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

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

#include "moto_geometry.h"
#include "general.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()
{
  set_ids();
  set_precision();
  set_saddle_rgb();
  set_engine_rgb();
  set_tire_rgb();
  set_headlight_rgb();
  set_handlebar_pos();
  set_left_wheel_pos();
}

// 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)
{
  set_point3f(handlebar_pos, x, y, z);
}

void Moto_geometry::set_left_wheel_pos(GLfloat x, GLfloat y, GLfloat z)
{
  set_point3f(left_wheel_pos, 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()
{
  glTranslatef(handlebar_pos[0], handlebar_pos[1], handlebar_pos[2]);
}

void Moto_geometry::position_left_wheel()
{
  glTranslatef(left_wheel_pos[0], left_wheel_pos[1], left_wheel_pos[2]);
}

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

  // 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],
		      hor_dim);
}

// 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()
{
  Point3f fork_base[2] = {{-0.6, 1.2, 0}, {-0.6, 1.6, 0}};
  gl_cylinder_strip(fork_base, 2, 0.22, precision);

  Point3f fork1[2] = {{-2, 1, 0.22}, {-0.5, 1.4, 0.22}};
  Point3f fork2[2] = {{-2, 1, -0.22}, {-0.5, 1.4, -0.22}};
  Point3f dir[2] = {{-0.035, 0.1, 0}, {-0.035, 0.1, 0}};

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

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

  Point3f middle_frame_coord[4] = {
    {-0.4, 1.7, 0.3}, {-0.3, 2.25, 0.3}, 
    {-0.4, 2.7, 0.3}, {-0.45, 2.95, 0.1}};
  gl_cylinder_strip(middle_frame_coord, 4, 0.05, precision);
  mirror_z(middle_frame_coord, 4);
  gl_cylinder_strip(middle_frame_coord, 4, 0.05, precision);
}

void Moto_geometry::engine_center()
{
  // Links between the engine ellipsoids.
  Point3f link1[2] = {{0, 1.5, 0}, {-0.65, 2.3, 0}};
  gl_cylinder_strip(link1, 2, 0.1, precision);
  Point3f link2[2] = {{-0.65, 2.3, 0}, {0.35, 2.7, 0}};
  gl_cylinder_strip(link2, 2, 0.1, precision);
  Point3f link3[2] = {{-0.65, 2.3, 0}, {-1.2, 2.4, 0.3}};
  gl_cylinder_strip(link3, 2, 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
  Point3f exhaust[2] = {{-1.2, 2.4, 0.3}, {-2.2, 2.7, 0.4}};
  gl_cylinder_strip(exhaust, 2, 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, wheel_radius = 1.0, 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;
  Point3f 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, 2, spike_width, precision);
  }

  Point3f ax[2] = {{0, 0, -ax_len}, {0, 0, ax_len}};
  gl_cylinder_strip(ax, 2, 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);

  Point3f left_bar[2] = {{0, 0, -0.3}, {-1.5, 2.7, -0.3}};
  Point3f right_bar[2] = {{0, 0, 0.3}, {-1.5, 2.7, 0.3}};
  Point3f bar_coords[6] ={
    {-1.7, 3, -1.7},
    {-1.5, 3, -1},
    {-1.5, 2.7, -0.5},
    {-1.5, 2.7, 0.5},
    {-1.5, 3, 1},
    {-1.7, 3, 1.7}
    };

  gl_cylinder_strip(left_bar, 2, bar_width, precision);
  gl_cylinder_strip(right_bar, 2, bar_width, precision);
  gl_cylinder_strip(bar_coords, 6, 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);

  Point3f middle_bar[2] = {{-0.95, 1.7, -0.3}, {-0.95, 1.7, 0.3}};
  gl_cylinder_strip(middle_bar, 2, bar_width, precision);

  Point3f protect_left[6] = {
    {0.4, 1.9, 0.2}, {0.7, 2.4, 0.2}, {1, 2.7, 0.2},
    {1.5, 2.8, 0.2}, {2, 2.7, 0.2}, {2.6, 2.4, 0.2}};
  Point3f protect_right[6];
  mirror_z(protect_left, protect_right, 6);

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

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