/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   moto_geometry.cc
   
   The geometrical implementation of the motorcycle containing the
   necessary OpenGL commands.  
*/

#include <iostream> 
#include <stdio.h>
#include <cmath>

#include "moto_geometry.h"
#include "general.h"
#include "gl_draw.h"


// Default constructor. Sets the id numbers and the default
// colors. Does not create the display lists to be sure that we don't
// want to set the colors to different values and draw the motorcycle
// only afterwards. It should not be invoked before the gl is
// initialized because it will create a segmentation fault.
Moto_geometry::Moto_geometry()
  : handlebar_pos(), left_wheel_pos(), mass(150)
{
  wheel_radius = 1.0;
  set_ids();
  set_precision();
  set_saddle_rgb();
  set_engine_rgb();
  set_tire_rgb();
  set_headlight_rgb();
  set_handlebar_pos();
  set_left_wheel_pos();
  wheel_distance = 2*left_wheel_pos.norm();
}

// Set the ids of the display lists in case we have more than one
// motorcycle.
void Moto_geometry::set_ids()
{
  fix_parts_id = glGenLists(1);
  wheel_id = glGenLists(1);
  handlebar_id = glGenLists(1);
}

// Set the precision of the rendering. 1 would be wireframe type, 2
// surfaced with raw polygons, 3 surfaced with lots of polygons.
void Moto_geometry::set_precision(int value)
{
  precision = value;
}

// Setting each of the colors and default values for each of them.
void Moto_geometry::set_saddle_rgb(GLfloat r, GLfloat g, GLfloat b)
{
  saddle_r = r;
  saddle_g = g;
  saddle_b = b;
}

void Moto_geometry::set_engine_rgb(GLfloat r, GLfloat g, GLfloat b)
{
  engine_r = r;
  engine_g = g;
  engine_b = b;
}

void Moto_geometry::set_tire_rgb(GLfloat r, GLfloat g, GLfloat b)
{
  tire_r = r;
  tire_g = g;
  tire_b = b;
}

void Moto_geometry::set_headlight_rgb(GLfloat r, GLfloat g, GLfloat b)
{
  headlight_r = r;
  headlight_g = g;
  headlight_b = b;
}
  
// Set the positions.
void Moto_geometry::set_handlebar_pos(GLfloat x, GLfloat y, GLfloat z)
{
  handlebar_pos.set_data(x, y, z);
}

void Moto_geometry::set_left_wheel_pos(GLfloat x, GLfloat y, GLfloat z)
{
  left_wheel_pos.set_data(x, y, z);
}

// Define some dimensions
float Moto_geometry::length()
{
  return 2*(fabs(left_wheel_pos[0]) + fabs(left_wheel_pos[1]));
}

float Moto_geometry::width()
{
  return 0.567*length();
}

float Moto_geometry::height()
{
  return 0.8*length();
}

// Print some of these values
void Moto_geometry::cout_headlight_rgb()
{
  cout << headlight_r << ' ' << headlight_g << ' ' << headlight_b << endl;
}

// Destructor. Delete the display lists hoping that it will restore
// the memory.
Moto_geometry::~Moto_geometry() 
{
  glDeleteLists(fix_parts_id, 1);
  glDeleteLists(wheel_id, 1);
  glDeleteLists(handlebar_id, 1);
}


// Creates the display lists containing the OpenGL commands for the
// independent parts of the motorcycle.
void Moto_geometry::draw()
{
  // Set the polygon mode to wireframe or surface depending on the
  // precision.
  glNewList(fix_parts_id, GL_COMPILE);
  if (precision == 1) 
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  else
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  
  saddle();
  engine();
  glEndList();

  glNewList(wheel_id, GL_COMPILE);
  wheel();
  glEndList();

  glNewList(handlebar_id, GL_COMPILE);
  handlebar();
  glEndList();
}

// Calls all of the display lists.
void Moto_geometry::display()
{
  display_fix_parts();

  //left_wheel
  glPushMatrix();
  position_left_wheel();
  display_wheel();
  glPopMatrix();

  //right_wheel
  glPushMatrix();
  position_handlebar();
  display_wheel();
  display_handlebar();
  glPopMatrix();
}

// We may need to diplay individual components. These functions should
// only call the display lists.
void Moto_geometry::display_wheel()
{
  glCallList(wheel_id);
}

void Moto_geometry::display_handlebar()
{
  glCallList(handlebar_id);
}

void Moto_geometry::display_fix_parts()
{
  glCallList(fix_parts_id);
}

// Position the handlebar and wheel so that we don't have to call the
// gl command explicitly every time.
void Moto_geometry::position_handlebar()
{
  handlebar_pos.gl_translate();
}

void Moto_geometry::position_left_wheel()
{
  left_wheel_pos.gl_translate();
}

// The OpenGL commands for the saddle. Not intended to be used by
// itself.
void Moto_geometry::saddle()
{
  int ver_dim=6;
  // Basic coordinates
  vector3f top_coord;
  top_coord.push_back(Point3f(-2.7, 3.95, 0.15));
  top_coord.push_back(Point3f(-1.15, 3.15, 0.2));
  top_coord.push_back(Point3f(-0.6, 3, 0.25));
  top_coord.push_back(Point3f(-0.25, 3, 0.25)); 
  top_coord.push_back(Point3f(0.3, 3.15, 0.2)); 
  top_coord.push_back(Point3f(0.65, 3.2, 0.15));
  top_coord.push_back(Point3f(0.75, 3.2, 0.15));
  int hor_dim=top_coord.size();
  vector3f middle_coord;
  middle_coord.push_back(Point3f(-2.7, 3.90, 0.15)); 
  middle_coord.push_back(Point3f(-1.15, 2.9, 0.3)); 
  middle_coord.push_back(Point3f(-0.8, 2.7, 0.38));
  middle_coord.push_back(Point3f(-0.25, 2.6, 0.45)); 
  middle_coord.push_back(Point3f(0.3, 2.6, 0.45)); 
  middle_coord.push_back(Point3f(0.65, 2.8, 0.55));
  middle_coord.push_back(Point3f(0.75, 2.85, 0.62));
  vector3f lateral_coord;
  lateral_coord.push_back(Point3f(-1.15, 2.6, 0.32)); 
  lateral_coord.push_back(Point3f(-1, 2.3, 0.5)); 
  lateral_coord.push_back(Point3f(-0.8, 2.7, 0.38));
  lateral_coord.push_back(Point3f(0, 2.25, 0.5)); 
  lateral_coord.push_back(Point3f(0.45, 2.45, 0.55)); 
  lateral_coord.push_back(Point3f(0.9, 2, 0.6));
  lateral_coord.push_back(Point3f(1.1, 2.6, 0.7));
    
  // Build the 6 lists of vectors for the saddle. The are supposed to
  // make a triangle grid together.
  matrix3f saddle_coord(ver_dim), saddle_normals(ver_dim);
  Point3f empty(hor_dim);
  int i, j, k;
  for (j=0; j<ver_dim; j++)
    for (i=0; i<hor_dim; i++) {
      saddle_coord[j].push_back(empty);
      saddle_normals[j].push_back(empty);
    }
  for (i=0; i<hor_dim; i++) {
    saddle_coord[0][i] = lateral_coord[i];
    saddle_coord[1][i] = middle_coord[i];
    saddle_coord[2][i] = top_coord[i];
    saddle_coord[3][i] = top_coord[i];
    saddle_coord[4][i] = middle_coord[i];
    saddle_coord[5][i] = lateral_coord[i];
    for (j=3; j<ver_dim; j++)
      saddle_coord[j][i][2] *= -1;
  }

  // Initialize all the normals to 0, add all of them from each pair
  // of coordinate vectors, and then normalize all of them.
  for (i=0; i<ver_dim; i++)
    for (j=0; j<hor_dim; j++)
      for (k=0; k<3; k++)
	saddle_normals[i][j][k] = 0.0;
  for (i=0; i<ver_dim-1; i++) 
    add_normals(saddle_coord[i], saddle_coord[i+1], 
		saddle_normals[i], saddle_normals[i+1]);
  normalize_vectors(saddle_normals);

  // Now we can generate those OpenGL commands.
  glColor3f(saddle_r, saddle_g, saddle_b);
  for (i=0; i<ver_dim-1; i++) 
    gl_triangle_strip(saddle_coord[i], saddle_coord[i+1], 
		      saddle_normals[i], saddle_normals[i+1]);
}

// The OpenGL commands for the engine. Not intended to be used by
// itself.
void Moto_geometry::engine()
{
  glColor3f(engine_r, engine_g, engine_b);
  fork();
  frame();
  engine_center();
}

// The OpenGL commands for the fork. Not intended to be used by
// itself.
void Moto_geometry::fork()
{
  vector3f fork_base;
  fork_base.push_back(Point3f(-0.6, 1.2, 0));
  fork_base.push_back(Point3f(-0.6, 1.6, 0));
  gl_cylinder_strip(fork_base, 0.22, precision);

  vector3f fork1;
  fork1.push_back(Point3f(-2, 1, 0.22)); 
  fork1.push_back(Point3f(-0.5, 1.4, 0.22));
  vector3f fork2; 
  fork2.push_back(Point3f(-2, 1, -0.22));
  fork2.push_back(Point3f(-0.5, 1.4, -0.22));
  vector3f dir; 
  dir.push_back(Point3f(-0.035, 0.1, 0));
  dir.push_back(Point3f(-0.035, 0.1, 0));

  gl_ribbon(fork1, dir);
  gl_ribbon(fork2, dir);
}

void Moto_geometry::frame()
{
  vector3f frame_coord;
  frame_coord.push_back(Point3f(-2.2, 3.65, 0.05));
  frame_coord.push_back(Point3f(-2, 3, 0.1));
  frame_coord.push_back(Point3f(-0.4, 1.7, 0.3));
  frame_coord.push_back(Point3f(-0.3, 1.3, 0.3));
  frame_coord.push_back(Point3f(-0.25, 1.1, 0.4));
  frame_coord.push_back(Point3f(0.25, 1, 0.35));
  frame_coord.push_back(Point3f(0.4, 1.1, 0.3));
  frame_coord.push_back(Point3f(0.9, 2.5, 0.2));
  frame_coord.push_back(Point3f(1.12, 2.7, 0.1));
  gl_cylinder_strip(frame_coord, 0.05, precision);
  mirror_z(frame_coord);
  gl_cylinder_strip(frame_coord, 0.05, precision);

  vector3f middle_frame_coord;
  middle_frame_coord.push_back(Point3f(-0.4, 1.7, 0.3));
  middle_frame_coord.push_back(Point3f(-0.3, 2.25, 0.3));
  middle_frame_coord.push_back(Point3f(-0.4, 2.7, 0.3));
  middle_frame_coord.push_back(Point3f(-0.45, 2.95, 0.1));
  gl_cylinder_strip(middle_frame_coord, 0.05, precision);
  mirror_z(middle_frame_coord);
  gl_cylinder_strip(middle_frame_coord, 0.05, precision);
}

void Moto_geometry::engine_center()
{
  // Links between the engine ellipsoids.
  vector3f link1;
  link1.push_back(Point3f(0, 1.5, 0));
  link1.push_back(Point3f(-0.65, 2.3, 0));
  gl_cylinder_strip(link1, 0.1, precision);
  vector3f link2;
  link2.push_back(Point3f(-0.65, 2.3, 0));
  link2.push_back(Point3f(0.35, 2.7, 0));
  gl_cylinder_strip(link2, 0.1, precision);
  vector3f link3;
  link3.push_back(Point3f(-0.65, 2.3, 0));
  link3.push_back(Point3f(-1.2, 2.4, 0.3));
  gl_cylinder_strip(link3, 0.1, precision);

  // center_eng
  gl_ellipsoid(0, 1.5, 0, 0.5, 0.3, 0.4, precision);
  // tank
  gl_ellipsoid(0.35, 2.7, 0, 0.5, 0.35, 0.25, precision);
  // back_eng
  gl_ellipsoid(-0.65, 2.3, 0, 0.2, 0.35, 0.3, precision);
  // footrests
  gl_ellipsoid(-0.25, 1.1, 0.6, 0.15, 0.05, 0.25, precision);
  gl_ellipsoid(-0.25, 1.1, -0.6, 0.15, 0.05, 0.25, precision);

  // exhaust
  vector3f exhaust;
  exhaust.push_back(Point3f(-1.2, 2.4, 0.3));
  exhaust.push_back(Point3f(-2.2, 2.7, 0.4));
  gl_cylinder_strip(exhaust, 0.15, precision);
  // exhaust link
  gl_sphere(-1.2, 2.4, 0.3, 0.15, precision); 
}

// The wheel centered in the origin of the space so as to rotate it
// easily. When drawing it, we have to translate it to 2 locations.
void Moto_geometry::wheel()
{
  //int precision = 30, speed = 2, i;
  int i; 
  const int tire_nr = 10;
  const GLfloat tire_width = 0.15, spike_width = 0.07, 
    ax_width = 0.12, ax_len = 0.4;

  glColor3f(tire_r, tire_g, tire_b);
  // Determine the precision of the rendering.
  switch (precision) {
  case 1:
    glutWireTorus(tire_width, wheel_radius-tire_width, 10, 10);
    break;
  case 2: 
    glutSolidTorus(tire_width, wheel_radius-tire_width, 10, 20);
    break;
  case 3: 
    glutSolidTorus(tire_width, wheel_radius-tire_width, 20, 30);
  }

  glColor3f(engine_r, engine_g, engine_b);

  float alpha = 2*M_PI/tire_nr;
  vector3f spike_coord(2);
  spike_coord[0][0] = 0;
  spike_coord[0][1] = 0;
  spike_coord[0][2] = 0;
  for (i=0; i<tire_nr; i++) {
    spike_coord[1][0] = (wheel_radius-tire_width) * cos(i*alpha);
    spike_coord[1][1] = (wheel_radius-tire_width) * sin(i*alpha);
    spike_coord[1][2] = 0;
    gl_cylinder_strip(spike_coord, spike_width, precision);
  }

  vector3f ax;
  ax.push_back(Point3f(0, 0, -ax_len));
  ax.push_back(Point3f(0, 0, ax_len));
  gl_cylinder_strip(ax, ax_width, precision);

  gl_sphere(0, 0, -ax_len, ax_width, precision);
  gl_sphere(0, 0, ax_len, ax_width, precision);
}

// The handlebar
void Moto_geometry::handlebar()
{
  // DVlines left_bar(2, 1, -0.3, 0.5, 3.7, -0.3);
  // DVlines right_bar(2, 1, 0.3, 0.5, 3.7, 0.3);
  const GLfloat bar_width=0.12;
  glColor3f(engine_r, engine_g, engine_b);

  vector3f left_bar;
  left_bar.push_back(Point3f(0, 0, -0.3));
  left_bar.push_back(Point3f(-1.5, 2.7, -0.3));
  vector3f right_bar;
  right_bar.push_back(Point3f(0, 0, 0.3));
  right_bar.push_back(Point3f(-1.5, 2.7, 0.3));
  vector3f bar_coords;
  bar_coords.push_back(Point3f(-1.7, 3, -1.7));
  bar_coords.push_back(Point3f(-1.5, 3, -1));
  bar_coords.push_back(Point3f(-1.5, 2.7, -0.5));
  bar_coords.push_back(Point3f(-1.5, 2.7, 0.5));
  bar_coords.push_back(Point3f(-1.5, 3, 1));
  bar_coords.push_back(Point3f(-1.7, 3, 1.7));
  gl_cylinder_strip(left_bar, bar_width, precision);
  gl_cylinder_strip(right_bar, bar_width, precision);
  gl_cylinder_strip(bar_coords, bar_width, precision);

  // close the top bars with spheres.
  gl_sphere(-1.7, 3, 1.7, bar_width, precision);
  gl_sphere(-1.7, 3, -1.7, bar_width, precision);

  vector3f middle_bar;
  middle_bar.push_back(Point3f(-0.95, 1.7, -0.3));
  middle_bar.push_back(Point3f(-0.95, 1.7, 0.3));
  gl_cylinder_strip(middle_bar, bar_width, precision);

  vector3f protect_left;
  protect_left.push_back(Point3f(0.4, 1.9, 0.2));
  protect_left.push_back(Point3f(0.7, 2.4, 0.2));
  protect_left.push_back(Point3f(1, 2.7, 0.2));
  protect_left.push_back(Point3f(1.5, 2.8, 0.2));
  protect_left.push_back(Point3f(2, 2.7, 0.2));
  protect_left.push_back(Point3f(2.6, 2.4, 0.2));
  vector3f protect_right(protect_left.size());
  mirror_z(protect_left, protect_right);

  glPushMatrix();
  glColor3f(saddle_r, saddle_g, saddle_b);
  glTranslatef(-2, -1, 0);
  gl_triangle_grid(protect_left, protect_right);
  glPopMatrix();

  glColor3f(headlight_r, headlight_g, headlight_b);
  gl_ellipsoid(-1.1, 2.1, 0, 0.2, 0.3, 0.3, precision);
}
