#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <pthread.h>
#include <math.h>
#include <phidget21.h>

#define CONTROL_RATE 8 // ms
#define SENSOR_RATE 8 //ms
#define START_DELAY 500 //ms
#define PI 3.14159

long ustarttime = 0;
int running = 0;
int spring=6;
int mode;
double theta = 0;

void *startControlThread();

int msleep(long milisec)
{
   if(milisec > 0)
   {
      struct timespec req={0};
      time_t sec=(int)(milisec/1000);
      milisec=milisec-(sec*1000);
      req.tv_sec=sec;
      req.tv_nsec=milisec*1000000L;
      while(nanosleep(&req,&req)==-1)
         continue;
   }
   return 1;
}

int microsleep(long microsec)
{
   if(microsec > 0)
   {
      struct timespec req={0};
      time_t sec=(int)(microsec/1000000);
      microsec=microsec-(sec*1000000);
      req.tv_sec=sec;
      req.tv_nsec=microsec*1000;
      while(nanosleep(&req,&req)==-1)
         continue;
   }
   return 1;
}

long getutime() // get time in microseconds
{
   struct timeval tv;
   gettimeofday(&tv, 0);
   return( (long)(tv.tv_sec * 1000000 + tv.tv_usec)-ustarttime );
}
long getmtime() // get time in milliseconds
{
   return( round(getutime()/1000) );
}
long getstime() // get time in seconds
{
   return( round(getutime()/1000000) );
}

int ErrorHandler(CPhidgetHandle IFK, void *userptr, int ErrorCode, const char *unknown)
{
   printf("Error handled. %d - %s", ErrorCode, unknown);
   return 0;
}


int main(int argc, char* argv[])
{
   
   char keypress;
   
   printf("Put wheel in zero position and press any key.\n");
   printf("define the mode you want: f-force feeback,  .....");
   keypress = getchar();   // add code to define mode here
   
   if ( keypress== 'z' || keypress == 'Z')
   {
      mode=spring;
   }
   
    /*---------------------------------------------------------------------------------------------*/
   /*-------------------------------------- Spawn Control Thread ---------------------------------*/
   /*---------------------------------------------------------------------------------------------*/
   
   pthread_t control_thread_id;
   pthread_attr_t control_attr;
   struct sched_param control_param;
   
   pthread_attr_init(&control_attr);
   pthread_attr_setinheritsched(&control_attr, PTHREAD_EXPLICIT_SCHED);
   pthread_create(&control_thread_id, &control_attr, startControlThread, NULL);
   
   while(keypress != 'q' & keypress != 'Q')
   {
      msleep(5);
      keypress = getchar();
   }
   running = 0;
   sleep(1);
   
   return 0;
}

void *startControlThread(){
   int result;
   const char *err;
   
    printf("here 1\n");
   
   // setup motor controller
   CPhidgetMotorControlHandle motoControl = 0;
   CPhidgetMotorControl_create(&motoControl);
   CPhidget_open((CPhidgetHandle)motoControl, -1);   
   printf("here 2\n");
    // setup encoder
   CPhidgetEncoderHandle encoder = 0;
   CPhidgetEncoder_create(&encoder);
   CPhidget_open((CPhidgetHandle)encoder, 1);
   printf("here 3\n");
   //   if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000)))
   //   {
   //      CPhidget_getErrorDescription(result, &err);
   //      printf("Problem waiting for attachment: %s\n", err);
   //      return 0;
   //   }
   printf("here 4\n");
   if((result = CPhidget_waitForAttachment((CPhidgetHandle)encoder, 10000)))
   {
      CPhidget_getErrorDescription(result, &err);
      printf("Problem waiting for attachment: %s\n", err);
      return 0;   
   }   printf("here 5\n");
   
   int output;
   CPhidgetEncoder_getPosition(encoder, 0, &output);
   
   CPhidgetMotorControl_setAcceleration (motoControl, 0, 100.00);
   CPhidgetMotorControl_setVelocity (motoControl, 0, 0.00);
   
   struct sched_param sp;
   sp.sched_priority=63;
   if (pthread_setschedparam(pthread_self(), SCHED_RR, &sp)  == -1) {
      printf("Failed to change priority.\n");
   }
   FILE *file_c;
   file_c = fopen("haptics_cursor.txt", "w");   
   long startTime = getutime() + START_DELAY*1000;
   double torque;
   
   double rate = 1.46;
   printf("\n\n\n");
   double theta_error = 0.0;
   double theta_error_prev = 0.0;
   double Kp;
   double theta = 0.0;
   double theta_prev = 0.0;
   double theta_give = 0.0;
   
    printf("here");
   
   
   while(running)
   {
      // sleep until the next startTime following this point
      while(startTime < getutime())
      {
         startTime = startTime+CONTROL_RATE*1000;
      }
      microsleep(startTime - getutime());
        printf("Mode = %d",mode);   
         
      if(mode == spring)
      {
         CPhidgetMotorControl_setAcceleration (motoControl, 0, 100.00);
         CPhidgetEncoder_getPosition(encoder, 0, &output);
         theta_prev=theta;
         theta = -output/1441.8*360.0;      
         theta_give=0;
         theta_error = (theta_give - theta);
         Kp =.75;
         torque =  Kp * theta_error;
         if (torque>100)
         {
            torque=100;
         }
         if (torque<-100)
         {
            torque=-100;
         }
         fprintf(file_c,"%2.1f\t %2.1f %2.1f %2.1f\n",theta_give,theta,theta_error,startTime/1000000.0);
         CPhidgetMotorControl_setVelocity (motoControl, 0, torque);
      }
      
   }

      CPhidgetMotorControl_setAcceleration (motoControl, 0, 0);
      CPhidgetMotorControl_setVelocity (motoControl, 0, 0);
      //fprintf(file_c,"%2.1f\t %2.1f %2.1f\n",theta_give,theta,theta_error);   
      fclose(file_c);
      printf("\n\nControl loop stopping\n");
   }