/*
  PID regulator.
 */

#include <stdio.h>
#include <stdint.h>
#include <sys/time.h>
#include "pid.h"

PID::PID(double min, double max, double kp, double kd, double ki) {
  Min = min;
  Max = max;
  Kp = kp;
  Kd = kd;
  Ki = ki;
  Integral = 0;
  PreviousError = 0;
  PreviousTime = getTime();
}

PID::~PID() {
}

int64_t PID::getTime(void) {

  struct timeval current_time;
  int64_t t;
  
  gettimeofday (&current_time, NULL);
  t = current_time.tv_sec * 1000000L + current_time.tv_usec;
  return t;
}

double PID::Update(double target, double value, int64_t time) {
  double error, out, dt;
  double P, I, D;
  double Derivative;

  // error
  error = target - value;

  // proportional part
  P = Kp * error;

  // integral part
  dt = time - PreviousTime;
  Integral += error * dt;
  I = Ki * Integral;

  // derivative part
  Derivative = (error - PreviousError) / dt;
  D = Kd * Derivative;

  // calculate output of regulator
  out = P + I + D;
  
  // Min/Max
  if (out > Max) out = Max;
  if (out < Min) out = Min;

  // save previous error value and time
  PreviousError = error;
  PreviousTime = time;

  return out;
}