/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   agent_breaks.cc
   
   An agent class controling the breaks. 
  
**/

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

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

// output the values
void Agent_breaks::output_data()
{
  cout << "nr config: " << nr_config << endl;  
  cout << "thr_speed " << thr_speed << endl;
  cout << "thr_speed_low " << thr_speed_low << 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_lat " << 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_breaks::set_nr_config()
{
  nr_config = 10;
  //cout << "setting the config nr to " << nr_config << endl;
}

// Agent-specific set data to be defined in the derivate class.
void Agent_breaks::set_data(float values[])
{
  int i=0;
  thr_speed = values[i++];
  thr_speed_low = 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_breaks::update_action()
{
  get_status();
  parent->lock();
  fbreak = boss->moto->get_front_break();
  bbreak = boss->moto->get_back_break();
  parent->unlock();
  float lat;
  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;
  float lim = speed_limit>thr_speed ? thr_speed : speed_limit;
  float lim_lat = c_lat_speed*pow(speed, 1/speed_pow_lat) -lat;
  float lim_front = c_fr_speed*pow(speed, 1/speed_pow_fr) - front;
  float add_break=0;
  if (speed > lim) 
    add_break += c_dec_speed *(speed -lim)-0.6*c_slope*slope;

  if (lim_lat > 0)
    add_break += c_dec_lat*lim_lat - 0.3*c_slope*slope;

  if (lim_front > 0)
    add_break += c_dec_fr*lim_front - 0.3*c_slope*slope;

  if (speed > thr_speed_low) {
    if (add_break > fbreak) {
      parent->lock();
      boss->moto->add_front_break(add_break);
      //cout << "adding front break " << add_break << endl;
      parent->unlock();
    }
    if (0.4*add_break > bbreak) {
      parent->lock();
      boss->moto->add_back_break(0.4*add_break);
      //cout << "adding back break " << add_break << endl;
      parent->unlock();
    }
  }
} 
