#include "BatterySaver.hh"

BatterySaver::BatterySaver (char* name, priority_t priorityLevel, 
			    RobotControl* robot, float minBatteryLevel) :
  Behavior (name, priorityLevel, 1, "localhost", 2000)
{
  this->robot           = robot;
  this->minBatteryLevel = minBatteryLevel;
  suspended             = false;

  cerr << name << ": Constructed a Batterysaver." << endl;
}

int 
BatterySaver::run ()
{
  sleep(3);			// Wait for the charge to settle

  while (true)
    {
      // Retrieve the current charge level of the battery
      float charge = robot->BatteryCharge ();

      // If the charge is below the allowed amount
      if (charge < minBatteryLevel && !suspended)
	{
	  cerr << name << ": Charge is too low (" 
	       << charge << "V).  Suspending." << endl;
	  sendCommand (SUSPEND);
	  suspended = true;
	}
      // If the charge is above the allowed amount
      else if (charge >= minBatteryLevel && suspended)
	{
	  /*
	  cerr << name << ": Charge is large enough ("
	       << charge << "V).  Resuming." << endl;
	       */
	  sendCommand (RESUME);
	  suspended = false;
	}
      // Otherwise print out the current battery charge
      
      sleep (1);
    }
}