/**********************************************************
    Shell of a Robot Program
    Steve Dannelly
    April 2005
**********************************************************/

#include "Aria.h"

/*** global variables ****************/
ArRobot         robot;   // robot
ArSonarDevice   sonar;   // sonar
ArTcpConnection con;     // connection
/*************************************/


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*    I N I T I A L I Z E   R O B O T   * * * * * * * * * * * * **/
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void Initialize_Robot()
{
  int portnum;

  /*** Read in the Port Number ***/
  cout << "\n\n\n-------------------------------------------\n";
  cout << "--- Enter the Simulator\'s PortNumber: ";
  cin  >> portnum;
  cout << "-------------------------------------------\n";
  if ((portnum > 8200) || (portnum < 8000) || (portnum == 8101))
    {
     cout << "\nError:\n";
     cout << "Port number must be between 8000 and 8200, but not 8101.\n";
     cout << "Kill the simulator and restart it with a proper port.\n";
     cout << "Can not continue, Exiting...\n\n";
     exit(0);
    }

  /*** Open the TCP port ***/
  printf("creating a connection channel with port %d ...\n",portnum);
  con.setPort("calgary",portnum);
  if (!con.openSimple())
    {
     printf("\nError: unable to open port %d\n",portnum);
     printf("can not continue, exiting...\n\n");
     Aria::shutdown();
     exit (1);
    }
    
  /*** Add Sonar Hardware Control ***/
  printf("adding sonar...\n");
  robot.addRangeDevice(&sonar);

  /*** Establish the Connection ***/
  printf("connecting client to server...\n");
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
    {
     printf("\nError: unable to connect to server...\n");
     printf("can not continue, exiting...\n\n");
     Aria::shutdown();
     exit(1);
    }

  /*** Wrap Up the Initializations ***/
  printf("turning on motors, etc...\n");
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  robot.runAsync(true);
  printf("this is process number %d\n",getpid());

  printf("\n\n*******************************\n");
  printf("   DONE WITH INITIALIZATIONS\n");
  printf("THE ROBOT IS READY AND RUNNING.\n");
  printf("*******************************\n\n");
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*  S H U T D O W N   * * * * * * * * * * * * * * * * * * * * * **/
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void Shut_Down() 
{
  char cmd[100];

  printf("\nShutting down the system...\n");
  robot.stop();
  robot.disconnect();
  robot.stopRunning();
  Aria::shutdown();
  printf("Shutdown complete\n");
  sleep(3);
  printf("Still not dead so...\n");
  printf("Performing Brutal Kill of Process %d\n",getpid());
  sprintf(cmd,"kill -9 %d\n",getpid());
  system(cmd);
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/

int main()
{
  Aria::init();
  Initialize_Robot();

  /**********************
  Put your code here!!!!
  **********************/

  Shut_Down();
}