#include "RandomMovement.hh"

RandomMovement::RandomMovement( char* name, priority_t priorityLevel, 
				Semaphore* sem, RobotControl* robot) :
  Behavior (name, priorityLevel, 1, "localhost", 2000, sem)
{
  this->robot = robot;

  cerr << name.data () << ": Constructed a RandomMovement." << endl;
}

int 
RandomMovement::run ()
{
  int FrontBackProb = 50;

  while (true)
    {
      int tvel = rand () % 5;
      if ((rand () % 100) < FrontBackProb)
	{
	  tvel = -tvel;
	}

      int rvel = rand () % 5;
      if ((rand () % 100) < 50)
	{
	  rvel = -rvel;
	}

      bool TorR = (rand () % 100) < 10;

      if (TorR)
	{
	  cerr << name.data () << ": Setting rotational velocity to "
	       << rvel << " degrees/sec." << endl;
	  
	  robot->RotationalVelocity (rvel);
	  sleep (1);
	  robot->RotationalVelocity (0);

	  sleep (1);

	  testSuspend ();
	}
      else
	{
	  cerr << name.data () << ": Setting translational velocity to "
	       << tvel << " mm/sec." << endl;
	  robot->TranslationalVelocity (tvel);
	  sleep (3);
	  robot->TranslationalVelocity (0);
	  
	  if (robot->LeftWheelStall () || robot->RightWheelStall ())
	    {
	      if (tvel > 0)
		{
		  cerr << name.data () << ": Increasing reverse prob to "
		       << FrontBackProb << "%" << endl;
		  FrontBackProb += 5;
		}
	      if (tvel < 0)
		{
		  cerr << name.data () << ": Increasing forward prob to "
		       << 100-FrontBackProb << "%" << endl;
		  FrontBackProb -= 5;
		}
	    }

	  sleep (1);

	  testSuspend ();
	}
    }
}

void
RandomMovement::onSuspend ()
{
  cerr << name.data () << ": Getting suspended.  Setting tvel and rvel "
       << "to 0." << endl;
  robot->TranslationalVelocity (0);
  robot->TranslationalVelocity (0);
  robot->TranslationalVelocity (0);

  robot->RotationalVelocity (0);
  robot->RotationalVelocity (0);
  robot->RotationalVelocity (0);
}