//******************************
//
// Filename = RobotAPI.cpp
// Author = Michael Wales
//
// Version History:
//  0.0   Complete listing of original design
//  0.1   Full support for CHANGE_ORIENTATION, CHANGE_SPEED, STATUS_CHECK, and CURRENT_STATUS commands
//
//******************************

#include"RobotAPI.h"
#include<stdio.h>
#include<stdlib.h>
#include<string.h>

// Include files for socket communications
#include<unistd.h>
#include<ctype.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<sys/uio.h>
#include<arpa/inet.h>

// Include files for threads
#define _REENTRANT
#include<pthread.h>
#include<semaphore.h>

#include<sys/time.h>

//*********************************//
// CLASS IMPLEMENTATION - RobotAPI //
//*********************************//

RobotAPI::RobotAPI ()
   {
   SocketFD = -1;
   PortNumber = -1;
   ConnectionOK = 0;
   ServerSocket = -1;

   for (int counter = 0; counter < MAX_CONNECTIONS; counter++ )
      CommunicationSockets[counter] = -1;
   }

void RobotAPI::startSafetyOverride (void)
   {
   return;

   pthread_t SafetyThread;
   pthread_create( &SafetyThread, 0, &safetyOverride, this);
   printf("RobotAPI:  Safety thread started\n");}

int RobotAPI::sendCommand( RobotCommand CommandToExecute)
   {
   int counter, status, mainstatus;
   sem_wait( &SendCommandLock );
   mainstatus = 0;
   
   // This was the parent method
   int SFD = getSocketFD();
   if ( SFD != -1) 
      mainstatus = sendCommandTo( CommandToExecute, SFD);
   
   // This was the child method
   // Send data to secondary connections
   for (counter=0; counter < MAX_CONNECTIONS; counter++)
      {
      if ( CommunicationSockets[counter] != -1 )
         {
         // printf("Send data to secondary connection #%i\n", counter+1);
         status = sendCommandTo( CommandToExecute, CommunicationSockets[counter]);
         if ( status == 1)
            {
            // Connections has gone bad
            printf("RobotAPI:  Lost secondary connections #%i\n", counter+1 );
            close( CommunicationSockets[counter] );
            CommunicationSockets[counter] = -1;
            }
         }
      }
   
   sem_post( &SendCommandLock );
   return mainstatus;
   }

int RobotAPI::sendCommandTo(RobotCommand CommandToExecute, int SFD)
   {
   // Check for good connections
   if ( SFD == -1)
      {
      printf("RobotAPI Error:  Socket not open.\n");
      return 1;
      }   
   
   int SentSize;
   int CommandSize = sizeof( CommandToExecute );
   char* Buffer;
   char InstructionCode = 'Z';
   
   // Create an instruction code
   if ( strcmp( CommandToExecute.CommandName, "CHANGE_ORIENTATION") == 0 )
      InstructionCode = 'A';
   if ( strcmp( CommandToExecute.CommandName, "CHANGE_SPEED") == 0 )
      InstructionCode = 'B';
   if ( strcmp( CommandToExecute.CommandName, "GOTO_XY") == 0 )
      InstructionCode = 'C';
   if ( strcmp( CommandToExecute.CommandName, "FOLLOW_PATH") == 0 )
      InstructionCode = 'D';
   if ( strcmp( CommandToExecute.CommandName, "STATUS_CHECK") == 0 )
      InstructionCode = 'E';
   if ( strcmp( CommandToExecute.CommandName, "CURRENT_STATUS") == 0 )
      InstructionCode = 'F';
   if ( strcmp( CommandToExecute.CommandName, "STEER_MOTOR") == 0 )
      InstructionCode = 'G';
   if ( strcmp ( CommandToExecute.CommandName, "NULL_COMMAND") == 0 )
      InstructionCode = 'Y';

   // printf("RobotAPI:  Sending command %s\n", CommandToExecute.CommandName);
      
   SentSize = send( SFD, &InstructionCode, 1, 0);
   if (SentSize != 1)
      {
      printf("RobotAPI ERROR:  Command not sent (Failed on instruction code)\n");
      return 1;
      }
   
   Buffer = (char*) &CommandSize;
   SentSize = send( SFD, Buffer, sizeof( CommandSize ), 0);
   if (SentSize != sizeof( CommandSize ))
      {
      printf("RobotAPI ERROR:  Command not sent (Failed on command size)\n");
      return 1;
      }
   
   Buffer = (char*) &CommandToExecute;
   SentSize = send( SFD, Buffer, CommandSize, 0);
   if (SentSize != CommandSize)
      {
      printf("RobotAPI ERROR:  Command not sent (Failed on command)\n");
      return 1;
      }
   
   return 0;
   }
   
void RobotAPI::setPort(int PortNum)
   {
   PortNumber = PortNum;
   }
   
void RobotAPI::setReceiver(TesterClass* InterfaceToAPI)
   {
   MessageReceiver = InterfaceToAPI;
   
   // Initialize the semaphore
   if ( sem_init( &SendCommandLock, 0, 1) != 0 )
      {
      printf("RobotAPI ERROR:  Couldn't initialize SendCommandLock semaphore\n");
      exit(1);
      }
   }
   
void RobotAPI::setSocketFD (int NewFD)
   {
   SocketFD = NewFD;
   }
   
int RobotAPI::getSocketFD (void)
   {
   return SocketFD;
   }
   
 
//*******************************************//
// CLASS IMPLEMENTATION - RobotAPIController //
//*******************************************//

int RobotAPIController::establishConnection(void)
   {
   int SFD;
   
   struct sockaddr_in ServerAddress = {AF_INET, PortNumber, INADDR_ANY};
   
   // Initialize the socket
   SFD = socket( AF_INET, SOCK_STREAM, 0);
   if (SFD == -1)
      {
      printf("RobotAPI ERROR:  Robot's side of socket initialization failed\n");
      return -1;
      }
      
   // Set the SocketFD variable in the RobotAPI parent class
   setSocketFD( SFD );
      
   // Bind the server socket
   if ( bind( SFD, (struct sockaddr*) &ServerAddress, sizeof( ServerAddress )) == -1)
      {
      printf("RobotAPI ERROR:  Robot's side of socket failed binding\n");
      return -1;
      }
      
   // Setup server socket to listen for connections
   if ( listen(SFD, 5) == -1)
      {
      printf("RobotAPI ERROR:  Robot's side unable to listen for connection\n");
      return -1;
      }
      
   // Start accepting connections
   int ComSocket = accept( SFD, NULL, NULL );
   if ( ComSocket == -1 )
      {
      printf("RobotAPI Error:  Robot's side unable to accept connection\n");
      return -1;
      }
      
   ServerSocket = SFD;
   setSocketFD( ComSocket );
      
   // Start the listening thread
   pthread_t ListeningThread;
   pthread_create( &ListeningThread, 0, &listenForData, (void*) this);

   // Start the secondary connection listening thread
   pthread_t SecondaryConnectionT;
   pthread_create( &SecondaryConnectionT, 0, &waitForNewConnections, (void*) this);
   
   // Start sending null commands every .1 second to main connection
   // pthread_t NullCommandSender;
   // pthread_create( &NullCommandSender, 0, &sendNullCommand, (void*) this);
   
   return 0;
   }
   

   

//**************************************//
// CLASS IMPLEMENTATION - RobotAPIRobot //
//**************************************//

int RobotAPIRobot::establishConnection(char* IPAddress)
   {
   int SFD;
   
   sockaddr_in ServerAddress = {AF_INET, PortNumber};
   ServerAddress.sin_addr.s_addr = inet_addr(IPAddress);
   
   // Initialize the socket
   SFD = socket( AF_INET, SOCK_STREAM, 0);
   if (SFD == -1)
      {
      printf("RobotAPI ERROR:  Controller's side of socket initialization failed");
      return -1;
      }
      
   // Set the SocketFD variable in the RobotAPI parent class
   setSocketFD( SFD );
      
   if ( connect( SFD, (struct sockaddr *) &ServerAddress, sizeof( ServerAddress) ) == -1 )
      {
      printf("RobotAPI ERROR:  Controller side failed to connect\n");
      return -1;
      }
      
   // Start the listening thread
   pthread_t ListeningThread;
   pthread_create( &ListeningThread, 0, &listenForData, (void*) this);
   
   // startSafetyOverride();
   return 0;
   }
   
   
//**********************************//
// CLASS IMPLEMENTATION - PointList //
//**********************************//

void PointList::addPoint(int X, int Y)
   {
   if ( (NumOfPoints+1) == MaxSize )
      {
      printf("RobotAPI Error:  You can't add any more points to the list\n");
      return;
      }
   
   // Store point information into array
   TheList[NumOfPoints].X = X;
   TheList[NumOfPoints].Y = Y;
   
   //Update the number of points
   NumOfPoints++;
   }
   
void PointList::pathToString(char* Path)
   {
   
   }
   
void PointList::printList(void)
   {
   printf("MaxSize = %i\tNumOfPoints = %i\n", MaxSize, NumOfPoints);
   for( int counter = 0; counter < NumOfPoints; counter++)
      {
      printf("#%i)\t(%i,%i)\n", counter + 1, TheList[counter].X, TheList[counter].Y );
      }
   }
   
PointList::PointList(int Size)
   {
   if (Size > 50)
      {
      printf("RobotAPI Error:  Maximum point list size limited to 50\n");
      Size = 50;
      }
   
   NumOfPoints = 0;
   MaxSize = Size;
   // TheList = new XYPoint[MaxSize];
   }
   

//*************************************//
// CLASS IMPLEMENTATION - RobotCommand //
//*************************************//

void RobotCommand::copyCommand(RobotCommand Source)
   {
   strcpy( CommandName, Source.CommandName );
   Value = Source.Value;
   ValueB = Source.ValueB;
   ValueC = Source.ValueC;
   XCoordinate = Source.XCoordinate;
   YCoordinate = Source.YCoordinate;
   // Path = Source.Path;
   }
   
   
//**********************************//
// CLASS IMPLEMENTATION - ConfigSys //
//**********************************//

void ConfigSys::loadConfiguration (void)
   {
   char ThrowAway[200];
   FILE* ConfigFD;
   
   ConfigFD = fopen("config.txt", "r");
   
   if (ConfigFD == NULL)
      {
      printf("RobotAPI ERROR:  Couldn't open config.txt file\n");
      exit(1);
      }
   
   fscanf( ConfigFD, "%s", ThrowAway);
   fscanf( ConfigFD, "%s", ServerIP);
   fscanf( ConfigFD, "%s", ThrowAway);
   fscanf( ConfigFD, "%i", &HorizontalArea);
   fscanf( ConfigFD, "%s", ThrowAway);
   fscanf( ConfigFD, "%i", &PortNumber);
      
   fclose( ConfigFD);   
   }

   
//**********************//
// FUNCTION - robotWait //
//**********************//

void robotWait(int MicroSeconds)
   {
   // timezone *StupidTimeZone = new timezone;
   timeval *CurrentTime = new timeval;
   timeval *EndTime = new timeval;
   
   gettimeofday( CurrentTime, 0);
   
   int SecondsAway = MicroSeconds / 100000;
   int MicroSecondsAway = MicroSeconds % 100000;
   
   EndTime->tv_sec = CurrentTime->tv_sec + SecondsAway;
   EndTime->tv_usec = CurrentTime->tv_usec + MicroSeconds;   
   
   while (1)
      {
      gettimeofday( CurrentTime, 0);
      
      if ( EndTime->tv_sec < CurrentTime->tv_sec )
         break;
         
      if ( EndTime->tv_sec == CurrentTime->tv_sec )
         {
         if ( EndTime->tv_usec <= CurrentTime->tv_usec )
            break;
         }
      }
   
   delete CurrentTime;
   delete EndTime;
   }


//**************************//   
// FUNCTION - listenForData //
//**************************//   

void* listenForData(void* RobotAPIObject)
   {
   RobotAPI* BotAPI = (RobotAPI*) RobotAPIObject;
   
   char InstructionCode;
   char* Buffer;
   int CommandSize, ErrorCode = 1;
   RobotCommand NextCommand;
   RobotCommand* SendMe;
   int SFD = BotAPI->getSocketFD();
   
   while (1)
      {      
      // Receive the instruction code      
      ErrorCode = read(SFD, &InstructionCode, 1);
      
      if (ErrorCode == 0)
         {
         printf("RobotAPI:  Main Connection Terminated\n");
         close ( SFD );
         BotAPI->setSocketFD(-1);
         break;
         }
      
      // printf("EC=%i\tIC=%c\t",ErrorCode, InstructionCode);     
      
      recv( SFD, &CommandSize, sizeof(CommandSize), 0);
            
      recv( SFD, &NextCommand, CommandSize, 0);      
      
      // printf("copy = %s, %i\n", SendMe->CommandName, SendMe->Value);
      
      BotAPI->ConnectionOK = 1;
      
      // If command is the null command, don't pass on command
      if ( InstructionCode == 'Y' )
         {
         // printf("RobotAPI:  Received a NULL command\n");
         continue;
         }

      // printf("RobotAPI:  Receiving normal command\n");
      SendMe = new RobotCommand;
      SendMe->copyCommand( NextCommand );
      BotAPI->MessageReceiver->receiveCommand( InstructionCode, SendMe );
      }
      
   printf("RobotAPI:  Listening thread finished\n");
   return 0;
   }
   
//**********************************//
// FUNCTION - waitForNewConnections //
//**********************************//

void* waitForNewConnections (void* RobotAPIControllerObject)
   {
   RobotAPIController* MyAPIC = (RobotAPIController*) RobotAPIControllerObject;
   
   // Initialize CommunicationSockets[] array
   int* CommunicationSockets;
   CommunicationSockets = MyAPIC->CommunicationSockets;   
   for(int counter=0; counter < MAX_CONNECTIONS; counter++)
      CommunicationSockets[counter] = -1;   
   
   int MyServerSocket = MyAPIC->ServerSocket;
   int ExtraConnections = 0;
   
   while (ExtraConnections < MAX_CONNECTIONS)
      {
      // Start accepting extra connections
      CommunicationSockets[ExtraConnections] = accept( MyServerSocket, NULL, NULL );
      if ( CommunicationSockets[ExtraConnections] != -1)
         {
         ExtraConnections++;
         printf("RobotAPI:  New secondary connection established! ((%i of %i)\n", ExtraConnections, MAX_CONNECTIONS);
         
         // Start secondary connection listening thread
         pthread_t SecondaryListeningThread;
         pthread_create( &SecondaryListeningThread, 0, &listenOnSecondaryConnection, (void*) RobotAPIControllerObject);         
         }              
      }   
   
   printf("RobotAPI:  Maximum connections established\n");
   return RobotAPIControllerObject;
   }
   
//****************************************//
// FUNCTION - listenOnSecondaryConnection //
//****************************************//

void* listenOnSecondaryConnection(void* RobotAPIControllerObject)
   {
   RobotAPIController* BotAPI = (RobotAPIController*) RobotAPIControllerObject;
   int ConnectionNumber;
   
   for(int counter=0; counter < MAX_CONNECTIONS; counter++)
      {
      if ( BotAPI->CommunicationSockets[counter]  != -1 )
         ConnectionNumber = counter;
      }
      
   printf("Listening on secondary connection #%i\n", ConnectionNumber+1);   
   
   char InstructionCode;
   char* Buffer;
   int CommandSize, ErrorCode = 1;
   RobotCommand NextCommand;
   RobotCommand* SendMe;
   
   while (1)
      {      
      // Receive the instruction code      
      ErrorCode = read( BotAPI->CommunicationSockets[ConnectionNumber], &InstructionCode, 1);
      
      if (ErrorCode == 0)
         {
         printf("RobotAPI:  Secondary Connection #%i Terminated\n", ConnectionNumber + 1);
         close( BotAPI->CommunicationSockets[ConnectionNumber] );
         BotAPI->CommunicationSockets[ConnectionNumber] = -1;
         pthread_exit(0);
         }
      
      // printf("EC=%i\tIC=%c\t",ErrorCode, InstructionCode);     
      
      recv( BotAPI->CommunicationSockets[ConnectionNumber], &CommandSize, sizeof(CommandSize), 0);
            
      recv( BotAPI->CommunicationSockets[ConnectionNumber], &NextCommand, CommandSize, 0);
      
      printf("RobotAPI:  Secondary Connection #%i had incoming data with IC=%c\n", ConnectionNumber+1, InstructionCode);
      }
      
   printf("RobotAPI ERROR:  This code should never execute (listenOnSecondaryConnection)\n");
   return 0;
   }


//****************************//
// FUNCTION - sendNullCommand //
//****************************//

void* sendNullCommand (void* RobotAPIObject)
   {
   RobotCommand MyNullCommand;
   int SentSize;
   char* Buffer;
   int SFD = 0;;
   RobotAPI* BotAPI = (RobotAPIController*) RobotAPIObject;
   // RobotAPIController BotAPINoPointer = *BotAPI;

   // Create the null instructions
   MyNullCommand.Value = 123;
   strcpy( MyNullCommand.CommandName, "NULL_COMMAND" );
   int CommandSize = sizeof( MyNullCommand );

   while (1)
      {
      // printf("RobotAPI:  Sending null commands\n");
      (*BotAPI).sendCommand( MyNullCommand );

      // Wait for a tenth of a second
      robotWait(30000);
      }

   return RobotAPIObject;
   }
   
   
//***************************//
// FUNCTION - safetyOverride //
//***************************//

void* safetyOverride (void* RobotAPIObject)
   {
   // Create the command to stop the robot incase of an emergency
   RobotCommand* StopMoving = new RobotCommand;
   StopMoving->Value = 0;
   strcpy( StopMoving->CommandName, "CHANGE_SPEED");
   
   RobotAPI* BotAPI = (RobotAPI*) RobotAPIObject;
   
   while( BotAPI->getSocketFD() != -1)
      {
      BotAPI->ConnectionOK = 0;
      robotWait(600000);
      
      if ( BotAPI->ConnectionOK == 1 )
         {
         // printf("Connection good\n");
         }
      else
         {
         printf("RobotAPI:  Safety override initiated!\n");
         
         // Send emergancy command
         BotAPI->MessageReceiver->receiveCommand('B', StopMoving);
         
         // Recreate the StopMoving data (would have been deleted by receiveCommand() )
         StopMoving = new RobotCommand;
         StopMoving->Value = 0;
         strcpy( StopMoving->CommandName, "CHANGE_SPEED");
         }
      }
   
   // Socket closed, make sure robot stopped
   printf("RobotAPI:  Safety override initiated!\n");
         
   // Send emergancy command
   BotAPI->MessageReceiver->receiveCommand('B', StopMoving);
   
   return RobotAPIObject;
   }