#include "AvoidObject.hh"
#include <float.h>

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

  cerr << name << ": Constructed an AvoidObject behavior." << endl;
}

int 
AvoidObject::run ()
{
  int delay = 50000;
  
  while ( true )
    {
      // find the minimum sonar value
      int min_sonar_num = findMinSonar();
      
      // If the sonar reading is too close, back away. Otherwise ignore.
      if ( testMinimum( min_sonar_num ) )
	{
	  Suspend( true );
	  AdjustVelocity( min_sonar_num );
	  testSuspend();
	}
      else
	{
	  Suspend( false );
	}

      usleep( delay );
      testSuspend();
    }
}

void
AvoidObject::AdjustVelocity( int min_sonar )
{
  double SonarScale[] = { 0.9, 0.8, 0.7, 1.4, 1.3, 1.2, 1.1 };

  double factor = SonarScale[ min_sonar ];

  double vr_new = robot->RightWheelVelocity() * factor;
  double vl_new = 2 * velocity - vr_new;

  cout << " - Sonar " << min_sonar << " is minimum" << endl;

  cout << "DeltaV: vt=" << velocity << ", vr=" << right_vel << ", vl="
       << left_vel << "\t New: vt=" << ((vr_new+vl_new)/2)
       << ", vr=" << vr_new << ", vl=" << vl_new << endl;

  robot->TranslationalVelocity( vr_new, vl_new );
  usleep( 100000 );
}

void
AvoidObject::ResetVelocity()
{
}

void
AvoidObject::Suspend( bool suspendValue )
{
  // Suspend other threads
  if ( !suspended && suspendValue )
    {
      sendCommand(SUSPEND);
      suspended = true;

      // Store previous wheel conditions
      left_vel = robot->LeftWheelVelocity();
      right_vel = robot->RightWheelVelocity();
      velocity = ( left_vel + right_vel ) / 2;
    }
  else if ( suspended && !suspendValue )
    {
      ResetVelocity();
      sendCommand(RESUME);
      suspended = false;

      // restore previous wheel characteristics
      cout << "Resetting Velocity to: Vl= " << left_vel 
	   << ", Vr=" << right_vel << endl;
      robot->TranslationalVelocity( left_vel, right_vel );
    }
}

bool
AvoidObject::testMinimum( int min_sonar )
{
  double threshold = 800;

  if ( min_sonar != -1 && robot->SonarReading( min_sonar ) < threshold )
    return true;
  return false;
}

int
AvoidObject::findMinSonar()
{
  double min_distance = DBL_MAX;
  double max_distance = 0;
  int min_sonar_num = -1;
  int max_sonar_num = -1;

  // find the minimum and maximum sonar readings
  for ( int sonar = 0 ; sonar < MAX_SONAR ; sonar++ )
    {
      if ( robot->SonarReading(sonar) < min_distance )
	{
	  min_distance = robot->SonarReading(sonar);
	  min_sonar_num = sonar;
	}
      else if ( robot->SonarReading(sonar) > max_distance )
	{
	  max_distance = robot->SonarReading(sonar);
	  max_sonar_num = sonar;
	}
    }
  /*
    cout << "Min distance is " << min_distance << " on sonar "
    << min_sonar_num << endl;
	
    cout << "Max distance is " << max_distance << " on sonar "
    << max_sonar_num << endl;
  */
  return min_sonar_num;
}

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