#include "Explore.hh"

Explore::Explore (char* name, priority_t priorityLevel, 
		  RobotControl* robot) :
  Behavior (name, priorityLevel, 1, "localhost", 2000)
{
  this->robot = robot;
  
  cerr << name << ": Constructed an Explore behavior." << endl;
}

int 
Explore::run ()
{
  double fan_range = 40.0; // Degree range possible in turning

  double speed = 200;      // velocity in mm/s

  while ( true )
    {
      testSuspend();

      // figure out a random direction to move in
      double new_direction = rand() % (int)fan_range;

      robot->RelativeHeading( (new_direction - ((double)fan_range/2.0)) );
      testSuspend();
      /*
       * Speed should be relative to the amount of room we have to 
       * work in. Query the sonar to see what the distance is in the
       * direction that we are moving in.
       */

      robot->TranslationalVelocity( speed );
      //(int)((distance / (double)max_distance)*(double)speed) );

      usleep( 5000000 );
      testSuspend();
    }
}

void
Explore::onSuspend ()
{
  cerr << name << ": Getting suspended." << endl;
}