/*
  The file implements a simple PID regulator.  The time resolution is
  1usec.  The update function should be called regularily as often as
  possible.
 */

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

/*
  This this the class constructor.  The min/max values are the minimum
  and maximum values output by the regulator.  The three regulator
  parameters must be adjusted by experiments.
 */
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();
}

/*
 * default parameter
 */
PID::PID() {
	  Min = 0.0;
	  Max = 1.0;
	  Kp = 1.0;
	  Kd = 0.0;
	  Ki = 0.0;
	  Integral = 0;
	  PreviousError = 0;
	  PreviousTime = GetTime();
}

/*
  Class destructor.
 */
PID::~PID() {
}

/*
  A static function to obtain a timestamp for the integrative/derivative
  parts of the regulator.
 */
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;
}

/*
  The function can be used to re-start the PID regulator.  Re-setting
  the integral and setting the current time as starting point.
 */
void PID::Start(void) {
  Integral = 0;
  PreviousError = 0;
  PreviousTime = GetTime();
}

/*
  This is the update function of the PID regulator.  Passing the
  target value of the regulator, the current process value and the
  timestamp when the value was obtained.
 */
double PID::Update(double target, double value, int64_t time) {
  double error, out;
  int64_t 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;

  if (dt > 0) {
    // derivative part
    Derivative = (error - PreviousError) / dt;
    D = Kd * Derivative;
  }
  else {
    D = 0;
  }

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

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

  return out;
}

/*
  This is a shortcut function of the update function without passing
  the timestamp.  The timestamp will be the time when the fuction is
  called.
 */
double PID::Update(double target, double value) {
  return Update(target, value, GetTime());
}


void PID::SetParam (double mi, double ma, double p, double i, double d) {
	Kp = p;
	Ki = i;
	Kd = d;
	Min = mi;
	Max = ma;
};


void PID::GetParam (double *mi, double *ma, double *p, double *i, double *d) {
	if (p != NULL) *p = Kp;
	if (i != NULL) *i = Ki;
	if (d != NULL) *d = Kd;
	if (mi != NULL) *mi = Min;
	if (ma != NULL) *ma = Max;
};