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

using namespace std;

void* ControlThread::Run(void* parameter)
{
	Labjack				ESP_Profiler((char*) "192.168.0.4");
	struct itimerval	timer;
	sigset_t 			signalsToCatch;
	int					signalCaught;
	double				winchDAC_voltage;
	double				levelWindDAC_voltage;
	double 				motorTension;
	double	 			flangeTension;
	bool				isEndOfTravelLeftSwitchClosed;
	bool				isEndOfTravelRightSwitchClosed;
	bool				isCableLeftSwitchClosed;
	bool				isCableRightSwitchClosed;

	#define CABLELR_ARRAY_LENGTH 100

	int				i, loopCtr = 0,
					iTemp;
	double			cableLRValue = 0.0;
	static short	cableLR[CABLELR_ARRAY_LENGTH] = {0};
	static long		cableLRIndex = 0;

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

	winchDAC_voltage = 1.0;
	levelWindDAC_voltage = 2.0;

	ESP_Profiler.ConnectToLabjack();

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

		if( (isCableLeftSwitchClosed == true) && (isCableRightSwitchClosed == false) )
			cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 1;
		else if( (isCableRightSwitchClosed == true) && (isCableLeftSwitchClosed == false) )
			cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = -1;
		else if( (isCableRightSwitchClosed == false) && (isCableLeftSwitchClosed == false) )
			cableLR[(cableLRIndex % CABLELR_ARRAY_LENGTH)] = 0;
		else if( (isCableRightSwitchClosed == true) && (isCableLeftSwitchClosed == true) )
		{}	//error

		cableLRIndex = cableLRIndex + 1;

		iTemp = 0;
		for(i=0; i<CABLELR_ARRAY_LENGTH; i++)
			iTemp = iTemp + cableLR[i];
		cableLRValue = iTemp / CABLELR_ARRAY_LENGTH;

		cout << "Loop Counter 				= " << loopCtr << endl;
		cout << "iTemp		 				= " << iTemp << endl;
		cout << "cableLRValue 				= " << cableLRValue << endl << endl;

		cout << "Motor Tension 				= " << motorTension << endl;
		cout << "Flange Tension 			= " << flangeTension << endl;
		cout << "isEndOfTravelLeftSwitch 	= " << ((isEndOfTravelLeftSwitchClosed) ? "closed" : "open") << endl;
		cout << "isEndOfTravelRightSwitch	= " << ((isEndOfTravelRightSwitchClosed) ? "closed" : "open") << endl;
		cout << "isCableLeftSwitch 			= " << ((isCableLeftSwitchClosed) ? "closed" : "open") << endl;
		cout << "isCableRightSwitch 		= " << ((isCableRightSwitchClosed) ? "closed" : "open") << endl << endl;
		cout.flush();

		loopCtr = loopCtr + 1;
	}
	return NULL;
}