-- Adds a smart failsafe that accounts for how far the plane is from home
-- the average battery consumption, and the wind to decide when to failsafe
--
-- CAUTION: This script only works for Plane

-- store the batt info as { instance, filtered, capacity, margin_mah }
-- instance: the battery monitor instance (zero indexed)
-- filtered: internal variable for current draw
-- capacity: internal variable populated from battery monitor capacity param
-- margin_mah: the mah to be remaining when you reach home and use the time margin, note that this is on top of CRT_MAH
local batt_info = { {0, 0, 0, 0}, -- main battery
                                  -- add more batteries to monitor here
                  }

local margin = 30 -- margin in seconds of flight time that should remain once we have reached 
local time_SF = 1 -- the return time safety factor, 1.1 gives 10% extra time for return flight
local filter = 0.9 -- filter gain
local min_flying_time = 30 -- seconds, must have been flying in a none Qmode above the min alt for this long before script will start sampling current
local print_time = 15 -- seconds between update prints (set to zero to disable)
local alt_min = 0 -- the minimum altitude above home the script will start working at, zero disables

-- if true current draw is normalized with dynamic pressure
-- this gives better prediction of current draws at other airspeeds
-- airspeed sensor recommended
local airspeed_normalize = false

-- hard code wind to match SITL
-- should get exact return time estimates < +- 5 seconds
local SITL_wind = false


-- Read in required params
local value = param:get('TRIM_ARSPD_CM')
if value then
  air_speed = value / 100
else
  error('LUA: get TRIM_ARSPD_CM failed')
end
local value = param:get('ARSPD_FBW_MIN')
if value then
  min_air_speed = value
else
  error('LUA: get ARSPD_FBW_MIN failed')
end
local value = param:get('MIN_GNDSPD_CM')
if value then
  min_ground_speed = value / 100
else
  error('LUA: get MIN_GNDSPD_CM failed')
end
local value = param:get('LIM_ROLL_CD')
if value then
  max_bank_angle = value / 100
else
  error('LUA: get LIM_ROLL_CD failed')
end

-- https://en.wikipedia.org/wiki/Standard_rate_turn#Radius_of_turn_formula
-- the radius is equal to the circumference per 1 radian
local turn_rad = (air_speed^2) / (9.81 * math.tan(math.rad(max_bank_angle)))

-- Read the radius we expect to circle at when we get home
local home_reached_rad
local value = param:get('RTL_RADIUS')
if value then
  value = math.abs(value)
  if value > 0 then
    home_reached_rad = math.abs(value) * 2
  else 
    value = param:get('WP_LOITER_RAD')
    if value then
      home_reached_rad = math.abs(value) * 2
    else
      error('LUA: get WP_LOITER_RAD failed')
    end
  end
else
  error('LUA: get RTL_RADIUS failed')
end

-- internal global variables
local return_start
local return_distance
local return_amps
local trigger_instance = batt_info[1][1]
local last_print = 0
local timer_start_time = 0
local timer_active = true

-- calculates the amount of time it will take for the vehicle to return home
-- returns 0 if there is no position, wind or home available
-- returns a negative number if it will take excessively long time, or is impossible
-- otherwise returns the time in seconds to get back
local function time_to_home()
  local home = ahrs:get_home()
  local position = ahrs:get_position()
  local wind = ahrs:wind_estimate()

  if home and position and wind then
    local bearing = position:get_bearing(home)

    -- wind is in NED, convert for readability
    local wind_north = wind:x()
    local wind_east = wind:y()

    -- hard code wind for testing
    if SITL_wind then
      -- only safe to read from params at a high rate because we are in SITL
      -- don't do this on a real vehicle
      wind_speed = param:get('SIM_WIND_SPD')
      wind_dir = param:get('SIM_WIND_DIR')
      if wind_speed and wind_dir then
        wind_dir = math.rad(wind_dir)
        wind_north = -math.cos(wind_dir) * wind_speed
        wind_east = -math.sin(wind_dir) * wind_speed
      else
        error('Could not read SITL wind')
      end
    end
    --gcs:send_text(0, string.format("Wind: north %0.2f, east  %0.2f",wind_north,wind_east))

    -- rotate the wind vector inline with the home bearing
    local tail_wind =  math.sin(bearing) * wind_east + math.cos(bearing) * wind_north
    local cross_wind = math.cos(bearing) * wind_east + math.sin(bearing) * wind_north   -- left to right

    -- we can't get home
    if math.abs(cross_wind) > air_speed then
      return -1, air_speed -- FIXME: this should really be infinity
    end

    -- calculate the crab angle required to cancel out the cross wind
    local crab_angle = math.asin(-cross_wind / air_speed)

    -- the resulting speed in the desired direction
    local home_airspeed = air_speed * math.cos(crab_angle)

    local effective_speed = home_airspeed + tail_wind

    -- Estimate the extra distance required to turn
    local yaw = ahrs:get_yaw()
    -- this is the estimated angle we have to turn to be at the home bearing and crab angle
    local turn_angle_rad = bearing - crab_angle - yaw 
    -- wrap to +- PI
    if turn_angle_rad < math.rad(-180) then
      turn_angle_rad = turn_angle_rad + math.rad(360)
    elseif turn_angle_rad > math.rad(180) then
      turn_angle_rad = turn_angle_rad - math.rad(360)
    end
    --gcs:send_text(0, "turn " .. tostring(math.deg(turn_angle_rad)) .. " deg")

    -- Take into account min ground speed and resulting increased RTL airspeed
    local return_air_speed = air_speed
    if min_ground_speed > 0 and effective_speed < min_ground_speed then
      -- we travel home at the min ground speed 
      effective_speed = min_ground_speed

      -- work out the resulting airspeed
      return_air_speed = math.sqrt((effective_speed-tail_wind)^2 + cross_wind^2)
    end

    -- distance to travel over ground speed + turn circumference over airspeed
    return (position:get_distance(home) / effective_speed) + (math.abs(turn_angle_rad*turn_rad) / air_speed), return_air_speed
  end

  return 0, air_speed -- nothing useful available
end

-- idle function
function idle()
  -- if disarmed and not flying reset for a potential second trigger
  if not arming:is_armed() and not vehicle:get_likely_flying() then
    return update, 100
  end
  return idle, 1000
end

-- time margin update function
function margin_update()
  if not vehicle:get_likely_flying() then
    -- no longer flying, idle function
    return idle, 10000
  end

  -- display the remaining battery capacity on the triggered monitor
  local capacity = battery:pack_capacity_mah(trigger_instance) 
  local consumed = battery:consumed_mah(trigger_instance)
  if capacity and consumed then
    gcs:send_text(0, string.format("Failsafe: %is margin elapsed %.2fmAh remain",margin, capacity - consumed))
  end

  -- idle function
  return idle, 10000
end

-- this is an alternate update function that is simply used to track how long it will take to get home
-- it's really only used for debugging how the prediction rules are working
function track_return_time()
  if not vehicle:get_likely_flying() then
      -- no longer flying, idle function
      return idle, 10000
  end

  local home = ahrs:get_home()
  local position = ahrs:get_position()
  if home and position then
    local now = millis()

    local home_dist = position:get_distance(home)
    if home_dist < home_reached_rad then
      -- calculate the extra time to fly the reached rad distance
      local time_home = time_to_home()
      local total_time = time_home + ((now-return_start)/1000)

      -- display the remaining battery capacity on the triggered monitor
      local capacity = battery:pack_capacity_mah(trigger_instance) 
      local consumed = battery:consumed_mah(trigger_instance)
      if capacity and consumed then
        -- estimate the extra capacity used for the reached rad distance
        return_capacity = return_amps * time_home  * (1000 / 60^2) -- convert from amp second's to mAh
        
        gcs:send_text(0, "Failsafe: RTL took " .. tostring(total_time) .. string.format("s, %.2fmAh remain", capacity - consumed - return_capacity) )

        if margin > 0 then
          return margin_update, margin*1000
        end
      else
        gcs:send_text(0, "Failsafe: RTL took " .. tostring(total_time) .. " s")
      end

      -- idle function
      return idle, 10000
    end

    -- print updates tracking progress
    local return_time = time_to_home()
    local total_time = return_time + ((now-return_start)/1000)
    if last_print + (print_time * 1000) < now and print_time > 0 then
      last_print = now
      if (return_time < 0) then
        gcs:send_text(6, "Failsafe: ground speed low can not get home")
      elseif (return_time > 0) then
        -- cannot get string.format() to work with total time, wrong variable type? ie not %f or %i?
        gcs:send_text(0, "Failsafe: Estimated " .. tostring(total_time) .. string.format("s, %.0fs remain", return_time) )
      end
    end

    logger.write('SFSC','total_return_time,remaining_return_time','If','ss','--',total_time,return_time)
  end
  return track_return_time, 100
end

-- the main update function that is used to decide when we should do a failsafe
function update()
  local now = millis();

  -- check armed
  if not arming:is_armed() then
    --gcs:send_text(0, "Failsafe: disabled: not armed")
    timer_start_time = now
    timer_active = true
    return update, 100
  end

  -- check flying
  if not vehicle:get_likely_flying() then
    --gcs:send_text(0, "Failsafe: disabled: not flying")
    timer_start_time = now
    timer_active = true
    return update, 100
  end

  -- check mode
  local current_mode = vehicle:get_mode()
  if current_mode >= 17 then
    --gcs:send_text(0, "Failsafe: disabled: Q mode")
    timer_start_time = now
    timer_active = true
    return update, 100
  end

  -- check altitude
  if alt_min ~= 0 then
    local dist = ahrs:get_relative_position_NED_home()
    if not dist or -1*dist:z() < alt_min then
      --gcs:send_text(0, "Failsafe: disabled: low alt")
      timer_start_time = now
      timer_active = true
      return update, 100
    end
  end

  -- check timer
  if now - timer_start_time < (min_flying_time * 1000) then
    --gcs:send_text(0, "Failsafe: disabled: timer")
    return update, 100
  end

  -- notify that we have started
  if timer_active then
    gcs:send_text(0, "Smart Battery RTL started monitoring")
    timer_active = false
  end

  -- check airspeed
  local air_speed_in = ahrs:airspeed_estimate()
  if not air_speed_in then
    error("Could not read airspeed")
  end
  if air_speed_in < min_air_speed * 0.75 then
    -- we are not flying fast enough, skip but don't reset the timer
    return update, 100
  end

  local min_remaining_time = 86400 -- 24 hours

  -- find the return time and airspeed
  local return_time, return_airspeed = time_to_home()

  -- default to no normalization
  local q = 1
  local return_q = 1

  -- normalize current with dynamic pressure
  if airspeed_normalize then

    -- we could probably just use air speed^2
    local press = baro:get_pressure()
    local temp = baro:get_external_temperature() + 273.2 -- convert deg centigrade to kelvin
    local density =  press / (temp * 287.058) -- calculate the air density, ideal gas law, constant is (R) specific gas constant for air
    q = 0.5 * density * air_speed_in^2

    return_q = 0.5 * density * return_airspeed^2 -- we could estimate the change in density also, but will be negligible
  end

  logger.write('SFSA','return_time,return_airspeed,Q,return_Q','ffff','snPP','----',return_time,return_airspeed,q,return_q)

  for i = 1, #batt_info do
    local instance, norm_filtered_amps, rated_capacity_mah = table.unpack(batt_info[i])
    local amps = battery:current_amps(instance)
    local consumed_mah = battery:consumed_mah(instance)

    if amps and consumed_mah then

      local norm_amps = amps / q

      -- update all the current consumption rates
      norm_filtered_amps = (norm_filtered_amps * filter) + (norm_amps * (1.0 - filter))
      batt_info[i][2] = norm_filtered_amps

      -- calculate the estimated return amps, estimate the return current if we were to fly at a different airspeed
      return_amps = norm_filtered_amps * return_q

      local remaining_capacity = (rated_capacity_mah - consumed_mah) * 3.6 -- amp seconds (60^2 / 1000)
      local remaining_time = remaining_capacity / return_amps

      local buffer_time = remaining_time - ((return_time * time_SF) + margin)
      logger.write('SFSB','Instance,current,rem_cap,rem_time,buffer','Bffff','#Aiss','--C--',i-1,return_amps,remaining_capacity,remaining_time,buffer_time)
      if  (return_time < 0) or buffer_time < 0 then
        if return_time < 0 then
          gcs:send_text(0, "Failsafe: ground speed low can not get home")
        elseif #batt_info == 1 then
          gcs:send_text(0, string.format("Failsafe: Estimated %.0fs to home", return_time))
        else
          trigger_instance = instance
          gcs:send_text(0, string.format("Failsafe: Estimated %.0fs to home, instance %i", return_time, instance))
        end

        last_print = now
        -- FIXME: We need more insight into what the vehicles already doing. IE don't trigger RTL if we are already landing
        vehicle:set_mode(11) -- plane RTL FIXME: we need a set of enums defined for the vehicles
        -- swap to tracking the time rather then re trigger
        return_start = now

        -- Print the return distance
        --[[local home = ahrs:get_home()
        local position = ahrs:get_position()
        if home and position then
          return_distance = position:get_distance(home)
        end
        gcs:send_text(0, string.format("Failsafe: %.0f m to home", return_distance))]]

        -- print the current draw we estimate
        --gcs:send_text(0, string.format("Failsafe: %.2fa", return_amps))

        return track_return_time, 100
      end
      min_remaining_time = math.min(min_remaining_time, buffer_time)
    end
  end

  -- print updates tracking progress
  if last_print + (print_time * 1000) < now and print_time > 0 then
    last_print = now
    gcs:send_text(6, string.format("%.0f seconds of flight remaining before RTL", min_remaining_time))
  end

  return update, 100
end

-- validate that all the expected monitors have current monitoring capability, and fetch initial values
for i = 1, #batt_info do
  -- check that the instance exists
  local instance = batt_info[i][1]
  if instance > battery:num_instances() then
    error("Battery " .. instance .. " does not exist")
  end
  -- check that we can actually read current from the instance
  if not battery:current_amps(instance) then
    error("Battery " .. instance .. " does not support current monitoring")
  end
  -- store the pack capacity for later use, it's assumed to never change mid flight
  -- subtract the capacity we want remaining when we get home
  local rated_cap = battery:pack_capacity_mah(instance)
  if rated_cap then
    -- read in the critical MAH
    local param_string = 'BATT' .. tostring(instance + 1) .. '_CRT_MAH'
    if instance == 0 then
      param_string = 'BATT_CRT_MAH'
    end

    local value = param:get(param_string)
    if  not value then
      error('LUA: get '.. param_string .. ' failed')
    end

    batt_info[i][3] = rated_cap - (batt_info[i][4] + value)
  else
    error("Battery " .. instance .. " does not support current monitoring")
  end
end

return update, 100