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

#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