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

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

Driver_animate::Driver_animate(float size, int name)
  :Driver_geometry(size)
{
  for (int i=0; i<vector_numbers; i++)
    vectors.push_back(Point3f(0,0,0));
  set_position();
  set_rotation();
  reset_angles();
  name_id = name;
}

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].set_data(x, y, z);
}

void Driver_animate::set_position(Point3f &pos)
{
  vectors[position].set_data(pos);
}
    
// Translate from the original position by some values.
void Driver_animate::add_position(float x, float y, float z)
{
  vectors[position] += Point3f(x, y, z);
}

// Translate from the original position by some values.
void Driver_animate::add_position(Point3f &pos)
{
  vectors[position] += pos;
}

// Translate from the original position by some values.
void Driver_animate::remove_position(Point3f &pos)
{
  vectors[position] -= pos;
}

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

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

// Set all the angles to 0.
void Driver_animate::reset_angles()
{
  for (int i=0; i<vectors.size(); i++)
    vectors[i].set_data(0, 0, 0);
  /*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;
    }*/
}

// Print a snaphot of all the angles.
void Driver_animate::print_angles()
{
  cout << "Angles:" << endl;
  for (int i=0; i<vectors.size(); i++)
    cout << vectors[i] << ' ';
  cout << endl;  
}

// Output a snaphot of all the angles to a file.
void Driver_animate::output_angles(ofstream &fout)
{
  fout << "// " << endl;
  for (int i=0; i<vectors.size(); i++)
    fout << vectors[i] << ' ';
  fout << endl;   
}

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

// Move some angles based on an identity. "id" is the name returned by
// the name selection process, and it is 1 + the subscript in the
// vectors array because we reserve the name 0 for the
// background. coord is one of x, y, z (0, 1, 2) that the identified
// angle should be incremented in. quant is the amount of change.
void Driver_animate::move_angle(int id, int coord, float quant)
{
  if (id > 0) {
    int which = (id - 1) % vector_numbers;
    vectors[which][coord] += quant;
  }
}

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

// Applies some general purpose translation and rotation to make the
// entire scene follow the driver around.
void Driver_animate::set_view()
{
  Point3f pos = vectors[position];
  pos *= -1;
  float angle = -vectors[rotation][1]; // rotation around y
  glRotatef(180+angle, 0, 1, 0); // rotate around y
  pos.gl_translate();
}

// Calls any display list and identifies the name of the primitives
// too for interactive purposes.
void Driver_animate::call_list_name(int id, int name)
{
  int load_name = name_id*vector_numbers + name + 1;
  glLoadName(load_name);
  glCallList(id);
}

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

  glPushMatrix();
  vectors[position].gl_translate();
  vectors[rotation].gl_rotate();

  glPushMatrix();
  glTranslatef(0, waist, 0);
  vectors[angle_shoulders].gl_rotate();
  glTranslatef(0, -waist, 0);
  call_list_name(shoulders_id,angle_shoulders);

  // The head angle_head
  glPushMatrix();
  glTranslatef(0, throat_hollow, 0);
  vectors[angle_head].gl_rotate();
  glTranslatef(0, -throat_hollow, 0);
  call_list_name(head_id, angle_head);
  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);
  vectors[angle_hips_center].gl_rotate();
  glTranslatef(0, -waist, 0);
  call_list_name(hips_id, angle_hips_center);

  // 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);
  vectors[angle_hips].gl_rotate();
  call_list_name(thigh_id, angle_hips);

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

  glPushMatrix();
  glTranslatef(0, -calf_length, 0);
  vectors[angle_ankle].gl_rotate();
  call_list_name(foot_id, angle_ankle);
  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);
  vectors[angle_arm].gl_rotate();
  call_list_name(forearm_id, angle_arm);

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

  glPushMatrix();
  glTranslatef(0, -arms_size, 0);
  vectors[angle_wrist].gl_rotate();
  call_list_name(hand_id, angle_wrist);
  glPopMatrix();
  glPopMatrix();
  glPopMatrix();
}

// This function generates a mirror left-right image of the current pose.
void Driver_animate::reverse_angles()
{
  Point3f temp;

  for (int i=0; i<3; i++) {
    // Knees
    temp[i] = vectors[angle_knee_left][i];
    vectors[angle_knee_left][i] = vectors[angle_knee_right][i];
    vectors[angle_knee_right][i] = temp[i];

    if (i == 2) {
      vectors[angle_knee_left][i] *= -1;
      vectors[angle_knee_right][i] *= -1;
    }

    // Elbows
    temp[i] = vectors[angle_elbow_left][i];
    vectors[angle_elbow_left][i] = vectors[angle_elbow_right][i];
    vectors[angle_elbow_right][i] = temp[i];

    if (i == 2){
      vectors[angle_elbow_left][i] *= -1;
      vectors[angle_elbow_right][i] *= -1;
    }
    
    // Hips (Center)
    if (i == 1 || i == 2)
      vectors[angle_hips_center][i] *= -1;

    if (i == 2)
      vectors[rotation][i] *= -1;

    // Shoulders
    if (i == 1 || i == 2)
      vectors[angle_shoulders][i] *= -1;

    if (i == 2)
      vectors[rotation][i] *= -1;
   
    // Head
    if (i == 1 || i == 2)
      vectors[angle_head][i] *= -1;

    if (i == 2)
      vectors[rotation][i] *= -1;


    // Hips (Legs)
    temp[i] = vectors[angle_hips_left][i];
    vectors[angle_hips_left][i] = vectors[angle_hips_right][i];
    vectors[angle_hips_right][i] = temp[i];

    if (i == 1 || i == 2) {
      vectors[angle_hips_left][i] *= -1;
      vectors[angle_hips_right][i] *= -1;
    }

    // Arms
    temp[i] = vectors[angle_arm_left][i];
    vectors[angle_arm_left][i] = vectors[angle_arm_right][i];
    vectors[angle_arm_right][i] = temp[i];

    if (i == 1 || i == 2) {
      vectors[angle_arm_left][i] *= -1;
      vectors[angle_arm_right][i] *= -1;
    }
    
    // Ankles
    temp[i] = vectors[angle_ankle_left][i];
    vectors[angle_ankle_left][i] = vectors[angle_ankle_right][i];
    vectors[angle_ankle_right][i] = temp[i];

    if (i == 1 || i == 2) {
      vectors[angle_ankle_left][i] *= -1;
      vectors[angle_ankle_right][i] *= -1;
    }

    // Wrists
    temp[i] = vectors[angle_wrist_left][i];
    vectors[angle_wrist_left][i] = vectors[angle_wrist_right][i];
    vectors[angle_wrist_right][i] = temp[i];

    if (i == 1 || i == 2) {
      vectors[angle_wrist_left][i] *= -1;
      vectors[angle_wrist_right][i] *= -1;
    } 
  }
}
