/**********************************************************************************************
* Arduino PID Library - Version 1.2.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under the MIT License
**********************************************************************************************/

#if ARDUINO >= 100
  #include "Arduino.h"
  #else
  #include "WProgram.h"
#endif

#include "PID_v1.h"

/*Constructor (...)*********************************************************
*    The parameters specified here are those for for which we can't set up
*    reliable defaults, so we need to have the user set them.
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
  double Kp, double Ki, double Kd, int POn, int ControllerDirection) {
  myOutput = Output;
  myInput = Input;
  mySetpoint = Setpoint;
  inAuto = false;
  //Tuner = new PID_ATune(Input, Output);
  PID::OutputLimits(0, 100);
  PID::Direction(ControllerDirection);
  PID::SetTunings(Kp, Ki, Kd, POn);

  // Autotune defaults
  noise_band = 0.5;
  autotune_running = false;
  oStep = 30;
  LookbackSec(10);

  SampleTime = 100;                           //default Controller Sample Time is 0.1 seconds
  lastTime = millis()-SampleTime;
}

/*Constructor (...)*********************************************************
*    To allow backwards compatability for v1.1, or for people that just want
*    to use Proportional on Error without explicitly saying so
***************************************************************************/

PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
:PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) {

}


/* Compute() **********************************************************************
*     This, as they say, is where the magic happens.  this function should be called
*   every time "void loop()" executes.  the function will decide for itself whether a new
*   pid Output needs to be computed.  returns true when the output is computed,
*   false when nothing has been done.
**********************************************************************************/
bool PID::Compute() {
  if(!inAuto) return false;
  unsigned long now = millis();
  unsigned long timeChange = (now - lastTime);
  if(timeChange>=SampleTime) {
    /*Compute all the working error variables*/
    double input = *myInput;
    double error = *mySetpoint - input;
    double dInput = (input - lastInput);
    outputSum+= (ki * error);

    /*Add Proportional on Measurement, if P_ON_M is specified*/
    if(!pOnE) outputSum-= kp * dInput;

    if(outputSum > outMax) outputSum= outMax;
    else if(outputSum < outMin) outputSum= outMin;

    /*Add Proportional on Error, if P_ON_E is specified*/
    double output;
    if(pOnE) output = kp * error;
    else output = 0;

    /*Compute Rest of PID Output*/
    output += outputSum - kd * dInput;

    if(output > outMax) output = outMax;
    else if(output < outMin) output = outMin;
    *myOutput = output;

    /*Remember some variables for next time*/
    lastInput = input;
    lastTime = now;
    return true;
  }
  else return false;
}

int PID::ComputeTune() {
  //~ justevaled=false;
  if(peakCount>9 && autotune_running) {
    autotune_running = false;
    FinishUp();
    return 1;
  }
  unsigned long now = millis();

  if((now-lastTime)<sampleTime) return false;

  lastTime = now;
  double refVal = *myInput;
  //~ justevaled=true;
  if(!autotune_running) {
    //initialize working variables the first time around
    peakType = 0;
    peakCount=0;
    atune_peak_change=false;
    absMax=refVal;
    absMin=refVal;
    *mySetpoint = refVal;
    autotune_running = true;
    outputStart = *myOutput;
    *myOutput = outputStart+oStep;
    } else {
    if(refVal>absMax)absMax=refVal;
    if(refVal<absMin)absMin=refVal;
    } else {
    if(refVal>absMax)absMax=refVal;
    if(refVal<absMin)absMin=refVal;
  }

  //oscillate the output base on the input's relation to the setpoint

  if(refVal>*mySetpoint+noise_band) *myOutput = outputStart-oStep;
  else if (refVal<*mySetpoint-noise_band) *myOutput = outputStart+oStep;


  //bool isMax=true, isMin=true;
  isMax=true;isMin=true;
  //id peaks
  for(int i=nLookBack-1;i>=0;i--) {
    double val = lastInputs[i];
    if(isMax) isMax = refVal>val;
    if(isMin) isMin = refVal<val;
    lastInputs[i+1] = lastInputs[i];
  }
  lastInputs[0] = refVal;
  if(nLookBack<9){
    //we don't want to trust the maxes or mins until the inputs array has been filled
    return 0;
  }

  if(isMax) {
    if(peakType==0)peakType=1;
    if(peakType==-1) {
      peakType = 1;
      atune_peak_change=true;
      peak2 = peak1;
    }
    peak1 = now;
    peaks[peakCount] = refVal;

    } else if(isMin) {
    if(peakType==0)peakType=-1;
    if(peakType==1){
      peakType=-1;
      peakCount++;
      atune_peak_change=true;
    }

    if(peakCount<10)peaks[peakCount] = refVal;
  }

  if(atune_peak_change && peakCount>2) {
    //we've transitioned.  check if we can autotune based on the last peaks
    double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2;
    if( avgSeparation < 0.05*(absMax-absMin)){
      FinishUp();
      autotune_running = false;
      return 1;

    }
  }
  atune_peak_change=false;
  return 0;
}

void PID::FinishUp(){
  *myOutput = outputStart;
  //we can generate tuning parameters!
  Ku = 4*(2*oStep)/((absMax-absMin)*3.14159);
  Pu = (double)(peak1-peak2) / 1000;
}

/* SetTunings(...)*************************************************************
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd, int POn){
  if (Kp<0 || Ki<0 || Kd<0) return;

  pOn = POn;
  pOnE = POn == P_ON_E;

  double SampleTimeInSec = ((double)SampleTime)/1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;

  if(controllerDirection ==REVERSE){
    kp = (0 - kp);
    ki = (0 - ki);
    kd = (0 - kd);
  }
}

/* SetTunings(...)*************************************************************
* Set Tunings using the last-rembered POn setting
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
  SetTunings(Kp, Ki, Kd, pOn);
}

/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SampleTime(int NewSampleTime){
  if (NewSampleTime > 0){
    double ratio  = (double)NewSampleTime
    / (double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime;
  }
}

/* OutputLimits(...)*******************************************************
*     This function will be used far more often than SetInputLimits.  while
*  the input to the controller will generally be in the 0-1023 range (which is
*  the default already,)  the output will be a little different.  maybe they'll
*  be doing a time window and will need 0-8000 or something.  or maybe they'll
*  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
*  here.
**************************************************************************/
void PID::OutputLimits(double Min, double Max){
  if(Min >= Max) return;
  outMin = Min;
  outMax = Max;

  if(inAuto){
    if(*myOutput > outMax) *myOutput = outMax;
    else if(*myOutput < outMin) *myOutput = outMin;

    if(outputSum > outMax) outputSum= outMax;
    else if(outputSum < outMin) outputSum= outMin;
  }
}

/* Mode(...)*******************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID::Mode(int Mode){
  bool newAuto = (Mode == 1);
  if(newAuto && !inAuto){  /*we just went from manual to auto*/
    PID::Initialize();
  }
  inAuto = newAuto;
  OpMode = Mode;
}

int PID::CycleMode() {
  if (OpMode + 1 == 3) {
    PID::Mode(0);
    } else {
    PID::Mode(OpMode + 1);
  }

  return OpMode;
}

/* Initialize()****************************************************************
*  does all the things that need to happen to ensure a bumpless transfer
*  from manual to automatic mode.
******************************************************************************/
void PID::Initialize(){
  outputSum = *myOutput;
  lastInput = *myInput;
  if(outputSum > outMax) outputSum = outMax;
  else if(outputSum < outMin) outputSum = outMin;
}

/* Direction(...)**************************************************************
* The PID will either be connected to a DIRECT acting process (+Output leads
* to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
* know which one, because otherwise we may increase the output when we should
* be decreasing.  This is called from the constructor.
******************************************************************************/
void PID::Direction(int Direction){
  if(inAuto && Direction !=controllerDirection){
    kp = (0 - kp);
    ki = (0 - ki);
    kd = (0 - kd);
  }
  controllerDirection = Direction;
}

/* Status Funcions*************************************************************
* Just because you set the Kp=-1 doesn't mean it actually happened.  these
* functions query the internal state of the PID.  they're here for display
* purposes.  this are the functions the PID Front-end uses for example
******************************************************************************/
double PID::Kp(){ return Kp; }
double PID::Ki(){ return Ki;}
double PID::Kd(){ return Kd;}
int PID::Mode(){ return OpMode;}
int PID::Direction(){ return controllerDirection;}
void PID::Cancel(){ autotune_running = false;}
double PID::TunedKp(){ return 0.6 * Ku;}
double PID::TunedKi(){ return 1.2*Ku / Pu ;  // Ki = Kc/Ti}
  double PID::TunedKd(){ return 0.075 * Ku * Pu;  //Kd = Kc * Td}
    void PID::SetOutputStep(double Step){ oStep = Step;}
    double PID::GetOutputStep(){ return oStep;}
    void PID::SetNoiseBand(double Band){ noise_band = Band;}
    double PID::GetNoiseBand(){ return noise_band;}

    void PID::LookbackSec(int value){
      if (value<1) value = 1;

      if(value<25) {
        nLookBack = value * 4;
        sampleTime = 250;
        } else {
        nLookBack = 100;
        sampleTime = value*10;
      }
    }

    int PID::LookbackSec(){ return nLookBack * sampleTime / 1000;}