#include "pid.h"

static inline float Saturation(float val, float min, float max)
{
  if (val < min) val = min;
  if (val > max) val = max;
  return val;
}

float PID_Update(float Value, float Current, PID_Data& Coef, float de)
{
  float p, i, d;

  float e = Value - Current;

  p = e * Coef.P.C;
  p = Saturation(p, Coef.P.Min, Coef.P.Max);

  i = Coef.i;
  i += e * Coef.I.C;
  i = Saturation(i, Coef.I.Min, Coef.I.Max);
  Coef.i = i;
  
  d = de * Coef.D.C;
  d = Saturation(d, Coef.D.Min, Coef.D.Max);

  return Saturation(p + i + d, Coef.Min, Coef.Max);
}