//******************************
//
// Filename = USRobot.cpp
// Author = Michael Wales
//
// Version History:
//  0.0   Initial version for USRobot robot from Russ Clifford
//
//******************************

#include"RobotAPI.h"
#include<stdio.h>
#include<string.h>
#include<pthread.h>
#include<sys/types.h>
#include<unistd.h>
#include<stdlib.h>

//******************//
// GLOBAL VARIABLES //
//******************//

#define WheelTurnTime 650000
#define PivotTime 6000
#define Pause 80000

RobotAPIRobot APIR;
unsigned char SpeedInput = 128;
unsigned char SteerInput = 128;
int Heading = 0;


//***********************//
// FUNCTION DECLERATIONS //
//***********************//

void sendSerialCommand( unsigned char Speed, unsigned char Steer );
void* senderFunction ( void* UselessObject );
int angle0To359( int Angle );


//***************//
// MAIN FUNCTION //
//***************//

int main (void)
   {
   TesterClass Test;
   Test.runMe();
   }

   
//************************************//
// CLASS IMPLEMENTATION - TesterClass //
//************************************//

int TesterClass::runMe (void)
   {
   ConfigSys Configuration;
   Configuration.loadConfiguration();
   
   printf("Press return to connect to server %s\n", Configuration.ServerIP );
   getchar();   
   
   APIR.setReceiver( this );
   APIR.setPort( Configuration.PortNumber );
   APIR.establishConnection( Configuration.ServerIP );
   
   pthread_t SerialPortThread;
   pthread_create( &SerialPortThread, 0, &senderFunction, (void*) this);
   
   printf("Press return to quit\n");
   getchar();   
   
   return 0;
   }
   

void TesterClass::receiveCommand(char InstructionCode, RobotCommand* NewCommand)
   {
     
   switch(InstructionCode)
      {
      case 'A':
         // CHANGE ORIENTATION
         int NewHeading, DeltaAngle, OldSpeed;
         
         printf("USRobot Received:  CHANGE_ORIENTATION\t");
         
         NewHeading = angle0To359( NewCommand->Value );
         
         // Easy case:  Don't need to turn at all
         if ( NewHeading == Heading )
            {
            delete NewCommand;
            break;
            }
         
         // Case I:  NewHeading > Heading
         if ( NewHeading > Heading )
            {
            DeltaAngle = NewHeading - Heading;     // Always positive
            
            // If DeltaAngle < 180, turn counter clockwise
            // If DeltaAngle >= 180, turn clockwise
            if ( DeltaAngle >= 180)
               DeltaAngle = DeltaAngle - 360;  // Always negative
            }
         // Case II:  NewHeading <= Heading
         else
            {
            DeltaAngle = Heading - NewHeading;     // Always positive
            
            // If DeltaAngle < 180, turn clockwise
            // If DeltaAngle >= 180, turn counter-clockwise
            if ( DeltaAngle >= 180)
               DeltaAngle = DeltaAngle - 360;  // Always negative
               
            DeltaAngle = DeltaAngle * (-1);     // Switch the sign for the DeltaAngle
            }
            
         printf("Delta=%i\t", DeltaAngle);
         
         // Stop the robot and dry steer the wheel, this will improve the accuracy for steering model
         OldSpeed = SpeedInput;
         SpeedInput = 128;
         
         // Tell the wheel to turn in the desired direction
         printf("Rotating Wheel 45 deg.\n");
         if ( DeltaAngle > 0 )
            SteerInput = 48;
         if ( DeltaAngle < 0 )
            SteerInput = 208;
         
         robotWait( Pause );
   
         // Change this number depending on how fast wheel turns on surface
         // Want enough time for the wheel to turn about 45 degrees
         robotWait( WheelTurnTime );
         
         // Stop the wheel from turning
         SteerInput = 128;
         
         robotWait( Pause );

         // Use the rear wheels to turn the robot
         // Change the below numbers depending on how fast the robot can pivot on surface
         printf("\tPivoting\t");
         SpeedInput = 196;
         robotWait( Pause );
         robotWait( abs(DeltaAngle) * PivotTime );
         SpeedInput = 128;
         
         robotWait( Pause );

         // Tell the wheel to turn back
         printf("Turning wheel back\t");
         if ( DeltaAngle > 0)
            SteerInput = 208;
         if ( DeltaAngle < 0)
            SteerInput = 48;
         robotWait( (int) (WheelTurnTime * 1.3) );
         SteerInput = 128;
         
         // Resume prior speed
         printf("Resuming speed %i", OldSpeed);
         SpeedInput = OldSpeed;            
         
         Heading = NewHeading;         
         printf("\n");
         delete NewCommand;
         break;
         
      case 'B':
         // CHANGE_SPEED
         
         // Cap off incoming value to the limits of the robot
         if ( NewCommand->Value <= -127 )
            SpeedInput = 0;
         else if ( NewCommand->Value >= 127 )
            SpeedInput = 255;
         else
            SpeedInput = NewCommand->Value + 128;
         
         printf("USRobot Received:  CHANGE_SPEED   Data = %i\tSerialOut = %i\n", NewCommand->Value, SpeedInput );
         
         delete NewCommand;
         break;
         
      case 'C':
         // GOTO_XY
         printf("USRobot Error   :  Command not implemented\n");
         
         delete NewCommand;
         break;
         
      case 'D':
         // FOLLOW_PATH
         printf("USRobot Error   :  Command not implemented\n");
         
         delete NewCommand;
         break;
         
      case 'E':
         // STATUS_CHECK
         printf("USRobot Received:  STATUS_CHECK\n");
         
         RobotCommand ReturnCommand;
         strcpy( ReturnCommand.CommandName, "CURRENT_STATUS");
         ReturnCommand.Value = SteerInput;
         ReturnCommand.ValueB = SpeedInput;
         ReturnCommand.ValueC = Heading;
         ReturnCommand.XCoordinate = 0;
         ReturnCommand.YCoordinate = 0;
         
         printf("USRobot Sending :  CURRENT_STATUS  SteerInput=%i  SpeedInput=%i  Heading=%i\n", SteerInput, SpeedInput, Heading);
         APIR.sendCommand( ReturnCommand );         
         
         delete NewCommand;
         break;
         
      case 'F':
         // CURRENT_STATUS
         printf("USRobot Error   :  Command not Implemented\n");
         
         delete NewCommand;
         break;
         
      case 'G':
         // STEER_MOTOR
         // Cap off incoming value to the limits of the robot
         if ( NewCommand->Value <= -127 )
            SteerInput = 0;
         else if ( NewCommand->Value >= 127 )
            SteerInput = 255;
         else
            SteerInput = NewCommand->Value + 128;
         
         printf("USRobot Received:  STEER_MOTOR   Data = %i\tSerialOut = %i\n", NewCommand->Value, SteerInput );
         
         delete NewCommand;
         break;    
      
      default:
         printf("Invalid Instruction Code\n");
         
         delete NewCommand;
         return;      
      }
   }
   
   
//*********************************************//
// FUNCTION IMPLEMENTATION - sendSerialCommand //
//*********************************************//

void sendSerialCommand( unsigned char speed, unsigned char steer)
   {
   FILE* SerialPort;
   unsigned char temp;

   SerialPort = fopen("/dev/ttyS0", "w");
   // fprintf( SerialPort, "%c%c\n", speed, steer);

   fputc( speed, SerialPort );
   fputc( steer, SerialPort );
   //temp = 23;
   //fputc( temp, SerialPort );
   temp = '\n';
   fputc( temp, SerialPort );

   fclose( SerialPort);
   
   return;
   }
   
   
//******************************************//
// FUNCTION IMPLEMENTATION - senderFunction //
//******************************************//

void* senderFunction (void* UselessObject)
   {
   int cnt=0;
      
   while(1)
      {
      sendSerialCommand( SpeedInput, SteerInput );
      robotWait(35000);
      }
   }
   
   
//***************************************//
// FUNCTION IMPLEMENTATION - angle0To359 //
//***************************************//

int angle0To359 (int Angle)
   {
   while ( (Angle<0) || (Angle>359) )
      {
      if ( Angle < 0 )
         Angle += 360;
      if ( Angle > 359 )
         Angle -= 360;
      }
   return Angle;
   }