#include <math.h>
#include <stdio.h>
#include <time.h>
#include <iostream>

using namespace std;

#include "pid.h"

FILE *dataFID;
char fileName[80];

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( int loopType, uint32_t tv_sec, uint32_t tv_usec, PIDConstants PIDConst )
{

	/* 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(PIDConst.m_Ti * PIDConst.m_Td);						//Anti-windup tracking time

	m_bi = PIDConst.m_K * m_h / PIDConst.m_Ti;						//Constant for I calculation
	m_ad = (2 * PIDConst.m_Td - PIDConst.m_N * m_h) / (2 * PIDConst.m_Td + PIDConst.m_N * m_h);		//Constant for D calculation
	m_bd = (2 * PIDConst.m_K * PIDConst.m_N * PIDConst.m_Td) / (2 * PIDConst.m_Td + PIDConst.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

	if( loopType == 1 )
	{
		cout << "Please enter a filename: ";
		cin >> fileName;
		cout << "File name = " << fileName << endl;
	}

	return;
}

double PID::ahCalcCV( int loopType, double setPoint, double PV,  PIDConstants PIDConst)
{
	long currentTime;
	long temp;

	currentTime = clock();
	temp = CLOCKS_PER_SEC;

	m_Ysetpoint = setPoint;
	m_Y = PV;

	m_P = PIDConst.m_K * (PIDConst.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 + m_I + m_D;													//temporary output

	//Check for saturation
	if(m_V > PIDConst.m_Umax)
		m_U = PIDConst.m_Umax;
	else if(m_V < PIDConst.m_Umin)
		m_U = PIDConst.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;

	if( loopType == 1 )
	{
		dataFID = fopen(fileName,"a");
		printf("Time: %ld, Desired depth: %8.3f, Actual depth: %8.3f,   m_P: %8.3f,  m_D: %8.3f,   m_I: %8.3f,   m_V: %8.3f,   m_U: %8.3f \r\n",
				currentTime, m_Ysetpoint, m_Y, m_P, m_D, m_I, m_V, m_U);
		fprintf(dataFID, "Time: %ld, Desired depth: %10.6g, Actual depth: %10.6g,   m_P: %10.6g,  m_D: %10.6g,   m_I: %10.6g,   m_V: %10.6g,   m_U: %10.6g \r\n",
				currentTime, m_Ysetpoint, m_Y, m_P, m_D, m_I, m_V, m_U);
		fclose(dataFID);
	}

	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