/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru (and Russel Mehler)

   agent_lean.cc

   An agent class controling the leaning. 
  
**/

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

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

// output the values
void Agent_lean::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_lean::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_lean::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_lean::update_action()

{ 
  static bool road_curves_right = false;
  static bool road_curves_left  = false;

  //cout << "in agent lean 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;
  static float left_side_dist = 3;
  static float right_side_dist = 3;

     if (left_dist + right_dist == 0)
	side_diff = 0;
      else {
	side_diff = (abs(left_dist - right_dist)/(left_dist +  right_dist));
	if (right_dist > left_dist)
	  side_diff = -side_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));
	if (front_right_dist > front_left_dist)
	  front_diff = -front_diff;      
      }

  if (front > 3)
    {
 

      if ((abs(side_diff) > 0.8)){
	parent->lock();
	if (side_diff > 0){
	  boss->moto->lean_left();
	  last_lean = 'L';
	}
	else {
	  boss->moto->lean_right();
	  last_lean = 'R';
	}
	parent->unlock();
      }
      //else if ((abs(side_diff) > old_diff)){
      //parent->lock();
      //if (side_diff > 0){
      //  boss->moto->lean_left();
      //  last_lean = 'L';
      //}
      //else {
      //  boss->moto->lean_right();
      //  last_lean = 'R';
      //}
      //parent->unlock();
      //}

      else if (boss->moto->get_lateral_momentum()){
	parent->lock();
	//     boss->moto->set_lateral_momentum(0);
	boss->moto->amortize_lateral_momentum();
	//if (last_lean == 'L')
      //boss->moto->lean_right();
      //else if (last_lean == 'R')
      //  boss->moto->lean_left();
	parent->unlock();
      }
      left_side_dist = left_dist;
      right_side_dist = right_dist;
      if (road_curves_right)
	old_diff = right_dist;
      else
	old_diff = left_dist;
    }
  else
    {
      parent->lock();
      if ( (front_left_dist>front_right_dist) && (left_dist>right_dist) )
	road_curves_right = true;
      else if ( (front_left_dist>front_right_dist) && (right_dist>left_dist) )
	road_curves_left = true;
      else if ( (front_right_dist>front_left_dist) && (left_dist>right_dist) )
	road_curves_right = true;
      else if ( (front_right_dist>front_left_dist) && (right_dist>left_dist) )
	road_curves_left = true;

      if (road_curves_right)
	{
	  if (right_dist < ( 0.85 * old_diff) )//right_dist) )
	    {      //right_dist was right_side_ dist
	      boss->moto->lean_left();
	      last_lean = 'L';
	    }
	  else if (right_dist > ( 1.15 * old_diff) )//right_dist) )
	    {     //right_dist was right_side_dist
	      boss->moto->lean_right();
	      last_lean = 'R';
	    }
	}
      else if (road_curves_left)
	{
	// road curves left
	  if (left_dist < ( 0.85 * old_diff) )//left_dist) )  
	    {    //left_dist was left_side_dist
	      boss->moto->lean_right();
	      last_lean = 'R';
	    }
	  else if (left_dist > ( 1.15 * old_diff) )//left_dist) )
	    {    //left_dist was left_side_dist
	      boss->moto->lean_left();
	      last_lean = 'L';
	    }
	}
	parent->unlock();
	//if (road_curves_right)
	//old_diff = right_dist;
	//else
	//old_diff = left_dist;
    }
  //old_diff = side_diff;
  
  // print_status();
}
