airship-autopilot/files/autopilot/autopilot/main.lua

-- main.lua -- loop + tilstandsmaskin for v3 observer-autopiloten.
-- All UI-tekst er ENGELSK; kommentarer er norske (husstil).
-- Modus (argument eller meny):
--   boot      oppstart: setup-wizard-prompt hvis profil mangler, ellers standby
--   setup     foerstegangs-wizard: wiring + sign test i ett (alt skip-spesifikt)
--   standby   lytter etter ENGAGE fra pilot-konsollen (default etter boot)
--   monitor   Lag 1: les sensorer+estimator, logg+HUD, styrer INGENTING
--   signtest  §13.2: selvgaaende -- loefter, maaler pitch-fortegn, lander igjen
--   manual    Lag 2: faste nivaaer (piltaster), finn hover
--   cal       Lag 3: full kalibrering (selv-laerende)
--   observe   Lag 4: observer ÅPEN SLØYFE -- valider at v_hat sporer v_meas
--   fly       Lag 5-8: full lukket sløyfe (IDLE->CALIBRATING->FLYING->FAULT)

local progDir = fs.getDir(shell.getRunningProgram())
if package and package.path and not string.find(package.path, progDir, 1, true) then
  package.path = progDir .. "/?.lua;" .. package.path
end

local cfg         = require("config")
local utils       = require("utils")
local sensors     = require("sensors")
local Estimator   = require("estimator")
local Logger      = require("logger")
local mixer       = require("mixer")
local calibration = require("calibration")
local Observer    = require("observer")
local altitude    = require("altitude")
local pitch       = require("pitch")
local safety      = require("safety")
local link        = require("link")

local clamp, round = utils.clamp, utils.round
local function now() return os.epoch("utc") / 1000 end

-- ============================ AKTUATOR (sigma-delta dither) ============================
local foreAcc, aftAcc = 0, 0
local function ditherSide(cmd, acc)
  if cfg.dither then
    acc = acc + cmd
    local out = math.floor(acc + 0.5)
    if out < 0 then out = 0 elseif out > cfg.maxSignal then out = cfg.maxSignal end
    acc = clamp(acc - out, -1, 1)
    return out, acc
  else
    return clamp(round(cmd), 0, cfg.maxSignal), 0
  end
end
local function driveValves(fore, aft)
  local of; of, foreAcc = ditherSide(fore, foreAcc)
  redstone.setAnalogOutput(cfg.foreSide, of)
  local oa = 0
  if cfg.aftSide then oa, aftAcc = ditherSide(aft, aftAcc); redstone.setAnalogOutput(cfg.aftSide, oa) end
  return of, oa
end
local function allOff()
  foreAcc, aftAcc = 0, 0
  redstone.setAnalogOutput(cfg.foreSide, 0)
  if cfg.aftSide then redstone.setAnalogOutput(cfg.aftSide, 0) end
end

-- ============================ PERSISTENS ============================
local CALIB_FILE, TARGET_FILE = "calib.txt", "target.txt"
local function loadCalib()
  local c = {}
  for k, v in pairs(cfg.calib) do c[k] = v end
  if fs.exists(CALIB_FILE) then
    local f = fs.open(CALIB_FILE, "r"); local s = f.readAll(); f.close()
    local g = textutils.unserialise(s or "")
    if type(g) == "table" then for k, v in pairs(g) do c[k] = v end end
  end
  return c
end
local function saveCalib(c)
  local f = fs.open(CALIB_FILE, "w"); f.write(textutils.serialise(c)); f.close()
end
local function loadTarget()
  if not fs.exists(TARGET_FILE) then return nil end
  local f = fs.open(TARGET_FILE, "r"); local s = f.readAll(); f.close(); return tonumber(s)
end
local function saveTarget(h) local f = fs.open(TARGET_FILE, "w"); f.write(tostring(h)); f.close() end

-- wiring (front/aft redstone-sider) -- persistert per skip, overstyrer config-defaults
local SIDES_FILE = "sides.txt"
local SIDES = { "top", "bottom", "left", "right", "front", "back" }
local function validSide(s) for _, x in ipairs(SIDES) do if x == s then return true end end return false end
local function loadSides()
  if not fs.exists(SIDES_FILE) then return nil end
  local f = fs.open(SIDES_FILE, "r"); local s = f.readAll(); f.close()
  local t = textutils.unserialise(s or ""); return (type(t) == "table" and t.fore) and t or nil
end
local function saveSides(t) local f = fs.open(SIDES_FILE, "w"); f.write(textutils.serialise(t)); f.close() end

-- komplett skip-profil = wiring valgt + pitch-fortegn faktisk MAALT paa DETTE skipet
-- (signMeasured settes av sign test / full kalibrering -- aldri av en shipped default)
local function profileComplete()
  if not (fs.exists(SIDES_FILE) and fs.exists(CALIB_FILE)) then return false end
  return loadCalib().signMeasured == true
end

-- live brukerinnstillinger (justeres fra pilot-konsollen), persistert. Syntetiske noekler -> cfg.
local SETTINGS_FILE = "settings.txt"
local function saveSettings(t) local f = fs.open(SETTINGS_FILE, "w"); f.write(textutils.serialise(t)); f.close() end
local function loadSettings()
  if not fs.exists(SETTINGS_FILE) then return nil end
  local f = fs.open(SETTINGS_FILE, "r"); local s = f.readAll(); f.close()
  local t = textutils.unserialise(s or ""); return (type(t) == "table") and t or nil
end
-- bruk en innstilling live paa cfg (clamp for sikkerhet). persist=true lagrer til disk.
local function applySetting(key, value, persist)
  value = tonumber(value); if not value then return end
  if key == "climbSpeed" then value = utils.clamp(value, 0.2, 2.0); cfg.vMaxUp = value; cfg.vMaxDown = value
  elseif key == "holdBand" then value = utils.clamp(value, 1, 10); cfg.altDeadband = value
  elseif key == "maxPower" then value = utils.clamp(math.floor(value + 0.5), 3, 15); cfg.maxSignal = value
  else return end
  if persist then saveSettings({ climbSpeed = cfg.vMaxUp, holdBand = cfg.altDeadband, maxPower = cfg.maxSignal }) end
end

-- bundet proporsjonal hover-hold (universal, brukt av observe + fault-tilstand)
local function velHoldCommon(calib, vDes, v)
  local hover = (calib.uHoverFront + calib.uHoverRear) / 2
  return clamp(hover + clamp(1.2 * (vDes - v), -1.5, 1.5), cfg.absFloor, cfg.maxSignal)
end

-- ============================ MONITOR (Lag 1) ============================
local function runMonitor()
  local est = Estimator.new(cfg)
  local log = Logger.new("monitor.log", "# monitor: t, height, pitch, vVel, pitchRate, valid")
  print("MONITOR -> monitor.log (controls nothing). CTRL+T to stop."); sleep(1.0)
  local t0 = now()
  while true do
    local t, h, p = now(), sensors.readAltitude(), sensors.readPitch()
    term.clear(); term.setCursorPos(1, 1); print("==== MONITOR (Layer 1) ====")
    if h == nil or p == nil then print("SENSOR ERROR (nil) -- check sensors.")
    else
      est:update(t, h, p)
      log:row({ t - t0, est.altitude, est.pitch, est.verticalVelocity, est.pitchRate, est.valid and 1 or 0 })
      local vtag = est.verticalVelocity > 0.15 and "(rising)" or est.verticalVelocity < -0.15 and "(sinking)" or "(still)"
      print(string.format("Height    %8.2f", est.altitude))
      print(string.format("Pitch     %8.2f deg", est.pitch))
      print(string.format("vVel      %8.2f b/s  %s", est.verticalVelocity, vtag))
      print(string.format("pitchRate %8.2f deg/s", est.pitchRate))
      print(est.valid and "estimate: OK" or "estimate: filling window...")
    end
    sleep(cfg.dt)
  end
end

-- ============================ SIGN TEST (§13.2, v3 -- GYNGE-PROBE) ============================
-- GRUNNPREMISS: et SKJEVT skip er nettopp skipet denne testen finnes for. Skjevhet med balansert
-- loft er MAALEDATA (det er ubalansen vi skal laere aa trimme bort), ikke en feiltilstand. Derfor
-- finnes ingen "for skjevt"-abort -- testen maaler seg gjennom skjevheten og retter den til slutt.
--
-- Fortegnet trenger INGEN balansert baseline: sign = sign(rate(+dytt) - rate(-dytt)). Differansen
-- mellom de to dyttene kansellerer enhver konstant skjev-drift eksakt. Og fordi vi VEKSLER mellom
-- dyttene gynger skipet rundt utgangspunktet i stedet for aa akkumulere tilt -- selvbegrensende.
--
-- BAKKE-ATTITYDEN BRUKES ALDRI: skipet er bygget for aa FLY, ikke for aa staa, saa paa bakken
-- ligger det gjerne paa siden. Vi venter til oppdriften har reist det opp OG ratene har roet seg
-- foer attityde-referansen settes -- ellers ville skipets egen oppretting blitt lest som respons.
--
--   1/5  trapper seg luftbaaren fra bunnen (laerer ~hover paa kjoepet, ingen fast liftLevel)
--   2/5  klatrer til maalehoyde, lar skipet RETTE SEG OPP, setter attityde-referanse i lufta
--   3/5  GYNGE-PROBE: front-dytt <-> bak-dytt, 2 sykluser; aksen som svarer blir pitchIndex.
--        Svak respons -> auto sterkere dytt. Maaler ALLE gimbal-akser (byggeretning likegyldig)
--   4/5  BALANSE: med fortegnet kjent laeres differanse-trimmen som holder skipet vannrett
--   5/5  lagrer, senker skipet kontrollert tilbake og HOLDER hover -- ALDRI frittfall

local signtestAirborne = false   -- for runMode-cleanup: hold hover hvis luftbaaren, ellers alt av

local function runSigntest()
  local s, est = cfg.signtest, Estimator.new(cfg)
  local log = Logger.new("signtest.log", "# signtest: t, phase, fore, aft, height, pitch, vVel, pitchRate")
  signtestAirborne = false
  local h0 = sensors.readAltitude()
  if not h0 then print("No altitude sensor."); return false end

  -- ett-ballong-skip: ingen differensiell pitch-autoritet -> testen er meningsloes
  if not cfg.aftSide then
    print("Single-balloon ship (no AFT side wired).")
    print("Pitch can't be steered differentially, so")
    print("the sign test does not apply. Marking done.")
    local c = loadCalib(); c.pitchSign = 1; c.signMeasured = true; saveCalib(c)
    print(""); print("[Enter]"); read()
    return true
  end

  local t0 = now()
  local hover = nil       -- laeres i trappa (1/4), trimmes videre av fart-holdet
  local levelDiff = 0     -- treg nivellering under nedstigning (naar fortegnet er kjent)

  -- MULTI-AKSE maaling: vi VET ikke hvilken gimbal-vinkel som er pitch paa DETTE skipet
  -- (avhenger av byggeretning!). Derfor maales rate-responsen paa ALLE aksene, og aksen
  -- som faktisk svarer paa dyttet blir pitchIndex. Orientering blir dermed likegyldig.
  local NAX = 0             -- antall gimbal-akser (settes ved foerste lesing, maks 3)
  local axRings = {}        -- per-akse (t, vinkel)-ring -> rate via least-squares slope
  local a0, lastAx = nil, nil   -- vinkler ved start (tilt-referanse) + siste lesing

  local function readAx()
    local a = sensors.readAngles()
    if type(a) ~= "table" or type(a[1]) ~= "number" then return nil end
    if NAX == 0 then
      NAX = math.min(3, #a)
      for i = 1, NAX do axRings[i] = utils.newRing(1.0) end
    end
    return a
  end

  -- Attityde-referansen settes AaLDRI paa bakken: et skip bygget for aa FLY staar ikke stoett,
  -- det ligger gjerne paa siden. Den attityden er meningsloes -- og oppdriftens egen oppretting
  -- etter avgang ville blitt lest som baade "tilt" og "rate-respons". Vi setter referansen
  -- FOERST naar skipet har rettet seg opp i lufta og ratene har roet seg.
  local function setAttitudeRef()
    if not lastAx then return end
    a0 = {}
    for i = 1, NAX do a0[i] = lastAx[i] end
  end

  local function axRates()
    local r = {}
    for i = 1, NAX do r[i] = axRings[i]:slope() end
    return r
  end

  -- stoerste vinkelavvik fra start-attityden paa NOEN akse (fortegns-/akse-uavhengig tilt-vakt)
  local function maxTilt()
    if not (lastAx and a0) then return 0 end
    local m = 0
    for i = 1, NAX do
      local v = lastAx[i]
      if type(v) == "number" then
        local d = math.abs(v - a0[i])
        if d > m then m = d end
      end
    end
    return m
  end

  local function tick(fore, aft, phase)
    local f, oa = driveValves(fore, aft)
    local h, t = sensors.readAltitude(), now()
    local ax = readAx()
    if ax then
      lastAx = ax
      for i = 1, NAX do
        local v = ax[i]
        if type(v) == "number" and v == v then axRings[i]:push(t, v) end
      end
    end
    local p = ax and ax[cfg.pitchIndex or 1] or nil
    if h and p then est:update(t, h, p) end
    log:row({ t - t0, phase, f, oa, est.altitude, est.pitch, est.verticalVelocity, est.pitchRate })
    sleep(cfg.dt)
  end

  local function hud(step, title, lines)
    term.clear(); term.setCursorPos(1, 1)
    print("==== SIGN TEST  " .. step .. " ====")
    print(title)
    for _, l in ipairs(lines or {}) do print(l) end
    print("")
    print(string.format("height +%.1f  pitch %+.1f  vVel %+.2f",
      est.altitude - h0, est.pitch, est.verticalVelocity))
    print("CTRL+T = abort (holds lift, no freefall)")
  end

  -- fart-P-hold rundt laert hover (+ hover-trim), med valgfri differanse (fore - aft).
  -- Trim-rate 0.12 = samme verdi som Ki_climb (verifisert stabil for skipets ~5 s lag);
  -- 0.05 var for tregt -- skipet rakk aa drive ut av maalebaandet foer trimmen tok igjen.
  local function velTick(vDes, diff, phase)
    hover = clamp(hover + 0.12 * (vDes - est.verticalVelocity) * cfg.dt, cfg.absFloor, cfg.maxSignal - 0.5)
    local u = hover + clamp(1.2 * (vDes - est.verticalVelocity), -1.5, 1.5)
    local f, a = mixer.preserveDifferenceClamp(u + diff, u - diff, cfg.absFloor, cfg.maxSignal)
    tick(f, a, phase)
  end

  -- kontrollert nedstigning + hover-hold -- den ENESTE maaten vi forlater lufta paa
  local function descendAndHold(pSign)
    local tD = now()
    while est.altitude > h0 + 3 and now() - tD < 120 do
      if pSign ~= 0 then
        levelDiff = clamp(levelDiff - pSign * 0.2 * est.pitch * cfg.dt, -1.5, 1.5)
      end
      velTick(-0.4, levelDiff, "descend")
      hud("5/5", "Coming back down...", {
        string.format("descending to +3   hover ~%.1f", hover),
        string.format("trim %+.2f holding it level", levelDiff),
      })
    end
    local tS = now()
    while now() - tS < 3.0 do velTick(0, levelDiff, "settle") end
    driveValves(hover, hover)   -- bli staaende paa laert hover (aldri null = stup)
  end

  -- felles avbrudd: luftbaaren -> kontrollert ned foerst; paa bakken -> bare av
  local function bail(msg)
    if signtestAirborne then descendAndHold(0) else allOff() end
    term.clear(); term.setCursorPos(1, 1)
    print("==== SIGN TEST: STOPPED ====")
    print(msg)
    if signtestAirborne then print(""); print("Ship is holding hover near the ground.") end
    print(""); print("[Enter]"); read()
    return false
  end

  local function abortMsg(code)
    if code == "time" then
      return "Ran out of time -- could not hold a\nsteady measuring height. Check burner\nfuel / ship weight and try again."
    end
    return "Aborted."
  end

  -- ---- 1/4: trappe seg luftbaaren (ingen fast liftLevel -- vi SOEKER effekten) ----
  local cmd, stepT, riseHold, tLift = cfg.absFloor, now(), 0, now()
  while true do
    tick(cmd, cmd, "lift")
    hud("1/5", "Lifting off (auto power search)...", {
      string.format("power %.1f / %d", cmd, cfg.maxSignal),
    })
    if est.verticalVelocity > 0.25 then riseHold = riseHold + cfg.dt else riseHold = 0 end
    if riseHold >= 1.0 and est.altitude - h0 > 1.5 then break end
    if now() - stepT >= 4.0 then
      if cmd >= cfg.maxSignal then
        return bail("No lift even at max power.\nCheck wiring (menu: Wiring) and that the\nburners have fuel.")
      end
      cmd = math.min(cfg.maxSignal, cmd + 0.5); stepT = now()
    end
    if now() - tLift > 120 then return bail("Timed out lifting off.") end
  end
  hover = math.max(cfg.absFloor, cmd - 0.25)   -- kryssingen er ~her: foerste hover-estimat
  signtestAirborne = true

  -- ---- 2/4: klatre til maalehoyde og SETTLE (hover-trimmen laerer ekte hover) ----
  -- SUPERVISOR-prinsipp: hoyden overvaakes gjennom hele maalefasen. Driver skipet ut av
  -- baandet -> fly tilbake til maalehoyden og proev maalingen igjen (selv-reparerende).
  -- Bare EN total-deadline kan felle testen -- ikke haartrigger-aborter paa drift.
  local BAND_LO, BAND_HI, BAND_TARGET = 4, 25, 12
  local deadline = now() + 420   -- hele testen ferdig innen 7 min, ellers ryddig bail

  local function recenter(step, reason)
    while now() < deadline do
      local dh = est.altitude - (h0 + BAND_TARGET)
      if math.abs(dh) < 1.5 then return true end
      velTick(dh > 0 and -0.4 or 0.4, levelDiff, "recenter")
      hud(step, "Flying to measuring height (" .. reason .. ")", {
        string.format("target +%d   now %+.1f   hover ~%.1f", BAND_TARGET, est.altitude - h0, hover),
        string.format("tilt %+.0f deg (fine -- that's what we fix)", maxTilt()),
      })
    end
    return false, "time"
  end

  local okC, cErr = recenter("2/5", "initial climb")
  if not okC then return bail(abortMsg(cErr)) end

  -- La skipet RETTE SEG OPP i lufta foer vi maaler noe som helst. Et flybygget skrog ligger
  -- gjerne paa siden paa bakken; oppdriften reiser det opp av seg selv naar det kommer i lufta.
  -- Vi venter til ALLE akse-ratene har roet seg -- og setter FOERST DA attityde-referansen.
  local tR, calmR, worst = now(), 0, 0
  while now() - tR < 120 and now() < deadline do
    velTick(0, 0, "righting")
    worst = 0
    for i = 1, NAX do worst = math.max(worst, math.abs(axRings[i]:slope())) end
    if worst < 0.5 then calmR = calmR + cfg.dt else calmR = 0 end
    hud("2/5", "Letting the ship right itself...", {
      "(a flight-built hull lies over on the ground;",
      " buoyancy levels it once airborne)",
      string.format("worst rate %.2f deg/s   steady %.1f/5.0 s", worst, calmR),
      string.format("hover ~%.2f", hover),
    })
    if calmR >= 5.0 then break end
    local rel = est.altitude - h0
    if rel < BAND_LO or rel > BAND_HI then
      local okD, dErr = recenter("2/5", "altitude drift")
      if not okD then return bail(abortMsg(dErr)) end
    end
  end
  setAttitudeRef()   -- NAA er referansen meningsfull: skipets faktiske flyattityde
  local settled = {}
  for i = 1, NAX do settled[i] = (lastAx and lastAx[i]) or 0 end

  -- rate-stabil maaling PER AKSE: hold hoyden (vDes=0) med gitt differanse til ratene roer
  -- seg. Returnerer tabell {avgRate per akse}. Lag-bevisst: minHold >= loft-etterslepet.
  local function measureRate(diff, step, title, minHold, maxWait)
    local tS, win = now(), {}
    local avg = nil
    while now() - tS < maxWait do
      velTick(0, diff, title)
      win[#win + 1] = { t = now(), r = axRates() }
      while #win > 1 and (now() - win[1].t) > 1.5 do table.remove(win, 1) end
      avg = {}
      local spanMax = 0
      for i = 1, NAX do
        local sum, lo, hi = 0, math.huge, -math.huge
        for _, e in ipairs(win) do
          local v = e.r[i]
          sum = sum + v; lo = math.min(lo, v); hi = math.max(hi, v)
        end
        avg[i] = sum / #win
        local sp = (#win > 1) and (hi - lo) or 99
        if sp > spanMax then spanMax = sp end
      end
      local rline = ""
      for i = 1, NAX do rline = rline .. string.format(" a%d %+.2f", i, avg[i]) end
      hud(step, title, {
        string.format("diff %+.0f  rates:%s", diff, rline),
        string.format("calm when spread < %.1f (now %.2f)", s.rateCalm, spanMax),
      })
      -- INGEN tilt-abort: skjevhet er maalingens formaal. Har raten allerede svart tydelig,
      -- slutter vi likevel aa dytte tidlig -- vi har det vi kom for, unoedig aa tippe mer.
      if maxTilt() > s.maxTiltAbort and #win >= 5 then return avg end
      local rel = est.altitude - h0
      if rel < BAND_LO or rel > BAND_HI then return nil, "band" end
      if now() > deadline then return nil, "time" end
      if now() - tS >= minHold and #win >= 5 and spanMax < s.rateCalm then return avg end
    end
    return avg   -- timeout: retningen er som regel riktig selv om roen aldri kom
  end

  -- selv-reparerende maaling: hoyde-drift -> re-sentrer og proev IGJEN i stedet for abort
  local function measureSafe(diff, title, minHold, maxWait)
    while true do
      local r, code = measureRate(diff, "3/5", title, minHold, maxWait)
      if r then return r end
      if code ~= "band" then return nil, code end
      local okR, rErr = recenter("3/5", "altitude drift")
      if not okR then return nil, rErr end
    end
  end

  -- ---- 3/5: GYNGE-PROBE -- front-dytt <-> bak-dytt; aksen som SVARER blir pitchIndex ----
  -- d = rate(+dytt) - rate(-dytt) trenger INGEN baseline: en konstant skjev-drift finnes i
  -- begge leddene og kanselleres eksakt. To sykluser som er ENIGE om fortegnet = verifisert.
  local nudge = s.nudge
  local maxNudge = math.max(s.nudge, math.floor(cfg.maxSignal / 2) + 1)
  local pick = nil
  for attempt = 1, 3 do
    local ds = {}
    for c = 1, 2 do
      local tag = string.format("  (cycle %d/2, push %d)", c, nudge)
      local rF, eF = measureSafe(nudge, "Rocking: push FRONT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
      if not rF then return bail(abortMsg(eF)) end
      local rA, eA = measureSafe(-nudge, "Rocking: push AFT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
      if not rA then return bail(abortMsg(eA)) end
      local d = {}
      for i = 1, NAX do d[i] = rF[i] - rA[i] end
      ds[c] = d
    end
    -- aksen med sterkest ENIG respons vinner (begge sykluser samme retning)
    for i = 1, NAX do
      local d1, d2 = ds[1][i], ds[2][i]
      local sg = utils.sign(d1)
      local mag = (math.abs(d1) + math.abs(d2)) / 2
      if sg ~= 0 and utils.sign(d2) == sg and mag >= s.drMin then
        if not pick or mag > pick.mag then pick = { axis = i, sign = sg, mag = mag } end
      end
    end
    if pick then break end
    nudge = math.min(maxNudge, nudge + 1)   -- auto: proev sterkere dytt
  end
  if not pick then
    return bail("No pitch response on ANY gimbal axis,\neven with a stronger push. Likely causes:\n- links cross-wired: did BOTH burners fire\n  on one pulse in Wiring? Re-run Wiring.\n- balloons not spaced along the hull.")
  end
  local sign, axis = pick.sign, pick.axis
  cfg.pitchIndex = axis   -- gjeld umiddelbart (estimator, balansefase, nedstigning)

  -- ---- 4/5: BALANSE -- laer differanse-trimmen som holder DETTE skipet vannrett ----
  -- Dette er hele poenget: skipet henger skjevt, og vi maaler nettopp trimmen som retter det.
  -- Integralet laerer den varige trimmen; P/D bare demper transienten og forsvinner ved likevekt.
  -- Konvergerer den ikke: vi lagrer likevel beste trim + advarer (begrenset pitch-autoritet).
  local Kp_lvl, Kd_lvl, Ki_lvl = 0.15, 0.08, 0.03
  local trimMax = cfg.maxSignal / 2
  local levelTrim, calm, tBal = 0, 0, now()
  local pitchNow, rateNow = 0, 0
  while now() - tBal < 90 and now() < deadline do
    pitchNow = (lastAx and lastAx[axis]) or 0
    rateNow  = axRings[axis]:slope()
    levelTrim = clamp(levelTrim + Ki_lvl * sign * (-pitchNow) * cfg.dt, -trimMax, trimMax)
    levelDiff = clamp(levelTrim + sign * (-Kp_lvl * pitchNow - Kd_lvl * rateNow), -trimMax, trimMax)
    velTick(0, levelDiff, "balance")
    hud("4/5", "Learning the balance trim...", {
      string.format("tilt %+.1f deg   rate %+.2f", pitchNow, rateNow),
      string.format("trim %+.2f   level for %.1f/4.0 s", levelTrim, calm),
    })
    if math.abs(pitchNow) < 2.0 and math.abs(rateNow) < 0.3 then calm = calm + cfg.dt else calm = 0 end
    if calm >= 4.0 then break end
    local rel = est.altitude - h0
    if rel < BAND_LO or rel > BAND_HI then
      local okR, rErr = recenter("4/5", "altitude drift")
      if not okR then return bail(abortMsg(rErr)) end
    end
  end
  local balanced = calm >= 4.0
  levelDiff = levelTrim   -- nedstigningen fortsetter fra den laerte trimmen

  -- ---- lagre FOER nedstigningen (Ctrl+T etterpaa mister ingenting) ----
  local c = loadCalib()
  c.pitchSign, c.pitchGain, c.alphaPitch = sign, 1.0, 1.0
  c.pitchIndex = axis                          -- MAALT: aksen som faktisk svarte paa dyttet
  c.pitchTrim = 2 * levelTrim                  -- diff -> D_des-enheter (mixer: front = u + D/2)
  c.uHoverFront, c.uHoverRear = hover, hover   -- grov men EKTE hover-maaling ('cal' kan forfine)
  c.hoverTrim = 0                              -- ny baseline -> gammel loft-trim er ugyldig
  c.signMeasured = true                        -- profilen er komplett (wizard/standby sjekker denne)
  saveCalib(c)

  -- ---- 5/5: kontrollert ned + hold hover (nivellert med den nylaerte trimmen) ----
  descendAndHold(sign)

  term.clear(); term.setCursorPos(1, 1)
  print("==== SIGN TEST: SUCCESS ====")
  print(string.format("pitch = gimbal axis a%d, sign %+d", axis, sign))
  local sline = ""
  for i = 1, NAX do sline = sline .. string.format(" a%d %+.0f", i, settled[i]) end
  print("airborne attitude:" .. sline)
  print(string.format("balance trim %+.2f  (saved warm-start)", 2 * levelTrim))
  if balanced then
    print("Ship levelled out under this trim.")
  else
    print(string.format("NOTE: still %+.0f deg off level -- limited", pitchNow))
    print("pitch authority. FLY keeps learning it.")
  end
  if math.abs(settled[axis]) > 30 then
    print("")
    print("WARNING: it settled far from level even")
    print("with balanced lift -- the hull may not be")
    print("righting fully. Check balloon spacing.")
  end
  print(string.format("hover ~%.1f (saved as baseline)", hover))
  print("")
  print("Ship is holding hover. Ready to FLY.")
  print(""); print("[Enter]"); read()
  return true
end

-- ============================ WIRING SETUP ============================
-- Probe tilkoblede ting + la brukeren identifisere hvilken redstone-side som driver FRONT- og
-- BAK-ballongen. Redstone-links er IKKE peripheraler (CC kan ikke "se" dem), saa vi pulser hver
-- ledige side og spoer hvilken brenner som taentes. Lagres til sides.txt (overstyrer config).
local function allSidesOff() for _, s in ipairs(SIDES) do redstone.setAnalogOutput(s, 0) end end

local function runWiring()
  term.clear(); term.setCursorPos(1, 1)
  print("==== WIRING SETUP ====")
  print("(best on the ground -- it pulses the burners)")
  print("")
  print("Connected per side:")
  local linkSides = {}
  for _, side in ipairs(SIDES) do
    if peripheral.isPresent(side) then
      print(string.format("  %-7s peripheral: %s", side, tostring(peripheral.getType(side))))
    else
      print(string.format("  %-7s free (possible redstone link)", side))
      linkSides[#linkSides + 1] = side
    end
  end
  print("")
  print("Redstone links aren't detectable, so I PULSE each free side --")
  print("watch which balloon's burner lights up.")
  print("[Enter] = pulse-walk   |   type 'm' = assign manually")
  local choice = read()

  local fore, aft = nil, nil
  if choice == "m" or choice == "M" then
    repeat write("FRONT balloon side: "); fore = read() until validSide(fore)
    write("AFT balloon side (Enter = none): "); local a = read()
    aft = (a ~= "" and validSide(a)) and a or nil
  else
    local level = math.min(cfg.maxSignal, 8)
    for _, side in ipairs(linkSides) do
      while true do   -- [r] pulser samme side igjen -- lett aa gaa glipp av et blaff
        term.clear(); term.setCursorPos(1, 1); print("==== WIRING SETUP: pulsing ====")
        print(string.format("Pulsing '%s' at %d -- WATCH THE BURNERS...", side, level))
        redstone.setAnalogOutput(side, level); sleep(3.0); redstone.setAnalogOutput(side, 0)
        write(string.format("'%s' fired: [f]ront / [a]ft / [n]one / [r]=again: ", side))
        local a = read():lower()
        if a == "f" then fore = side; break
        elseif a == "a" then aft = side; break
        elseif a ~= "r" then break end
      end
    end
  end
  allSidesOff()

  term.clear(); term.setCursorPos(1, 1); print("==== WIRING RESULT ====")
  print("FRONT side: " .. tostring(fore))
  print("AFT side:   " .. tostring(aft or "none (single balloon)"))
  if not fore then
    print("")
    print("No FRONT side identified -- NOT saving. Re-run.")
  else
    saveSides({ fore = fore, aft = aft })
    cfg.foreSide, cfg.aftSide = fore, aft   -- bruk umiddelbart denne oekten
    print(""); print(">>> Saved to sides.txt and applied.")
  end
  print(""); print("[Enter]"); read()
  return fore ~= nil
end

-- ============================ MANUAL (Lag 2) ============================
local function runManual(foreArg, aftArg)
  local est, c = Estimator.new(cfg), loadCalib()
  local fore = clamp(round(tonumber(foreArg) or c.uHoverFront), 0, cfg.maxSignal)
  local aft = clamp(round(tonumber(aftArg) or c.uHoverRear), 0, cfg.maxSignal)
  local log = Logger.new("manual.log", "# manual: t, fore, aft, height, pitch, vVel, pitchRate")
  local t0 = now()
  local function render()
    local h, p = sensors.readAltitude(), sensors.readPitch()
    if h and p then est:update(now(), h, p) end
    driveValves(fore, aft)
    log:row({ now() - t0, fore, aft, est.altitude, est.pitch, est.verticalVelocity, est.pitchRate })
    term.clear(); term.setCursorPos(1, 1); print("==== MANUAL (Layer 2) ====")
    print(string.format("fore %d   aft %d   (max %d)", fore, aft, cfg.maxSignal))
    print(string.format("Height %8.2f   vVel  %6.2f b/s", est.altitude, est.verticalVelocity))
    print(string.format("Pitch  %8.2f   pRate %6.2f deg/s", est.pitch, est.pitchRate))
    print(""); print("UP/DOWN both  LEFT/RIGHT front  W/S rear  SPACE off  Q quit")
  end
  render()
  local timer = os.startTimer(cfg.dt)
  while true do
    local ev = { os.pullEvent() }
    if ev[1] == "timer" and ev[2] == timer then render(); timer = os.startTimer(cfg.dt)
    elseif ev[1] == "key" then
      local k = ev[2]
      if k == keys.up then fore = fore + 1; aft = aft + 1
      elseif k == keys.down then fore = fore - 1; aft = aft - 1
      elseif k == keys.right then fore = fore + 1 elseif k == keys.left then fore = fore - 1
      elseif k == keys.w then aft = aft + 1 elseif k == keys.s then aft = aft - 1
      elseif k == keys.space then fore = 0; aft = 0 elseif k == keys.q then break end
      fore = clamp(fore, 0, cfg.maxSignal); aft = clamp(aft, 0, cfg.maxSignal); render()
    end
  end
  allOff(); print("Manual ended. Valves off.")
end

-- ============================ CALIBRATE (Lag 3) ============================
local function doCalibrate()
  print("CALIBRATION. Open sky above. It lifts off and learns by itself.")
  print("CTRL+T aborts (holds last command). Starting in 2s..."); sleep(2.0)
  local calib, msg = calibration.run(cfg, sensors, driveValves)
  if calib then
    saveCalib(calib)
    driveValves(calib.uHoverFront, calib.uHoverRear)
  end
  return calib, msg
end
local function runCal()
  local calib, msg = doCalibrate()
  term.clear(); term.setCursorPos(1, 1)
  if calib then
    print("==== CALIBRATION DONE (saved) ====")
    print(string.format("uHoverFront %.2f  uHoverRear %.2f", calib.uHoverFront, calib.uHoverRear))
    print(string.format("lambda %.3f  g %.3f", calib.lambda, calib.g))
    print(string.format("pitchSign %d  pitchGain %.3f", calib.pitchSign, calib.pitchGain))
    if calib.pitchResidual and math.abs(calib.pitchResidual) > 5 then
      print(string.format("!! residual pitch %.1f deg -- limited diff authority", calib.pitchResidual))
    end
    print("Holding hover (valves stay set).")
  else
    print("==== CALIBRATION ABORTED ====\nReason: " .. tostring(msg))
    local c = loadCalib(); driveValves(c.uHoverFront, c.uHoverRear)
  end
  print(""); print("[Enter]"); read()
end

-- ============================ OBSERVE (Lag 4, ÅPEN SLØYFE) ============================
-- Driver et bundet opp/ned-profil (observeren styrer IKKE) og logger v_hat mot v_meas + L_hat.
-- Slik validerer/tuner vi observeren: leder estimatet, henger det etter, eller svinger det?
local function runObserve()
  if not fs.exists(CALIB_FILE) then print("No calibration. Run 'cal' first."); print("[Enter]"); read(); return end
  local calib = loadCalib()
  local est, obs = Estimator.new(cfg), Observer.new(cfg, calib)
  local log = Logger.new("observe.log", "# observe: t, cmd, height, v_meas, v_hat, L_hat, bias, e_v")
  print("OBSERVE (open-loop). Observer watches, does NOT steer. CTRL+T to stop."); sleep(1.0)
  local t0, lastT, phaseT, phase = now(), now(), now(), 0
  while true do
    local t = now(); local dt = t - lastT; lastT = t
    if dt <= 0 or dt > 1 then dt = cfg.dt end
    if now() - phaseT > 12 then phase = (phase + 1) % 2; phaseT = now() end
    local vDes = (phase == 0) and 0.4 or -0.4
    local cmd = velHoldCommon(calib, vDes, est.verticalVelocity)
    driveValves(cmd, cmd)
    local h, p = sensors.readAltitude(), sensors.readPitch()
    if h and p then est:update(t, h, p) end
    local e_v = obs:step(cmd, cmd, est.verticalVelocity, dt, false)
    local Lh = obs:L_hat()
    log:row({ t - t0, cmd, est.altitude, est.verticalVelocity, obs.v_hat, Lh, obs.bias, e_v })
    term.clear(); term.setCursorPos(1, 1); print("==== OBSERVE (Layer 4, open-loop) ====")
    print(string.format("cmd %.2f  (vDes %+.1f)", cmd, vDes))
    print(string.format("v_meas %6.2f   v_hat %6.2f   e_v %+.2f", est.verticalVelocity, obs.v_hat, e_v))
    print(string.format("L_hat  %6.3f   bias %+.3f", Lh, obs.bias))
    print(string.format("height %.1f", est.altitude))
    print("")
    print("Good: v_hat tracks v_meas (leads slightly, no lag/oscillation).")
    print("If v_hat lags -> raise Kf/lambda. If it oscillates -> lower Kf.")
    sleep(cfg.dt)
  end
end

-- ============================ FLY (Lag 5-8, LUKKET SLØYFE) ============================
local function runFly()
  local calib = loadCalib()
  -- v3 trenger IKKE den skjore staircase-kalibreringen lenger (hoyde er g-fri, pitch self-learning).
  -- Det eneste profilen maa ha er pitchSign (fra Sign test) + riktig wiring. Mangler den -> veiled.
  if not fs.exists(CALIB_FILE) then
    term.clear(); term.setCursorPos(1, 1)
    print("No profile for this ship yet.")
    print("")
    print("Run the setup wizard first:")
    print("  ap setup    (or menu choice 1)")
    print("")
    print("It maps the wiring and measures the pitch")
    print("sign in one short guided auto-flight.")
    print("Pitch & altitude self-learn from there.")
    print(""); print("[Enter]"); read(); return
  end

  local launchAlt = sensors.readAltitude() or 0
  local target = loadTarget() or (launchAlt + cfg.takeoffOffset)
  local est = Estimator.new(cfg)
  local obs = Observer.new(cfg, calib)
  local div = safety.newDivergence(cfg.evDivergeAlpha)
  local log = Logger.new("fly.log",
    "# fly: t, state, height, target, pitch, v_meas, v_hat, L_front, L_rear, L_hat, u_sum, bias, fore, aft, e_v, iTrim, hTrim")
  print(string.format("FLY -> target %.1f  (sign %d). CTRL+T to stop.", target, calib.pitchSign)); sleep(1.0)

  local pctl = pitch.new(cfg, calib)               -- self-learning pitch-kontroller (integral-trim)
  local actl = altitude.new(cfg, calib)            -- self-learning g-fri hoyde-kontroller
  local state, reason, lastT, t0 = "FLYING", "", now(), now()
  local prevSat, lastTrimSave = false, now()

  -- pilot-lenke: kringkast status + ta imot enkle kommandoer fra pilot-konsollen (om modem finnes)
  if link.open() then print("Pilot link: ON (rednet)") else print("Pilot link: none (solo)") end
  local stopReq, mode = false, "fly"   -- mode = siste pilot-intensjon: fly / hold / land
  local function handleCmd(msg)
    if type(msg) ~= "table" then return end
    if msg.cmd == "target" and tonumber(msg.value) then target = tonumber(msg.value); saveTarget(target); mode = "fly"
    elseif msg.cmd == "adjust" and tonumber(msg.delta) then target = target + tonumber(msg.delta); saveTarget(target); mode = "fly"
    elseif msg.cmd == "hold" then target = est.altitude; saveTarget(target); mode = "hold"
    elseif msg.cmd == "land" then target = launchAlt + 3; saveTarget(target); mode = "land"
    elseif msg.cmd == "set" then applySetting(msg.key, msg.value, true)
    elseif msg.cmd == "stop" then stopReq = true end
  end
  -- vent ~dt sekunder MEN behandle innkommende kommandoer underveis (ikke-blokkerende styring)
  local function serve(secs)
    local timer = os.startTimer(secs)
    while true do
      local e1, e2, e3, e4 = os.pullEvent()
      if e1 == "timer" and e2 == timer then return
      elseif e1 == "rednet_message" and e4 == link.CMD then handleCmd(e3) end
    end
  end

  while true do
    local t = now(); local dt = t - lastT; lastT = t
    if dt <= 0 or dt > 1 then dt = cfg.dt end
    local h, p = sensors.readAltitude(), sensors.readPitch()
    local valid = safety.sensorOk(h, p)
    if valid then est:update(t, h, p) end

    -- pitch runaway-vakt: aldri la skipet tippe ukontrollert (siste skanse, fortegns-uavhengig)
    if valid and state == "FLYING" and math.abs(est.pitch) > cfg.pitchFaultDeg then
      state = "FAULT"; reason = "pitch out of range"
    end

    local frontHeat, rearHeat, sat, diag, u_diff = nil, nil, true, {}, 0
    if state == "FLYING" and valid then
      local lowAlt = (est.altitude - launchAlt) < cfg.lowAltCaution
      local uFc, uRc, ad = actl:compute(est.altitude, target, est.verticalVelocity, dt, prevSat, lowAlt)
      u_diff = pctl:compute(est.pitch, est.pitchRate, dt, prevSat)
      if not cfg.aftSide then u_diff = 0 end   -- ett-ballong-skip: ingen differensiell autoritet
      local floor
      frontHeat, rearHeat, floor = mixer.mix(cfg, calib, uFc, uRc, u_diff)
      sat = (frontHeat <= floor + 0.01 or frontHeat >= cfg.maxSignal - 0.01
          or rearHeat <= floor + 0.01 or rearHeat >= cfg.maxSignal - 0.01)
      diag = ad
    else
      -- FAULT eller ugyldig sensor: HOLD hover-baseline (aldri null = stup)
      frontHeat, rearHeat = safety.holdOutputs(calib)
    end

    local of, oa = driveValves(frontHeat, rearHeat)

    -- observer oppdateres med FAKTISK kommandert (float) loft + maalt fart
    local e_v = 0
    if valid then
      e_v = obs:step(frontHeat, rearHeat, est.verticalVelocity, dt, sat)
      local evAvg = div:update(e_v)
      if evAvg > cfg.evDivergeFault then state = "FAULT"; reason = "observer divergence"
      elseif state == "FAULT" and evAvg < cfg.evDivergeFault * 0.5
             and math.abs(est.pitch) < cfg.pitchFaultDeg * 0.6 and reason ~= "" then state = "FLYING" end
    else
      state = "FAULT"; reason = "sensor invalid"
    end

    log:row({ t - t0, state, est.altitude, target, est.pitch, est.verticalVelocity, obs.v_hat,
      obs.L_front, obs.L_rear, obs:L_hat(), diag.u_sum or 0, obs.bias, of, oa, e_v, pctl.iTrim, actl.uTrim })

    -- persister laerte trims som warm-start (self-updating; taaler restart + last-endring)
    if now() - lastTrimSave > 10 then
      calib.pitchTrim = pctl.iTrim; calib.hoverTrim = actl.uTrim; saveCalib(calib); lastTrimSave = now()
    end
    prevSat = sat

    term.clear(); term.setCursorPos(1, 1)
    if state == "FAULT" then
      print("!!!! FAULT: " .. reason .. " !!!!")
      print("Holding hover baseline (NOT zero). Take manual control / recalibrate.")
    else
      print(diag.hold and "==== FLY: HOLDING (band) ====" or "==== FLY: moving to band ====")
    end
    print(string.format("Height %7.1f / %.0f  (e %+.1f)", est.altitude, target, target - est.altitude))
    print(string.format("Pitch  %7.1f  pRate %+.1f  trim %+.2f", est.pitch, est.pitchRate, pctl.iTrim))
    print(string.format("v_meas %6.2f  v_hat %6.2f  (e_v %+.2f)", est.verticalVelocity, obs.v_hat, e_v))
    print(string.format("u_sum %.1f  hTrim %+.2f  vDes %+.2f", diag.u_sum or 0, actl.uTrim, diag.v_des or 0))
    print(string.format("fore %2d   aft %2d", of, oa))

    -- kringkast status til pilot-konsollen
    link.publishStatus({
      state = state, reason = reason, height = est.altitude, target = target,
      pitch = est.pitch, vVel = est.verticalVelocity, pitchRate = est.pitchRate,
      uSum = diag.u_sum or 0, hold = diag.hold or false, mode = mode,
      iTrim = pctl.iTrim, hTrim = actl.uTrim, fore = of, aft = oa,
      climbSpeed = cfg.vMaxUp, holdBand = cfg.altDeadband, maxPower = cfg.maxSignal,
    })

    serve(cfg.dt)   -- vent dt + behandle pilot-kommandoer (erstatter sleep)
    if stopReq then print("Pilot STOP -- holding hover, back to standby."); break end
  end
end

-- ============================ SETUP-WIZARD (foerstegang / nytt skip) ============================
-- ETT stoppested for alt skip-spesifikt: wiring (hvilken side driver hvilken ballong) + sign test
-- (kort selvgaaende flytur som maaler pitch-fortegn og ~hover). Etterpaa er skipet klart til FLY.
local function runSetup()
  signtestAirborne = false   -- nullstill foer wizarden (ellers kan cleanup arve forrige kjoering)
  term.clear(); term.setCursorPos(1, 1)
  print("==== SHIP SETUP WIZARD ====")
  print("")
  print("Two steps, fully guided:")
  print(" 1) Wiring    -- map front/aft burner sides")
  print(" 2) Sign test -- short auto-flight that")
  print("    measures pitch response + balance")
  print("")
  print("Just needs open sky above. It's FINE if the")
  print("ship lies over on the ground -- it rights")
  print("itself airborne, and nothing is measured")
  print("until it has.")
  print("")
  write("[Enter] = start, q = cancel: ")
  if read():lower() == "q" then return end

  if not runWiring() then
    print("Wiring was not saved -- wizard stopped."); print("[Enter]"); read(); return
  end

  term.clear(); term.setCursorPos(1, 1)
  print("==== STEP 2: SIGN TEST ====")
  print("")
  print("The ship lifts off BY ITSELF, waits until")
  print("it has righted and steadied in the air,")
  print("rocks front/aft to find the pitch axis,")
  print("learns the trim that levels it, then comes")
  print("back down to hover. Takes 2-4 minutes.")
  print("")
  write("[Enter] = lift off, q = cancel: ")
  if read():lower() == "q" then return end
  runSigntest()

  term.clear(); term.setCursorPos(1, 1)
  if profileComplete() then
    print("==== SETUP COMPLETE ====")
    print("")
    print("Profile saved. The ship is ready:")
    print("  - FLY from the menu, or")
    print("  - standby + ENGAGE from the cockpit")
  else
    print("==== SETUP INCOMPLETE ====")
    print("")
    print("Sign test did not finish -- run the")
    print("wizard again (menu choice 1).")
  end
  print(""); print("[Enter]"); read()
end

-- ============================ STANDBY (pilot-fjernstart) ============================
-- Lytter etter ENGAGE fra pilot-konsollen og starter fly. Kringkaster IDLE-heartbeat slik at
-- konsollen vet at maskinrommet er online og klart. Default etter boot; [S] aapner setup-wizarden.
local function awaitCmd(secs)
  local timer = os.startTimer(secs)
  while true do
    local e1, e2, e3, e4 = os.pullEvent()
    if e1 == "timer" and e2 == timer then return nil
    elseif e1 == "char" and (e2 == "s" or e2 == "S") then return "setup"
    elseif e1 == "rednet_message" and e4 == link.CMD and type(e3) == "table" then
      if e3.cmd == "engage" or e3.cmd == "stop" then return e3.cmd end
      if e3.cmd == "set" then applySetting(e3.key, e3.value, true) end   -- juster innstillinger i standby
      -- target/hold/land ignoreres mens vi staar i standby
    end
  end
end

local function runStandby()
  while true do
    -- re-proev modem hver runde: fanger et modem som plugges til mens vi staar i standby
    local hasModem = link.open()
    local h = sensors.readAltitude() or 0
    local p = sensors.readPitch() or 0
    local flyable = fs.exists(CALIB_FILE)    -- kan fly (evt. eldre profil uten maalt-flagg)
    local complete = profileComplete()       -- wiring + faktisk MAALT pitch-fortegn
    link.publishStatus({ state = "IDLE", height = h, target = loadTarget() or h, pitch = p,
      vVel = 0, hold = false, ready = flyable, fore = 0, aft = 0, uSum = 0, iTrim = 0, hTrim = 0,
      climbSpeed = cfg.vMaxUp, holdBand = cfg.altDeadband, maxPower = cfg.maxSignal })
    term.clear(); term.setCursorPos(1, 1)
    print("==== STANDBY (pilot remote) ====")
    if complete then print("Profile: READY")
    elseif flyable then print("Profile: LEGACY   ([S] = re-run setup)")
    else print("Profile: NOT SET UP -> press [S]") end
    if hasModem then print("Link: modem OK (broadcasting to cockpit)")
    else print("Link: NO MODEM -- cockpit shows OFFLINE!")
         print("      attach a modem to this PC") end
    print(string.format("Height %.1f   Pitch %+.1f", h, p))
    print("Waiting for ENGAGE from the pilot console...")
    print("[S] setup wizard    CTRL+T then 'ap' = menu")
    local cmd = awaitCmd(0.5)
    if cmd == "setup" then
      runSetup()
    elseif cmd == "engage" then
      if flyable then
        runFly()                                   -- blokkerer til pilot STOP / FAULT / CTRL+T
        local c = loadCalib(); driveValves(c.uHoverFront, c.uHoverRear)  -- hold hover etter stop
      else
        link.publishStatus({ state = "SETUP NEEDED", reason = "press S on autopilot PC", ready = false })
        sleep(2.0)
      end
    end
  end
end

-- ============================ BOOT (startup-inngang) ============================
-- Foerste boot uten komplett profil -> tilby wizarden. Timeout -> standby, slik at en ubemannet
-- reboot (chunk-reload) aldri blir staaende og vente paa tastetrykk.
local function runBoot()
  if not profileComplete() then
    term.clear(); term.setCursorPos(1, 1)
    print("==== AIRSHIP AUTOPILOT ====")
    print("")
    print(fs.exists(CALIB_FILE)
      and "Ship profile: LEGACY (sign not measured)."
      or "No ship profile on this computer yet.")
    print("")
    print("[Enter] = first-time setup wizard")
    print("(any other key, or 10 s -> standby)")
    local timer = os.startTimer(10)
    while true do
      local e = { os.pullEvent() }
      if e[1] == "timer" and e[2] == timer then break
      elseif e[1] == "key" then
        if e[2] == keys.enter then runSetup() end
        break
      end
    end
  end
  runStandby()
end

-- ============================ PROFIL / MENY ============================
local function showProfile()
  local c = loadCalib()
  print("==== PROFILE ====")
  print(string.format("uHoverFront %.2f  uHoverRear %.2f", c.uHoverFront, c.uHoverRear))
  print(string.format("lambda %.3f  g %.3f", c.lambda, c.g))
  print(string.format("pitchSign %d (%s)  axis a%d", c.pitchSign,
    c.signMeasured and "measured" or "DEFAULT -- run setup", c.pitchIndex or cfg.pitchIndex))
  print(string.format("wiring: fore=%s  aft=%s%s", cfg.foreSide, tostring(cfg.aftSide),
    fs.exists(SIDES_FILE) and "" or " (config default)"))
  print("calib.txt: " .. (fs.exists(CALIB_FILE) and "saved" or "none (defaults)"))
  local tH = loadTarget()
  print("Target: " .. (tH and string.format("%.1f", tH) or "auto (start + offset)"))
end

local function menu()
  while true do
    term.clear(); term.setCursorPos(1, 1)
    print("===== AIRSHIP AUTOPILOT v3 =====")
    print(" Setup")
    print("  1) Setup wizard  (wiring + sign test)")
    print("  2) Wiring only        3) Sign test only")
    print(" Fly")
    print("  4) FLY / hold         5) Standby (pilot)")
    print("  6) Set target altitude")
    print(" Tools")
    print("  7) Monitor  8) Manual  9) Full calibrate")
    print("  o) Observe  p) Profile/reset  q) Quit")
    print("")
    local prof = profileComplete() and "Profile: OK"
      or (fs.exists(CALIB_FILE) and "Profile: LEGACY (1 = re-setup)" or "Profile: NOT SET UP -> 1")
    print((sensors.present() and "Sensors: OK   " or "SENSORS MISSING!   ") .. prof)
    print(string.format("Wiring: fore=%s aft=%s", cfg.foreSide, tostring(cfg.aftSide)))
    write("Choice: ")
    local k = read():lower()
    if k == "1" then return "setup"
    elseif k == "2" then return "wiring"
    elseif k == "3" then return "signtest"
    elseif k == "4" then return "fly"
    elseif k == "5" then return "standby"
    elseif k == "6" then
      write("Target (empty = auto): "); local s = read()
      if s == "" then if fs.exists(TARGET_FILE) then fs.delete(TARGET_FILE) end; print("Target cleared.")
      elseif tonumber(s) then saveTarget(tonumber(s)); print("Target = " .. s) else print("Invalid number.") end
      print("[Enter]"); read()
    elseif k == "7" then return "monitor"
    elseif k == "8" then return "manual"
    elseif k == "9" then return "cal"
    elseif k == "o" then return "observe"
    elseif k == "p" then
      term.clear(); term.setCursorPos(1, 1); showProfile()
      print("\nR = reset SHIP PROFILE (new ship), Enter = back"); local a = read():lower()
      if a == "r" then
        for _, f in ipairs({ CALIB_FILE, SIDES_FILE, TARGET_FILE, SETTINGS_FILE }) do
          if fs.exists(f) then fs.delete(f) end
        end
        print("Ship profile reset -- setup wizard on next boot."); print("[Enter]"); read()
      end
    elseif k == "q" then return "exit" end
  end
end

-- ============================ DISPATCH ============================
sensors.init(cfg)
local args = { ... }
-- bruk persistert wiring (front/aft-sider) hvis satt -- overstyrer config-defaults per skip
local sv = loadSides()
if sv then cfg.foreSide = sv.fore; cfg.aftSide = sv.aft end
-- bruk MAALT gimbal-akse for pitch hvis sign-testen har funnet den (bygg-orientering varierer)
local cal0 = loadCalib()
if cal0.pitchIndex then cfg.pitchIndex = cal0.pitchIndex end
-- bruk persisterte live-innstillinger (klatre-fart / hold-baand / maks-effekt)
local sset = loadSettings()
if sset then
  applySetting("climbSpeed", sset.climbSpeed, false)
  applySetting("holdBand", sset.holdBand, false)
  applySetting("maxPower", sset.maxPower, false)
end
if not sensors.present() then print("Sensors missing (altitude_sensor + gimbal_sensor)."); return end

-- kjor EN modus + opprydding. Returnerer (ok, err) saa menyloopen kan avslutte ved CTRL+T.
local function runMode(mode, a2, a3)
  local ok, err = pcall(function()
    if mode == "boot" then runBoot()
    elseif mode == "setup" then runSetup()
    elseif mode == "standby" then runStandby()
    elseif mode == "monitor" then runMonitor()
    elseif mode == "signtest" then runSigntest()
    elseif mode == "wiring" then runWiring()
    elseif mode == "manual" then runManual(a2, a3)
    elseif mode == "cal" then runCal()
    elseif mode == "observe" then runObserve()
    elseif mode == "fly" then runFly()
    else print("Unknown mode: " .. tostring(mode))
      print("Valid: setup wiring signtest fly standby monitor manual cal observe boot") end
  end)
  -- opprydding: aktive driv-modus -> hold hover (aldri etterlat drivende uten beskjed);
  -- monitor styrer ingenting (la utgangene staa).
  if mode == "manual" or mode == "wiring" then
    allOff()
  elseif mode == "signtest" or mode == "setup" then
    -- Ctrl+T midt i sign-testen: luftbaaren -> hold hover (ALDRI frittfall); paa bakken -> alt av
    if signtestAirborne then
      local c = loadCalib(); driveValves(c.uHoverFront, c.uHoverRear)
    else
      allOff()
    end
  elseif mode == "cal" or mode == "fly" or mode == "observe" or mode == "standby" or mode == "boot" then
    if fs.exists(CALIB_FILE) then
      local c = loadCalib(); driveValves(c.uHoverFront, c.uHoverRear)   -- hold, ikke null (aldri stup)
    else
      allOff()   -- aldri floeyet under vaar kontroll -> skipet staar paa bakken; ikke varm brennere
    end
  end
  if not ok and err ~= "Terminated" then print("ERROR: " .. tostring(err)); print("[Enter]"); read() end
  return ok, err
end

if args[1] then
  -- CLI-arg: kjor en gang og avslutt
  runMode(args[1], args[2], args[3])
else
  -- meny-drevet: tilbake til hovedskjermen etter hver modus (CTRL+T avslutter appen)
  while true do
    local m = menu()
    if m == "exit" then break end
    local ok, err = runMode(m)
    if not ok and err == "Terminated" then break end
  end
end