#include "pid.h"

pid::pid() {
   // initial states
   lastarg = 0;
   argint = 0;
   init = true;

   // safe default gain if no setup (do nothing)
   p = 0.0;
   i = 0.0;
   d = 0.0;
   dt = 1.0;
}

void pid::set(double k_p, double k_i, double k_d, double delta_t) {
   p = k_p;
   i = k_i;
   d = k_d;
   dt = delta_t;
}

double pid::run(double arg) {

   double argdot;
   double res;

   if (init) {

      // first pass, set integral and derivative to 0
      //   to minimize startup transients
      argint = 0;                   // integral state
      argdot = 0;                   // derivative state
      init = false;                 // once and done

   } else {

      // else propagate states
      argint = argint + arg*dt;     // integral state
      argdot = (arg - lastarg)/dt;  // derivative state
   }

   // save last input
   lastarg = arg;

   // output calculation
   res = p*arg + i*argint + d*argdot;

   // return PID output
   return res;
}