--------------------------------------------------------------------
-- lib/pilotage.lua : regulation et mixage.
--
-- ASSERVISSEMENT EN CASCADE + FEEDFORWARD :
--   1. feedforward : rpm d'equilibre (sustentation) calcule depuis
--      POIDS et POUSSEE_HELICE_MAX; le PID ne corrige qu'autour de
--      ce point au lieu de devoir le trouver par integrale
--   2. boucle externe : erreur d'altitude -> consigne de vitesse
--      verticale (P simple, bornee V_MONTEE_MAX / V_DESCENTE_MAX)
--   3. boucle interne : PID sur la vitesse verticale MESUREE
--      (velocity sensor vertical calibre; repli: derivee filtree de
--      l'altitude) -> delta rpm autour du feedforward
--   4. assiette : PID tangage + roulis, corrections signees par coin
--   5. propulseurs : mixage differentiel avance / virage
--------------------------------------------------------------------
local Pilotage = {}

local SIGNES = {
  lb = { bf = -1, lr =  1 },
  rb = { bf = -1, lr = -1 },
  lf = { bf =  1, lr =  1 },
  rf = { bf =  1, lr = -1 },
}

-- assiette normalisee: une helice AVANT pousse plus quand le tangage
-- est negatif (nez bas), une helice GAUCHE quand le roulis est
-- negatif (penche a gauche)
local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1

function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
  local p = {}
  local VMAX = conf.VITESSE_RSC_MAX
  local gains = etat.pid or conf.PID

  local pidMontee   = Pid.nouveau(gains.montee,   -VMAX, VMAX, 20)
  local pidDescente = Pid.nouveau(gains.descente, -VMAX, VMAX, 20)
  local pidTangage  = Pid.nouveau(gains.tangage,  -VMAX / 2, VMAX / 2)
  local pidRoulis   = Pid.nouveau(gains.roulis,   -VMAX / 2, VMAX / 2)

  local consigneRampe = nil
  local regimePrec = nil
  local facteurProp = 1.0
  local altPrec, vFiltre = nil, 0   -- repli de mesure de vitesse

  -- ADAPTATION (transport a charge variable, remise a zero au
  -- decollage via razAdaptation)
  local rpmAdapt = 0                -- correction adaptative du ff
  local trimTangage, trimRoulis = 0, 0  -- trim d'assiette adaptatif

  ------------------------------------------------------------------
  -- FEEDFORWARD : rpm d'equilibre d'UNE helice
  -- poussee(rpm) = pousseeEffective * (rpm/VMAX)^exp, avec la
  -- poussee CORRIGEE PAR LA PRESSION (elle diminue avec l'altitude):
  --   pousseeEffective = POUSSEE_HELICE_MAX * pression / PRESSION_REF
  -- equilibre: 4 * poussee(rpm) = POIDS (a vide) + charge estimee
  ------------------------------------------------------------------
  local function pousseeEffective()
    local facteur = materiel.lirePression() / conf.PRESSION_REF
    return conf.POUSSEE_HELICE_MAX * math.max(facteur, 0.05)
  end

  function p.rpmSustentation()
    local ratio = (conf.POIDS / 4) / pousseeEffective()
    if ratio <= 0 then return 0 end
    if ratio >= 1 then return VMAX end
    return VMAX * ratio ^ (1 / conf.POUSSEE_EXPOSANT)
  end

  -- poids total estime (a vide + charge), inverse de la courbe de
  -- poussee au rpm de sustentation adapte, a la pression courante
  function p.poidsEstime()
    local rpm = math.max(0, math.min(VMAX, p.rpmSustentation() + rpmAdapt))
    return 4 * pousseeEffective() * (rpm / VMAX) ^ conf.POUSSEE_EXPOSANT
  end

  -- plafond de poids total (regle des 60% a la pression de reference)
  function p.poidsMax()
    return conf.RATIO_CHARGE_MAX * 4 * conf.POUSSEE_HELICE_MAX
  end

  function p.trim()
    return trimTangage, trimRoulis
  end

  function p.razAdaptation()
    rpmAdapt, trimTangage, trimRoulis = 0, 0, 0
  end

  ------------------------------------------------------------------
  -- MESURE DE VITESSE VERTICALE
  -- capteur calibre si present, sinon derivee filtree de l'altitude
  ------------------------------------------------------------------
  function p.vitesseVerticale(altitude, dt)
    if materiel.veloParAxe["vertical"] then
      return materiel.lireVitesseAxe("vertical")
    end
    if altPrec and dt > 0 then
      local brute = (altitude - altPrec) / dt
      vFiltre = vFiltre + 0.3 * (brute - vFiltre)
    end
    altPrec = altitude
    return vFiltre
  end

  function p.gains()
    return gains
  end

  function p.reglerGains(nouveaux)
    gains = nouveaux
    etat.pid = nouveaux
    pidMontee:reglerGains(gains.montee)
    pidDescente:reglerGains(gains.descente)
    pidTangage:reglerGains(gains.tangage)
    pidRoulis:reglerGains(gains.roulis)
  end

  function p.reglerFacteurProp(f)
    facteurProp = math.max(0, math.min(1, f))
  end

  function p.raz()
    pidMontee:raz()
    pidDescente:raz()
    pidTangage:raz()
    pidRoulis:raz()
    consigneRampe = nil
    regimePrec = nil
    altPrec, vFiltre = nil, 0
  end

  local function borner(v, mini, maxi)
    if v < mini then return mini elseif v > maxi then return maxi end
    return v
  end

  -- applique base + trim + corrections d'assiette aux 4 helices.
  -- `adapter` (booleen): en quasi-stationnaire, le trim absorbe
  -- lentement la composante statique des corrections (CG decale par
  -- la cargaison); le PID reste centre sur la dynamique.
  local function appliquerHelices(base, tangage, roulis, dt,
                                  anglesCibles, adapter)
    local corrTangage = SIGNE_TANGAGE
      * pidTangage:calculer(anglesCibles.tangage or 0, tangage, dt)
    local corrRoulis = SIGNE_ROULIS
      * pidRoulis:calculer(anglesCibles.roulis or 0, roulis, dt)
    if adapter then
      local taux = conf.ADAPTATION.TAUX_TRIM * dt
      trimTangage = trimTangage + taux * corrTangage
      trimRoulis  = trimRoulis + taux * corrRoulis
    end
    for role, s in pairs(SIGNES) do
      materiel.reglerRsc(role, borner(base
        + s.bf * (trimTangage + corrTangage)
        + s.lr * (trimRoulis + corrRoulis), 0, VMAX))
    end
  end

  ------------------------------------------------------------------
  -- UN PAS DE REGULATION EN VOL
  ------------------------------------------------------------------
  function p.reguler(consigneY, angles, avance, virage, dt)
    local altitude = materiel.lireAltitude()
    local tangage, roulis = materiel.lireAssiette()

    -- rampe de consigne
    if consigneRampe == nil then consigneRampe = altitude end
    local pas = conf.VITESSE_RAMPE * dt
    consigneRampe = consigneRampe
      + borner(consigneY - consigneRampe, -pas, pas)

    -- boucle externe: erreur alt -> vitesse verticale cible
    local vCible = borner(gains.alt.kp * (consigneRampe - altitude),
      -conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX)

    -- boucle interne: vitesse verticale -> delta rpm (double regime)
    local vMesuree = p.vitesseVerticale(altitude, dt)
    local montee = vCible >= 0
    local pid = montee and pidMontee or pidDescente
    if regimePrec ~= nil and regimePrec ~= montee then pid:raz() end
    regimePrec = montee
    local deltaRpm = pid:calculer(vCible, vMesuree, dt)

    -- GAIN SCHEDULING: la boucle de vitesse est mise a l'echelle du
    -- poids (delta de poussee requis proportionnel a la masse)
    if etat.poidsCalibration and etat.poidsCalibration > 0 then
      deltaRpm = deltaRpm
        * borner(p.poidsEstime() / etat.poidsCalibration, 0.5, 2.0)
    end

    -- ADAPTATION DE POIDS: en quasi-stationnaire, le residu du PID
    -- est transfere lentement vers le feedforward (le drone se pese)
    local stationnaire = math.abs(vCible) < 0.2
      and math.abs(vMesuree) < 0.3
    if stationnaire then
      rpmAdapt = borner(
        rpmAdapt + conf.ADAPTATION.TAUX_POIDS * dt * deltaRpm,
        -VMAX / 2, VMAX / 2)
    end

    local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm,
      0, VMAX)
    appliquerHelices(base, tangage, roulis, dt, angles, stationnaire
      and math.abs(tangage) < conf.ANGLE_MAX
      and math.abs(roulis) < conf.ANGLE_MAX)

    -- propulseurs (differentiel, delestables en surcharge)
    local function b1(v) return borner(v, -1, 1) end
    materiel.reglerRsc("prop_r", b1(avance + virage) * VMAX * facteurProp)
    materiel.reglerRsc("prop_l", b1(avance - virage) * VMAX * facteurProp)

    return altitude, consigneRampe
  end

  ------------------------------------------------------------------
  -- PAS EN BOUCLE OUVERTE (identification pour pidmath)
  -- applique rpmSustentation + deltaRpm (assiette toujours asservie)
  -- et retourne la vitesse verticale mesuree
  ------------------------------------------------------------------
  function p.pasOuvert(deltaRpm, dt)
    local altitude = materiel.lireAltitude()
    local tangage, roulis = materiel.lireAssiette()
    local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm,
      0, VMAX)
    appliquerHelices(base, tangage, roulis, dt,
      { tangage = 0, roulis = 0 }, false)
    materiel.reglerRsc("prop_l", 0)
    materiel.reglerRsc("prop_r", 0)
    return p.vitesseVerticale(altitude, dt), altitude
  end

  function p.plaquer()
    for role in pairs(SIGNES) do
      materiel.reglerRsc(role, -VMAX)
    end
    materiel.reglerRsc("prop_l", 0)
    materiel.reglerRsc("prop_r", 0)
  end

  function p.arreter()
    materiel.toutArreter()
    p.raz()
  end

  return p
end

return Pilotage