-- mount-poi.lua: finds the point-of-interest that the gimbal mount is pointing at using the vehicle's current Location, mount attitude and terrain database
--
-- How To Use
--   1. set RCx_OPTION to 300 to enable triggering the POI calculation from an auxiliary switch
--   2. optionally set POI_DIST_MAX to the maximum distance (in meters) that the POI point could be from the vehicle
--   3. fly the vehicle and point the camera gimbal at a point on the ground
--   4. raise the RC auxiliary switch and check the GCS's messages tab for the latitude, longitude and alt (above sea-level)
--
-- How It Works
--   1. retrieve the POI_DIST_MAX and TERRAIN_SPACING param values
--   2. get the vehicle Location (lat, lon, height above sea-level), initialise test-loc and prev-test-loc
--   3. get the vehicle's current alt-above-terrain
--   4. get gimbal attitude (only pitch and yaw are used)
--   5. "test_loc" is initialised to the vehicle's location
--   6. "prev_test_loc" is a backup of test_loc
--   7. test_loc is moved along the line defined by the gimbal's pitch and yaw by TERRAIN_SPACING (meters)
--   8. retrieve the terrain's altitude (above sea-level) at the test_loc
--   9. repeat step 6, 7 and 8 until the test_loc's altitude falls below the terrain altitude
--  10. interpolate between test_loc and prev_test_loc to find the lat, lon, alt (above sea-level) where alt-above-terrain is zero
--  11. display the POI to the user

-- global definitions
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local ALT_FRAME_ABSOLUTE = 0
local UPDATE_INTERVAL_MS = 100

-- add new param POI_DIST_MAX
local PARAM_TABLE_KEY = 78
assert(param:add_table(PARAM_TABLE_KEY, "POI_", 1), "could not add param table")
assert(param:add_param(PARAM_TABLE_KEY, 1, "DIST_MAX", 10000), "could not add POI_DIST_MAX param")

--[[
  // @Param: POI_DIST_MAX
  // @DisplayName: Mount POI distance max
  // @Description: POI's max distance (in meters) from the vehicle
  // @Range: 0 10000
  // @User: Standard
--]]
local POI_DIST_MAX = Parameter("POI_DIST_MAX")

-- bind to other parameters this script depends upon
TERRAIN_SPACING = Parameter("TERRAIN_SPACING")

-- local variables and definitions
local last_rc_switch_pos = 0            -- last known rc switch position.  Used to detect change in RC switch position

-- helper functions
function wrap_360(angle_deg)
  local res = math.fmod(angle_deg, 360.0)
  if res < 0 then
    res = res + 360.0
  end
  return res
end

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

function swap_float(f1, f2)
  return f2, f1
end

function interpolate(low_output, high_output, var_value, var_low, var_high)
  -- support either polarity
  if (var_low > var_high) then
    var_low, var_high = swap_float(var_low, var_high)
    low_output, high_output = swap_float(low_output, high_output)
  end
  if (var_value <= var_low) then
    return low_output
  end
  if (var_value > var_high) then
    return high_output
  end
  local p = (var_value - var_low) / (var_high - var_low)
  return (low_output + p * (high_output - low_output))
end


-- the main update function that performs a simplified version of RTL
function update()

  -- find RC channel used to trigger POI
  rc_switch_ch = rc:find_channel_for_option(300) --scripting ch 1
  if (rc_switch_ch == nil) then
      gcs:send_text(MAV_SEVERITY.ERROR, 'MountPOI: RCx_OPTION = 300 not set')    -- MAV_SEVERITY_ERROR
      return update, 10000  -- check again in 10 seconds
  end

  -- check if user has raised RC switch
  local rc_switch_pos = rc_switch_ch:get_aux_switch_pos()
  if rc_switch_pos == last_rc_switch_pos then
    return update, UPDATE_INTERVAL_MS
  end

  -- switch has changed position
  last_rc_switch_pos = rc_switch_pos
  if rc_switch_pos ~= 2 then
    return update, UPDATE_INTERVAL_MS
  end

  -- POI has been requested

  -- retrieve vehicle location
  local vehicle_loc = ahrs:get_location()
  if vehicle_loc == nil then
    gcs:send_text(MAV_SEVERITY.ERROR, "POI: vehicle pos unavailable") -- MAV_SEVERITY_ERROR
    return update, UPDATE_INTERVAL_MS
  end
  
  -- change vehicle location to ASML
  vehicle_loc:change_alt_frame(ALT_FRAME_ABSOLUTE)

  -- retrieve gimbal attitude
  local _, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0)
  if pitch_deg == nil or yaw_bf_deg == nil then
    gcs:send_text(MAV_SEVERITY.ERROR, "POI: gimbal attitude unavailable") -- MAV_SEVERITY_ERROR
    return update, UPDATE_INTERVAL_MS
  end

  -- project forward from vehicle looking for terrain
  -- start testing at vehicle's location
  local test_loc = vehicle_loc:copy()
  local prev_test_loc = test_loc:copy()

  -- get terrain altitude (asml) at test_loc
  local terrain_amsl_m = terrain:height_amsl(test_loc, true)  -- terrain alt (above amsl) at test_loc
  local prev_terrain_amsl_m = terrain_amsl_m    -- terrain alt (above amsl) at prev_test_loc

  -- fail if terrain alt cannot be retrieved
  if terrain_amsl_m == nil then
    gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR
    return update, UPDATE_INTERVAL_MS
  end

  -- get gimbal mount's pitch and yaw
  local mount_pitch_deg = pitch_deg
  local mount_yaw_ef_deg = wrap_180(yaw_bf_deg + math.deg(ahrs:get_yaw()))
  local dist_increment_m = TERRAIN_SPACING:get()

  -- initialise total distance test_loc has moved
  local total_dist = 0
  local dist_max = POI_DIST_MAX:get()

  -- iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
  while (total_dist < dist_max) and ((test_loc:alt() * 0.01) > terrain_amsl_m) do
    total_dist = total_dist + dist_increment_m

    -- take backup of previous test location and terrain asml
    prev_test_loc = test_loc:copy()
    prev_terrain_amsl_m = terrain_amsl_m

    -- move test location forward
    test_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m)

    -- get terrain's alt-above-sea-level (at test_loc)
    terrain_amsl_m = terrain:height_amsl(test_loc, true)

    -- fail if terrain alt cannot be retrieved
    if terrain_amsl_m == nil then
      gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR
      return update, UPDATE_INTERVAL_MS
    end
  end

  -- check for errors
  if (total_dist >= dist_max) then
    gcs:send_text(MAV_SEVERITY.ERROR, "POI: unable to find terrain within " .. tostring(dist_max) .. " m")
  elseif not terrain_amsl_m then
    gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to retrieve terrain alt")
  else
    -- test location has dropped below terrain
    -- interpolate along line between prev_test_loc and test_loc
    local dist_interp_m = interpolate(0, dist_increment_m, 0, prev_test_loc:alt() * 0.01 - prev_terrain_amsl_m, test_loc:alt() * 0.01 - terrain_amsl_m)
    local poi_loc = prev_test_loc:copy()
    poi_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m)
    gcs:send_text(MAV_SEVERITY.INFO, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01))
  end

  return update, UPDATE_INTERVAL_MS
end

return update()