/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   moto_animate.h
   
   The control of the basic animation of the motorcycle without any
   physical considerations.
  
*/

#include "moto_animate.h"
#include "general.h"
#include <iostream.h>

// Default constructor. Sets all the speeds and angles to 0 and all of
// the other parameters to their default values. Since it invokes the
// constructor for Moto_geometry, it should not be invoked before the
// gl is initialized.
Moto_animate::Moto_animate() : Moto_geometry()
{
  reset();
  set_handlebar_axis();
  set_max_handlebar_angle();
  set_orientation_angle();
  set_road_pos();
}

// Reset all the speeds and angles to 0.
void Moto_animate::reset()
{
  wheel_speed = 0; 
  handlebar_speed = 0; 
  lateral_speed = 0;
  wheel_angle = 0;
  handlebar_angle = 0;
  lateral_angle = 0;
}

// Destructor. Probably nothing to do.
Moto_animate::~Moto_animate()
{;}

// Set and get the speeds.
void Moto_animate::set_wheel_speed(float speed)
{
  wheel_speed = speed;
}

void Moto_animate::set_handlebar_speed(float speed)
{
  handlebar_speed = speed;
}

void Moto_animate::set_lateral_speed(float speed)
{
  lateral_speed = speed;
}

GLfloat Moto_animate::get_wheel_speed()
{
  return wheel_speed;
}

GLfloat Moto_animate::get_handlebar_speed()
{
  return handlebar_speed;
}

GLfloat Moto_animate::get_lateral_speed()
{
  return lateral_speed;
}

// Initialize the road position and orientation angle.
void Moto_animate::set_road_pos(GLfloat x, GLfloat y, GLfloat z)
{
  set_point3f(road_pos, x, y, z);
}

void Moto_animate::set_orientation_angle(GLfloat angle)
{
  orientation_angle = angle;
}

// Set this axis once and for all.
void Moto_animate::set_handlebar_axis(GLfloat x, GLfloat y, GLfloat z)
{
  set_point3f(handlebar_axis, x, y, z);
}

// The degree of freedom of the handlebar.
void Moto_animate::set_max_handlebar_angle(float angle)
{
  max_handlebar_angle = angle;
}

// Creates the display lists containing the OpenGL commands for the
// independent parts of the motorcycle.
void Moto_animate::draw()
{
  Moto_geometry::draw();
}

// Performs one move of the motorcycle according to the speeds and
// angles. Just updates the angles for now.
void Moto_animate::move()
{
  update_angles();
}

// Calls all of the display lists.
void Moto_animate::display()
{
  // Rotate the entire motorcycle laterally
  glPushMatrix(); // 1
  glTranslatef(road_pos[0], road_pos[1], road_pos[2]);
  glRotatef(orientation_angle, 0.0, 1.0, 0.0);
  glRotatef(lateral_angle, 1.0, 0.0, 0.0);
  display_fix_parts();

  //left_wheel
  glPushMatrix(); // 2
  position_left_wheel();
  glRotatef(wheel_angle, 0.0, 0.0, 1.0);
  display_wheel();
  glPopMatrix(); // 2 

  //right_wheel
  glPushMatrix(); // 3
  position_handlebar();
  rotate_handlebar();
  glPushMatrix(); // 4
  glRotatef(wheel_angle, 0.0, 0.0, 1.0);
  display_wheel();
  glPopMatrix(); // 4
  display_handlebar();
  glPopMatrix(); // 3
  glPopMatrix(); //1
}

// Making this a function so we don't have to do it manually every
// time.
void Moto_animate::rotate_handlebar()
{
  glRotatef(handlebar_angle, 
	    handlebar_axis[0], handlebar_axis[1], handlebar_axis[2]);
}

// Updating the speeds and angles.
void Moto_animate::update_angles()
{
  update_angle(wheel_angle, wheel_speed);
  update_angle(handlebar_angle, handlebar_speed);

  // Check the degree of freedom of the handlebar.
  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;

  update_angle(lateral_angle, lateral_speed);
}

// Handlebar loop left-right. Written for testing purposes.
void Moto_animate::handlebar_loop()
{
  if (handlebar_angle >= 30 && handlebar_angle < 180)
    handlebar_speed = -0.1;
  else if (handlebar_angle <= 330 && handlebar_angle > 180)
    handlebar_speed = 0.1;
}