/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
      Modified by: Russel Mehler

   agent_hbar.cc
   
   An agent class controling the handlebar. 
  
**/

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

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

// output the values
void Agent_hbar::output_data()
{
  cout << "nr config: " << nr_config << endl;
  cout << "thr_diff " << thr_diff  << endl;
  cout << "thr_front " << thr_diff  << endl;
  cout << "c_push " << c_push  << endl;  
  cout << "speed_pow " << speed_pow  << endl;
}

// Set the number of configuration values. To be defined by the
// derivate classes.
void Agent_hbar::set_nr_config()
{
  nr_config = 4;
  //cout << "setting the config nr to " << nr_config << endl;
}

// Agent-specific set data to be defined in the derivate class.
void Agent_hbar::set_data(float values[])
{
  int i=0;
  thr_diff = values[i++];
  thr_front = values[i++];
  c_push = values[i++];  
  speed_pow = values[i++];
}

// This will be specific to each agent.
void Agent_hbar::update_action()
{
  //cout << "in agent hbar update" << endl;
  get_status();
  parent->lock();
  hbar_angle = boss->moto->get_handlebar_angle();
  lpush = boss->moto->get_left_push();
  rpush = boss->moto->get_right_push();
  parent->unlock();

  float push=0, diff;
  float front_diff, side_diff, forward_diff;
  float front = front_dist / moto_length;

  if (left_dist + right_dist == 0)
    side_diff = 0;
  else {
    side_diff = (abs(left_dist - right_dist)/(left_dist +  right_dist));
    //side_diff = (abs(left_dist - right_dist)/max(left_dist, right_dist));
    //side_diff = sqrt(abs(left_dist - right_dist)/max(left_dist, right_dist));
    if (right_dist > left_dist)
      side_diff = -side_diff;
  }
  if (forward_left_dist + forward_right_dist == 0)
    forward_diff = 0;
  else {
    forward_diff = (abs(forward_left_dist - forward_right_dist)
		    /(forward_left_dist +  forward_right_dist));
    if (forward_right_dist > forward_left_dist)
      forward_diff = -forward_diff;
  }

  if (front_left_dist + front_right_dist == 0)
    front_diff = 0;
  else {
    front_diff = (abs(front_left_dist - front_right_dist)
    		      /max(front_left_dist, front_right_dist));
    //front_diff = sqrt(abs(front_left_dist - front_right_dist)
    //	     /max(front_left_dist, front_right_dist));
    if (front_right_dist > front_left_dist)
      front_diff = -front_diff;
  }
  if ((abs(side_diff) > 0.8)) //&& (front > 4))
    diff = side_diff;
  //else if ( front > 12 )
  //  diff = forward_diff / 8;
  else if ( front > 8 )
    diff = forward_diff / 4;
  else if ( front > 4 )
    diff = forward_diff / 2;
  else
    diff = forward_diff;

  //cout << "in agent handlebar diff " << diff << endl;
  //print_status();
  
  if (abs(diff) > thr_diff || front < thr_front) {
    push = abs(c_push * ((diff-thr_diff) + (thr_front-front)/thr_front) 
	       / pow(log(1.0+speed), 1.0/speed_pow));
    if (abs(diff) > 0.8)
      push += c_push;
    if (abs(diff) > 0.9)
      push += c_push;
  }
  if (diff < 0)
    push = -push;
  float factor = 0.9+0.2*((rand() % 1000) /1000.0);
  push *= factor;
  //cout << push << " l " << lpush << " r " << rpush << endl;
  if (push != 0) {
    //if (push > lpush || push < -rpush) {
    parent->lock();
    if (push > 0) { //(push > lpush) {
      //cout << "turning left by " << push-lpush << endl;
      boss->moto->set_right_push(0.0);
      boss->moto->add_left_push(push-lpush/2);
    }
    else {
      //cout << "turning right by " << rpush-push << endl;
      boss->moto->set_left_push(0.0);
      boss->moto->add_right_push(-push-rpush/2);
    }
    parent->unlock();	
  }
  else {
    parent->lock();
    boss->moto->amortize_left_push();
    boss->moto->amortize_right_push();
    parent->unlock();
  }
}
