-- TerraHawk charge interlock. Runs on the flight controller (Pixhawk 6C Mini first, then our own H743).
--
-- The aircraft carries a charge-enable switch between the landing-shoe
-- rectifier and the pack, driven by the charge node over DroneCAN. This
-- script closes that switch only while every condition below holds, and
-- opens it the moment one fails. The ground-side charger has its own
-- limits; both controllers must agree before current flows.
--
-- Battery instances on the FC (0-based here, BATT1.. in parameters):
--   0  the FC's own sensor on the main leads
--   1  charge node, pack: voltage, charge current, cell voltages, temperature (id 10)
--   2  charge node, shoe side of the rectifier: voltage only (id 11)
-- Relays on the FC (0-based here, RELAY1.. in parameters):
--   2  charge enable   -> DroneCAN hardpoint 0 (RELAY3_PIN 1000)
--   3  balancer power  -> DroneCAN hardpoint 1 (RELAY4_PIN 1001)
-- See docs/charge-node.md for the parameters that go with this script.
--
-- Full-charge reset: instance 0 counts mAh used since boot and never counts the charge, so after a
-- dock charge with no reboot the capacity triggers still see the last flight and refuse to arm
-- (th-testpilot, 2026-10-01: the dock harness hung on "Battery 1 low capacity failsafe"). When a
-- charge seen in this session tapers off with every cell at or above FULL_CELL, the script sets
-- instance 0 back to 100 %. A partial charge does not reset it: the count then overstates what was
-- used, which is the safe side.

local PACK        = 1
local SHOE        = 2
local RLY_CHARGE  = 2
local RLY_BAL     = 3

local NUM_CELLS   = 6
local CELL_MAX    = 4.20   -- V, open above this
local CELL_REARM  = 4.15   -- V, close again only below this
local CELL_MIN    = 2.80   -- V, do not charge a cell below this
-- Pack temperature band (docs/charge-node.md, th-eda 00ba24c; Ryan via th-hangar: never above 45 C): start only
-- from 2 to 40 C; once charging, continue up to 45 C; after a trip, close again only below 40 C. A reading below
-- -20 or above 80 C (an ADC rail, or INT16_MAX) is an open or shorted thermistor: unknown, so the switch stays open.
-- The P50B data sheet allows more (70 C cell surface while charging); 40/45 is a pack-life margin.
local TEMP_MIN    = 2.0    -- degC, never below
local TEMP_START  = 40.0   -- degC, highest temperature to start, and to close again after a trip
local TEMP_MAX    = 45.0   -- degC, open above this while charging
local TEMP_VALID_LO, TEMP_VALID_HI = -20.0, 80.0
local SHOE_MIN    = 3.0    -- V on the shoe side means the pad is probing us
local SUM_TOL     = 0.6    -- V, cells must add up to the pack voltage
local BAL_ON_AMPS = 0.2    -- balancer runs while this much charge current flows
local MAIN        = 0      -- the FC's own sensor, which the failsafes use
local FULL_CELL   = 4.15   -- V, every cell at or above this for a full charge
local FULL_AMPS   = 0.5    -- A, charge current tapered below this (C/20 on the 10 Ah P50B)
local FULL_HOLD_MS = 30000 -- ms the full condition must hold

local PERIOD_MS   = 500
local MSG_MIN_MS  = 5000

local cell_tripped = false
local temp_tripped = false
local last_reason  = ""
local last_msg_ms  = 0
local charged      = false -- a charge flowed since boot or since the last reset
local full_since   = nil

local function say(reason)
  local now = millis()
  if reason ~= last_reason or (now - last_msg_ms) > MSG_MIN_MS then
    gcs:send_text(6, "charge: " .. reason)   -- 6 = MAV_SEVERITY_INFO
    last_reason = reason
    last_msg_ms = now
  end
end

-- returns ok, reason
local function conditions()
  if arming:is_armed() then return false, "armed" end
  if vehicle:get_likely_flying() then return false, "flying" end

  if not battery:healthy(PACK) then return false, "node pack data stale" end
  if not battery:healthy(SHOE) then return false, "node shoe data stale" end

  local shoe_v = battery:voltage(SHOE)
  if shoe_v < SHOE_MIN then return false, "no pad contact" end

  local temp = battery:get_temperature(PACK)
  if temp == nil then return false, "no pack temperature" end
  if temp < TEMP_VALID_LO or temp > TEMP_VALID_HI then
    return false, string.format("pack temperature unknown (%.0fC: thermistor open or shorted)", temp)
  end
  if temp < TEMP_MIN then return false, string.format("pack cold %.1fC", temp) end
  local charging = relay:get(RLY_CHARGE) ~= 0
  if temp > TEMP_MAX then temp_tripped = true end
  if temp_tripped and temp < TEMP_START then temp_tripped = false end
  if temp_tripped then return false, string.format("pack hot %.1fC, waiting for %.0fC", temp, TEMP_START) end
  if not charging and temp > TEMP_START then return false, string.format("pack warm %.1fC, start below %.0fC", temp, TEMP_START) end

  local sum = 0
  local vmax = 0
  for c = 0, NUM_CELLS - 1 do
    local v = battery:get_cell_voltage(PACK, c)
    if v == nil then return false, string.format("cell %d missing", c + 1) end
    if v < CELL_MIN then return false, string.format("cell %d low %.2fV", c + 1, v) end
    if v > vmax then vmax = v end
    sum = sum + v
  end

  local pack_v = battery:voltage(PACK)
  if math.abs(sum - pack_v) > SUM_TOL then
    return false, string.format("cells %.2fV vs pack %.2fV", sum, pack_v)
  end

  if vmax > CELL_MAX then cell_tripped = true end
  if cell_tripped and vmax < CELL_REARM then cell_tripped = false end
  if cell_tripped then return false, string.format("cell high %.3fV", vmax) end

  return true, "ok"
end

local function set_relay(idx, on)
  if on then
    if relay:get(idx) == 0 then relay:on(idx) end
  else
    if relay:get(idx) ~= 0 then relay:off(idx) end
  end
end

local function min_cell()
  local vmin = 99
  for c = 0, NUM_CELLS - 1 do
    local v = battery:get_cell_voltage(PACK, c)
    if v == nil then return nil end
    if v < vmin then vmin = v end
  end
  return vmin
end

-- after a charge in this session tapers off with every cell full, set the FC's count to 100 %
local function full_reset(amps)
  if not charged or arming:is_armed() or not battery:healthy(PACK) or amps == nil then
    full_since = nil
    return
  end
  local vmin = min_cell()
  if vmin == nil or vmin < FULL_CELL or amps >= FULL_AMPS then
    full_since = nil
    return
  end
  local now = millis()
  full_since = full_since or now
  if now - full_since >= FULL_HOLD_MS then
    battery:reset_remaining(MAIN, 100)
    charged, full_since = false, nil
    gcs:send_text(6, string.format("charge: full, min cell %.2fV, battery count reset", vmin))
  end
end

function update()
  local ok, reason = conditions()
  set_relay(RLY_CHARGE, ok)

  local amps = battery:current_amps(PACK)
  local charging = ok and amps ~= nil and amps > BAL_ON_AMPS
  set_relay(RLY_BAL, charging)
  if charging then charged = true end
  full_reset(amps)

  if ok then
    if charging then say(string.format("charging %.1fA", amps)) else say("enabled, waiting") end
  else
    say("blocked: " .. reason)
  end
  return update, PERIOD_MS
end

-- fail safe on start: both switches open until the first good check
relay:off(RLY_CHARGE)
relay:off(RLY_BAL)
gcs:send_text(6, "charge interlock loaded")
return update, PERIOD_MS