using namespace std;
#include <iostream>
#include <sys/time.h>
#include "settings.h"
#include "servos.h"
#include "wheels.h"
#include "manual.h"
#include "vision.h"
#include "io.h"

#ifdef linux
#include <signal.h>
#endif
int visionState;
bool doVision;
int fd;
IO oIO; // there can only be one of these instantiated
Wheels oWheels(&oIO);
Servos oServos(&oIO);
Vision oVision(&oWheels, &oServos, &oIO);
Manual oManual(&oWheels, &oServos, &oIO);
int motorTimerCount;
int curTimerThreshold;
bool motorTimerOn;

void timer_handler(int x);
void sigint_handler(int x);

int main() {
  fd = oVision.fd;
       visionState=0;  
        doVision=false;
     
	struct itimerval timer1;

	signal(SIGALRM, timer_handler);

        if(doVision)
         signal(SIGINT, sigint_handler);
	
	timer1.it_interval.tv_sec = 0;       // reset val
	timer1.it_interval.tv_usec = 100000; // reset val usec=microseconds=0.1seconds
	timer1.it_value.tv_sec = 0;          // initial val \_ 0 = stopped
	timer1.it_value.tv_usec = 100000;    // initial val /
	setitimer(ITIMER_REAL, &timer1, NULL);

	
	cout << "Start  ii" << endl;

//      oWheels.GoDesired(10,10,10);
//	oWheels.SetDesired(10,10);
//	oWheels.Demo();
//	oServos.Demo()
 oManual.Start();
//	oManual.Runner(motorTimerOn,motorTimerCount,curTimerThreshold);
cout<<"After"<<endl;
//		oVision.Runner(1,motorTimerOn,motorTimerCount,curTimerThreshold);



	cout << "Stopping, please wait" << endl;
	double i;
	for (i = 0; i < 200000000; i++) { // give time for last step to complete
	}

	return true;
}

void sigint_handler(int x)
{

    close(fd);
    if ( visionState >=4)
        visionState=0;
    else
    {
       visionState++;
       oVision.Runner(visionState, motorTimerOn, motorTimerCount,curTimerThreshold);  //need to restore this prototype and link correctly
    }

    printf("SIGINT");
}

void timer_handler(int x) {
      // cout<<"SIGALRM Call"<<endl;
        if ( motorTimerOn )
	{
     
      cout<<"MTO mtc="<<motorTimerCount<<" ctt="<<curTimerThreshold<<endl;
		motorTimerCount++;
                if (motorTimerCount>curTimerThreshold)
		{
                   // cout<<"\nMT STOP\n"<<endl;
                      //have gone enough, stop the motors
                     oWheels.SetDesired(0,0.0);
                     motorTimerOn=0;
                    motorTimerCount=0;
		}
	}

	signal(SIGALRM, timer_handler); // reset, so it timers more than once
	oWheels.Update();
}