#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"
#include "pid.h"
#include "profilerConstants.h"

using namespace std;

enum LWDir
{
	toRight = -1,
	toLeft = 1

};

int EOTLeftMode_test = 0;
int EOTRightMode_test = 0;

PID lwPID;
PID wmPID;

void* ControlThread::Run(void* parameter)
{
	Labjack				espProfiler((char*) parameter);

	struct itimerval	timer;
	sigset_t 			signalsToCatch;
	int					signalCaught;
	int					iTemp;
	double				motorTensionVolts;
	double				flangeTensionVolts;
	double				busVoltage;
	double				lwPID_U;
	double				wmPID_U;

	uint32_t			cableLR[CABLELR_ARRAY_LENGTH] = {0};
	uint32_t			cableLRIndex = 0;

	PID::PIDConstants	lwPIDConst;
	PID::PIDConstants	wmPIDConst;

	int exitEOTLeft_test = 0;
	int exitEOTRight_test = 0;

	bool atEOTRight = false;
	bool atEOTLeft = false;
	bool isEOTRightSwitchActive = false;
	bool isEOTLeftSwitchActive = false;
	bool isCableRightSwitchActive = false;
	bool isCableLeftSwitchActive = false;

	LWDir lwDir;

//GM

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

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

	espProfiler.ConnectToLabjack();

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

	lwPIDConst.m_Ti = lwTi;			//Integral time constant
	lwPIDConst.m_K = lwK;			//Controller gain
	lwPIDConst.m_Td = lwTd;			//Derivative time constant
	lwPIDConst.m_N = lwN;			//Derivative smoothing factor
	lwPIDConst.m_B = lwB;			//Setpoint weight
	lwPIDConst.m_Umax = lwUmax;		//Maximum output value
	lwPIDConst.m_Umin = lwUmin;		//Minimum output value
	lwPID.ahSetConstants( 0, TV_SEC, TV_uSEC, lwPIDConst );

	wmPIDConst.m_Ti = wmTi;			//Integral time constant
	wmPIDConst.m_K = wmK;				//Controller gain
	wmPIDConst.m_Td = wmTd;			//Derivative time constant
	wmPIDConst.m_N = wmN;				//Derivative smoothing factor
	wmPIDConst.m_B = wmB;				//Setpoint weight
	wmPIDConst.m_Umax = wmUmax;		//Maximum output value
	wmPIDConst.m_Umin = wmUmin;		//Minimum output value
	wmPID.ahSetConstants( 1, TV_SEC, TV_uSEC, wmPIDConst );

	m_LevelWindMode = traverse;
	m_LevelWindDirection = 1; // +1 = toward LW motor, -1 is away
	lwDir = toLeft;

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

		//GM
		/* espProfiler.Update((m_Started) ? m_WinchDAC_voltage : 0,
								   (m_Started) ? m_LevelWindDAC_voltage : lwElmoAS1,
								   motorTensionVolts,
								   flangeTensionVolts,
								   busVoltage,
								   m_IsEndOfTravelLeftSwitchClosed,
								   m_IsEndOfTravelRightSwitchClosed,
								   m_IsCableLeftSwitchClosed,
								   m_IsCableRightSwitchClosed);
		*/

	   espProfiler.Update(	m_WinchDAC_voltage,
										m_LevelWindDAC_voltage,
										motorTensionVolts,
										flangeTensionVolts,
										busVoltage,
										m_IsEndOfTravelLeftSwitchClosed,
										m_IsEndOfTravelRightSwitchClosed,
										m_IsCableLeftSwitchClosed,
										m_IsCableRightSwitchClosed );

/*
 * 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;
		m_BusVoltage = busVoltage * voltageDividerRatio;
		isEOTRightSwitchActive = !m_IsEndOfTravelRightSwitchClosed;
		isEOTLeftSwitchActive = !m_IsEndOfTravelLeftSwitchClosed;
		isCableRightSwitchActive = !m_IsCableRightSwitchClosed;
		isCableLeftSwitchActive = !m_IsCableLeftSwitchClosed;


//1. Check for EOT condition
		if ( isEOTRightSwitchActive && isEOTLeftSwitchActive )
		{
				// error, both EOT switches shouldn't be closed at the same time
		}
		else if( (m_LevelWindMode == traverse) && (lwDir == toLeft) )  //only check for left EOT condition if traversing left
		{
			atEOTLeft = CheckEOTLeft( isEOTLeftSwitchActive );
		}
		else if( (m_LevelWindMode == traverse) && (lwDir == toRight) )  //only check for right EOT condition if traversing right
		{
			atEOTRight = CheckEOTRight( isEOTRightSwitchActive );
		}

		if( atEOTLeft && atEOTRight )
		{
			// error, shouldn't ever be in both EOTRight and EOTLeft
		}
		else
		{
			if( atEOTLeft )
				m_LevelWindMode = EOTLeft;
			if( atEOTRight )
				m_LevelWindMode = EOTRight;
		}

		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;

				lwPID_U = lwPID.ahCalcCV(0, 0.0,  m_LevelWindError, lwPIDConst);

				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( (exitEOTLeft_test > 0) && (m_IsCableRightSwitchClosed == true) )
					exitEOTLeft_test = 0;		//must have consecutive Cable Right switch closures
				else if( m_IsCableRightSwitchClosed == false )
					exitEOTLeft_test = exitEOTLeft_test + 1;

				if( exitEOTLeft_test >= exitEOTLEFT_TEST)
				{
					m_LevelWindDAC_voltage = -m_LevelWindBaseDAC_Voltage;
					exitEOTLeft_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( (exitEOTRight_test > 0) && (m_IsCableLeftSwitchClosed == true) )
					exitEOTRight_test = 0;		//must have consecutive Cable Left switch closures
				else if( m_IsCableLeftSwitchClosed == false )
					exitEOTRight_test = exitEOTRight_test + 1;
				if( exitEOTRight_test >= exitEOTRIGHT_TEST)
				{
					m_LevelWindDAC_voltage = m_LevelWindBaseDAC_Voltage;
					exitEOTRight_test = 0;
					for (uint32_t i = 0; i < CABLELR_ARRAY_LENGTH; i++)
						cableLR[i] = 0;
					m_LevelWindMode  = traverse;
					m_LevelWindDirection = +1;
				}
				break;
			}
		}

		/* 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  */

		if( m_DesiredDepthInMeters != 0.0)
		{
			wmPID_U = wmPID.ahCalcCV(1, m_DesiredDepthInMeters, m_CopyOfDepthInMeters, wmPIDConst);
//			printf("Desired Depth (m): %f    Actual Depth(m): %f   wmPID_U: %f \n",
//					m_DesiredDepthInMeters, m_CopyOfDepthInMeters, wmPID_U);
			m_WinchDAC_voltage = wmElmoAS1 + wmPID_U;
		}

//		m_LevelWindDAC_voltage = (m_LevelWindDirection * m_LevelWindBaseDAC_Voltage) + lwElmoAS1 + lwPID_U;
//		printf("LW Error: %f DAC V: %f \n" ,m_LevelWindError, m_LevelWindDAC_voltage);

		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.busVoltage = m_BusVoltage;
	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(double depthInMeters)
{
	m_ExclusiveAccess.Acquire();
	m_DepthInMeters = depthInMeters;
	m_ExclusiveAccess.Release();
}

void ControlThread::DesiredDepth(double desiredDepthInMeters)
{
	m_ExclusiveAccess.Acquire();
	m_DesiredDepthInMeters = desiredDepthInMeters;
	m_ExclusiveAccess.Release();
}

stringstream& operator<< (stringstream& out, const ControlThread& controller)
{
	out.str("");
	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.busVoltage 					<< ",";
	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);
}

bool ControlThread::CheckEOTLeft( bool isEOTLSwitchActive )
{
	bool atEOTLeft = false;

	if( (EOTLeftMode_test > 0) && !isEOTLSwitchActive  )
	{
		EOTLeftMode_test = 0;		//must have consecutive EOT switches
		atEOTLeft = false;
	}
	else if( isEOTLSwitchActive )
	{
		EOTLeftMode_test = EOTLeftMode_test + 1;
		if(EOTLeftMode_test >= EOTLEFT_MODE_TEST)
		{
			EOTLeftMode_test = 0;
			atEOTLeft = true;
		}
		else
			atEOTLeft = false;
	}
	else
		atEOTLeft = false;

	return(atEOTLeft);
}

bool ControlThread::CheckEOTRight( bool isEOTRSwitchActive )
{
	bool atEOTRight = false;

	if( (EOTRightMode_test > 0) && !isEOTRSwitchActive  )
	{
		EOTRightMode_test = 0;		//must have consecutive EOT switches
		atEOTRight = false;
	}
	else if( isEOTRSwitchActive )
	{
		EOTRightMode_test = EOTRightMode_test + 1;
		if( EOTRightMode_test >= EOTRIGHT_MODE_TEST)
		{
			EOTRightMode_test = 0;
			atEOTRight = true;
		}
		else
			atEOTRight = false;
	}
	else
		atEOTRight = false;

	return(atEOTRight);
}