/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   moto_behave.cc
   
   The class that handles behavior for the motorcycle, 
   meaning the kinematics - speed, acceleration, center of gravity.
  
**/

#include <cmath>
#include "moto_behave.h"
#include "general.h"

// Default constructor
Moto_behave::Moto_behave()
  : Moto_animate()
{
  set_consts();
  set_throttle_twist();
  set_left_push();
  set_right_push();
  set_front_break();
  set_back_break();
  set_lateral_momentum();
  set_weight();
  set_kfriction(0.05);
  set_wheel_width();
  crashed = false;
}

// Destructor
Moto_behave::~Moto_behave()
{;}

// Reset everything about the moto.
void Moto_behave::reset()
{
  set_road_pos(0, 0, 0);
  set_orientation_angle(0);
  hor_speed = 0;
  throttle_twist = 0;
  front_break = 0;
  back_break = 0;
  left_push = 0; 
  right_push = 0;
  lateral_momentum = 0;
  Moto_animate::reset();
  crashed = false;
}

//added by pk
//After crash make the moto get up
void Moto_behave::getup()
{
  hor_speed = 0;
  throttle_twist = 0;
  front_break = 0;
  back_break = 0;
  left_push = 0; 
  right_push = 0;
  lateral_momentum = 0;
  Moto_animate::reset();
  crashed = false;
}

//set const speed forward
void Moto_behave::set_const_speed_forward()
{  
    ds= 0.5; 
    Point3f dsv(ds, 0, 0);
    dsv.rotate_y(d2r(-orientation_angle));
    dsv[0] *= cos(d2r(slope));
    dsv[1] = ds * sin(d2r(slope));
    dsv[2] *= cos(d2r(slope));
    road_pos += dsv;
    Moto_animate::move();
    
}

//set const speed backward
void Moto_behave::set_const_speed_backward()
{  
    ds= -0.5; 
    Point3f dsv(ds, 0, 0);
    dsv.rotate_y(d2r(-orientation_angle));
    dsv[0] *= cos(d2r(slope));
    dsv[1] = ds * sin(d2r(slope));
    dsv[2] *= cos(d2r(slope));
    road_pos += dsv;
    Moto_animate::move();
    
}
//---------------------------------------------------
// Set some of the global constants and initial conditions.
void Moto_behave::set_consts()
{ 
  g_scale = 0.01;
  g = 9.80665 * g_scale; // mm / s^2
  max_throttle_twist = 2.5*g;
  hor_speed = 0;
  scale = 1; // 0.5;
  wradius = wheel_radius * scale;
  max_front_break = 30*max_throttle_twist;
  max_back_break = 20*max_throttle_twist;
  dt = 0.2;
  max_speed_rate = 50;
  air_density = 1.22145;
  ref_coef_lift = 0.5;
  ref_coef_deg = 30;
  surface_perc = 0.25;
  surface = surface_perc * length() * height();
  r_scale = g_scale * 15;
  kdrag=0.0005;
  bkdrag=0.05;
  bkfriction = 0.1;
}

// Set, add, and get all of these values.
void Moto_behave::set_throttle_twist(float val)
{
  throttle_twist = val;
  if (throttle_twist > max_throttle_twist)
    throttle_twist = max_throttle_twist;
  if (throttle_twist <0)
    throttle_twist =0;
}

void Moto_behave::add_throttle_twist(float val)
{
  if (!crashed) {
    throttle_twist += val;
    if (throttle_twist > max_throttle_twist)
      throttle_twist = max_throttle_twist;
    if (throttle_twist <0)
      throttle_twist =0;
  }
  else
    throttle_twist =0;
}

void Moto_behave::set_left_push(float val)
{
  if (!crashed) {
    left_push = val;
    if (left_push > max_handlebar_angle)
      left_push = max_handlebar_angle;
    if (left_push < 0)
      left_push = 0;
  }
  else
    left_push = 0;
}

void Moto_behave::add_left_push(float val)
{
  if (val > 0 && !crashed) {
    if (left_push + val > max_handlebar_angle)
      left_push = max_handlebar_angle;
    else
      left_push += val;
    if (left_push < 0)
      left_push = 0;
  }
}

void Moto_behave::amortize_left_push()
{
  left_push -= 5;
  if (left_push < 0)
    left_push = 0;
}

void Moto_behave::set_right_push(float val)
{
  if (!crashed) {
    right_push = val;
    if (right_push > max_handlebar_angle)
      right_push = max_handlebar_angle;
    if (right_push < 0)
      right_push = 0;
  }
  else 
    right_push = 0;
}

void Moto_behave::add_right_push(float val)
{
  if (val > 0 && !crashed) {
    if (right_push + val > max_handlebar_angle)
      right_push = max_handlebar_angle;
    else
      right_push += val;
    if (right_push < 0)
      right_push = 0;
  }
}

void Moto_behave::amortize_right_push()
{
  right_push -= 5;
  if (right_push < 0)
    right_push = 0;
}

void Moto_behave::set_front_break(float val)
{
  if (!crashed) {
    front_break = val;
    if (front_break > max_front_break)
      front_break = max_front_break;
    if (front_break < 0)
      front_break = 0;
  }
  else
    front_break = 0;
}

void Moto_behave::add_front_break(float val)
{
  if (!crashed) {
    front_break += val;
    if (front_break > max_front_break)
      front_break = max_front_break;
    if (front_break < 0)
      front_break = 0;
  }
  else
    front_break = 0;
}

// A natural decrease in the break if the user doesn't continue to
// push on it.
void Moto_behave::amortize_front_break()
{
  if (0.5 * front_break > 0.005)
    add_front_break(-0.5 * front_break);
  else
    add_front_break(-0.05);
}

void Moto_behave::set_back_break(float val)
{
  if (!crashed) {
    back_break = val;
    if (back_break > max_back_break)
      back_break = max_back_break;
    if (back_break < 0)
      back_break = 0;
  }
  else
    back_break = 0;
}

void Moto_behave::add_back_break(float val)
{
  if (!crashed) {
    back_break += val;
    if (back_break > max_back_break)
      back_break = max_back_break;
    if (back_break < 0)
      back_break = 0;
  }
  else
    back_break = 0;
}

// A natural decrease in the break if the user doesn't continue to
// push on it.
void Moto_behave::amortize_back_break()
{
  if (back_break > 0.01)
    add_back_break(-0.3 * back_break);
  else
    add_back_break(-0.01);
}

void Moto_behave::set_lateral_momentum(float val)
{
  if (!crashed) {
    lateral_momentum = val;
    if (lateral_momentum < -90)
      lateral_momentum = -90;
    else if (lateral_momentum > 90)
      lateral_momentum = 90;
  }
  else
    lateral_momentum = 0;
}

void Moto_behave::add_lateral_momentum(float val)
{
  if (!crashed) {
    lateral_momentum += val;
    if (lateral_momentum < -90)
      lateral_momentum = -90;
    else if (lateral_momentum > 90)
      lateral_momentum = 90;
  }
  else
    lateral_momentum = 0;
}

// Explicit lean functions
void Moto_behave::lean_left(float val)
{
  add_lateral_momentum(-val);
}

void Moto_behave::lean_right(float val)
{
  add_lateral_momentum(val);
}

// Amortize the lateral momentum.
void Moto_behave::amortize_lateral_momentum()
{
  if (fabs(lateral_momentum) > 5)
    add_lateral_momentum(-0.3*lateral_momentum);
  else
    set_lateral_momentum(0);
}

void Moto_behave::set_weight(float val)
{
  weight = val;
}

void Moto_behave::set_kfriction(float val)
{
  kfriction = val;
}

void Moto_behave::set_wheel_width(float val)
{
  wheel_width = val;
}

float Moto_behave::get_throttle_twist()
{
  return throttle_twist;
}

float Moto_behave::get_left_push()
{
  return left_push;
}

float Moto_behave::get_right_push()
{
  return right_push;
}

float Moto_behave::get_front_break()
{
  return front_break;
}

float Moto_behave::get_back_break()
{
  return back_break;
}

float Moto_behave::get_lateral_momentum()
{
  return lateral_momentum;
}

float Moto_behave::get_weight()
{
  return weight;
}

float Moto_behave::get_kfriction()
{
  return kfriction;
}

float Moto_behave::get_wheel_width()
{
  return wheel_width;
}

float Moto_behave::get_speed()
{
  return hor_speed;
}

float Moto_behave::get_max_speed_rate()
{
  return max_speed_rate;
}

// Find out if the moto has crashed.
bool Moto_behave::has_crashed()
{
  return crashed;
}

// Need this for some statistics.
float Moto_behave::get_ds()
{
  return ds;
}

// Amortize the horizontal and angular speed in a crash situation.
void Moto_behave::amortize_speed()
{
  hor_speed *= 0.5;
  if (hor_speed < 0.5)
    hor_speed = 0;
  wheel_speed *= 0.8;
  if (wheel_speed < 1)
    wheel_speed = 0;
}

// Decrease all the components that need user input to keep going.
void Moto_behave::amortize()
{
  amortize_front_break();
  amortize_back_break();
  amortize_left_push();
  amortize_right_push();
  amortize_lateral_momentum();
  if (crashed)
    amortize_speed();
}

// Compute the gravity centers. 
// The static center is based on the geometry.
void Moto_behave::compute_static_gcenter()
{
  static_gcenter.set_data(0.0, 0.5*height(), 0.0);
}

// The dynamic one depends on the weight and position of the driver.
void Moto_behave::compute_dynamic_gcenter(const Point3f &driver_center, 
					  float driver_weight)
{
  dynamic_gcenter.set_data(static_gcenter);
  dynamic_gcenter.rotate_z(lateral_angle);
  dynamic_gcenter *= weight;
  Point3f temp(driver_center);
  temp *= driver_weight;
  dynamic_gcenter += temp;
  dynamic_gcenter *= (1.0/(weight+driver_weight));
}

// Compute the horizontal acceleration based on the slope, the
// gravitation, the friction, and .
void Moto_behave::compute_acceleration()
{
  float gravt = -sin(d2r(slope)) * g;
  float gravn = cos(d2r(slope)) * g;
  // if (fabs(gravt) >= 0.0001) {
  /*  cout << "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b";
  if (fabs(slope) < 0.001)
    cout << "........................"; 
  else if (slope < 0)
    cout << "------------------------"; 
  else
  cout << "++++++++++++++++++++++++"; 
  */
    //cout << endl;
  //cout << "gravt: " << gravt << " slope: " << slope << endl;
    //status();
  // }
  float ffriction = -0.5*kfriction * gravn - front_break * bkfriction
                    - (bkdrag*front_break+kdrag)*front_speed*front_speed;
  float bfriction = -0.5*kfriction* gravn - front_break * bkfriction
                    - (bkdrag*back_break+kdrag)*back_speed*back_speed;
  if (hor_speed < 0) {
    ffriction = -ffriction;
    bfriction = -bfriction;
  }
  float friction = ffriction + bfriction;
  float control = throttle_twist + gravt;
  front_accl = ffriction + 0.5*gravt;
  back_accl = bfriction + throttle_twist + 0.5*gravt;
  acceleration = front_accl + back_accl;
  if (fabs(control) < fabs(friction) && 
      fabs(acceleration) * dt > fabs(hor_speed)) {
      acceleration = -hor_speed/dt;
  }
  if (fabs(front_accl*dt) > fabs(front_speed) && front_accl*front_speed < 0)
    front_accl = -front_speed/dt;
  if (fabs(back_accl*dt) > fabs(back_speed) && back_accl*back_speed < 0)
    back_accl = -back_speed/dt;
  if (((throttle_twist > 0 && 
       hor_speed < max_speed_rate * throttle_twist) ||
       front_break != back_break) && hor_speed != 0);
    //    move_rotation_center((front_accl - back_accl) / 
    //			 (fabs(front_accl) + fabs(back_accl)));
}

// Compute the front, back, general, and angular speed.
void Moto_behave::compute_speed()
{
  if (front_break) {
    front_speed += front_accl * dt;
    back_speed += back_accl * dt;
    hor_speed = (front_speed + back_speed)/2;
  }
  else {
    hor_speed += acceleration * dt;
    front_speed = back_speed = hor_speed;
  }
  if (throttle_twist > 0 && hor_speed > max_speed_rate * throttle_twist)
    hor_speed = max_speed_rate * throttle_twist;
  if (fabs(hor_speed) < 0.0001)
    hor_speed = 0;
  hor_2_angular();
}

// Translate the angular wheel speed into horizontal speed.
void Moto_behave::angular_2_hor()
{
  hor_speed = wradius * d2r(wheel_speed);
}

// Translate the horizontal speed into angular wheel speed.
void Moto_behave::hor_2_angular()
{
  wheel_speed = r2d(hor_speed / wradius); //r2d(hor_speed / wradius);
}

// Changes the angle of the handlebar based on the left and right
// push.
void Moto_behave::push_handlebar()
{
  if (left_push)
    add_handlebar_angle(left_push);
  if (right_push)
    add_handlebar_angle(-right_push);

  if (handlebar_angle > max_handlebar_angle && handlebar_angle <= 180)
    handlebar_angle = max_handlebar_angle;
  if (handlebar_angle < 360-max_handlebar_angle && handlebar_angle > 180)
    handlebar_angle = 360-max_handlebar_angle;
}

// Lean the motorcycle left and right based on its lateral momentum.
void Moto_behave::push_lateral()
{
  if (lateral_momentum != 0) {
    if (lateral_momentum * lateral_angle >= 0)
      lateral_angle += lateral_momentum;
    else if (fabs(lateral_momentum) > fabs(lateral_angle))
      lateral_angle = 0;
    else 
      lateral_angle += lateral_momentum;
  }
  if (lateral_angle < -90)
    lateral_angle = -90;
  else if (lateral_angle > 90)
    lateral_angle = 90;
}

// When the motorcycle turns on a circle of radius given by the
// parameter.
void Moto_behave::move_by_radius(float radius)
{
  Point3f pos = road_pos;
  float angle = d2r(-orientation_angle);
  Point3f norm1(cos(angle+0.5*M_PI), 0, sin(angle+0.5*M_PI));
  Point3f rad_vector(norm1);
  rad_vector *= radius;
  Point3f center(pos);
  center += rad_vector;
  Point3f norm2(norm1);
  norm2 *= -1;
  float beta = ds / radius, cosb = cos(beta), sinb = sin(beta);
  norm2.set_data(cosb*norm2[0]-sinb*norm2[2], 0, 
		 sinb*norm2[0]+cosb*norm2[2]);
  norm2 *= radius;
  road_pos = center;
  road_pos += norm2;
  orientation_angle += r2d(beta);
  if (orientation_angle < 0)
    orientation_angle += 360;
  if (orientation_angle >= 360)
    orientation_angle -= 360;
}

// Compute the radius of turning based on the lateral angle and the
// lift force. The radius will be negative if the angle is negative.
float Moto_behave::compute_radius()
{
  float ref_surface = surface * cos(d2r(lateral_angle));
  float clift = lateral_angle * ref_coef_lift / ref_coef_deg;
  float speed2 = hor_speed * hor_speed;
  float flift = 0.25 * air_density * ref_surface * clift * speed2 * g_scale;
  float divby = (g * mass - flift) * sin(d2r(-lateral_angle));
  float radius = mass * pow(double(speed2), double(0.75)) * r_scale / divby;
  return radius;
}

// Compute the modification to the orientation based on the handlebar
// angle and the lateral bending (later).
void Moto_behave::compute_orientation()
{
  push_handlebar();
  push_lateral();
  float dangle = 0;
  if (handlebar_angle != 0) {
    float move_len = length() - 2 * wradius;
    if (ds >= move_len)
      dangle = handlebar_angle;
    else {
      float cosda = ds * sin(d2r(handlebar_angle))/ move_len;
      dangle = r2d(asin(cosda));
    }
    if (handlebar_angle < 0)
      dangle = -dangle;
  }
  orientation_angle += dangle;
  if (orientation_angle < 0)
    orientation_angle += 360;
  if (orientation_angle >= 360)
    orientation_angle -= 360;
  handlebar_angle -= dangle;
}

// A function that is invoked when the moto falls down. 
void Moto_behave::crash()
{
  crashed = true;
  //cout << "lateral_angle" << lateral_angle << endl;
  if (lateral_angle < 0) 
    lateral_angle = -75;
  else 
    lateral_angle = 75;
}

// Main function which will be called from the display.
void Moto_behave::move()
{
  if (!crashed) {
    compute_acceleration();
    ds = hor_speed * dt + 0.5 * acceleration * dt * dt;
    compute_orientation();
    compute_speed();
  }
  else
    ds = hor_speed * dt;
  if (lateral_angle != 0 && !crashed) {
    float radius = compute_radius();
    float crash_angle = 40*(1+0.2*(5-min(fabs(hor_speed),float(5))));
    if (fabs(lateral_angle) > (90 - crash_angle ))
      crash();
    else if (fabs(radius) < 0.8*wheel_distance) {
      if (fabs(hor_speed)> 0.1)
	crash();
      else
	lateral_angle = 0;
    }
    else
      move_by_radius(radius);
  }
  else {
    Point3f dsv(ds, 0, 0);
    dsv.rotate_y(d2r(-orientation_angle));
    dsv[0] *= cos(d2r(slope));
    dsv[1] = ds * sin(d2r(slope));
    dsv[2] *= cos(d2r(slope));
    road_pos += dsv;
  }
  Moto_animate::move();
  amortize();
}

// Write out all the speed-acceleration-break-throttle.
void Moto_behave::status()
{
  cout << endl 
       << "acceleration: " <<  acceleration << " speed: " << hor_speed
       << " breaks: " << front_break << ' ' <<  back_break
       << " throttle: " << throttle_twist << endl;
}
