#include <math.h>
#include "pid.h"

double PID::CalculateGain( double position )
{
	double error = m_Setpoint - position;
	m_ErrorSum += error;
	m_ProportionalGain = m_KProportional * error;
	m_IntegralGain = m_KIntegral * m_ErrorSum;

	// Prevent integrator wind up
	if ( fabs( m_IntegralGain ) > m_IntegralLimit ) {
	   if( m_IntegralGain > 0 )
		   m_IntegralGain = m_IntegralLimit;
	   else
		   m_IntegralGain = -m_IntegralLimit;
	   m_ErrorSum -= error;
	}

	// Slow down derivative response if needed
	if ( ++m_DifferentialCycleCount >= m_DifferentialCycle ) {
	   m_DifferentialGain =  m_KDifferential * ( error - m_LastError );
	   m_DifferentialCycleCount = 0;
	}

	m_LastError = error;
	m_Gain = m_ProportionalGain + m_IntegralGain + m_DifferentialGain;
	return m_Gain;
}

void PID::ahSetConstants( uint32_t tv_sec, uint32_t tv_usec, PVConsts PVC )
{
	/* Calculate constants for PID calculation
	 * Reference: 	PID Controllers, Theory, Design and Tuning
	 * 				2nd ed.
	 * 				Astrom and Hagglund
	 */


//*** Need to link I & T time constants to actual cycle time
	m_h = tv_sec + (tv_usec / 1000000.0 );
	m_Tt = sqrt(PVC.m_Ti * PVC.m_Td);						//Anti-windup tracking time

	m_bi = PVC.m_K * m_h / PVC.m_Ti;						//Constant for I calculation
	m_ad = (2 * PVC.m_Td - PVC.m_N * m_h) / (2 * PVC.m_Td + PVC.m_N * m_h);		//Constant for D calculation
	m_bd = (2 * PVC.m_K * PVC.m_N * PVC.m_Td) / (2 * PVC.m_Td + PVC.m_N * m_h);			//Constant for D calculation
	m_a0 = m_h / m_Tt;							//Anti-windup constant

	//NOTE: check for max/min values here

	m_I = 0.0;		//Initial I value for PID calculation
	m_Dold = 0.0;		//Initial D value for PID calculation
	m_Yold = 0.0;		//Initial process output for PID calculation

	return;
}

double PID::ahCalcCV( double setPoint, double PV,  PVConsts PVC)
{
	m_Ysetpoint = setPoint;
	m_Y = PV;

	m_P = PVC.m_K * (PVC.m_B * m_Ysetpoint - m_Y);		//proportional part
	m_D = m_ad * m_Dold - m_bd *(m_Y - m_Yold);				//update derivative part
	m_V = m_P; //+ I + D;													//temporary output

	//Check for saturation
	if(m_V > PVC.m_Umax)
		m_U = PVC.m_Umax;
	else if(m_V < PVC.m_Umin)
		m_U = PVC.m_Umin;
	else
		m_U = m_V;

	m_Iold = m_I;
	m_I = m_Iold + m_bi * (m_Ysetpoint - m_Y) + m_a0 * (m_U - m_V);
	m_Yold = m_Y;
	m_Dold = m_D;

	return m_U;
}

#if 0
PID::PID(double kP, double kI, double kD, double lI, uint32_t cD )
	: m_ProportionalGain(kP), m_IntegralGain(kI), m_DifferentialGain(kD),
	m_IntegralLimit(lI), m_DifferentialCycle(cD)
{
}
#endif