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

using namespace std;

const double 	WINCH_COUNTSPERREV 		= 4096.0;	//Winch motor resolver counts per revolution
const double 	WINCH_GEARRATIO			= 220.0;	//Winch gear ratio
const double 	LW_COUNTSPERREV			= 4096.0;	//Levelwind motor resolver counts per revolution
const double 	LW_GEARRATIO			= 100.0;	//Winch gear ratio
const uint32_t 	CABLELR_ARRAY_LENGTH	= 10;		//Length of array (and integration time) for cable L/R switch values
const uint32_t 	EOTLEFT_MODE_TEST		= 3;		//How many EOT Left switch closures in a row are required to enter EOT Left mode
const uint32_t 	EOTRIGHT_MODE_TEST		= 3;		//How many EOT Left switch closures in a row are required to enter EOT Left mode
const uint32_t 	EOTLEFT_DWELL_TEST		= 3;		//How many EOT Right switch closures in a row are required to exit EOT Left mode
const uint32_t	EOTRIGHT_DWELL_TEST 	= 3;		//How many EOT Left switch closures in a row are required to exit EOT Right mode

// The following "constants" need to be variables that can be changed from the user interface
const double lwElmoAG2 			= 44500.0;	//Elmo analog input scale factor: counts/sec per volt
const double lwElmoAS1 			= 2.0;		//Elmo analog input offset: volts

void* ControlThread::Run(void* parameter)
{
	Labjack				espProfiler((char*) parameter);
	struct itimerval	timer;
	sigset_t 			signalsToCatch;
	int					signalCaught;
	int					iTemp;
	double				motorTensionVolts;
	double				flangeTensionVolts;
	double				h;
	double				Ti;
	double				Td;
	double				Tt;
	double				K;
	double				N;
	double				V;
	double				U;
	double				bi;
	double				ad;
	double				bd;
	double				a0;
	double				Ysetpoint;
	double				Y;
	double				P;
	double				D;
	double				B;
	double				Umax;
	double				Umin;
	double				Yold;
	double				Iold;
	double				Dold;
	double				I;

	uint32_t			cableLR[CABLELR_ARRAY_LENGTH] = {0};
	uint32_t			EOTLeftMode_test = 0;
	uint32_t			EOTRightMode_test = 0;
	uint32_t			EOTLeftDwell_test = 0;
	uint32_t			EOTRightDwell_test = 0;
	uint32_t			cableLRIndex = 0;

//GM

	// Configure the timer to expire after one second
	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);

	SetWinchRPM(0);
	m_WinchDAC_voltage = 0.0;
	m_LevelWindDAC_voltage = 0.0;

	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

	//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
	m_LevelWindMode = traverse;
	m_LevelWindDirection = 1; // +1 = toward LW motor, -1 is away

	for(;;) {
		sigwait(&signalsToCatch,&signalCaught);
		m_ExclusiveAccess.Acquire();

		espProfiler.Update((m_Started) ? m_WinchDAC_voltage : 0,
						   (m_Started) ? m_LevelWindDAC_voltage : lwElmoAS1,
						   motorTensionVolts,
						   flangeTensionVolts,
						   m_IsEndOfTravelLeftSwitchClosed,
						   m_IsEndOfTravelRightSwitchClosed,
						   m_IsCableLeftSwitchClosed,
						   m_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.
 */

		m_MotorTensionPounds = motorTensionVolts * 500.0 / 5.0;	//500 lbf full scale = 5.0 VDC full scale;
		m_FlangeTensionPounds = flangeTensionVolts * 500.0 / 5.0;	//500 lbf full scale = 5.0 VDC full scale;
		m_TotalTensionPounds = m_MotorTensionPounds + m_FlangeTensionPounds;

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

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

		switch( m_LevelWindMode  )
		{
			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( (m_IsCableLeftSwitchClosed == false) && (m_IsCableRightSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = -1;
				else if( (m_IsCableRightSwitchClosed == false) && (m_IsCableLeftSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 1;
				else if( (m_IsCableRightSwitchClosed == true) && (m_IsCableLeftSwitchClosed == true) )
					cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 0;
				else if( (m_IsCableRightSwitchClosed == false) && (m_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 (uint32_t i = 0; i< CABLELR_ARRAY_LENGTH; i++)
					iTemp = iTemp + cableLR[i];
				m_LevelWindError = (double)iTemp / (double)CABLELR_ARRAY_LENGTH;

				Ysetpoint = 0.0;
				Y = m_LevelWindError;

				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  */
				m_LevelWindDAC_voltage = (m_LevelWindDirection * m_LevelWindBaseDAC_Voltage) + lwElmoAS1 + U;

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

				break;
			}
			case EOTLeft:
			{
				m_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) && (m_IsCableRightSwitchClosed == true) )
					EOTLeftDwell_test = 0;		//must have consecutive Cable Right switch closures
				else if( m_IsCableRightSwitchClosed == false )
					EOTLeftDwell_test = EOTLeftDwell_test + 1;
				if( EOTLeftDwell_test >= EOTLEFT_DWELL_TEST)
				{
					m_LevelWindDAC_voltage = -m_LevelWindBaseDAC_Voltage;
					EOTLeftDwell_test = 0;
					for (uint32_t i = 0; i < CABLELR_ARRAY_LENGTH; i++)
						cableLR[i] = 0;
					m_LevelWindMode = traverse;
					m_LevelWindDirection = -1;
				}
				break;
			}
			case EOTRight:
			{
				m_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) && (m_IsCableLeftSwitchClosed == true) )
					EOTRightDwell_test = 0;		//must have consecutive Cable Left switch closures
				else if( m_IsCableLeftSwitchClosed == false )
					EOTRightDwell_test = EOTRightDwell_test + 1;
				if( EOTRightDwell_test >= EOTRIGHT_DWELL_TEST)
				{
					m_LevelWindDAC_voltage = m_LevelWindBaseDAC_Voltage;
					EOTRightDwell_test = 0;
					for (uint32_t i = 0; i < CABLELR_ARRAY_LENGTH; i++)
						cableLR[i] = 0;
					m_LevelWindMode  = traverse;
					m_LevelWindDirection = +1;
				}
				break;
			}
		}

		m_ExclusiveAccess.Release();

	}
	return NULL;
}

void ControlThread::SetWinchRPM(double rpm)
{
	double			lwBaseRPM;
	double			lwCPS;

	m_ExclusiveAccess.Acquire();
	m_RequestedWinchRPM = rpm;
	lwBaseRPM = 326.0 * m_RequestedWinchRPM;	//326 LW motor revs advances cable guide 1 cable diameter
	lwCPS = lwBaseRPM * LW_COUNTSPERREV / 60.0;	//CPS = encoder counts per second
	m_LevelWindBaseDAC_Voltage = lwCPS / lwElmoAG2;

	m_WinchCountsPerSecond = m_RequestedWinchRPM *
								WINCH_GEARRATIO * WINCH_COUNTSPERREV / 60.0;
	m_Winch.JogVelocity(m_WinchCountsPerSecond);
	m_ExclusiveAccess.Release();
}

void ControlThread::StartController(void)
{
	m_ExclusiveAccess.Acquire();
	m_Started = true;
	m_ExclusiveAccess.Release();
}

void ControlThread::StopController(void)
{
	m_ExclusiveAccess.Acquire();
	m_Started = false;
	m_ExclusiveAccess.Release();
}

void ControlThread::Query(void)
{
	// TODO: Replace the following with a structure assignment
	m_ExclusiveAccess.Acquire();
	m_l.requestedWinchRPM = m_RequestedWinchRPM;
	m_l.levelWindBaseDAC_Voltage = m_LevelWindBaseDAC_Voltage;
	m_l.winchDAC_Voltage = m_WinchDAC_voltage;
	m_l.levelWindDAC_Voltage = m_LevelWindBaseDAC_Voltage;
	m_l.motorTensionPounds = m_MotorTensionPounds;
	m_l.flangeTensionPounds = m_FlangeTensionPounds;
	m_l.totalTensionPounds = m_TotalTensionPounds;
	m_l.levelWindError = m_LevelWindError;
	m_l.winchCountsPerSecond = m_WinchCountsPerSecond;
	m_l.levelWindDirection = m_LevelWindDirection;
	m_l.levelWindMode = m_LevelWindMode;
	m_l.isEndOfTravelLeftSwitchClosed = m_IsEndOfTravelLeftSwitchClosed;
	m_l.isEndOfTravelRightSwitchClosed = m_IsEndOfTravelRightSwitchClosed;
	m_l.isCableLeftSwitchClosed = m_IsCableLeftSwitchClosed;
	m_l.isCableRightSwitchClosed = m_IsCableRightSwitchClosed;
	m_CopyOfK = m_K;
	m_CopyOfDepthInMeters = m_DepthInMeters;
	m_ExclusiveAccess.Release();
}

void ControlThread::ReportKinematics(Kinematics& k)
{
	m_ExclusiveAccess.Acquire();
	m_K = k;
	m_ExclusiveAccess.Release();
}

void ControlThread::ReportDepth(float depth)
{
	m_ExclusiveAccess.Acquire();
	m_DepthInMeters = depth;
	m_ExclusiveAccess.Release();
}

stringstream& operator<< (stringstream& out, const ControlThread& controller)
{
	out << "$EPLJ"														<< ",";
	switch (controller.m_l.levelWindMode) {
		case traverse:	out << "T,";
						break;
		case EOTLeft:	out << "L,";
						break;
		case EOTRight:	out << "R,";
						break;
		default:		out << "?,";
						break;
	}
	out << setprecision(5) << controller.m_l.requestedWinchRPM				<< ',';
	out << setprecision(5) << controller.m_l.levelWindBaseDAC_Voltage		<< ",";
	out << setprecision(5) << controller.m_l.winchDAC_Voltage				<< ",";
	out << setprecision(5) << controller.m_l.levelWindDAC_Voltage			<< ",";
	out << setprecision(5) << controller.m_l.motorTensionPounds				<< ",";
	out << setprecision(5) << controller.m_l.flangeTensionPounds 			<< ",";
	out << setprecision(5) << controller.m_l.totalTensionPounds 			<< ",";
	out << setprecision(5) << controller.m_l.levelWindError 				<< ",";
	out << controller.m_l.winchCountsPerSecond 								<< ",";
	out << controller.m_l.levelWindDirection 								<< ",";
	out << controller.m_l.isEndOfTravelLeftSwitchClosed						<< ",";
	out << controller.m_l.isEndOfTravelRightSwitchClosed					<< ",";
	out << controller.m_l.isCableLeftSwitchClosed							<< ",";
	out << controller.m_l.isCableRightSwitchClosed							<< ",;";
	out << "\r\n";

	out.setf(ios::showpos);
	out.setf(ios::scientific);
	out.width(8);
	out.precision(3);
	out << "$EPAD"										<< ",";
	out << controller.m_CopyOfK.x_AccelerationInG		<< ',';
	out << controller.m_CopyOfK.y_AccelerationInG		<< ",";
	out << controller.m_CopyOfK.z_AccelerationInG		<< ",";
	out << controller.m_CopyOfK.x_AngularRateInRadSec	<< ",";
	out << controller.m_CopyOfK.y_AngularRateInRadSec	<< ",";
	out << controller.m_CopyOfK.y_AngularRateInRadSec	<< ",";
	out << controller.m_CopyOfDepthInMeters				<< ",;";
	out << "\r\n";
	return (out);
}
