/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   agent_gas.cc
   
   An agent class controling the gas. For the motorcycle it means the
   throttle. This determines the acceleration.
  
**/

#include <iostream>
using namespace std;
#include <cmath>
#include "agent_gas.h"
#include "manager.h"

// constructor
Agent_gas::Agent_gas() : Agent()
{
  set_nr_config();
}

// output the values
void Agent_gas::output_data()
{
  cout << "nr config: " << nr_config << endl;  
  cout << "thr_lateral " << thr_lateral<< endl;
  cout << "thr_front " << thr_front << endl;
  cout << "thr_speed " << thr_speed << endl;
  cout << "thr_speed_low " << thr_speed_low << endl;
  
  cout << "c_inc_speed " << c_inc_speed << endl;
  cout << "c_slope " << c_slope << endl;
  cout << "c_lat_speed " << c_lat_speed << endl;
  cout << "c_fr_speed " << c_fr_speed << endl;
  cout << "c_dec_speed " << c_dec_speed << endl;
  cout << "c_dec_latt " << c_dec_lat << endl;
  cout << "c_dec_fr " << c_dec_fr << endl;
  
  cout << "speed_pow_lat " << speed_pow_lat << endl;
  cout << "speed_pow_fr " << speed_pow_fr  << endl;
}

// Set the number of configuration values. To be defined by the
// derivate classes.
void Agent_gas::set_nr_config()
{
  nr_config = 13;
}

// Agent-specific set data to be defined in the derivate class.
void Agent_gas::set_data(float values[])
{
  int i=0;
  thr_lateral = values[i++];
  thr_front = values[i++];
  thr_speed = values[i++];
  thr_speed_low = values[i++];
  
  c_inc_speed = values[i++];
  c_slope = values[i++];
  c_lat_speed = values[i++];
  c_fr_speed = values[i++];
  c_dec_speed = values[i++];
  c_dec_lat = values[i++];
  c_dec_fr = values[i++];
  
  speed_pow_lat = values[i++];
  speed_pow_fr = values[i++];
}

// This will be specific to each agent.
void Agent_gas::update_action()
{
  get_status();
  parent->lock();
  // Get the status values from the manager.
  throttle = boss->moto->get_throttle_twist();
  msr = boss->moto->get_max_speed_rate();
  parent->unlock();
  float add_gas = 0, lat;
  //cout << "in agent gas update" << endl;

  if (left_dist+right_dist > 0)
    lat = abs(left_dist-right_dist) / (left_dist+right_dist);
  else
    lat = 1;
  float front = front_dist / moto_length;
  //cout << "front " << front << " throttle " << throttle << endl;
  //max_speed_rate * throttle_twist
  if (speed < thr_speed_low) {
    //print_status();
    //cout << "front " << front << endl;
    //cout << "increasing gas by " << 5*c_inc_speed*(thr_speed_low - speed)
    // << endl;
    add_gas += c_inc_speed*(thr_speed_low - speed);
  }
  else if (lat < thr_lateral &&
	  front > thr_front &&
	  speed < speed_limit &&
	  speed >= msr * throttle) {
    //print_status();
    //cout << "front " << front << " msrt " << msr * throttle<<endl;
    //cout << "increasing gas by " << c_inc_speed*(front - thr_front) *
    //  (speed_limit-speed)+c_slope*slope << endl;
    add_gas += (2*(c_inc_speed*(front - thr_front)*(speed_limit-speed)+
		c_slope*slope));
  }
  else {
    float lim = speed_limit>thr_speed ? thr_speed : speed_limit;
    float lim_lat = lat - c_lat_speed/pow(1+speed, 1/speed_pow_lat);
    float lim_front  = c_fr_speed/pow(1+speed, 1/speed_pow_fr) - front;
    if (speed > lim)
      add_gas += -c_dec_speed *(speed -lim)+0.3*c_slope*slope;
    if (lim_lat > 0)
      add_gas += -c_dec_lat*lim_lat+0.3*c_slope*slope;
    if (lim_front > 0)
      add_gas += -c_dec_fr*lim_front+0.3*c_slope*slope;
    //print_status();
    //cout << "decreasing the gas by " << add_gas << endl;
  }
  //cout << "adding gas " << add_gas << endl;
  if (add_gas != 0) {
    float factor = 0.9 + 0.2 * (rand() % 1000)/1000.0;
    add_gas *= factor;
    parent->lock();
    //cout << "adding gas " << add_gas << endl;
     
    //----------------------------------------
    //added by pk
    if((add_gas > 0) || (speed > 0.3))
      boss->moto->add_throttle_twist(add_gas);
    //----------------------------------------
    parent->unlock();
  }
}
