--------------------------------------------------------------------
-- lib/pid.lua : regulateur PID concu pour le zero-depassement
--   - derivee calculee sur la MESURE (pas de coup de derivee au
--     changement de consigne)
--   - integrale conditionnelle: gelee quand la sortie sature dans le
--     sens de l'erreur (anti-emballement)
--------------------------------------------------------------------
local Pid = {}
Pid.__index = Pid

-- gains: { kp, ki, kd } ; bornes de sortie ; borne d'integrale
function Pid.nouveau(gains, borneMin, borneMax, iMax)
  return setmetatable({
    gains = gains,
    borneMin = borneMin,
    borneMax = borneMax,
    iMax = iMax or 40,
    integrale = 0,
    mesurePrec = nil,
  }, Pid)
end

function Pid:raz()
  self.integrale = 0
  self.mesurePrec = nil
end

function Pid:reglerGains(gains)
  self.gains = gains
end

function Pid:calculer(consigne, mesure, dt)
  local g = self.gains
  local erreur = consigne - mesure

  local derivee = 0
  if self.mesurePrec and dt > 0 then
    derivee = (mesure - self.mesurePrec) / dt
  end
  self.mesurePrec = mesure

  -- tentative d'integration bornee
  local integrale = self.integrale + erreur * dt
  if integrale > self.iMax then integrale = self.iMax end
  if integrale < -self.iMax then integrale = -self.iMax end

  local sortie = g.kp * erreur + g.ki * integrale - g.kd * derivee

  if sortie > self.borneMax then
    -- sature haut: on n'integre que si l'erreur tire vers le bas
    if erreur < 0 then self.integrale = integrale end
    return self.borneMax
  elseif sortie < self.borneMin then
    if erreur > 0 then self.integrale = integrale end
    return self.borneMin
  end
  self.integrale = integrale
  return sortie
end

return Pid