#include <stdint.h>
#include <signal.h>
#include <sys/time.h>
#include <string.h>
#include <iostream>
#include "ControlThread.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;

	// 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);

		// Your PID stuff
		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;
		cout.flush();
	}
	return NULL;
}