/*
 * Blocky humanoid robot (better known as "Lanjobot")
 *
 * Copyright (C) 2006-2008  Alejandro Valenzuela Roca & Hugo Mendieta Pacheco
 *
 * lanjoe9@mexinetica.com, scherezar_crusade@hotmail.com
 *
 * This file is part of MotorJ, a free framework for videogame development.
 * <http://wiki.lidsol.net/wiki/index.php?title=MotorJ >
 * <http://motorj.mexinetica.com/ >
 *
 * MotorJ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MotorJ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MotorJ.  If not, see <http://www.gnu.org/licenses/>.
 */



#include "lanjobot.h"

 // Height: 18.75
 // Arm length: 9

/* Implementación de funciones */


void colorcube(vector3 * color1, vector3 * color2)
{
	static bool LJBOTcolorCube = true;
	vector3 * currcolor;
	vector3 colorRed={1,0,0};
	vector3 colorWhite={1,1,1};
	
	if (LJBOTcolorCube)
	{
		if (!color1)
		{
			currcolor = &colorRed;
			
		}else
		{
			currcolor = color1;
		}
	} else
	{
		if (!color2)
		{
			currcolor = &colorWhite;
		} else
		{
			currcolor = color2;
		}
	}
	glColor3f(currcolor->x, currcolor->y, currcolor->z);
	
	mjSolidCube();
	LJBOTcolorCube = ! LJBOTcolorCube;
}


void lanjobot(ljbot_angles & parameters, vector3 * color1, vector3 * color2){
  push(/**/);
  push(/*Tronco*/);
  glTranslatef(0.0, 10.0, 0.0);
  push(/*Escala tr inf*/);
  glScalef(4.0,2.0,2.0);
  /* pelvis */
	colorcube(color1, color2);
  
  pop(/*Escala tr inf*/);

  push (/*piernas*/);
  push(/*left_l*/);
  glTranslatef(-1.1,0.0,0.0);
  ljbot_leg(parameters.left_l, 1, color1, color2);
  pop(/*left_l*/);
  push(/*right_l*/);
  glTranslatef(1.1,0.0,0.0);
  ljbot_leg(parameters.right_l, 0, color1, color2);
  pop(/*right_l*/);
  pop(/*piernas*/);

  push(/* Tronco sup */);
  glTranslatef(0.0,1.0,0.0);
  glRotatef(parameters.torax.rot,0.0,1.0,0.0);
  glRotatef(parameters.torax.flex_x,0.0,0.0,1.0);
  glRotatef(parameters.torax.flex_z,1.0,0.0,0.0);
  glTranslatef(0.0,2.0,0.0);
  push(/* Escala tronco sup*/);
  glScalef(4.0,4.0,2.0);
  colorcube(color1, color2);
  pop(/* Escala tr  sup*/);

  push(/*Cabeza/cuello*/);
  /*Cuello*/
  glTranslatef(0.0,2.0,0.0);
  glRotatef(parameters.head.neck_rot/2,0.0,1.0,0.0);
  glRotatef(parameters.head.neck_x,0.0,0.0,1.0);
  glRotatef(parameters.head.neck_z,1.0,0.0,0.0);
  glTranslatef(0.0,0.25,0.0);
  push(/*Escala cuello*/);
  glScalef(0.5,0.5,0.5);
  colorcube(color1, color2);
  pop(/*Escala cuello*/);
  glTranslatef(0.0,1.5,0.0);
  push(/* Cabeza*/);
  glRotatef(parameters.head.neck_rot/2,0.0,1.0,0.0);
  glScalef(1.9,2.0,1.9);
  //glRotatef(180,0.0,0.0,1.0);
  colorcube(color1, color2);
  pop(/* Cabeza*/);
  pop(/*Cabeza/cuello*/);

  push(/*Brazos*/);
  glTranslatef(0.0,1.5,0.0);

  push(/*arm_left*/);
  
  glRotatef(90,0.0,0.0,1.0);
  ljbot_arm(parameters.b_left,0, color1, color2);
  pop(/*arm_left*/);

  push(/*arm_right*/);
  glRotatef(-90,0.0,0.0,1.0);
  ljbot_arm(parameters.b_right,1, color1, color2);
  pop(/*arm_right*/);

  pop(/*Brazos*/);
  pop(/*Tronco sup*/);
  pop(/*Tronco*/);
  pop(/**/);
}

void ljbot_leg( leg_angles & params_p, int izq, vector3 * color1, vector3 * color2){
  push(/*pierna */);
  glTranslatef(0.0,-2.0,0.0);
  glRotatef(params_p.thigh_rot,0.0,1.0,0.0);
  glRotatef(params_p.thigh_x,0.0,0.0,1.0);
  glRotatef(params_p.thigh_z,1.0,0.0,0.0);

  glTranslatef(0.0,-1.0,0.0);
  push(/*Escala muslo */);
  glScalef(2.0,4.0,2.0);
  colorcube(color1, color2);
  pop(/*Escala muslo */);

  glTranslatef(0.0,-2.0,0.0);
  glRotatef(params_p.knee,1.0,0.0,0.0);
  glTranslatef(0.0,-2.0,0.0);
  push(/*Escala pantorrilla */);
  glScalef(2.0,4.0,1.0);
  colorcube(color1, color2);
  pop(/*Escala pantorrila */);
  push(/*Pie izquierdo*/);

  glTranslatef(0.0, -2.0, 0.0); 
  glRotatef(params_p.foot_rot,0.0,1.0,0.0);
  glRotatef(params_p.foot_z,1.0,0.0,0.0);
  glTranslatef(0.0, -0.5, 1.0);
  glScalef(2,0.5,2.0);
  colorcube(color1, color2);
  pop(/*Pie izquierdo*/);
  pop(/*Pierna izquierda*/);
}

void ljbot_arm( arm_angles &params_b, int isRightArm, vector3 * color1, vector3 * color2){
  int mod=1;
  push(/*Brazo izq*/);
  if (isRightArm) mod = -1;
  glTranslatef(0.0,3.0,0.0);
  glRotatef(params_b.arm_rot,0.0,1.0,0.0);
  glRotatef(params_b.arm_x*mod,0.0,0.0,1.0);
  glRotatef(params_b.arm_z,1.0,0.0,0.0);
  glTranslatef(0.0,1.0,0.0);
  push(/*Esc Brazo*/);
  glScalef(1.9,4.0,1.9);
  colorcube(color1, color2);
  pop(/*Esc Brazo*/);
  push(/*antebrazo*/);
  glTranslatef(0.0,2.0,0.0);
  glRotatef(params_b.elbow_rot,0.0,1.0,0.0);
  glRotatef(params_b.elbow_x,1.0,0.0,0.0);
  glTranslatef(0.0,2.0,0.0);
  push(/*Escala/rot antebr*/);
  glScalef(1.8,4.0,1.8);
  //glRotatef(180,0.0,0.0,1.0);
  colorcube(color1, color2);
  pop(/*Escala/rot antebr*/);
  push(/*Mano*/);
  glTranslatef(0.0, 2.0, 0.0);
  glRotatef(params_b.munieca_x,1.0,0.0,0.0);
  glTranslatef(0.0, 0.5, 0.0);
  glScalef(2.0,1.0,1.0);
  //glRotatef(180,0.0,0.0,1.0);
  colorcube(color1, color2);
  pop(/*Mano*/);
  pop(/*antebrazo*/);
  pop(/*Brazo izq*/);
}

void ljbot_set_angles(ljbot_angles * parameters, 
			   float c_x, float c_z, float c_r, 
			   float bi_x, float bi_z, float bi_r, float ci_x, float ci_r, float mi_x,
			   float bd_x, float bd_z, float bd_r, float cd_x, float cd_r, float md_x,
			   float tf_x, float tf_z, float t_r,
			   float pim_x, float pim_z, float pim_r, float ri, float pip_x, float pip_r,
			   float pdm_x, float pdm_z, float pdm_r, float rd, float pdp_x, float pdp_r, float anim_angle){
  parameters->head.neck_x = c_x;
  parameters->head.neck_z = c_z;
  parameters->head.neck_rot = c_r;
  parameters->b_left.arm_x = bi_x;
  parameters->b_left.arm_z = bi_z;
  parameters->b_left.arm_rot = bi_r;
  parameters->b_left.elbow_x = ci_x;
  parameters->b_left.elbow_rot = ci_r;
  parameters->b_left.munieca_x = mi_x;
  parameters->b_right.arm_x = bd_x;
  parameters->b_right.arm_z = bd_z;
  parameters->b_right.arm_rot = bd_r;
  parameters->b_right.elbow_x = cd_x;
  parameters->b_right.elbow_rot = cd_r;
  parameters->b_right.munieca_x = md_x;
  parameters->torax.flex_x = tf_x; 
  parameters->torax.flex_z = tf_z;
  parameters->torax.rot = t_r;
  parameters->left_l.thigh_x = pim_x;
  parameters->left_l.thigh_z = pim_z;
  parameters->left_l.thigh_rot = pim_r;
  parameters->left_l.knee = ri;
  parameters->left_l.foot_z = pip_x;
  parameters->left_l.foot_rot = pip_r;
  parameters->right_l.thigh_x = pdm_x;
  parameters->right_l.thigh_z = pdm_z;
  parameters->right_l.thigh_rot = pdm_r;
  parameters->right_l.knee = rd;
  parameters->right_l.foot_z = pdp_x;
  parameters->right_l.foot_rot = pdp_r;
}

void ljbot_base_pose(ljbot_angles * parameters) {
  ljbot_set_angles(parameters, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
}

void ljbot_anim(ljbot_angles * parameters, float t_elapsed, ljbot_action accion)
{
	
	float ang_mov;
	switch (accion)
	{
		case LJBOT_INACTIVE:
			
			parameters->anim_angle = 0;
		
		parameters->b_right.arm_x = 85;		//[-90 - 0 -  90]	[up - right - down]
		parameters->b_right.arm_z = 0;
		parameters->b_right.arm_rot = 0;
		
		parameters->b_left.elbow_x = 0;	//[-150 - -90  -  0]		[up - front - down]
		parameters->b_left.elbow_rot = 0;	//[]	[]
		parameters->b_left.arm_x = 85;
		parameters->torax.flex_z = 0;		//[ 20 - 0 - -60]	[back - up - front]
		
		
		
		parameters->head.neck_x = 0;		//[ -40 L   0   40 R ]	
		parameters->head.neck_z = 0;		//[ -45 F   0   45 B ]
		parameters->head.neck_rot = 0;		//[ -80 L   0   80 R ]
		
		
		parameters->b_left.arm_x = 80;		//[ -90   0    90  ]
		parameters->b_left.arm_z = 0;
		parameters->b_left.arm_rot = 0;
		parameters->b_left.elbow_x = 0;
		parameters->b_left.elbow_rot = 0;
		parameters->b_left.munieca_x = 0;
		
		
		parameters->b_right.arm_x = 80;
		parameters->b_right.arm_z = 0;
		parameters->b_right.arm_rot = 0;
		parameters->b_right.elbow_x = 0;
		parameters->b_right.elbow_rot = 0;
		parameters->b_right.munieca_x = 0;
		
		
		parameters->torax.flex_x = 0;
		parameters->torax.flex_z = 0;
		parameters->torax.rot = 0;
		
		
		parameters->left_l.thigh_x = 0;
		parameters->left_l.thigh_z = 0;
		parameters->left_l.thigh_rot = 0;
		parameters->left_l.knee = 0;
		parameters->left_l.foot_z = 0;
		parameters->left_l.foot_rot = 0;
		
		
		parameters->right_l.thigh_x = 0;
		parameters->right_l.thigh_z = 0;
		parameters->right_l.thigh_rot = 0;
		parameters->right_l.knee = 0;
		parameters->right_l.foot_z = 0;
		parameters->right_l.foot_rot = 0;
		
		
		parameters->b_right.arm_z = parameters->left_l.thigh_z = 0;
		parameters->b_left.arm_z = parameters->right_l.thigh_z = 0;
		
		
		
		break;
		case LJBOT_WALK:
			
			parameters->anim_angle += 80.0*t_elapsed;
		if (parameters->anim_angle >360) parameters->anim_angle -=360;
		ang_mov = 15*sin(deg_to_rad(parameters->anim_angle));		//arc mov angle


		parameters->b_left.elbow_x = 10;
		
		parameters->b_left.arm_x = 80;


		parameters->b_left.arm_z = 15;

		parameters->b_right.elbow_x = 10;
		
		
		parameters->b_right.arm_x = 80;

		
		
		parameters->b_right.arm_z = 15;
		
		parameters->torax.flex_z = 6;
		
		parameters->b_right.arm_z = parameters->right_l.thigh_z = ang_mov;
		parameters->b_left.arm_z = parameters->left_l.thigh_z = -ang_mov;
		
		break;
		case LJBOT_RUN:
			
			parameters->anim_angle += 680.0*t_elapsed;
		if (parameters->anim_angle >360) parameters->anim_angle -=360;
		ang_mov = 55.0*sin(deg_to_rad(parameters->anim_angle));		//arc mov angle
		
		parameters->b_left.elbow_x = 60;

		parameters->b_left.arm_x = 80;
		

		//parameters->b_left.arm_z = 40;
		
		

		parameters->b_right.elbow_x = 60;
		
		parameters->b_right.arm_x = 80;

		//parameters->b_right.arm_z = 40;
		
		parameters->torax.flex_z = 15;

		parameters->b_right.arm_z = parameters->right_l.thigh_z = ang_mov;
		parameters->b_left.arm_z = parameters->left_l.thigh_z = -ang_mov;
		
		
		break;
		case LJBOT_PUNCH:
			parameters->anim_angle += 154.0*t_elapsed;
			parameters->b_right.arm_z = -90;
			parameters->b_right.elbow_x = 0;
		
		break;
	}
}