// These includes pull in interface definitions and utilities.
#include "robot.h"
#include <iostream>
#include <math.h>

using namespace std;

// Start of main program.
int main( int argc, char **argv )
{
  Robot R( argc, argv );
  
  // Now, here is a loop that continually checks robot sensors and
  // sends drive commands
  MobilityGeometry::SegmentData_var irData;
  float haltdist = 0.5;    // 50cm halts
  float tempdist;          // Computed temp dist value 
  float x, y, theta;

  cout << "********** Mobility IR Example ***********" << endl;
  
  while( 1 ) {
    // forward velocity, no rotational velocity
    R.setVelocity( 0.1, 0.0 );

    // grab the sonar data
    irData = R.getIR();

    tempdist = sqrt(
	(irData->org[0].x - irData->end[0].x) *
	(irData->org[0].x - irData->end[0].x) +
	(irData->org[0].y - irData->end[0].y) *
	(irData->org[0].y - irData->end[0].y));

    cout << "dist: " << tempdist << endl;
    
    if( tempdist < haltdist )
      break;
    
    // if the user pressed return
    if( mbyUtility::chars_ready() > 0 ) {
      cout << "stopped by user" << endl;
      break;
    }

    // Wait a small time before the next loop. 0.1 second, keep going.
    omni_thread::sleep( 0, 100000000 );
  }

  R.setVelocity( 0.0, 0.0 );	  
  R.getOdometry( x, y, theta );
  cout << "X: " << x << endl;
  cout << "Y: " << y << endl;
  cout << "theta: " << theta << endl;
  return 0;
}