-- support takeoff and landing on moving platforms for VTOL planes

local PARAM_TABLE_KEY = 7
local PARAM_TABLE_PREFIX = "SHIP_"

local MODE_MANUAL = 0
local MODE_RTL = 11
local MODE_QRTL = 21
local MODE_AUTO = 10
local MODE_QLOITER = 19

local NAV_TAKEOFF = 22
local NAV_VTOL_TAKEOFF = 84

local ALT_FRAME_ABSOLUTE = 0

-- 3 throttle position
local THROTTLE_LOW = 0
local THROTTLE_MID = 1
local THROTTLE_HIGH = 2

-- bind a parameter to a variable
function bind_param(name)
   local p = Parameter()
   assert(p:init(name), string.format('could not find %s parameter', name))
   return p
end

-- add a parameter and bind it to a variable
function bind_add_param(name, idx, default_value)
   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
   return bind_param(PARAM_TABLE_PREFIX .. name)
end

-- setup SHIP specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table')
SHIP_ENABLE     = bind_add_param('ENABLE', 1, 0)
SHIP_LAND_ANGLE = bind_add_param('LAND_ANGLE', 2, 0)
SHIP_AUTO_OFS   = bind_add_param('AUTO_OFS', 3, 0)

-- other parameters
RCMAP_THROTTLE  = bind_param("RCMAP_THROTTLE")
ALT_HOLD_RTL    = bind_param("ALT_HOLD_RTL")
Q_RTL_ALT       = bind_param("Q_RTL_ALT")
TRIM_ARSPD_CM   = bind_param("TRIM_ARSPD_CM")
TECS_LAND_ARSPD = bind_param("TECS_LAND_ARSPD")
Q_TRANS_DECEL   = bind_param("Q_TRANS_DECEL")
WP_LOITER_RAD   = bind_param("WP_LOITER_RAD")
RTL_RADIUS      = bind_param("RTL_RADIUS")
FOLL_OFS_X      = bind_param("FOLL_OFS_X")
FOLL_OFS_Y      = bind_param("FOLL_OFS_Y")
FOLL_OFS_Z      = bind_param("FOLL_OFS_Z")

-- an auth ID to disallow arming when we don't have the beacon
local auth_id = arming:get_aux_auth_id()
arming:set_aux_auth_failed(auth_id, "Ship: no beacon")

-- current target
local target_pos = Location()
local current_pos = Location()
local target_velocity = Vector3f()
local target_heading = 0.0

-- landing stages
local STAGE_HOLDOFF = 0
local STAGE_DESCEND = 1
local STAGE_APPROACH = 2
local STAGE_IDLE = 2
local landing_stage = STAGE_HOLDOFF

-- other state
local vehicle_mode = MODE_MANUAL
local reached_alt = false
local throttle_pos = THROTTLE_HIGH
local have_target = false

-- square a variable
function sq(v)
   return v*v
end

-- check key parameters
function check_parameters()
  --[[
     parameter values which are auto-set on startup
  --]]
   local key_params = {
      FOLL_ENABLE = 1,
      FOLL_OFS_TYPE = 1,
      FOLL_ALT_TYPE = 0,
   }

   for p, v in pairs(key_params) do
      local current = param:get(p)
      assert(current, string.format("Parameter %s not found", p))
      if math.abs(v-current) > 0.001 then
         param:set_and_save(p, v)
         gcs:send_text(0,string.format("Parameter %s set to %.2f was %.2f", p, v, current))
      end
   end
end

-- update the pilots throttle position
function update_throttle_pos()
   local tpos
   if not rc:has_valid_input() then
      tpos = THROTTLE_LOW
   else
      local tchan = rc:get_channel(RCMAP_THROTTLE:get())
      local tval = (tchan:norm_input_ignore_trim()+1.0)*0.5
      if tval >= 0.40 then
         tpos = THROTTLE_HIGH
      elseif tval >= 0.1 then
         tpos = THROTTLE_MID
      else
         tpos = THROTTLE_LOW
      end
   end
   if tpos ~= throttle_pos then
      reached_alt = false
      if landing_stage == STAGE_HOLDOFF and tpos <= THROTTLE_MID then
         landing_stage = STAGE_DESCEND
         gcs:send_text(0, string.format("Descending for approach (hd=%.1fm h=%.1f th=%.1f)",
                                        get_holdoff_distance(), current_pos:alt()*0.01, get_target_alt()))
      end
      if landing_stage == STAGE_DESCEND and tpos == THROTTLE_HIGH then
         gcs:send_text(0,"Climbing for holdoff")
         landing_stage = STAGE_HOLDOFF
      end
   end
   throttle_pos = tpos
end

-- get landing airspeed
function get_land_airspeed()
   if TECS_LAND_ARSPD:get() < 0 then
      return TRIM_ARSPD_CM:get() * 0.01
   end
   return TECS_LAND_ARSPD:get()
end

--[[
  calculate stopping distance assuming we are flying at
  TECS_LAND_ARSPD and are approaching the landing target from
  behind. Take account of the wind estimate to get approach
  groundspeed
--]]
function stopping_distance()
   -- get the target true airspeed for approach
   local tas = get_land_airspeed() * ahrs:get_EAS2TAS()

   -- add in wind in direction of flight
   local wind = ahrs:wind_estimate():xy()

   -- rotate wind to be in approach frame
   wind:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get()))

   -- ship velocity rotated to the approach frame
   local ship2d = target_velocity:xy()
   ship2d:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get()))

   -- calculate closing speed
   -- use pythagoras theorem to solve for the wind triangle
   local tas_sq = sq(tas)
   local y_sq = sq(wind:y())
   local closing_speed
   if tas_sq >= y_sq then
      closing_speed = math.sqrt(tas_sq - y_sq)
   else
      -- min 1 m/s
      closing_speed = 1.0
   end

   -- include the wind in the direction of the ship
   closing_speed = closing_speed + wind:x()

   -- account for the ship velocity
   closing_speed = closing_speed - ship2d:x()

   -- calculate stopping distance
   return sq(closing_speed) / (2.0 * Q_TRANS_DECEL:get())
end

-- get holdoff distance
function get_holdoff_radius()
   if RTL_RADIUS:get() ~= 0 then
      return RTL_RADIUS:get()
   end
   return WP_LOITER_RAD:get()
end

-- get holdoff distance
function get_holdoff_distance()
   local radius = get_holdoff_radius()
   local holdoff_dist = math.abs(radius*1.5)
   local stop_distance = stopping_distance()

   -- increase holdoff distance by up to 50% to ensure we can stop
   holdoff_dist = math.max(holdoff_dist, math.min(holdoff_dist*2.5, stop_distance*2))
   return holdoff_dist
end

-- get the holdoff position
function get_holdoff_position()
   local radius = get_holdoff_radius()
   local heading_deg = target_heading + SHIP_LAND_ANGLE:get()
   local holdoff_dist = get_holdoff_distance()

   local ofs = Vector2f()
   ofs:x(-holdoff_dist)
   ofs:y(radius)
   ofs:rotate(math.rad(heading_deg))
   local target = target_pos:copy()
   target:offset(ofs:x(), ofs:y())
   return target
end

function wrap_360(angle)
   local res = math.fmod(angle, 360.0)
    if res < 0 then
        res = res + 360.0
    end
    return res
end

function wrap_180(angle)
    local res = wrap_360(angle)
    if res > 180 then
       res = res - 360
    end
    return res
end

--[[
   check if we have reached the tangent to the landing location
--]]
function check_approach_tangent()
   local distance = current_pos:get_distance(target_pos)
   local holdoff_dist = get_holdoff_distance()
   if landing_stage == STAGE_HOLDOFF and throttle_pos <= THROTTLE_MID and distance < 4*holdoff_dist then
      gcs:send_text(0, string.format("Descending for approach (hd=%.1fm)", holdoff_dist))
      landing_stage = STAGE_DESCEND
   end
   if reached_alt and landing_stage == STAGE_DESCEND then
      -- go to approach stage when throttle is low, we are
      -- pointing at the ship and have reached target alt.
      -- Also require we are within 2.5 radius of the ship, and our heading is within 20
      -- degrees of the target heading
      local target_bearing_deg = wrap_180(math.deg(current_pos:get_bearing(target_pos)))
      local ground_bearing_deg = wrap_180(math.deg(ahrs:groundspeed_vector():angle()))
      local margin = 10
      local distance = current_pos:get_distance(target_pos)
      local holdoff_dist = get_holdoff_distance()
      local error1 = math.abs(wrap_180(target_bearing_deg - ground_bearing_deg))
      local error2 = math.abs(wrap_180(ground_bearing_deg - (target_heading + SHIP_LAND_ANGLE:get())))
      logger.write('SLND','TBrg,GBrg,Dist,HDist,Err1,Err2','ffffff',target_bearing_deg, ground_bearing_deg, distance, holdoff_dist, error1, error2)
      if (error1 < margin and
          distance < 2.5*holdoff_dist and
          distance > 0.7*holdoff_dist and
          error2 < 2*margin) then
         -- we are on the tangent, switch to QRTL
         gcs:send_text(0, "Starting approach")
         landing_stage = STAGE_APPROACH
         vehicle:set_mode(MODE_QRTL)
      end
   end
end

--[[
   check if we should abort a QRTL landing
--]]
function check_approach_abort()
   local alt = current_pos:alt() * 0.01
   local target_alt = get_target_alt()
   if alt > target_alt then
      gcs:send_text(0, "Aborting landing")
      landing_stage = STAGE_HOLDOFF
      vehicle:set_mode(MODE_RTL)
   end
end

-- update state based on vehicle mode
function update_mode()
   local mode = vehicle:get_mode()
   if mode == vehicle_mode then
      return
   end
   vehicle_mode = mode
   if mode == MODE_RTL then
      landing_stage = STAGE_HOLDOFF
      reached_alt = false
   elseif mode ~= MODE_QRTL then
      landing_stage = STAGE_IDLE
      reached_alt = false
   end
end

-- update target state
function update_target()
   if not follow:have_target() then
      if have_target then
         gcs:send_text(0,"Lost beacon")
         arming:set_aux_auth_failed(auth_id, "Ship: no beacon")
      end
      have_target = false
      return
   end
   if not have_target then
      gcs:send_text(0,"Have beacon")
      arming:set_aux_auth_passed(auth_id)
   end
   have_target = true

   target_pos, target_velocity = follow:get_target_location_and_velocity_ofs()
   target_heading = follow:get_target_heading_deg()
   -- zero vertical velocity to reduce impact of ship movement
   target_velocity:z(0)
end

-- get the alt target for holdoff, AMSL
function get_target_alt()
   local base_alt = target_pos:alt() * 0.01
   if landing_stage == STAGE_HOLDOFF then
      return base_alt + ALT_HOLD_RTL:get() * 0.01
   end
   return base_alt + Q_RTL_ALT:get()
end

function update_alt()
   local alt = current_pos:alt() * 0.01
   local target_alt = get_target_alt()
   if landing_stage == STAGE_HOLDOFF or landing_stage == STAGE_DESCEND then
      if math.abs(alt - target_alt) < 3 then
         if not reached_alt then
            gcs:send_text(0,"Reached target altitude")
         end
         reached_alt = true
      end
   end
end

--[[
  update automatic beacon offsets
--]]

function update_auto_offset()
   if arming:is_armed() or math.floor(SHIP_AUTO_OFS:get()) ~= 1 then
      return
   end

   -- get target without offsets applied
   target_no_ofs, vel = follow:get_target_location_and_velocity()

   -- setup offsets so target location will be current location
   local new = target_no_ofs:get_distance_NED(current_pos)
   new:rotate_xy(-math.rad(target_heading))

   gcs:send_text(0,string.format("Set follow offset (%.2f,%.2f,%.2f)", new:x(), new:y(), new:z()))
   FOLL_OFS_X:set_and_save(new:x())
   FOLL_OFS_Y:set_and_save(new:y())
   FOLL_OFS_Z:set_and_save(new:z())

   SHIP_AUTO_OFS:set_and_save(0)
end

-- main update function
function update()
   if SHIP_ENABLE:get() < 1 then
      return
   end

   update_target()
   if not have_target then
      return
   end

   current_pos = ahrs:get_position()
   if not current_pos then
      return
   end
   current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)

   update_throttle_pos()
   update_mode()
   update_alt()
   update_auto_offset()

   ahrs:set_home(target_pos)

   local next_WP = vehicle:get_target_location()
   if not next_WP then
      -- not in a flight mode with a target location
      return
   end

   if vehicle_mode == MODE_RTL then
      local holdoff_pos = get_holdoff_position()
      holdoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)
      holdoff_pos:alt(math.floor(get_target_alt()*100))
      vehicle:update_target_location(next_WP, holdoff_pos)

      if throttle_pos == THROTTLE_LOW then
         check_approach_tangent()
      end

   elseif vehicle_mode == MODE_QRTL then
      vehicle:set_velocity_match(target_velocity:xy())
      target_pos:alt(next_WP:alt())
      vehicle:update_target_location(next_WP, target_pos)

      if throttle_pos == THROTTLE_HIGH then
         check_approach_abort()
      end
      
   elseif vehicle_mode == MODE_AUTO then
      local id = mission:get_current_nav_id()
      if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then
         vehicle:set_velocity_match(target_velocity:xy())
         local tpos = current_pos:copy()
         tpos:alt(next_WP:alt())
         vehicle:update_target_location(next_WP, tpos)
      end

   elseif vehicle_mode == MODE_QLOITER then
      vehicle:set_velocity_match(target_velocity:xy())
   end

end

function loop()
   update()
   -- run at 20Hz
   return loop, 50
end

check_parameters()

-- wrapper around update(). This calls update() at 20Hz,
-- and if update faults then an error is displayed, but the script is not
-- stopped
function protected_wrapper()
  local success, err = pcall(update)
  if not success then
     gcs:send_text(0, "Internal Error: " .. err)
     -- when we fault we run the update function again after 1s, slowing it
     -- down a bit so we don't flood the console with errors
     return protected_wrapper, 1000
  end
  return protected_wrapper, 50
end

-- start running update loop
return protected_wrapper()