--------------------------------------------------------------------
-- lib/pilotage.lua : regulation et mixage.
--
--   base   = PID d'altitude (double regime montee / descente)
--   assiette = PID tangage + PID roulis vers des angles cibles
--              (0 sauf en mode drone ou le joystick les commande)
--   helice(coin) = base + signeBF*corrTangage + signeLR*corrRoulis
--   propulseurs  = mixage differentiel avance / virage
--
-- La consigne d'altitude passe par une rampe (VITESSE_RAMPE) pour
-- garantir des transitions douces et sans depassement.
--------------------------------------------------------------------
local Pilotage = {}

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

-- materiel.lireAssiette() est NORMALISE par la calibration gimbal:
-- tangage negatif = nez bas, roulis negatif = penche a gauche.
-- Une helice AVANT pousse plus quand le tangage est negatif, une
-- helice GAUCHE pousse plus quand le roulis est negatif => signes -1
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,   0, VMAX)
  local pidDescente = Pid.nouveau(gains.descente, 0, VMAX)
  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   -- reduit en cas de surcharge du moteur

  function p.gains()
    return gains
  end

  -- 0..1 : applique aux propulseurs (priorite de delestage en
  -- cas de surcharge du reseau de stress)
  function p.reglerFacteurProp(f)
    facteurProp = math.max(0, math.min(1, f))
  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.raz()
    pidMontee:raz()
    pidDescente:raz()
    pidTangage:raz()
    pidRoulis:raz()
    consigneRampe = nil
    regimePrec = nil
  end

  -- Un pas de regulation en vol.
  --   consigneY      : altitude cible
  --   angles         : { tangage = deg, roulis = deg } cibles
  --   avance, virage : commande propulseurs dans -1..1
  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
    local ecart = consigneY - consigneRampe
    if ecart > pas then ecart = pas elseif ecart < -pas then ecart = -pas end
    consigneRampe = consigneRampe + ecart

    -- double regime : montee ou descente
    local montee = (consigneRampe - altitude) >= 0
    local pid = montee and pidMontee or pidDescente
    if regimePrec ~= nil and regimePrec ~= montee then
      pid:raz()
    end
    regimePrec = montee
    local base = pid:calculer(consigneRampe, altitude, dt)

    -- assiette
    local corrTangage = SIGNE_TANGAGE
      * pidTangage:calculer(angles.tangage or 0, tangage, dt)
    local corrRoulis = SIGNE_ROULIS
      * pidRoulis:calculer(angles.roulis or 0, roulis, dt)

    for role, s in pairs(SIGNES) do
      local v = base + s.bf * corrTangage + s.lr * corrRoulis
      if v < 0 then v = 0 elseif v > VMAX then v = VMAX end
      materiel.reglerRsc(role, v)
    end

    -- propulseurs (differentiel)
    local function borner(v)
      if v > 1 then return 1 elseif v < -1 then return -1 end
      return v
    end
    materiel.reglerRsc("prop_r", borner(avance + virage) * VMAX * facteurProp)
    materiel.reglerRsc("prop_l", borner(avance - virage) * VMAX * facteurProp)

    return altitude, consigneRampe
  end

  -- Stationnement: helices a pleine puissance inversee (plaquage),
  -- propulseurs a 0
  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