/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   driver_animate.cc
   
   The class that handles all possible movements of the body of the
   driver.
  
**/

#include <math.h>
#include "driver_animate.h"
#include "general.h"

Driver_animate::Driver_animate(float size)
  :Driver_geometry(size)
{
  set_position();
  set_rotation();
  reset_angles();
}

Driver_animate::~Driver_animate()
{;}

// Inialize the position. Default values 0, 0, 0.
void Driver_animate::set_position(float x, float y, float z)
{
  vectors[position][0] = x;
  vectors[position][1] = y;
  vectors[position][2] = z;
}

// Translate from the original position by some values.
void Driver_animate::add_position(float x, float y, float z)
{
  vectors[position][0] += x;
  vectors[position][1] += y;
  vectors[position][2] += z;
}

// Inialize the rotation. Default values 0, 0, 0.
void Driver_animate::set_rotation(float ax, float ay, float az)
{
  vectors[rotation][0] = ax;
  vectors[rotation][1] = ay;
  vectors[rotation][2] = az;
}

// Add some values to the initial rotation.
void Driver_animate::add_rotation(float ax, float ay, float az)
{
  vectors[rotation][0] = +ax;
  vectors[rotation][1] = +ay;
  vectors[rotation][2] = +az;
}

// Set all the angles to 0.
void Driver_animate::reset_angles()
{

  for (int i=0; i<3; i++) {
    // Should never be negative values.
    vectors[angle_knee_left][i] = 0;
    vectors[angle_knee_right][i] = 0;
    
    // These should only be negative values.
    vectors[angle_elbow_left][i] = 0;
    vectors[angle_elbow_right][i] = 0;
    // Positive values for the left up and back, and to bend forward.
    vectors[angle_hips_center][i] = 0;
    vectors[angle_shoulders][i] = 0; 
    // Negative values to bend forward.
    vectors[angle_hips_left][i] = 0;
    vectors[angle_hips_right][i] = 0;
    // Positive values to bend back, negative values to bend forward.
    vectors[angle_arm_left][i] = 0;
    vectors[angle_arm_right][i] = 0;
    // Negative values to bend up, positive ones to bend down.
    vectors[angle_ankle_left][i] = 0;
    vectors[angle_ankle_right][i] = 0;
    // Positive 0 values to bend forward, negative values to bend back.
    // Positive 1 values to twist right, negative to twist left.
    // Positive 2 values to tilt right, negative to tilt left.
    // 3 possible twists for the head: up-down, bend left-right, tilt
    // left-right.
    vectors[angle_head][i] = 0;
    // Positive values to bend down, negative values to bend up.
    vectors[angle_wrist_left][i] = 0;
    vectors[angle_wrist_right][i] = 0;
  }
}

// Some weird angle values to test the model. Thought it was worth
// saving.
void Driver_animate::test_angles()
{
  for (int i=0; i<3; i++) {
    // Should never be negative values.
    vectors[angle_knee_left][i] = 10;
    vectors[angle_knee_right][i] = 90;
    
    // These should only be negative values.
    vectors[angle_elbow_left][i] = -60;
    vectors[angle_elbow_right][i] = -90;
    
    // 3 possible twists for the head: up-down, bend left-right, tilt
    // left-right.
    // Positive values for the left up and back, and to bend forward.
    vectors[angle_hips_center][i] = 20;
    vectors[angle_shoulders][i] = 20; 
    // Negative values to bend forward.
    vectors[angle_hips_left][i] = -30;
    vectors[angle_hips_right][i] = 60;
    // Positive values to bend back, negative values to bend forward.
    vectors[angle_arm_left][i] = 60;
    vectors[angle_arm_right][i] = -30;
    // Negative values to bend up, positive ones to bend down.
    vectors[angle_ankle_left][i] = 20;
    vectors[angle_ankle_right][i] = -30;
    // Positive 0 values to bend forward, negative values to bend back.
    // Positive 1 values to twist right, negative to twist left.
    // Positive 2 values to tilt right, negative to tilt left.
    vectors[angle_head][i] = 30;
    // Positive values to bend down, negative values to bend up.
    vectors[angle_wrist_left][i] = -60;
    vectors[angle_wrist_right][i] = 30;
  }
}

// Some particular functions that describe the mobility of the body.

// Bend a knee and raise it from the ground too. To move it back down
// call the function with a negative angle.
void Driver_animate::bend_left_knee(float angle)
{
  vectors[angle_knee_left][0] += angle;
  vectors[angle_hips_left][0] -= angle;
}

void Driver_animate::bend_right_knee(float angle)
{
  vectors[angle_knee_right][0] += angle;
  vectors[angle_hips_right][0] -= angle;
}

void Driver_animate::bend_knees(float angle)
{
  bend_left_knee(angle);
  bend_right_knee(angle);
}

// Perform the rotation only at the level of the hips.
void Driver_animate::rotate_left_thigh_up(float angle)
{
  vectors[angle_hips_left][0] -= angle;
}

void Driver_animate::rotate_right_thigh_up(float angle)
{
  vectors[angle_hips_right][0] -= angle;
}

void Driver_animate::rotate_thighs_up(float angle)
{
  rotate_left_thigh_up(angle);
  rotate_right_thigh_up(angle);
}

// Rotate an entire leg out.
void Driver_animate::rotate_left_thigh_out(float angle)
{
  vectors[angle_hips_left][1] -= angle;
  vectors[angle_hips_left][2] -= 1.5*angle;
}

void Driver_animate::rotate_right_thigh_out(float angle)
{
  vectors[angle_hips_right][1] += angle;
  vectors[angle_hips_right][2] += 1.5*angle;
}

void Driver_animate::rotate_thighs_out(float angle)
{
  rotate_left_thigh_out(angle);
  rotate_right_thigh_out(angle);
}

// Raise an arm in a sort of natural movement by bending both at the
// elbow and at the shoulder level.
void Driver_animate::rotate_left_arm_up(float angle)
{
  vectors[angle_arm_left][0] -= angle;
  vectors[angle_elbow_left][0] -= 2*angle;
  if (vectors[angle_elbow_left][0] > 0)
    vectors[angle_elbow_left][0] = 0;
}

void Driver_animate::rotate_right_arm_up(float angle)
{
  vectors[angle_arm_right][0] -= angle;
  vectors[angle_elbow_right][0] -= 2*angle;
  if (vectors[angle_elbow_right][0] > 0)
    vectors[angle_elbow_right][0]=0;
}

void Driver_animate::rotate_arms_up(float angle)
{
  rotate_left_arm_up(angle);
  rotate_right_arm_up(angle);
}

// Rotate only the second part of the arm. Involves the elbow angles.
void Driver_animate::bend_left_forearm(float angle)
{
  vectors[angle_elbow_left][0] -= angle;
  if (vectors[angle_elbow_left][0] > 0)
    vectors[angle_elbow_left][0] = 0;
}

void Driver_animate::bend_right_forearm(float angle)
{
  vectors[angle_elbow_right][0] -= angle;
  if (vectors[angle_elbow_right][0] > 0)
    vectors[angle_elbow_right][0]=0;
}

void Driver_animate::bend_forearms(float angle)
{
  bend_left_forearm(angle);
  bend_right_forearm(angle);
}

// Spread the arms out.
void Driver_animate::rotate_left_arm_out(float angle)
{
  vectors[angle_arm_left][1] -= angle;
  vectors[angle_elbow_left][1] -= 0.2*angle;
  vectors[angle_arm_left][2] -= 0.3*angle;
  vectors[angle_elbow_left][2] -= 0.1*angle;
}

void Driver_animate::rotate_right_arm_out(float angle)
{
  vectors[angle_arm_right][1] += angle;
  vectors[angle_elbow_right][1] += 0.2*angle;
  vectors[angle_arm_right][2] += 0.3*angle;
  vectors[angle_elbow_right][2] += 0.1*angle;
}

void Driver_animate::rotate_arms_out(float angle)
{
  rotate_left_arm_out(angle);
  rotate_right_arm_out(angle);
}

// Builds the display lists.
void Driver_animate::draw()
{
  Driver_geometry::draw();
}

// Calls the display lists with appropriate transformations.
void Driver_animate::display()
{  
  glColor3f(r, g, b);

  glPushMatrix();
  glTranslatef(vectors[position][0], vectors[position][1], 
	       vectors[position][2]);
  rotate_point(vectors[rotation]);

  glPushMatrix();
  glTranslatef(0, waist, 0);
  rotate_point(vectors[angle_shoulders]);
  glTranslatef(0, -waist, 0);
  glCallList(shoulders_id);

  // The head angle_head
  glPushMatrix();
  glTranslatef(0, throat_hollow, 0);
  rotate_point(vectors[angle_head]);
  glTranslatef(0, -throat_hollow, 0);
  glCallList(head_id);
  glPopMatrix();

  // Left arm
  display_arm(angle_arm_left, angle_elbow_left, angle_wrist_left, 
	      -0.5*shoulders_width, throat_hollow);

  // Right arm
  display_arm(angle_arm_right, angle_elbow_right, angle_wrist_right, 
	      0.5*shoulders_width, throat_hollow);

  glPopMatrix(); // shoulders rotation
  
  glPushMatrix();
  glTranslatef(0, waist, 0);
  rotate_point(vectors[angle_hips_center]);
  glTranslatef(0, -waist, 0);
  glCallList(hips_id);

  // Left leg
  display_leg(angle_hips_left, angle_knee_left, angle_ankle_left,  
	      -0.5*(hips_width - 1.2*knee_radius), legs_size);

  // Right leg
  display_leg(angle_hips_right, angle_knee_right, angle_ankle_right, 
	      0.5*(hips_width - 1.2*knee_radius), legs_size);

  glPopMatrix(); // hips rotation

  glPopMatrix();
}

// Display one leg with given angles and position.
void Driver_animate::display_leg(angles_ids angle_hips, angles_ids angle_knee, 
				 angles_ids angle_ankle,
				 float x_trans, float y_trans)
{
  //thigh
  glPushMatrix();
  glTranslatef(x_trans, y_trans, 0);
  rotate_point(vectors[angle_hips]);
  glCallList(thigh_id);

  glPushMatrix();
  glTranslatef(0, -thigh_length, 0);
  glRotatef(vectors[angle_knee][0], 1, 0, 0);
  glCallList(leg_id);

  glPushMatrix();
  glTranslatef(0, -calf_length, 0);
  rotate_point(vectors[angle_ankle]);
  glCallList(foot_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();
}

// Display one arm with given angles and position.
void Driver_animate::display_arm(angles_ids angle_arm, angles_ids angle_elbow, 
				 angles_ids angle_wrist,
				 float x_trans, float y_trans)
{
  glPushMatrix();
  glTranslatef(x_trans, y_trans, 0);
  rotate_point(vectors[angle_arm]);
  glCallList(forearm_id);

  glPushMatrix();
  glTranslatef(0, -forearms_size, 0);
  glRotatef(vectors[angle_elbow][0], 1, 0, 0);
  glCallList(arm_id);

  glPushMatrix();
  glTranslatef(0, -arms_size, 0);
  rotate_point(vectors[angle_wrist]);
  glCallList(hand_id);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();
}