/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   driver_geometry.cc
   
   The geometry of the driver.
  
*/

#include <iostream>
#include <cmath>
#include "driver_geometry.h"

// Default constructor. Since it initializes the id for the display
// lists with calls to gl functions, it should not be invoked before
// gl is initialized because it would generate a segmentation fault.
Driver_geometry::Driver_geometry(float size)
{
  set_ids();
  height = size;
  set_dimensions();
  set_precision();
  set_color(0.7, 0.3, 0.3);
}

// Set the ids of the display lists in case we have more than one
// driver. Get them from gl so that we know for sure that each display
// list has a different available id.
void Driver_geometry::set_ids()
{
  head_id = glGenLists(1);
  hand_id = glGenLists(1);
  forearm_id = glGenLists(1);
  arm_id = glGenLists(1);
  shoulders_id = glGenLists(1);
  hips_id = glGenLists(1);
  thigh_id = glGenLists(1);
  leg_id = glGenLists(1);
  foot_id = glGenLists(1);
}

// Destructor. It should delete all of the display lists.
Driver_geometry::~Driver_geometry()
{
  glDeleteLists(hand_id, 1);
  glDeleteLists(forearm_id, 1);
  glDeleteLists(arm_id, 1);
  glDeleteLists(shoulders_id, 1);
  glDeleteLists(hips_id, 1);
  glDeleteLists(head_id, 1);
  glDeleteLists(thigh_id, 1);
  glDeleteLists(leg_id, 1);
  glDeleteLists(foot_id, 1);
}

// Set the dimensions of the human body considering that the head is
// 1/7 of the whole body length and other proportions follow from
// that.
void Driver_geometry::set_dimensions()
{
  head_length = height / 7.5;
  head_width = head_length * 0.8;
  head_pos = head_length * 7;
  neck_size = 0.25 * head_length;
  neck_radius = 0.25 * head_length;

  legs_size = 4.0 * head_length;
  thigh_length = 2.0 * head_length;
  calf_length = 1.75 * head_length;
  knee_radius = 0.25 * head_length;
  foot_hight = 0.25 * head_length;
  foot_length = head_length;

  forearms_size = 1.25 * head_length;
  arms_size = 1.25 * head_length;
  hand_length = 0.8 * head_length;
  hand_width = 0.6 * hand_length;

  shoulders_width = 2*head_length;
  body_size = 2.5 * head_length;
  hips_width = 2 * head_width;
  throat_hollow = height - head_length - 0.25*head_length;
  waist = throat_hollow - 2*head_length;
}

// Set the precision.
void Driver_geometry::set_precision(int value)
{
  precision = value;
}

// Set the color of the driver.
void Driver_geometry::set_color(float rv, float gv, float bv)
{
  r = rv;
  g = gv;
  b = bv;
}

// Builds the display lists.
void Driver_geometry::draw()
{
  glNewList(head_id, GL_COMPILE);
  glColor3f(r/2, g/2, b/2);
  head();
  glColor3f(r, g, b);
  glEndList();

  glNewList(shoulders_id, GL_COMPILE);
  shoulders();
  glEndList();

  glNewList(hips_id, GL_COMPILE);
  hips();
  glEndList();

  glNewList(forearm_id, GL_COMPILE);
  forearm();
  glEndList();

  glNewList(arm_id, GL_COMPILE);
  arm();
  glEndList();
  
  glNewList(hand_id, GL_COMPILE);
  hand();
  glEndList();
  
  glNewList(thigh_id, GL_COMPILE);
  thigh();
  glEndList();
  
  glNewList(leg_id, GL_COMPILE);
  calf();
  glEndList();
  
  glNewList(foot_id, GL_COMPILE);
  foot();
  glEndList();
}

// Calls the OpenGL commands to draw the head. Not intended for use by
// itself.
void Driver_geometry::head()
{
  // the skull
  float top_center = head_pos+0.5*(head_length-head_width);
  float radx = 0.5*head_width, rady = 0.5*head_length;
  //gl_ellipsoid(0.0, top_center, radx-rady, radx, radx, rady, precision);
  //gl_ellipsoid(0.0, head_pos, 0.0, radx, rady, 1.2*radx, precision);
  gl_ellipsoid(0.0, head_pos, 0, radx, rady, rady, precision);

  // the neck
  vector3f neck_coord(2);
  neck_coord[0][1] = height - head_length - neck_size;
  neck_coord[1][1] = top_center;
  gl_cylinder_strip(neck_coord, neck_radius, precision);
}

// The shoulders
void Driver_geometry::shoulders()
{
  gl_ellipsoid(0.0, throat_hollow, 0.0, 
	       0.5*shoulders_width, 0.25*head_length, 0.5*head_width, 
	       precision);
  int i, nr_points = 10 * precision+1;
  vector3f center_line(nr_points);
  vector3f top_line(nr_points);
  float radius = 0.5 * head_width;
  float dangle = 0.2 * M_PI / precision;

  for (i=0; i<nr_points; i++) {
    center_line[i][0] = radius * cos(i*dangle);
    center_line[i][1] = waist;
    center_line[i][2] = radius * sin(i*dangle);
    top_line[i][0] = head_length * cos(i*dangle);
    top_line[i][1] = throat_hollow;
    top_line[i][2] = radius * sin(i*dangle);
  }
  gl_triangle_grid(center_line, top_line);
}

// The hips
void Driver_geometry::hips()
{
  gl_ellipsoid(0.0, throat_hollow-body_size+0.5*head_length, 0.0, 
	       0.5*hips_width, 0.5*head_length, 0.5*head_width, 
	       precision);
}

// Forearms
void Driver_geometry::forearm()
{
  gl_sphere(0, 0, 0, knee_radius, precision);
  vector3f coords;
  coords.push_back(Point3f(0, 0, 0)); 
  coords.push_back(Point3f(0, -0.33*forearms_size, 0));
  coords.push_back(Point3f(0, -0.67*forearms_size, 0)); 
  coords.push_back(Point3f(0, -forearms_size, 0));
  float radia[4] = {0, knee_radius, knee_radius, 0};
  gl_cone_strip(coords, radia, precision);
}

// arms
void Driver_geometry::arm()
{
  gl_sphere(0, 0, 0, 0.8*knee_radius, precision);
  vector3f coords;
  coords.push_back(Point3f(0, 0, 0));
  coords.push_back(Point3f(0, -0.33*arms_size, 0));
  coords.push_back(Point3f(0, -0.67*arms_size, 0)); 
  coords.push_back(Point3f(0, -arms_size, 0));
  float radia[4] = {0, 0.8*knee_radius, 0.8*knee_radius, 0};
  gl_cone_strip(coords, radia, precision);
}

// Hand
void Driver_geometry::hand()
{
  gl_sphere(0, 0, 0, 0.6*knee_radius, precision);
  gl_ellipsoid(0, -0.5*hand_length, 0, 
	       0.5*hand_width, 0.5*hand_length, 0.25*hand_width,
	       precision);
}

// Legs

// Thigh calf_length
void Driver_geometry::thigh()
{
  gl_sphere(0, 0, 0, 1.2*knee_radius, precision);
  vector3f coords;
  coords.push_back(Point3f(0, 0, 0)); 
  coords.push_back(Point3f(0, -0.2*thigh_length, 0));
  coords.push_back(Point3f(0, -0.5*thigh_length, 0)); 
  coords.push_back(Point3f(0, -thigh_length, 0));
  float radia[4] = {0, 1.2*knee_radius, knee_radius, 0};
  gl_cone_strip(coords, radia, precision);
}

// Calf
void Driver_geometry::calf()
{
  gl_sphere(0, 0, 0, knee_radius, precision);
  vector3f coords;
  coords.push_back(Point3f(0, 0, 0)); 
  coords.push_back(Point3f(0, -0.4*calf_length, 0));
  coords.push_back(Point3f(0, -0.67*calf_length, 0)); 
  coords.push_back(Point3f(0, -calf_length, 0));
  float radia[4] = {0, knee_radius, 0.8*knee_radius, 0};
  gl_cone_strip(coords, radia, precision);
}

// Foot
void Driver_geometry::foot()
{
  gl_sphere(0, 0, 0, 0.8*knee_radius, precision);
  
  gl_ellipsoid(0, -0.5*foot_hight, 0.5*foot_length, 
	       0.5*hand_width, 0.5*foot_hight, 0.5*foot_length,
	       precision);
}

// Calls the display lists.
void Driver_geometry::display()
{
  glColor3f(r, g, b);
  glCallList(head_id);
  glCallList(shoulders_id);
  glCallList(hips_id);

  // Left arm
  glPushMatrix();
  glTranslatef(-0.5*shoulders_width, throat_hollow, 0);
  glCallList(forearm_id);

  glPushMatrix();
  glTranslatef(0, -forearms_size, 0);
  glCallList(arm_id);

  glPushMatrix();
  glTranslatef(0, -arms_size, 0);
  glCallList(hand_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();

  // Right arm
  glPushMatrix();
  glTranslatef(0.5*shoulders_width, throat_hollow, 0);
  glCallList(forearm_id);

  glPushMatrix();
  glTranslatef(0, -forearms_size, 0);
  glCallList(arm_id);

  glPushMatrix();
  glTranslatef(0, -arms_size, 0);
  glCallList(hand_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();

  // Left leg
  glPushMatrix();
  glTranslatef(-0.5*(hips_width - 1.2*knee_radius), legs_size, 0);
  glCallList(thigh_id);

  glPushMatrix();
  glTranslatef(0, -thigh_length, 0);
  glCallList(leg_id);

  glPushMatrix();
  glTranslatef(0, -calf_length, 0);
  glCallList(foot_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();

  // Right leg
  glPushMatrix();
  glTranslatef(0.5*(hips_width - 1.2*knee_radius), legs_size, 0);
  glCallList(thigh_id);

  glPushMatrix();
  glTranslatef(0, -thigh_length, 0);
  glCallList(leg_id);

  glPushMatrix();
  glTranslatef(0, -calf_length, 0);
  glCallList(foot_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();
}
