/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   manager.h
   
   The class that manages the motorcycle, the driver, and the road.
  
**/

#include <cmath>

#include "manager.h"
#include "general.h"
#include "gl_draw.h"
#include "scenery.h"

double ga_steer[] = 
  {2.39258, 6.67969, 4.45312, 2.32422, 0.078125, 8.78906, 9.29688, 2.5, 
   2.51953, 3.84766, 0.839844, 2.67578, 1.46484, 3.90625, 7.05078, 6.69922, 
   8.08594, 1.32812, 3.65234, 2.57812, 6.50391, 6.19141, 7.07031, 0.214844, 
   3.08594, 4.33594, 0.605469, 1.54297, 7.01172, 6.66016, 4.21875, 0.263672};
double ga_lean[] =
  {1.31836, 5.66406, 8.30078, 5.83984, 4.57031, 2.51953, 1.01562, 5.625, 
   7.36328, 6.99219, 7.96875, 6.58203, 9.62891, 1.54297, 6.77734, 7.83203, 
   7.94922, 3.33984, 5.25391, 0.605469, 4.35547, 0, 5.13672, 3.86719, 2.85156, 
   8.28125, 6.54297, 3.00781, 4.57031, 0.585938, 0.859375, 3.75977};
double ga_comb[] =
  {1.12305, 2.20703, 9.31641, 5.29297, 9.58984, 2.30469, 2.94922, 0.0976562, 
   1.07422, 9.6875, 9.78516, 0.371094, 3.10547, 6.28906, 1.40625, 6.91406, 
   8.02734, 8.84766, 1.17188, 0.605469, 5, 2.30469, 7.34375, 7.26562, 0.15625, 
   2.83203, 5.78125, 3.06641, 8.20312, 4.72656, 1.91406, 8.96484, 2.30469, 
   3.63281, 2.40234, 3.99414};
double ga_cmp_steer10[] =
  {9.89258, 1.42578, 3.75, 9.16016, 3.47656, 9.23828, 2.01172, 4.33594, 
   7.92969, 6.01562, 5.27344, 9.92188, 1.66016, 7.05078, 1.13281, 1.5625, 
   8.82812, 5.95703, 9.0625, 8.53516, 7.30469, 3.73047, 2.71484, 1.99219, 
   1.79688, 5.68359, 2.55859, 8.65234, 1.75781, 8.57422, 2.65625, 0.878906, 
   6.19141, 2.85156, 7.07031, 7.34375};
double ga_cmp_lean10[]=
  {6.24023, 4.72656, 4.17969, 8.78906, 0.117188, 7.40234, 4.375, 2.05078, 
   8.55469, 6.28906, 4.41406, 7.71484, 8.61328, 4.27734, 9.04297, 9.29688, 
   3.39844, 0.078125, 6.67969, 4.66797, 5.07812, 7.51953, 0.332031, 0.644531, 
   2.36328, 8.10547, 5.13672, 9.58984, 0.78125, 4.60938, 5.17578, 4.84375, 
   3.47656, 7.10938, 8.92578, 1.73828};
double ga_cmp_comb10[] =
  {9.16992, 0.410156, 7.98828, 8.37891, 7.75391, 6.67969, 9.33594, 2.5, 
   7.87109, 7.61719, 5.41016, 2.94922, 2.24609, 9.375, 4.6875, 9.00391, 
   7.16797, 9.25781, 4.58984, 7.94922, 9.12109, 0.820312, 1.75781, 8.61328, 
   4.53125, 4.58984, 0.683594, 2.83203, 3.26172, 3.00781, 7.77344, 5.41016, 
   4.0625, 2.96875, 6.89453, 2.25586};

//Initialize global variables
int passed_by[53];
int old_k = -1;
int new_k = -1;
static int num = 0;
int sent_flag = 0;
int count_neg_triangle = 0;
// Default constructor: make all the pointers null.
Manager::Manager()
{
  moto = NULL;
  driver = NULL;
  road = NULL;
  pilot = NULL;
  marker_id = glGenLists(1);
  triangle = -1; 
  thr_slope_low = 5;
  thr_slope_high = 20;
  pilot_on = false;
  time_on = false;
  circuit_completed = false;
  input_taken = false;
}

// Destructor: delete all the pointers.
Manager::~Manager()
{
  cout << endl;
  delete moto;
  delete driver;
  delete road;
  delete pilot;
  glDeleteLists(marker_id, 1);
}

// Toggle the pilot on and off.
void Manager::toggle_pilot()
{
  pilot_on = !pilot_on;
  toggle_timer();
}

//-------------------------------------------------------------
// added by pk
// Toggle the pilot on and off given the number of trials.
void Manager::toggle_pilot1(int sflag)
{
  sent_flag = sflag;
  //cout << "set flag is" << sflag<<endl;
  if(!input_taken && !pilot_on){
    cout << " Please enter the number of trials " << endl;
    cin >> num;
    cout << "Total number of trials is  " << num <<endl;
    //freopen ("myfile.txt","w",stdout);
    pilot_on = !pilot_on;
    num = num - 1;
    toggle_timer();
    input_taken = true;
  }
  else{  
    //cout << "num here is " << num << endl;
      if(num > 0){ 
	num  = num - 1;
	if(!pilot_on){
	  pilot_on = !pilot_on; 
	  toggle_timer();
	}
	else{
	  pilot_on = !pilot_on;
	  toggle_timer(); 
	  moto->reset();
	  pilot_on = !pilot_on;
	  toggle_timer();
	}	
    }
    else{
      pilot_on = !pilot_on;
      moto->reset();
      sent_flag = 0;
      toggle_timer();
      exit(0);
    }  
  }
}
//-------------------------------------------------------------
// Toggle the timer on and off.
void Manager::toggle_timer()
{
  if (time_on) {
    time_t new_timer;
    time(&new_timer);
    cout << endl << "Total time: " << new_timer - timer << endl;
    timer = new_timer;
    time_on = false;
    output_stats();
  }
  else {
    time_on = true;
    time(&timer);
    reset_stats();
  }
}

// Default initializer. Move all the murky stuff from the application
// in here. Initialize the moto, driver, and road, and make the
// display lists for each of them.
void Manager::init()
{
  moto = new Moto_behave;
  moto->draw();
  driver = new Driver_behave;
  driver->draw();
  driver->set_position(0, 0, 3);
  driver->move_on_moto(moto->length(), moto->width(), moto->height());
  road = new Road_manager("./road/traj_3loops.raw");
  //road = new Road_manager("/home/danav/develop/moto_drive/road/traj_3loops.raw");
  //road = new Road_manager("/home/danav/develop/moto_drive/road/road1.raw");
  road->zoom(20);
  road->rotate_x(-90);
  road->rotate_y(-90);
  road->translate(0, 0, 10);
  road->compute_normals();
  road->compute_d_pars();
  road->compute_bbox();
  road->compute_neighbors();
  road->read_center("./road/traj_3l_center.raw");
  road->draw();
  pilot = new Pilot_manager(this, 0.4);
  //pilot->read("pilot_config.txt");
  //pilot->read("pilot_comb.txt");
  pilot->read_param(ga_steer, 36);
  //---------------------------
  //commented by pk
  //  pilot->set_speed_limit(5);
  //---------------------------
  pilot->make_seq();
  //pilot->launch();
  //pilot->unlock();
  //pilot->turn_on();
  //pilot->turn_off();
  //pilot->turn_on();
  //pilot->terminate();
  draw_marker(20);
  scenery_draw();
}

void Manager::redraw_small()
{
  road->draw_sml();
}

// Draw a lil sphere that we can use to mark the horizon point and the
// lateral distances. For debugging purposes mostly.
void Manager::draw_marker(float radius)
{
  glNewList(marker_id, GL_COMPILE);
  gl_sphere(0, 0, 0, radius, 2);
  glEndList();
}

// Display the marker with a given color an position.
void Manager::display_marker(Point3f &position, 
			     float r, float g, float b)
{
  glPushMatrix();
  position.gl_translate();
  glColor3f(r, g, b);
  gl_sphere(0, 0, 0, 0.4, 2);
  //glCallList(marker_id);  
  glPopMatrix();
}

// Computes the intersection of the direction of movement with the
// border of the road (and 1 degree to either side 
// and 5 degrees to either side), 
// as well as of two lateral vectors going left
// and right at a little distance in front of the motorcycle.
void Manager::compute_horizon(int triangle)
{
  horizon = position;
  left_horizon = position;
  right_horizon = position;
  second_left_horizon = position;
  second_right_horizon = position;
  left_pos = position;
  right_pos = position;
  forward_left_pos = position;
  forward_right_pos = position;
  if (triangle >= 0) {
    float move_angle=moto->get_orientation_angle();
    float rmove_angle = d2r(-move_angle);
    float leng = moto->length();
    side_watch = 2*leng;
    front_watch = 8*leng; 
    Point3f move_dir(cos(rmove_angle), 0, sin(rmove_angle));
    
    // compute the horizon in the direction of movement
    road->trace_out(triangle, position, move_dir, horizon);

    // compute the horizon 1 degrees to the left and right 
    // of the direction of movement
    float rmove_angle_left = d2r(-(move_angle+1));
    Point3f move_dir_left(cos(rmove_angle_left), 0, sin(rmove_angle_left));
    road->trace_out(triangle, position, move_dir_left, left_horizon);
    float rmove_angle_right = d2r(-(move_angle-1));   
    Point3f move_dir_right(cos(rmove_angle_right), 0, sin(rmove_angle_right));
    road->trace_out(triangle, position, move_dir_right, right_horizon);

    // compute the horizon 5 degrees to the left and right 
    // of the direction of movement
    float rmove_angle_second_left = d2r(-(move_angle+5));
    Point3f move_dir_second_left(cos(rmove_angle_second_left), 0,
				 sin(rmove_angle_second_left));
    road->trace_out(triangle, position, move_dir_second_left, 
		    second_left_horizon);
    float rmove_angle_second_right = d2r(-(move_angle-5));   
    Point3f move_dir_second_right(cos(rmove_angle_second_right), 0, 
				  sin(rmove_angle_second_right));
    road->trace_out(triangle, position, move_dir_second_right, 
		    second_right_horizon);

    // Determine a position on the road in front of the motorcycle, at
    // most at 3 lengths of the motorcycle ahead.
    pos=position;
    if (0.75 * position.distance(horizon) <= side_watch) 
      side_watch = 0.75 * position.distance(horizon);
    move_dir *= side_watch;
    pos += move_dir;
    triangle = road->locate_neighborhood(pos, triangle);
    if (triangle == -1)
      triangle = road->locate(pos);

         // From that position trace left and right in a direction that is
         // perpendicular to the direction of movement.
         move_dir.set_data(cos(rmove_angle+0.5*M_PI), 0, 
		      sin(rmove_angle+0.5*M_PI));
         road->trace_out(triangle, pos, move_dir, right_pos);

         move_dir *= -1;
         road->trace_out(triangle, pos, move_dir, left_pos);

    // Determine a position on the road in front of the motorcycle, at
    // the navigating horizon of the motorcycle ahead.
    Point3f move_dir_forward(cos(rmove_angle), 0, sin(rmove_angle));
    fpos=position;
    if (0.75 * position.distance(horizon) <= front_watch) 
      front_watch = 0.75 * position.distance(horizon);
    move_dir_forward *= front_watch;
    fpos += move_dir_forward;
    triangle = road->locate_neighborhood(fpos, triangle);
    if (triangle == -1)
      triangle = road->locate(fpos);

         // From that position trace left and right in a direction that is
         // perpendicular to the direction of movement.
         move_dir_forward.set_data(cos(rmove_angle+0.5*M_PI), 0, 
		      sin(rmove_angle+0.5*M_PI));
         road->trace_out(triangle, fpos, move_dir_forward, forward_right_pos);

         move_dir_forward *= -1;
         road->trace_out(triangle, fpos, move_dir_forward, forward_left_pos);  
  }
}

// Compute some statistics. For now just the average and maximal speed.
// Also right and left turn counts and off road counts
// left and right lean counts
void Manager::compute_stats()
{
  // average and maximal speed
  frame_count++;
  float speed = moto ->get_speed();
  speed_sum += speed;
  if (speed_max < speed)
    speed_max = speed;
  double leftd = pos.distance(left_pos);
  double rightd = pos.distance(right_pos);
  if (leftd+rightd > 0)
    lateral_sum += abs(leftd-rightd)/(leftd+rightd);
  total_ds += moto->get_ds();

  // left and right turn counts
  float left_turn =  moto->get_left_push();
  if (left_turn > old_left_push)
    left_turn_count++;
  old_left_push = left_turn;
  float right_turn =  moto->get_right_push();  
  if (right_turn > old_right_push)
    right_turn_count++;
  old_right_push = right_turn;  

  // left and right lean counts
  float lean = moto->get_lateral_angle();
  if ( lean < old_lean)
    left_lean_count++;
  else if ( lean > old_lean)
    right_lean_count++;
  if ( lean < 0) {
    left_lean_frame_count++;
    left_lean_sum += lean;
  }
  if ( lean > 0) {
    right_lean_frame_count++;
    right_lean_sum += lean;
  }
  old_lean = lean;

  // off road counts
  if (triangle == -1) {
    off_road_time++;
    if (old_triangle != -1)
      off_road_count++;
  }
  old_triangle = triangle;
}

// Reset all the statistics values so we can start again
void Manager::reset_stats()
{
  speed_sum = 0;
  speed_max = 0;
  frame_count = 0;
  lateral_sum = 0;
  total_ds = 0;
  off_road_count = 0;
  off_road_time = 0;
  left_turn_count = 0;
  right_turn_count = 0;
  left_lean_count = 0;
  right_lean_count = 0;
  left_lean_frame_count = 0;
  right_lean_frame_count = 0;
  left_lean_sum = 0;
  right_lean_sum = 0;
  old_left_push = 0;
  old_right_push = 0;
  old_lean = 0;
  old_triangle = triangle;
}

// Output the statistics
void Manager::output_stats()
{
  if (frame_count > 0) {
    speed_sum = speed_sum / frame_count;
    lateral_sum = lateral_sum / frame_count;
    if (left_lean_frame_count)
      left_lean_sum = left_lean_sum / left_lean_frame_count;
    else
      left_lean_sum = 0;
    if (right_lean_frame_count)
      right_lean_sum = right_lean_sum / right_lean_frame_count;
    else
      right_lean_sum = 0;   
    if (off_road_count)
      off_road_average = off_road_time / off_road_count;
    else
      off_road_average = 0;
  }
  cout << "Average speed: " << speed_sum << " maximum speed:"
       << speed_max << endl;
  cout << "Average lateral balance: " << lateral_sum << endl;
  cout << "Total distance covered: " << total_ds << endl;
  cout << "Times left the road: " << off_road_count 
       << " average time off the road: "
       << off_road_average << endl;
  cout << "Total left turns: " << left_turn_count << " ,total right turns: " 
       << right_turn_count << endl;
  cout << "Total left leans: " << left_lean_count << " ,total right leans: "
       << right_lean_count << endl;
  cout << "Average lean angle when leaning left: " << left_lean_sum 
       << " , for " << left_lean_frame_count << " frames" << endl;
  cout << "Average lean angle when leaning right: " << right_lean_sum 
       << " , for " << right_lean_frame_count << " frames" << endl;

  if(circuit_completed == false)
    cout << "Circuit is not completed by the moto" << endl;
  else
    cout << "Circuit is completed by the moto" << endl;
}

// Reset the values checking for the circuit being completed.
void Manager::reset_complete(int length)
{
  for(int i = 0; i  < length ; i++)
    passed_by[i] = 0;
  new_k = -1; 
  old_k = -1;
}

// Move all the objects.
void Manager::move()
{
  static float old_pos = 0;
  //------------------position on center line----------------------

  int length = road->center_size();
  // bool flag =  true;

  if(triangle != -1)
    new_k = road->locate_center(position,old_k);
  else
    new_k = -1;
  //if (old_k == 0 && new_k != 0)
  //cout << "new_k " << new_k << endl;

  /*  if(new_k == length){
    for(int i = 0; i  < length ; i++){
      cout <<"passed by i" << i << " value is "
	   << passed_by[i]<<endl;
    }
    }*/
  if (old_pos * position[0] < 0 && (new_k == length || new_k == 0)){
    flag = complete_st;
    for(int i = 1; i  < length ; i++)
      if(passed_by[i] == 0){
	//cout <<"not passed by " << i << " out of " << length <<  endl;
	flag = incomplete_st;
	break;
      }
    //in pilot mode 1 time
    if(sent_flag == run_once_st){
      // if the pilot completes the circuit
      // output the stats,turn off the pilot
      // and state it completed the circuit
      if(flag == complete_st){
	reset_complete(length);
	circuit_completed = true;
	toggle_pilot();	  
	return;
      }

      //if the pilot comes back to the start without
      //completing the entire circuit
      //output stats stating that moto did not
      // complete the circuit

      if(flag == incomplete_st){
	circuit_completed = false;
	reset_complete(length);
	toggle_pilot();
	return;
      }
    }
    // in pilot mode given n trials
    else{
      // if the pilot completes the circuit
      // output the stats,turn off the pilot
      // and state it completed the circuit
      if(flag == complete_st){
	circuit_completed = true;
	reset_complete(length);
	moto->reset();
	toggle_pilot1(sent_flag);	  
	
      }

      //if the pilot comes back to the start without
      //completing the entire circuit
      //output stats stating that moto did not
      // complete the circuit

      if(flag == incomplete_st){
	circuit_completed = false;
	reset_complete(length);
	moto->reset();
	toggle_pilot1(sent_flag);
      }
    }
 
  }

  if (position[0] != 0)
    old_pos = position[0];

  if((new_k != old_k) && (new_k != -1))
    {
      old_k = new_k;
      passed_by[new_k]++;
    }

   
  //--------------------------------------------------------------
 
  //if (pilot)
  //pilot->lock();
  static int old_trig = -1;
  int trig=-1;
  float move_angle=moto->get_orientation_angle();
  float rmove_angle = d2r(-move_angle);
  Point3f move_dir(cos(rmove_angle), 0, sin(rmove_angle));

  position = moto->get_road_pos();

  //driver->remove_position(moto->get_road_pos());
  //driver->add_rotation(0.0, -moto->get_orientation_angle(), 0.0);
  compute_stats();
  moto->move();
  if (old_trig != -1)
    if (road->is_inside(position, old_trig)) 
      triangle = old_trig;
    else 
      triangle = road->locate_neighborhood(position, old_trig);
 
  //----------------------------------------------------------
  //added by pk
  // to check if the moto is off the road for too long
  // then if so turn off the pilot and output the stats 
  // and state that moto has not completed the circuit

  if(sent_flag == run_once_st){
     if(triangle == -1){
      count_neg_triangle++;
      //cout << "count negative triangle" << count_neg_triangle << endl;
      if(pilot && pilot_on){
	if(off_road_count  > 30) {// || count_neg_triangle > 60){
	  circuit_completed = false;
	  count_neg_triangle = 0;
	  toggle_pilot();
	  moto->reset();
	}
      }
    }  
  }
  else{
     if(triangle == -1){
      count_neg_triangle++;
      //cout << "count negative triangle" << count_neg_triangle << endl;
      if(pilot && pilot_on){
	if(off_road_count  > 30){// || count_neg_triangle > 60){
	  circuit_completed = false;
	  count_neg_triangle = 0;
	  toggle_pilot1(sent_flag);
	}
      }
    }  
  }
    

  //---------------------------------------------------------
  
  if (triangle == -1) {
    triangle = road->locate(position);
    //if (triangle != -1)
    //cout << endl << "tri " << triangle << endl;
  }
  compute_horizon(triangle);

  float diff_y = road->project_y_diff(position, triangle);
  slope = road->get_slope(position, 
			  moto->get_orientation_angle(),
			  triangle);
  //cout << slope << ' ' << endl;
  moto->add_road_pos(0, diff_y, 0);
  moto->set_slope(slope);
  old_trig = triangle;
  //if (pilot)
    //pilot->unlock();

  //---------------------------------------------------
  // added by pk commented down some code duplicate
  // to check if the moto crashed in the pilot mode
  // turn off the pilot and output the stats
  // and state that moto did not complete the circuit
  //---------------------------------------------------
  if(sent_flag == run_once_st){
    if (moto->has_crashed()){
      if (pilot && pilot_on){
	circuit_completed = false;
        cout << "The moto has crashed" << endl;
	toggle_pilot();
      }
    }
  }
  else{
    if (moto->has_crashed()){
      if (pilot && pilot_on){
	circuit_completed = false;
        cout << "The moto has crashed" << endl;
	toggle_pilot1(sent_flag);
      }
    }
  }
  /*if (moto->has_crashed())
    if (pilot && pilot_on)
    toggle_pilot();*/
   
  //--------------------------------------------------
  if (pilot && pilot_on)
    pilot->sequential_update();
  //driver->add_position(moto->get_road_pos());
  //driver->add_rotation(0.0, moto->get_orientation_angle(), 0.0);
}

// Call the display functions of all the components.
void Manager::display()
{
  //if (pilot)
    //pilot->lock();
  road->display();
  scenery_display();

  display_marker(horizon);
  display_marker(left_horizon, 0, 1, 1);
  display_marker(right_horizon, 0, 0, 1);
  display_marker(second_left_horizon, 0, 0.5, 1);
  display_marker(second_right_horizon, 0, 0, 0.5);
  display_marker(left_pos, 1, 1, 0);
  display_marker(right_pos, 1, 0, 1);
  display_marker(forward_left_pos, 1, 1, 0);
  display_marker(forward_right_pos, 1, 0, 1);

  // Rotate the entire motorcycle laterally
  glPushMatrix(); // 1
  moto->rotate_trans_all();

  moto->display();
  driver->display();

  glPopMatrix(); //1
  //if (pilot)
    //pilot->unlock();
}

// Print the status of the motorcycle on the road
void Manager::status()
{
  moto->status();
  float move_angle=moto->get_orientation_angle();
  float rmove_angle = d2r(-move_angle);
  Point3f move_dir(cos(rmove_angle), 0, sin(rmove_angle));
  cout << "triangle: " << triangle 
       << " position: " << position 
       << " orientation: " << move_dir << endl;
}

// Discretize the slope to 5 values, from -2 to 2.
int Manager::discrete_slope()
{
  if (abs(slope) <= thr_slope_low) 
    return 0;
  else if (slope < -thr_slope_high)
    return -2;
  else if (slope < -thr_slope_low)
    return -1;
  else if (slope <= thr_slope_high)
    return 1;
  else
    return 2;
}
