/**********************************************************************************************
* Arduino PID Library - Version 2.0.0
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
**********************************************************************************************/
#include "PID_v2.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.
***************************************************************************/
PIDv2::PIDv2(double* Input, double* Output, double* Setpoint,
         double Kp, double Ki, double Kd, Direction ControllerDirection)
{

    myOutput = Output;
    myInput = Input;
    mySetpoint = Setpoint;
    inAuto = false;

    PIDv2::SetOutputLimits(0, 255);    //default output limit corresponds to 
                                     //the arduino pwm limits

    SampleTime = 100;                //default Controller Sample Time is 0.1 seconds

    PIDv2::SetControllerDirection(ControllerDirection);
    PIDv2::SetTunings(Kp, Ki, Kd);

    lastTime = 0;
    lastError = 0;
    isClipping = false;
    i_limit = 0;
}


/* 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 PIDv2::Compute(unsigned int now)
{
    if(!inAuto)
        return false;
    if((now - lastTime)>= SampleTime)
    {
        lastTime = now;

        /*Compute all the working error variables*/
        double input = *myInput;
        double p_error = *mySetpoint - input;

        /* if output is in limit, don't let integrator wind up */
        if(!isClipping)
            i_error += ki * p_error;

        if(i_error != 0.0)
        {
          if(i_error > i_limit)
            i_error = i_limit;
          else if(i_error < -i_limit)
            i_error = -i_limit;
        }
        else
        {
          if(i_error > outMax)
              i_error= outMax;
          else if(i_error < outMin)
              i_error= outMin;
        }

        double d_error = (p_error - lastError);
        lastError = p_error;

        /*Compute PID Output*/
        double output = kp * p_error + i_error + kd * d_error;

        if(output > outMax)
        {
            isClipping = true;
            output = outMax;
        }
        else if(output < outMin)
        {
            isClipping = true;
            output = outMin;
        }
        else
            isClipping = false;

        *myOutput = output;

        return true;
    }
    else
        return false;
}


/* 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 PIDv2::SetTunings(double Kp, double Ki, double Kd)
{
    if (Kp<0 || Ki<0 || Kd<0) return;

    dispKp = Kp;
    dispKi = Ki;
    dispKd = Kd;

    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);
    }
}

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

/* SetOutputLimits(...)****************************************************
*     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 PIDv2::SetOutputLimits(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(i_error > outMax) i_error= outMax;
        else if(i_error < outMin) i_error= outMin;
    }
}

/* SetMode(...)****************************************************************
* 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 PIDv2::SetMode(Mode mode)
{
    bool newAuto = (mode == AUTOMATIC);
    if(newAuto == !inAuto)
    {  /*we just went from manual to auto*/
        PIDv2::Initialize();
    }
    inAuto = newAuto;
}

/* Initialize()****************************************************************
*    does all the things that need to happen to ensure a bumpless transfer
*  from manual to automatic mode.
******************************************************************************/ 
void PIDv2::Initialize()
{
    i_error = 0;
}

/* SetControllerDirection(...)*************************************************
* 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 PIDv2::SetControllerDirection(Direction 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 PIDv2::GetKp()
{
    return dispKp;
}

double PIDv2::GetKi()
{
    return dispKi;
}

double PIDv2::GetKd()
{
    return dispKd;
}

PIDv2::Mode PIDv2::GetMode()
{
    return  inAuto ? AUTOMATIC : MANUAL;
}

PIDv2::Direction PIDv2::GetDirection()
{
    return controllerDirection;
}