//******************************
//
// Filename = Tester.cpp
// Author = Michael Wales
//
// Version History:
//  0.0   No networking, simply creating simple data structures
//  0.1   Full support for CHANGE_ORIENTATION, CHANGE_SPEED, STATUS_CHECK, and CURRENT_STATUS commands
//
//******************************

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

int main (void)
   {
   TesterClass Test;
   Test.runMe();
   }
   
//************************************//
// CLASS IMPLEMENTATION - TesterClass //
//************************************//

int TesterClass::runMe (void)
   {
   printf("Main function\n");
   
   ConfigSys Configuration;
   Configuration.loadConfiguration();
   printf("Server IP = %s\n", Configuration.ServerIP);
   printf("Horizontal Area = %i\n\n", Configuration.HorizontalArea);
   
   printf("\n1. Controller\n2. Robot\n");
   int menuchoice;
   scanf("%i", &menuchoice);
   getchar();
   
   RobotCommand DumbCommand;
   strcpy ( DumbCommand.CommandName, "CHANGE_ORIENTATION");
   DumbCommand.Value = 100;
   
   if (menuchoice == 1)
      {
      // Controller Mode
      RobotAPIController APIC;
      APIC.setReceiver( this );
      APIC.setPort( 7000 );
      APIC.establishConnection();
      
      int CommandChoice = 0;
      int ErrorFlag = 0;
      
      while (1)
         {
         printf("\n\nRobot Controller Interface:\n");
         printf(" 1) Send CHANGE_ORIENTATION\n");
         printf(" 2) Send CHANGE_SPEED\n");
         printf(" 3) Send GOTO_XY (not implemented)\n");
         printf(" 4) Send FOLLOW_PATH (not implemented)\n");
         printf(" 5) Send STATUS_CHECK\n");
         
         scanf("%i", &CommandChoice);
         
         switch (CommandChoice)
            {
            case 1:
               // CHANGE_ORIENTATION command
               printf("CHANGE_ORIENTATION:  Enter the new heading in degrees\n");
               scanf("%i", &(DumbCommand.Value));
               strcpy( DumbCommand.CommandName ,"CHANGE_ORIENTATION" );
               ErrorFlag = APIC.sendCommand( DumbCommand ); 
               printf("CHANGE_ORIENTATION(%i) sent with ErrorFlag=%i\n", DumbCommand.Value, ErrorFlag);
               break;
            
            case 2:
               // CHANGE_SPEED command
               printf("CHANGE_SPEED:  Enter the new speed\n");
               scanf("%i", &(DumbCommand.Value));
               strcpy( DumbCommand.CommandName ,"CHANGE_SPEED" );
               ErrorFlag = APIC.sendCommand( DumbCommand ); 
               printf("CHANGE_SPEED(%i) sent with ErrorFlag=%i\n", DumbCommand.Value, ErrorFlag);
               break;
               
            case 3:
               // GOTO_XY command
               printf("Do you not understand NOT IMPLEMENTED\n");
               break;
               
            case 4:
               // FOLLOW_PATH command
               printf("Do you not understand NOT IMPLEMENTED\n");
               break;
               
            case 5:
               // STATUS_CHECK command
               strcpy (DumbCommand.CommandName, "STATUS_CHECK");
               ErrorFlag = APIC.sendCommand( DumbCommand ); 
               printf("STATUS_CHECK sent with ErrorFlag=%i\n", ErrorFlag);
               break;
               
            default:
               printf("You entered an invalid command\n");
               break;
            }         
         }
      }
      
   if (menuchoice == 2)
      {
      RobotAPIRobot APIR;
      APIR.setReceiver( this );
      APIR.setPort( 7000 );
      APIR.establishConnection( Configuration.ServerIP );
      
      getchar();
      while (1)
         {
         printf("\nPress return to send a command\n");
         getchar();
         printf("ReturnVal = %i\n", APIR.sendCommand( DumbCommand ) );
         }
      }  
      
   printf("DONE!\n");
   
   /* ***** PointList Test Code *****
   char ThePath[] = new char[256];
   for(int cnt=0; cnt < 256; cnt++)
      ThePath[cnt] = 0;
   
   PointList MyList;
   
   printf("Adding point (1,2) and (3,4)\n");   
   
   MyList.addPoint(1,2);
   printf("In the middle of adding em'\n");
   MyList.addPoint(3,4);
   
   MyList.printList();
   getchar();   

   printf("Retrieving path:\n");
   MyList.pathToString( ThePath );

   printf("Here is the path: %s\n", ThePath);   
   ***** End of PointList test code *****/
      
   getchar();
   
   return 0;
   }
   
void TesterClass::receiveCommand(char InstructionCode, RobotCommand* NewCommand)
   {
      
   printf("\nTC-Incoming command = %c\n", InstructionCode);   
   
   switch(InstructionCode)
      {
      case 'A':
         // CHANGE ORIENTATION
         printf("Changing orientation...\n");
         printf("Orientation = %i\n\n", NewCommand->Value);
         
         delete NewCommand;
         break;
         
      case 'B':
         // CHANGE_SPEED
         printf("Changing speed...\n");
         
         break;
         
      case 'C':
         // GOTO_XY
         printf("Going to a specific point...\n");
         
         break;
         
      case 'D':
         // FOLLOW_PATH
         printf("Going to follow path...\n");
         
         break;
         
      case 'E':
         // STATUS_CHECK
         printf("Need a status check...\n");
         
         break;
         
      case 'F':
         // CURRENT_STATUS
         printf("Current status...\n");
         
         break;
         
      default:
         printf("Invalid Instruction Code\n");
         return;      
      }
   }