--[[
solar_hop.lua  v2.2.1  -  land / solar-charge / resume state machine for link-free,
multi-day ArduCopter missions (store-and-forward data-logging platform).

Target: ArduCopter 4.5+ (Lua scripting).  Validated in SITL (Copter-4.5); not
flown. Follow the README test sequence before any link-free flight.

v2.2 closes three SITL findings on v2.1:
  S1  ArduPilot's bundled Lua limits a chunk to 100 local variables (stock Lua
      allows 200). v2.1 declared 123 and never loaded; with ARMING_CHECK on,
      the failed load also blocked arming. Tuning now lives in one CFG table
      and runtime state in one V table.
  S2  Copter-4.5 never reports MISSION_COMPLETE after a NAV_LAND: the landing
      disarms, mission.reset() runs, and the mission restarts at item 1 while
      disarmed. v2.1 therefore never reached DONE, and a reboot at the
      recovery point relaunched to re-fly the last legs. Completion is now
      detected from the last nav command seen while armed (NAV_LAND at the
      final index) followed by a disarm.
  S3  The divert decision was a fixed percentage with no regard to distance:
      with a small pack it ran out 150 m into a 700 m leg. The divert is now
      energy-aware: it triggers when the remaining energy above the floor is
      no longer enough to reach the nearest perch with margin, and a perch
      that cannot be reached is not attempted (land in place instead).
v2.1 closed ten review findings on v2 (README, Review record).

Behaviour
  FLY       Auto mission runs. Diverts when SOC <= LOW_PCT or when the nearest
            perch is about to become unreachable; waypoint index is saved.
  DIVERT    GUIDED, terrain-relative, to the nearest reachable perch.
  LANDING   LAND until touchdown and auto-disarm.
  CHARGING  Companion off, PV on inside the cell charge window, periodic
            resting-voltage SOC resync. Wait for SOC >= RESUME_PCT.
  CHECK     Temperature, tilt, daylight, launch window, GNSS, AHRS, pre-arm,
            mission audit, hop limits, HOLD flag. Companion booted. HOME = perch.
  CLIMB     GUIDED takeoff above the perch, then AUTO at the saved waypoint.
  PAUSED    A human or detect-and-avoid has the aircraft; wait.
  DONE      Mission complete or operator stop; charge-manage, never relaunch.

Parameters, wiring, mission rules and the test sequence are in README.md.
Persistent flags: SCR_USER1 saved waypoint (-1 = mission complete),
SCR_USER2 mission fingerprint, SCR_USER3 hop count, SCR_USER4 = 1 HOLD.
]]

------------------------------------------------------------------------------
-- Tuning (one table: ArduPilot Lua allows at most 100 locals per chunk)
------------------------------------------------------------------------------
local CFG = {
  LOW_PCT             = 30,     -- divert/land at or below this SOC regardless of distance
  RESUME_PCT          = 85,
  TAKEOFF_ALT_M       = 30,
  DIVERT_AGL_M        = 40,     -- divert leg height above terrain
  DIVERT_MAX_M        = 1500,   -- never consider a perch farther than this
  DIVERT_FLOOR_PCT    = 20,     -- energy below this is reserved for the landing
  DIVERT_RESERVE_S    = 60,     -- time allowance for the landing itself
  DIVERT_MARGIN       = 1.3,    -- required energy x margin must fit above the floor
  DIVERT_MIN_POWER_W  = 150,    -- floor on the measured power used in the estimate
  DIVERT_SPEED_MS     = 8,      -- fallback if WPNAV_SPEED is unreadable
  ARRIVE_M            = 5,
  DIVERT_TIMEOUT_MS   = 6 * 60 * 1000,
  LANDING_TIMEOUT_MS  = 5 * 60 * 1000,

  LAUNCH_TEMP_MIN_C   = 5,
  LAUNCH_TEMP_MAX_C   = 45,
  CHARGE_TEMP_MIN_C   = 0,      -- Li-ion NMC charge window per the cell datasheet
  CHARGE_TEMP_MAX_C   = 45,
  TEMP_REQUIRED       = true,   -- nil temperature blocks charging and launch
  TEMP_FALLBACK_BARO  = false,  -- if not required: board temperature minus margin
  TEMP_FALLBACK_MARGIN_C = 5,
  MAX_TILT_RAD        = 0.35,

  CHARGE_STALL_MS     = 6 * 3600 * 1000,
  OCV_SAMPLE_MS       = 45 * 60 * 1000,  -- resting-voltage resync interval
  OCV_SETTLE_MS       = 3 * 60 * 1000,   -- PV open this long before the read
  OCV_RETRY_MS        = 60 * 1000,
  REST_I_A            = 0.3,

  LAUNCH_WINDOW       = { start = 8.0, stop = 13.0 },  -- local solar hours; nil = off

  BATT_INST           = 0,
  SOLAR_INST          = 1,      -- -1 = none (pack current used for daylight)
  DAYLIGHT_PV_V       = 10.0,
  DAYLIGHT_A          = 0.1,
  CELLS               = 6,
  CELL_NOMINAL_V      = 3.6,

  RELAY_COMP_PWR      = 0,      -- ON = companion powered
  RELAY_PV            = 1,      -- ON = PV connected to the MPPT
  RELAY_COMP_SHDN     = 2,      -- ON = shutdown request to companion GPIO
  COMP_SHDN_WAIT_MS   = 30 * 1000,
  COMP_BOOT_WAIT_MS   = 60 * 1000,
  COMPANION_IN_FLIGHT = true,
  COMPANION_SITE_SELECT   = false,
  SITE_SELECT_TIMEOUT_MS  = 3 * 60 * 1000,

  HOP_MAX             = 60,
  SAME_IDX_MAX        = 3,
  SITL_TEST           = false,  -- bypass daylight, window, temperature; flight = false
}

-- Resting V/cell -> %, generic NMC 21700; replace with a measured table
local OCV = {
  {3.00,  0}, {3.30,  5}, {3.45, 10}, {3.55, 20}, {3.62, 30}, {3.68, 40},
  {3.75, 50}, {3.84, 60}, {3.92, 70}, {4.00, 80}, {4.08, 90}, {4.15, 97}, {4.20, 100},
}

-- Preplanned perches, decimal degrees
local LAND_PTS = {
  -- {lat = 47.674000, lng = -122.121000},
}

------------------------------------------------------------------------------
-- Constants, state
------------------------------------------------------------------------------
local M = { AUTO = 3, GUIDED = 4, LAND = 9, AVOID_ADSB = 19,
            NAV_LAND = 21, NAV_RTL = 20, FRAME_RELATIVE = 3, ALT_ABOVE_TERRAIN = 3,
            SEV_ERR = 3, SEV_WARN = 4, SEV_INFO = 6 }

local S = { FLY = 1, DIVERT = 2, SITE_SELECT = 3, LANDING = 4, CHARGING = 5,
            CHECK = 6, CLIMB = 7, DONE = 8, PAUSED = 9 }
local SNAME = { "FLY", "DIVERT", "SITE_SELECT", "LANDING", "CHARGING",
                "CHECK", "CLIMB", "DONE", "PAUSED" }

local V = {
  state = S.FLY, state_t = 0,
  target = nil, prev_mode = -1,
  ref_pct = 0, ref_t = 0,
  ground_alt_cm = nil,
  next_ocv_t = 0, ocv_phase = 0, ocv_open_t = 0, last_ocv_pct = nil, ocv_fail_n = 0,
  comp_state = "unknown", comp_t = 0,
  last_resume_idx = -1, same_idx_n = 0,
  pv_inhibited = false,
  mission_ok = true, mission_why = "",
  fly_seen = false, fly_nav_idx = 0, fly_nav_id = 0,
  p_ema = nil,                   -- smoothed flight power (W) for the divert model
}

local function msg(sev, t) gcs:send_text(sev, "HOP: " .. t) end
local function info(t) msg(M.SEV_INFO, t) end
local function now_ms() return millis():toint() end

local function set_state(s, why)
  V.state, V.state_t = s, now_ms()
  info(SNAME[s] .. ": " .. why)
end

local function try(f, ...)
  local ok, v = pcall(f, ...)
  if ok then return v end
  return nil
end

local get_roll, get_pitch
do
  local ok = pcall(function() return ahrs:get_roll_rad() end)
  if ok then
    get_roll  = function() return ahrs:get_roll_rad() end
    get_pitch = function() return ahrs:get_pitch_rad() end
  else
    get_roll  = function() return ahrs:get_roll() end
    get_pitch = function() return ahrs:get_pitch() end
  end
end

-- Persistence
local function pget(n) return param:get(n) or 0 end
local function pset(n, v) param:set_and_save(n, v) end
local function saved_idx() return math.floor(pget('SCR_USER1') + 0.5) end
local function save_idx(i) pset('SCR_USER1', i) end
local function mission_fp() return mission:num_commands() or 0 end
local function saved_fp() return math.floor(pget('SCR_USER2') + 0.5) end
local function save_fp() pset('SCR_USER2', mission_fp()) end
local function hops() return math.floor(pget('SCR_USER3') + 0.5) end
local function add_hop() pset('SCR_USER3', hops() + 1) end
local function hold_flag() return pget('SCR_USER4') >= 1 end
local function set_hold(why)
  pset('SCR_USER4', 1)
  msg(M.SEV_WARN, "HOLD set (" .. why .. "); clear SCR_USER4 to allow launch")
end

-- Relays
local function relay_set(idx, on) if on then relay:on(idx) else relay:off(idx) end end
local function pv_connect(on) relay_set(CFG.RELAY_PV, on) end

-- Battery
local function pack_v() return battery:voltage(CFG.BATT_INST) or 0 end
local function pack_i() return battery:current_amps(CFG.BATT_INST) or 0 end   -- + discharge, - charge
local function pack_wh() return pget('BATT_CAPACITY') / 1000 * CFG.CELLS * CFG.CELL_NOMINAL_V end

local function pack_temp()
  local t = try(function() return battery:get_temperature(CFG.BATT_INST) end)
  if t ~= nil then return t, "cell" end
  if not CFG.TEMP_REQUIRED and CFG.TEMP_FALLBACK_BARO then
    local b = try(function() return baro:get_temperature() end)
    if b ~= nil then return b - CFG.TEMP_FALLBACK_MARGIN_C, "board" end
  end
  return nil, "none"
end

local function ocv_to_pct(v_cell)
  if v_cell <= OCV[1][1] then return OCV[1][2] end
  for i = 2, #OCV do
    local v0, p0, v1, p1 = OCV[i-1][1], OCV[i-1][2], OCV[i][1], OCV[i][2]
    if v_cell <= v1 then return p0 + (p1 - p0) * (v_cell - v0) / (v1 - v0) end
  end
  return 100
end

local function soc()
  local p = battery:capacity_remaining_pct(CFG.BATT_INST) or 0
  if V.last_ocv_pct and p > V.last_ocv_pct + 25 then p = V.last_ocv_pct end
  return math.floor(p + 0.5)
end

local function ocv_gate_ok()
  if V.comp_state ~= "off" then return false, "companion on" end
  if math.abs(pack_i()) > CFG.REST_I_A then return false, string.format("I %.2f A", pack_i()) end
  return true, ""
end

local function resync_from_ocv()
  local v_cell = pack_v() / CFG.CELLS
  local p = math.floor(ocv_to_pct(v_cell) + 0.5)
  V.last_ocv_pct = p
  local ok = try(function() return battery:reset_remaining(CFG.BATT_INST, p) end)
  info(string.format("OCV %.3f V/cell -> SOC %d%% (%s)", v_cell, p, ok and "resync" or "no resync"))
  V.ocv_fail_n = 0
  return p
end

local function daylight()
  if CFG.SITL_TEST then return true end
  if CFG.SOLAR_INST >= 0 then
    local pv_v = battery:voltage(CFG.SOLAR_INST) or 0
    local pv_i = battery:current_amps(CFG.SOLAR_INST) or 0
    return pv_v > CFG.DAYLIGHT_PV_V or pv_i > CFG.DAYLIGHT_A
  end
  return pack_i() < -CFG.DAYLIGHT_A
end

local function solar_hour(here)
  if CFG.SITL_TEST or not here or gps:status(0) < 3 then return nil end
  local tow_ms = try(function() return gps:time_week_ms(0):toint() end)
  if not tow_ms then return nil end
  local utc_s = ((tow_ms / 1000) - 18) % 86400
  return ((utc_s / 3600) + (here:lng() / 1e7) / 15) % 24
end

local function in_launch_window(here)
  if not CFG.LAUNCH_WINDOW then return true, "" end
  local h = solar_hour(here)
  if not h then return true, "" end
  return (h >= CFG.LAUNCH_WINDOW.start and h <= CFG.LAUNCH_WINDOW.stop), string.format("solar %.1fh", h)
end

local function manage_pv()
  local t, src = pack_temp()
  local inhibit
  if t == nil then inhibit = CFG.TEMP_REQUIRED
  else inhibit = (t < CFG.CHARGE_TEMP_MIN_C or t > CFG.CHARGE_TEMP_MAX_C) end
  if inhibit ~= V.pv_inhibited then
    V.pv_inhibited = inhibit
    if inhibit then
      msg(M.SEV_WARN, t and string.format("PV inhibited, cell %.0f C (%s)", t, src) or "PV inhibited, no temperature")
    else info("PV enabled") end
  end
  if V.ocv_phase == 0 then pv_connect(not inhibit) end
end

-- Companion
local function companion_on()
  relay_set(CFG.RELAY_COMP_SHDN, false)
  relay_set(CFG.RELAY_COMP_PWR, true)
  V.comp_state, V.comp_t = "on", now_ms()
end

local function companion_begin_shutdown()
  if V.comp_state == "off" or V.comp_state == "shutting" then return end
  info("SHUTDOWN")
  relay_set(CFG.RELAY_COMP_SHDN, true)
  V.comp_state, V.comp_t = "shutting", now_ms()
end

local function companion_service()
  if V.comp_state == "shutting" and now_ms() - V.comp_t > CFG.COMP_SHDN_WAIT_MS then
    relay_set(CFG.RELAY_COMP_PWR, false)
    relay_set(CFG.RELAY_COMP_SHDN, false)
    V.comp_state = "off"
    info("companion off")
  end
end

local function log_tick(pct, idx)
  local pv_v = (CFG.SOLAR_INST >= 0) and (battery:voltage(CFG.SOLAR_INST) or 0) or 0
  local pv_i = (CFG.SOLAR_INST >= 0) and (battery:current_amps(CFG.SOLAR_INST) or 0) or 0
  local t = pack_temp() or -99
  logger:write('HOP', 'St,Pct,V,I,PVv,PVi,T,Idx,Hop', 'BffffffHH',
               V.state, pct, pack_v(), pack_i(), pv_v, pv_i, t, math.max(idx or 0, 0), hops())
end

-- Mission audit: refuse HOME-relative nav altitudes (HOME moves to each perch)
local function audit_mission()
  local n = mission_fp()
  if n < 2 then return false, "no mission" end
  for i = 1, n - 1 do
    local it = try(function() return mission:get_item(i) end)
    if it then
      local cmd, frame = it:command(), it:frame()
      local is_nav = (cmd >= 16 and cmd <= 31) or cmd == 82 or cmd == 84 or cmd == 85
      if is_nav and frame == M.FRAME_RELATIVE then return false, string.format("wp %d uses relative alt", i) end
    end
  end
  local last = try(function() return mission:get_item(n - 1) end)
  if last and last:command() ~= M.NAV_LAND then return false, "last cmd not NAV_LAND" end
  return true, ""
end

-- Divert energy model: can we reach `loc` and still have the floor left?
local function divert_energy(here, loc, pct)
  local d = here:get_distance(loc)
  local v = pget('WPNAV_SPEED') / 100
  if v < 1 then v = CFG.DIVERT_SPEED_MS end
  local t_s = d / v + CFG.DIVERT_RESERVE_S
  local p_w = math.max(V.p_ema or (pack_i() * pack_v()), CFG.DIVERT_MIN_POWER_W)
  local need = p_w * t_s / 3600 * CFG.DIVERT_MARGIN
  local avail = math.max(pct - CFG.DIVERT_FLOOR_PCT, 0) / 100 * pack_wh()
  return d, need, avail
end

-- Nearest perch that is both within range and reachable on the energy model
local function nearest_perch(here, pct)
  local best, best_d, best_why = nil, CFG.DIVERT_MAX_M, ""
  local any_d = nil
  for _, p in ipairs(LAND_PTS) do
    local loc = here:copy()
    loc:lat(math.floor(p.lat * 1e7 + 0.5))
    loc:lng(math.floor(p.lng * 1e7 + 0.5))
    local d, need, avail = divert_energy(here, loc, pct)
    if d < CFG.DIVERT_MAX_M and (not any_d or d < any_d) then any_d = d end
    if d < best_d and need <= avail then
      best, best_d, best_why = loc, d, string.format("%.0f m, need %.1f Wh, have %.1f Wh", d, need, avail)
    end
  end
  return best, best_why, any_d
end

-- True when the nearest in-range perch is about to stop being reachable
local function divert_now(here, pct)
  if pct <= CFG.LOW_PCT then return true, string.format("low %d%%", pct) end
  local nearest, nearest_d = nil, CFG.DIVERT_MAX_M
  for _, p in ipairs(LAND_PTS) do
    local loc = here:copy()
    loc:lat(math.floor(p.lat * 1e7 + 0.5))
    loc:lng(math.floor(p.lng * 1e7 + 0.5))
    local d = here:get_distance(loc)
    if d < nearest_d then nearest, nearest_d = loc, d end
  end
  if nearest then
    local d, need, avail = divert_energy(here, nearest, pct)
    if need <= avail and need > avail * 0.85 then
      return true, string.format("%d%%, perch at %.0f m reachable only now", pct, d)
    end
  end
  return false, ""
end

-- Send the divert target: terrain frame first, absolute fallback; false = refused
local function send_divert()
  if not V.target then return false end
  local t = V.target:copy()
  local ok = try(function() return t:change_alt_frame(M.ALT_ABOVE_TERRAIN) end)
  if ok then
    t:alt(CFG.DIVERT_AGL_M * 100)
    if vehicle:set_target_location(t) then return true end
  end
  local a = V.target:copy()
  local th = try(function() return terrain:height_amsl(a, true) end)
  if th and math.floor((th + CFG.DIVERT_AGL_M) * 100) > a:alt() then a:alt(math.floor((th + CFG.DIVERT_AGL_M) * 100)) end
  return vehicle:set_target_location(a)
end

local function mode_is(m, ...)
  for _, x in ipairs({...}) do if m == x then return true end end
  return false
end

------------------------------------------------------------------------------
-- State machine
------------------------------------------------------------------------------
local function enter_landing(why)
  vehicle:set_mode(M.LAND)
  set_state(S.LANDING, why)
end

local function enter_charging(pct, why)
  V.ref_pct, V.ref_t = pct, now_ms()
  V.next_ocv_t = 0
  companion_begin_shutdown()
  set_state(S.CHARGING, string.format("%s at %d%%", why, pct))
end

local function enter_done(why, persist_hold)
  companion_begin_shutdown()
  if persist_hold then set_hold(why) end
  set_state(S.DONE, why)
end

local function do_low_battery(here, pct, why)
  local idx = mission:get_current_nav_index()
  save_idx(idx); save_fp()
  local best, best_why, any_d = nearest_perch(here, pct)
  V.target = best
  if best then
    vehicle:set_mode(M.GUIDED)
    V.prev_mode = M.GUIDED
    if send_divert() then
      set_state(S.DIVERT, string.format("%s, to perch (%s), saved wp %d", why, best_why, idx))
    else
      enter_landing(string.format("%s, divert refused, landing in place, saved wp %d", why, idx))
    end
  elseif CFG.COMPANION_SITE_SELECT and V.comp_state == "on" then
    vehicle:set_mode(M.GUIDED)
    info("REQ_SITE")
    set_state(S.SITE_SELECT, string.format("%s, companion site select, saved wp %d", why, idx))
  else
    local reason = any_d and string.format("nearest perch %.0f m unreachable", any_d) or "no perch in range"
    enter_landing(string.format("%s, %s, landing in place, saved wp %d", why, reason, idx))
  end
end

local function update()
  local now    = now_ms()
  local here   = ahrs:get_location()
  local armed  = arming:is_armed()
  local flying = vehicle:get_likely_flying()
  local mode   = vehicle:get_mode()
  local pct    = soc()
  local idx    = saved_idx()
  local state  = V.state

  log_tick(pct, idx)
  companion_service()
  local mode_changed = (mode ~= V.prev_mode); V.prev_mode = mode

  ------------------------------------------------------------------ DONE
  if state == S.DONE then
    if not armed then manage_pv() end
    return update, 10000

  ------------------------------------------------------------------ PAUSED
  elseif state == S.PAUSED then
    if not armed and not flying then
      enter_done("external landing", true)
    elseif armed and mode == M.AUTO then
      V.fly_seen = false
      set_state(S.FLY, "control returned")
    end
    return update, 2000

  ------------------------------------------------------------------ FLY
  elseif state == S.FLY then
    if armed and flying and mode == M.AUTO then
      V.fly_seen = true
      V.fly_nav_idx = mission:get_current_nav_index()
      V.fly_nav_id  = try(function() return mission:get_current_nav_id() end) or 0
      local p_now = pack_i() * pack_v()
      V.p_ema = V.p_ema and (0.9 * V.p_ema + 0.1 * p_now) or p_now
    end
    -- Completion: Copter disarms on the final NAV_LAND and restarts the
    -- mission at item 1 while disarmed, so MISSION_COMPLETE never shows.
    local complete = (mission:state() == mission.MISSION_COMPLETE)
    if not complete and V.fly_seen and not armed and not flying then
      if V.fly_nav_id == M.NAV_LAND and V.fly_nav_idx >= mission_fp() - 1 then
        complete = true
      else
        enter_done(string.format("disarmed mid-mission at wp %d", V.fly_nav_idx), true)
        return update, 10000
      end
    end
    if complete then
      save_idx(-1)
      enter_done("mission complete", false)
      return update, 10000
    end
    if armed and flying and not mode_is(mode, M.AUTO, M.AVOID_ADSB) then
      set_state(S.PAUSED, "external control, mode " .. mode)
      return update, 2000
    end
    if here and armed and flying and mode == M.AUTO then
      if V.fly_nav_id ~= M.NAV_LAND and V.fly_nav_id ~= M.NAV_RTL then
        local go, why = divert_now(here, pct)
        if go then do_low_battery(here, pct, why) end
      end
    end

  ------------------------------------------------------------------ DIVERT
  elseif state == S.DIVERT then
    if armed and not mode_is(mode, M.GUIDED, M.AVOID_ADSB) then
      set_state(S.PAUSED, "external control during divert")
      return update, 2000
    end
    if here and V.target and here:get_distance(V.target) < CFG.ARRIVE_M then
      enter_landing("at perch")
    elseif pct <= CFG.DIVERT_FLOOR_PCT then
      enter_landing(string.format("floor %d%%, landing in place", pct))
    elseif now - V.state_t > CFG.DIVERT_TIMEOUT_MS then
      enter_landing("divert timeout, landing in place")
    elseif mode == M.GUIDED and mode_changed then
      if not send_divert() then enter_landing("divert re-send refused") end
    end

  ------------------------------------------------------------------ SITE_SELECT
  elseif state == S.SITE_SELECT then
    if mode == M.LAND then
      set_state(S.LANDING, "companion commanded land")
    elseif pct <= CFG.DIVERT_FLOOR_PCT or now - V.state_t > CFG.SITE_SELECT_TIMEOUT_MS then
      enter_landing("site select timeout/floor, landing in place")
    elseif armed and not mode_is(mode, M.GUIDED, M.AVOID_ADSB) then
      set_state(S.PAUSED, "external control during site select")
    end

  ------------------------------------------------------------------ LANDING
  elseif state == S.LANDING then
    if not armed and not flying then
      enter_charging(pct, "perched")
    elseif armed and flying and not mode_is(mode, M.LAND, M.AVOID_ADSB) then
      set_state(S.PAUSED, "external control during landing")
    elseif armed and not flying and now - V.state_t > CFG.LANDING_TIMEOUT_MS then
      msg(M.SEV_WARN, "landed but still armed, disarming")
      arming:disarm()
    elseif armed and flying and now - V.state_t > CFG.LANDING_TIMEOUT_MS then
      msg(M.SEV_WARN, "landing timeout, re-commanding LAND")
      vehicle:set_mode(M.LAND)
      V.state_t = now
    end

  ------------------------------------------------------------------ CHARGING
  elseif state == S.CHARGING then
    manage_pv()
    if V.ocv_phase == 0 and now >= V.next_ocv_t then
      pv_connect(false); V.ocv_phase, V.ocv_open_t = 1, now
      return update, 5000
    elseif V.ocv_phase == 1 then
      if now - V.ocv_open_t >= CFG.OCV_SETTLE_MS then
        local ok, why = ocv_gate_ok()
        if ok then
          resync_from_ocv()
          V.next_ocv_t = now + CFG.OCV_SAMPLE_MS
        else
          V.ocv_fail_n = V.ocv_fail_n + 1
          V.next_ocv_t = now + ((V.ocv_fail_n >= 5) and CFG.OCV_SAMPLE_MS or CFG.OCV_RETRY_MS)
          if V.ocv_fail_n == 1 or V.ocv_fail_n % 10 == 0 then msg(M.SEV_WARN, "OCV read skipped: " .. why) end
        end
        V.ocv_phase = 0
        pv_connect(not V.pv_inhibited)
      end
      return update, 5000
    end

    pct = soc()
    if pct >= CFG.RESUME_PCT and V.last_ocv_pct then
      set_state(S.CHECK, string.format("charged %d%%", pct))
      return update, 1000
    elseif pct > V.ref_pct then
      V.ref_pct, V.ref_t = pct, now
    elseif now - V.ref_t > CFG.CHARGE_STALL_MS then
      msg(M.SEV_WARN, "no charge gain for 6 h, holding")
      V.ref_t = now
    end
    return update, 30000

  ------------------------------------------------------------------ CHECK
  elseif state == S.CHECK then
    local reasons = {}
    V.mission_ok, V.mission_why = audit_mission()
    local t, src = pack_temp()
    if not CFG.SITL_TEST then
      if t == nil then
        if CFG.TEMP_REQUIRED then reasons[#reasons+1] = "no temp" end
      elseif t < CFG.LAUNCH_TEMP_MIN_C or t > CFG.LAUNCH_TEMP_MAX_C then
        reasons[#reasons+1] = string.format("temp %.0fC %s", t, src)
      end
    end
    if math.abs(get_roll()) > CFG.MAX_TILT_RAD or math.abs(get_pitch()) > CFG.MAX_TILT_RAD then reasons[#reasons+1] = "tilt" end
    if not daylight() then reasons[#reasons+1] = "dark" end
    local win_ok, win_why = in_launch_window(here)
    if not win_ok then reasons[#reasons+1] = win_why end
    if gps:status(0) < 3 then reasons[#reasons+1] = "gps" end
    if not ahrs:healthy() then reasons[#reasons+1] = "ahrs" end
    if hops() >= CFG.HOP_MAX then reasons[#reasons+1] = "hop limit" end
    if hold_flag() then reasons[#reasons+1] = "HOLD flag" end
    if not V.mission_ok then reasons[#reasons+1] = "mission: " .. V.mission_why end
    if idx <= 0 then reasons[#reasons+1] = "no saved wp" end
    if idx == V.last_resume_idx and V.same_idx_n >= CFG.SAME_IDX_MAX then
      reasons[#reasons+1] = "no progress"
      if not hold_flag() then set_hold(string.format("no progress past wp %d", idx)) end   -- survives reboot
    end
    if not here then reasons[#reasons+1] = "no position" end
    if pct < CFG.RESUME_PCT then reasons[#reasons+1] = "soc" end

    if #reasons == 0 then
      if CFG.COMPANION_IN_FLIGHT and V.comp_state ~= "on" then
        companion_on()
        info("companion booting")
        return update, CFG.COMP_BOOT_WAIT_MS
      end
      if CFG.COMPANION_IN_FLIGHT and now - V.comp_t < CFG.COMP_BOOT_WAIT_MS then return update, 5000 end
      pv_connect(false)
      ahrs:set_home(here)
      V.ground_alt_cm = here:alt()
      vehicle:set_mode(M.GUIDED)
      if arming:pre_arm_checks() and arming:arm() then
        if vehicle:start_takeoff(CFG.TAKEOFF_ALT_M) then
          add_hop()
          if idx == V.last_resume_idx then V.same_idx_n = V.same_idx_n + 1 else V.last_resume_idx, V.same_idx_n = idx, 1 end
          set_state(S.CLIMB, string.format("hop %d, taking off", hops()))
          return update, 1000
        end
        arming:disarm()
        reasons[#reasons+1] = "takeoff refused"
      else
        reasons[#reasons+1] = "arm refused"
      end
    end

    info("hold: " .. table.concat(reasons, ","))
    if V.comp_state == "on" then companion_begin_shutdown() end
    pv_connect(not V.pv_inhibited)
    V.state, V.state_t = S.CHARGING, now
    return update, 60000

  ------------------------------------------------------------------ CLIMB
  elseif state == S.CLIMB then
    if not armed then
      enter_done("disarmed during climb", true)
      return update, 5000
    end
    if not mode_is(mode, M.GUIDED, M.AVOID_ADSB) then
      set_state(S.PAUSED, "external control during climb")
      return update, 2000
    end
    if here and V.ground_alt_cm and (here:alt() - V.ground_alt_cm) >= CFG.TAKEOFF_ALT_M * 90 then
      mission:set_current_cmd(idx)
      vehicle:set_mode(M.AUTO)
      V.fly_seen = false
      set_state(S.FLY, "resuming at wp " .. idx)
    elseif now - V.state_t > 60000 then
      enter_landing("climb timeout")
    end
  end

  return update, 1000
end

local function protected()
  local ok, fn, delay = pcall(update)
  if ok then return protected, delay or 1000 end
  msg(M.SEV_ERR, "error: " .. tostring(fn))
  return protected, 5000
end

------------------------------------------------------------------------------
-- Init
------------------------------------------------------------------------------
local function init()
  relay_set(CFG.RELAY_COMP_SHDN, false)
  V.mission_ok, V.mission_why = audit_mission()
  if not V.mission_ok then msg(M.SEV_WARN, "mission audit: " .. V.mission_why) end

  local idx = saved_idx()
  if idx > 0 and saved_fp() ~= mission_fp() then
    msg(M.SEV_WARN, "mission changed, clearing saved waypoint")
    save_idx(0); idx = 0
  end

  if idx == -1 then
    pv_connect(true)
    V.comp_state = "unknown"; companion_begin_shutdown()
    V.state, V.state_t = S.DONE, now_ms()
    info("boot: mission complete, holding at recovery point")
  elseif idx > 0 and not arming:is_armed() then
    pv_connect(false)
    V.last_ocv_pct = 0
    V.comp_state = "unknown"; companion_begin_shutdown()
    V.state, V.state_t = S.CHARGING, now_ms()
    V.ref_pct, V.ref_t = 0, now_ms()
    V.next_ocv_t = now_ms() + CFG.COMP_SHDN_WAIT_MS + 5000
    info("boot: resuming charge cycle at wp " .. idx .. (hold_flag() and " (HOLD set)" or ""))
  else
    companion_on()
    pv_connect(false)
    V.state, V.state_t = S.FLY, now_ms()
    info("boot: ready" .. (hold_flag() and " (HOLD set)" or ""))
  end
  return protected, 2000
end

return init()