/*

Third robot test program. Moving the robot to a bunch
of landmarks given in a local coordinate frame.

You can copy this and put your own material between these comments.

dml 2009
 */


#include <stdlib.h>
#include <limits.h>
#include<math.h>
#include<string.h>
#include "Aria.h"


int main(int argc, char **argv)
{
  /* DECLARATIONS */

  ArArgumentParser parser(&argc, argv);    // set up our parser
  ArSimpleConnector 
             simpleConnector(&parser);     // set up our simple connector
  ArRobot robot;                           // declare robot object


  /* INITIALIZATION OF CONNECTION TO ROBOT */

  Aria::init();                             // mandatory init

  parser.loadDefaultArguments();           // load the default arguments 

  if (!simpleConnector.parseArgs()         // check command line args
                || !parser.checkHelpAndWarnUnparsed(1))
  {    
    simpleConnector.logOptions();
    Aria::shutdown();
    return 1;
  }


  if (!simpleConnector.connectRobot(&robot)) // ask for connection to robot
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  /* INITIALIZATION OF ROBOT*/

  robot.runAsync(true);
  robot.enableMotors();


  /* ****** DO SOMETHING INTERESTING WITH THE ROBOT HERE ****** */

  printf("Ten readings from the gyro at 2s intervals \n");
  ArAnalogGyro myGyro(&robot);
  int i;
  /*
  for (i=0; i<10; i++) {
   printf("gyro %10f robot %10f mixed %10f temp %d ave %g\n", 
	 myGyro.getHeading(), 
	 robot.getRawEncoderPose().getTh(), 
	 robot.getTh(), 
	 myGyro.getTemperature(), 
	 myGyro.getAverage());
   ArUtil::sleep(2000);
  }
  printf("GYRO Done.\n");

  */
  printf("One hunder readings from the TCM2 compass at 1s intervals \n");
  ArTCM2 *tcm2=0;
  tcm2 = new ArTCM2(&robot);
  tcm2->commandContinuousPackets();
 for ( i=0; i<100; i++) {
   printf("Compass %f, Pitch %f, Roll %f, temp %f \n",
	  tcm2->getCompass(),
	  tcm2->getPitch(),
	  tcm2->getRoll(),
	  tcm2->getTemperature()
	  );
   ArUtil::sleep(1000);
 }

  printf("TCM2 Done.\n");
  /* ****** END OF SOMETHING INTERESTING WITH THE ROBOT HERE ****** */

  Aria::shutdown();
  return 0;


}