#include <stdint.h>
#include <signal.h>
#include <sys/time.h>
#include <string>
#include <iostream>
#include <ctime>
#include "control_thread.h"
#include "mutex.h"
#include "labjack.h"

//GM
#include <math.h>
//GM

using namespace std;

ControlThread::ControlThread()
{
	// This is the constructor
}

ControlThread::~ControlThread()
{
	// This is the destructor
}

int ControlThread::genesMethod( int p1 )
{
	cout << "genesMethod p1 = " << p1 << endl;
}

void* ControlThread::Run(void* parameter)
{
	Labjack				espProfiler((char*) parameter);
	struct itimerval	timer;
	sigset_t 			signalsToCatch;
	int					signalCaught;
	double				winchDAC_voltage = 0.0;
	double				levelWindDAC_voltage = 0.0;
	double 				motorTension;
	double	 			flangeTension;
	bool				isEndOfTravelLeftSwitchClosed;
	bool				isEndOfTravelRightSwitchClosed;
	bool				isCableLeftSwitchClosed;
	bool				isCableRightSwitchClosed;
//GM
	#define CABLELR_ARRAY_LENGTH 10		//Length of array (and integration time) for cable L/R switch values
	#define EOTLEFT_MODE_TEST 	3		//How many EOT Left switch closures in a row are required to enter EOT Left mode
	#define EOTRIGHT_MODE_TEST 	3		//How many EOT Left switch closures in a row are required to enter EOT Left mode
	#define EOTLEFT_DWELL_TEST 	3		//How many EOT Right switch closures in a row are required to exit EOT Left mode
	#define EOTRIGHT_DWELL_TEST 3		//How many EOT Left switch closures in a row are required to exit EOT Right mode
	#define WINCH_COUNTSPERREV 	4096.0	//Winch motor resolver counts per revolution
	#define WINCH_GEARRATIO 	220.0	//Winch gear ratio
	#define LW_COUNTSPERREV 	4096.0	//Levelwind motor resolver counts per revolution
	#define LW_GEARRATIO 		100.0	//Winch gear ratio

	enum lwMode{traverse, EOTLeft, EOTRight};

	int				i, loopCtr = 0,
					iTemp;
	double			h, Ti, Td, Tt, K, N,
					bi, ad, bd, a0,
					Ysetpoint, Y,
					P, D, B,
					V, U, Umax, Umin,
					lwError,
					winchCPS, lwCPS,
					lwBaseDAC_voltage = 0.0, lwElmoAG2, lwElmoAS1;
	static short	cableLR[CABLELR_ARRAY_LENGTH] = {0},
					EOTLeftMode_test = 0, EOTRightMode_test = 0,
					EOTLeftDwell_test = 0, EOTRightDwell_test = 0,
					lwMode;
	static short	lwDirection;
	static long		cableLRIndex = 0;
	static double	winchRequestedRPM, lwBaseRPM,
					Yold = 0.0, Iold = 0.0, Dold = 0.0, I;
	static bool		firstTime = true;

//GM

	// Configure the timer to expire after one second
	if( firstTime ) {
	timer.it_value.tv_sec 	= 0;
	timer.it_value.tv_usec	= 100000;
	// .... and every second after that
	timer.it_interval.tv_sec 	= 0;
	timer.it_interval.tv_usec	= 100000;
	sigemptyset(&signalsToCatch);
	sigaddset(&signalsToCatch,SIGALRM);
	setitimer(ITIMER_REAL, &timer, NULL);

	espProfiler.ConnectToLabjack();

	cout.setf(ios::showpos);
	cout.setf(ios::scientific);
	cout.width(8);
	cout.precision(3);

	/* Calculate constants for PID calculation
	 * Reference: 	PID Controllers, Theory, Design and Tuning
	 * 				2nd ed.
	 * 				Astrom and Hagglund
	 */
	h = 0.1;			//Sample rate
	Ti = 0.01;			//Integral time constant
	K = 1.5;			//Controller gain
	Td = 1.0;			//Derivative time constant
	N = 8.0;			//Derivative smoothing factor
	B = 1.0;			//Setpoint weight
	Umax = 2.0;			//Maximum output value
	Umin = -2.0;		//Minimum output value
	Tt = sqrt(Ti*Td);						//Anti-windup tracking time

	bi = K * h / Ti;						//Constant for I calculation
	ad = (2*Td - N*h) / (2*Td + N*h);		//Constant for D calculation
	bd = (2*K*N*Td) / (2*Td + N*h);			//Constant for D calculation
	a0 = h / Tt;							//Anti-windup constant

//Need to make lw and winch ElmoAG2 and ElmoAS1 user settable
	winchRequestedRPM = 1.0;
	winchCPS = winchRequestedRPM * WINCH_GEARRATIO * WINCH_COUNTSPERREV / 60.0;	//CPS = encoder counts per second

	lwBaseRPM = 326.0 * winchRequestedRPM;	//326 LW motor revs advances cable guide 1 cable diameter
	lwCPS = lwBaseRPM * LW_COUNTSPERREV / 60.0;	//CPS = encoder counts per second
	lwElmoAG2 = 44500.0;					//Elmo analog input scale factor: counts/sec per volt
	lwElmoAS1 = 2.0;						//Elmo analog input offset: volts
	lwBaseDAC_voltage = lwCPS / lwElmoAG2;
	//NOTE: check for max/min values here

	I = 0.0;		//Initial I value for PID calculation
	Dold = 0.0;		//Initial D value for PID calculation
	Yold = 0.0;		//Initial process output for PID calculation
	lwMode = traverse;
	lwDirection = -1;

	firstTime = false;
	}

	for(;;) {
		sigwait(&signalsToCatch,&signalCaught);
		m_ExclusiveAccess.Acquire();
		espProfiler.Update(winchDAC_voltage,
						   levelWindDAC_voltage,
						   motorTension,
						   flangeTension,
						   isEndOfTravelLeftSwitchClosed,
						   isEndOfTravelRightSwitchClosed,
						   isCableLeftSwitchClosed,
						   isCableRightSwitchClosed);

		// Your PID stuff goes here...
/*
 * The basic structure for every control thread cycle is:
 * 1. Check for EOT condition.
 * 2. If not EOT, traverse at levelwind base traverse speed using analog input velocity
 * control to Elmo.  While traversing, add/subtract to base traverse voltage based on
 * cable L/R switch history.
 * 3. If EOT, stop the level wind.  EOT switch must be false for EOTXXX_MODE_TEST
 * consecutive cycles.  Wait for cable to wind up at flange and start moving in the
 * opposite directions as indicated by cable L/R switches.  Appropriate cable L/R switch
 * must be false for EOTXXX_DWELL_TEST consecutive cycles.
 */

		// Check for EOT Left
		if( (EOTLeftMode_test > 0) && (isEndOfTravelLeftSwitchClosed == true) )
			EOTLeftMode_test = 0;		//must have consecutive EOT switches
		else if( isEndOfTravelLeftSwitchClosed == false )
		{
			EOTLeftMode_test = EOTLeftMode_test + 1;
			if( EOTLeftMode_test >= EOTLEFT_MODE_TEST)
			{
				lwMode = EOTLeft;
				EOTLeftMode_test = 0;
			}
		}

		// Check for EOT Right
		if( (EOTRightMode_test > 0) && (isEndOfTravelRightSwitchClosed == true) )
			EOTRightMode_test = 0;		//must have consecutive EOT switches
		else if( isEndOfTravelRightSwitchClosed == false )
		{
			EOTRightMode_test = EOTRightMode_test + 1;
			if( EOTRightMode_test >= EOTRIGHT_MODE_TEST)
			{
				lwMode = EOTRight;
				EOTRightMode_test = 0;
			}
		}

		switch( lwMode )
		{
			case traverse:
			{
				// Read Cable L/R switches and insert a 1 or -1 (left or right) into the cableLR array each time either LR switch triggers
				if( (isCableLeftSwitchClosed == false) && (isCableRightSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = -1;
				else if( (isCableRightSwitchClosed == false) && (isCableLeftSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 1;
				else if( (isCableRightSwitchClosed == true) && (isCableLeftSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 0;
				else if( (isCableRightSwitchClosed == false) && (isCableLeftSwitchClosed == false) )
					{}	//error
				cableLRIndex = cableLRIndex + 1;

				//Calculate Cable L/R "error" signal and output to lw DAC
				//This really needs to be a low pass filter to emphasize most recent data
				iTemp = 0;
				for(i=0; i<CABLELR_ARRAY_LENGTH; i++)
					iTemp = iTemp + cableLR[i];
				lwError = (double)iTemp / (double)CABLELR_ARRAY_LENGTH;

				Ysetpoint = 0.0;
				Y = lwError;

				P = K * (B*Ysetpoint - Y);		//proportional part
				D = ad*Dold - bd*(Y - Yold);	//update derivative part
				V = P; //+ I + D;				//temporary output

				//Check for saturation
				if(V > Umax)
					U = Umax;
				else if(V < Umin)
					U = Umin;
				else
					U = V;

				/* NOTE: Labjack DAC is unipolar with about a 0 to +4.5VDC range.
				 * scale DAC voltage to +/- 2.0VDC and add a 2.0VDC offset.
				 * Make sure Elmo AIN offset is set for 2.0VDC: AS[1] = 2.0  */
				levelWindDAC_voltage = (lwDirection * lwBaseDAC_voltage) + lwElmoAS1 + U;

				Iold = I;
				I = Iold + bi*(Ysetpoint - Y) + a0*(U - V);
				Yold = Y;
				Dold = D;

				break;
			}
			case EOTLeft:
			{
				levelWindDAC_voltage = lwElmoAS1;	//Stop the LW motor

				//wait for a couple of Cable Right switch closures to exit EOT Left mode
				if( (EOTLeftDwell_test > 0) && (isCableRightSwitchClosed == true) )
					EOTLeftDwell_test = 0;		//must have consecutive Cable Right switch closures
				else if( isCableRightSwitchClosed == false )
					EOTLeftDwell_test = EOTLeftDwell_test + 1;
				if( EOTLeftDwell_test >= EOTLEFT_DWELL_TEST)
				{
					levelWindDAC_voltage = -lwBaseDAC_voltage;
					EOTLeftDwell_test = 0;
					for(i=0; i<CABLELR_ARRAY_LENGTH; i++)
						cableLR[i] = 0;
					lwMode = traverse;
					lwDirection = -1;
				}
				break;
			}
			case EOTRight:
			{
				levelWindDAC_voltage = lwElmoAS1;	//Stop the LW motor

				//wait for a couple of Cable Left switch closures to exit EOT Left mode
				if( (EOTRightDwell_test > 0) && (isCableLeftSwitchClosed == true) )
					EOTRightDwell_test = 0;		//must have consecutive Cable Left switch closures
				else if( isCableLeftSwitchClosed == false )
					EOTRightDwell_test = EOTRightDwell_test + 1;
				if( EOTRightDwell_test >= EOTRIGHT_DWELL_TEST)
				{
					levelWindDAC_voltage = lwBaseDAC_voltage;
					EOTRightDwell_test = 0;
					for(i=0; i<CABLELR_ARRAY_LENGTH; i++)
						cableLR[i] = 0;
					lwMode = traverse;
					lwDirection = +1;
				}
				break;
			}
		}

		loopCtr = loopCtr + 1;

		cout << "Loop Counter	= " << loopCtr << endl;
		cout << "Cable L/R 	= " << isCableLeftSwitchClosed << "  " << isCableRightSwitchClosed << endl;
		cout << "EOT L/R 	= " << isEndOfTravelLeftSwitchClosed << "  " << isEndOfTravelRightSwitchClosed << endl;
		cout << "Level wind mode	= " << lwMode << endl;
		cout << "Level wind base voltage= " << lwBaseDAC_voltage << endl;
		cout << "Level wind error 	= " << lwError << endl;
		cout << "Level wind direction	= " << lwDirection << endl;
		cout << "U & V 	= " << U << "   " << V << endl;
		cout << "Level wind DAC voltage = " << levelWindDAC_voltage << endl << endl;
		cout.flush();

		m_ExclusiveAccess.Release();

	}
	return NULL;
}