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

-- gains { kp, ki, kd } lus PAR REFERENCE a chaque pas (edition vive)
function Pid.nouveau(gains, borneMin, borneMax, iMax, filtreD)
  return setmetatable({
    gains = gains,
    borneMin = borneMin,
    borneMax = borneMax,
    iMax = iMax or 40,
    filtreD = filtreD or 0.5,   -- 1 = brut, 0.5 = lisse
    integrale = 0,
    mesurePrec = nil,
    deriveeF = 0,
  }, Pid)
end

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

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

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

  if self.mesurePrec and dt > 0 then
    local brute = (mesure - self.mesurePrec) / dt
    self.deriveeF = self.deriveeF
      + self.filtreD * (brute - self.deriveeF)
  end
  self.mesurePrec = mesure

  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 * self.deriveeF

  if sortie > self.borneMax then
    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