/*
Very simple robot movement:
   move foward until you almost hit something
   turn around and come back to origin
*/

#include "Aria.h"

int main(int argc, char **argv)
{
  ArRobot robot; 		// robot
  ArSonarDevice sonar; 		// sonar

  /*** Initialize the System ***/
  printf("initializing aria...\n");
  Aria::init(); 		// mandatory init
  printf("initializing connection...\n");
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  printf("initializing sonar...\n");
  robot.addRangeDevice(&sonar);
  printf("connecting to robot...\n");
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }
  printf("turning on motors...\n");
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  printf("running the robot...\n");
  robot.runAsync(false);
  
  /**************************************/
  printf("original location = %f %f\n",robot.getX(),robot.getY());
  printf("moving at speed 200\n");
  robot.setVel(200);
  while(1)
    { 
      ArUtil::sleep(500);
      printf("sonars 2,3 - %d %d\n", 
             robot.getSonarRange(2), robot.getSonarRange(3));
      if ((robot.getSonarRange(2) < 300) ||
          (robot.getSonarRange(3) < 300))
         break;
    }
  robot.setVel(0);

  printf("done moving to wall, now at %f %f\n",robot.getX(),robot.getY());
  robot.setDeltaHeading(180);
  ArUtil::sleep(4000);

  printf("moving back toward home at 200\n");
  robot.setVel(200);
  while(1)
    { 
      ArUtil::sleep(500);
      if ((robot.getX() < 200.0) && (robot.getY() < 200.0))
         break;
    }
  robot.setVel(0);
  printf("done moving back, now at %f %f\n",robot.getX(),robot.getY());
  
  /*** Shut down ***/
  robot.disconnect();
  robot.stopRunning();
  Aria::shutdown();
  return 0;
}