/**********************************************************************************************
 * Basic PID Library - Version 1.0
 * by Jay Wilhelm <jwilhelm@ohio.edu>
 * original by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
 *
 * This Library is licensed under the MIT License
 **********************************************************************************************/

#ifdef ARDUINO
#include "Arduino.h"
#endif

#include "BasicPIDLibraryClass.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.
 ***************************************************************************/
BasicPIDLibrary::BasicPIDLibrary(double Kp, double Ki, double Kd)
{
    BasicPIDLibrary::SetOutputLimits(0, 255);	//default output limit corresponds to
												//the arduino pwm limits
    this->mSampleTime = 50;							//default Controller Sample Time is 0.1 seconds
    BasicPIDLibrary::SetTunings(Kp, Ki, Kd);
#ifdef ARDUINO
	this->mLastTime = millis()-mSampleTime;
#endif
	EnableController();
}


/* 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
 *   BasicPIDLibrary Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 **********************************************************************************/
bool BasicPIDLibrary::Compute(DECIMAL iSetpoint,DECIMAL iInput,DECIMAL &pOutput)
{
	if(!this->mEnabled)
		return false;
	unsigned long now = 0;
#ifdef ARDUINO
	now = millis();
	unsigned long timeChange = (now - mLastTime);
	if(timeChange >= this->mSampleTime) //only run at a set controller sample rate
#endif
	{	
		DECIMAL error = iSetpoint - iInput;
		DECIMAL dInput = (error - mLastInput) / (this->mSampleTime/1000.0);
		this->mOutputSum += (error)*(this->mSampleTime/1000.0);
		//keep track for debug
		this->mLastChange = dInput;

		//calculate the output
		DECIMAL output;
		output = (kp * error) +  (this->mOutputSum * ki) + (dInput * kd); //dt assumed in ki and kd

		if(output > mOutMax) 
			pOutput = mOutMax;
		else if(output < mOutMin) 
			pOutput = mOutMin;
		else{
			pOutput = output;	//assign output value to the pointer
		}

		//Remember some variables for next time 
		this->mLastInput = error;
		this->mLastTime = now;
		return true;
	}
#ifdef ARDUINO
	else 
		return false;
#endif
}


/* SetTunings(...)*************************************************************
 * Set Tunings using the last-rembered POn setting
 ******************************************************************************/
void BasicPIDLibrary::SetTunings(DECIMAL iKp, DECIMAL iKi, DECIMAL iKd){
   this->kp = iKp;
   this->ki = iKi;// * (DECIMAL)this->mSampleTime/1000.0;
   this->kd = iKd;// / (DECIMAL)this->mSampleTime/1000.0;
   EnableController();
}

/* SetSampleTime(...) *********************************************************
 * sets the period, in Milliseconds, at which the calculation is performed
 ******************************************************************************/
void BasicPIDLibrary::SetSampleTimeMilliseconds(int NewSampleTimeMilliseconds)
{
   if (NewSampleTimeMilliseconds > 0)
   {
      this->mSampleTime = (unsigned long)NewSampleTimeMilliseconds;
   }
}

/* 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 BasicPIDLibrary::SetOutputLimits(DECIMAL Min, DECIMAL Max)
{
   if(Min >= Max) 
	   return;
   this->mOutMin = Min;
   this->mOutMax = Max;
}

void BasicPIDLibrary::DisableController(void)
{
	this->mEnabled = false;
}
void BasicPIDLibrary::EnableController(void)
{
	this->mEnabled = true;
	this->mOutputSum = 0.0;
	this->mLastInput = 0.0;
	this->mLastTime = 0;
}