/*
 * Biped character class
 *
 * Copyright (C) 2008  Hugo Mendieta Pacheco & Alejandro Valenzuela Roca
 *
 * scherezar_crusade@hotmail.com, lanjoe9@mexinetica.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 "biped-class.h"

//FIXME: make a version for gl and another for Wii

#ifdef NINTENDO_DS

void mjBiped::SetDef(mjBipedDef * def)
{
	iDef=def;
}

void mjBiped::SetAng(mjBipedAngles * ang)
{
	iAng=ang;
}


void mjBiped::SetAngles(
			   float h_x, float h_z, float h_r,
			   float ra_x, float ra_z, float ra_r, float la_x, float la_r, float la_z,
			   float re_x, float re_r, float le_x, float le_r,
			   float rf_z, float rf_r, float lf_z, float lf_r,
			   float lk_z, float rk_z, float lw_x, float rw_x,
			   float rt_x, float rt_z, float rt_r, float lt_x, float lt_z, float lt_r,
			   float tf_x, float tf_z, float t_r){

	iAng->head_x = h_x;
	iAng->head_z = h_z;
	iAng->head_rot = h_r;
	iAng->rightArm_x = ra_x;
	iAng->rightArm_z = ra_z;
	iAng->rightArm_rot = ra_r;
	iAng->leftArm_x = la_x;
	iAng->leftArm_z = la_z;
	iAng->leftArm_rot = la_r;
	iAng->rightElbow_x = re_x;
	iAng->rightElbow_rot = re_r;
	iAng->leftElbow_x = le_x;
	iAng->leftElbow_rot = le_r;
	iAng->rightFoot_z = rf_z;
	iAng->rightFoot_rot = rf_r;
	iAng->leftFoot_z = lf_z;
	iAng->leftFoot_rot = lf_r;
	iAng->rightKnee_z = rk_z;
	iAng->leftKnee_z = lk_z;
	iAng->rightWrist_x = rw_x;
	iAng->leftWrist_x = lw_x;
	iAng->rightThigh_x = rt_x;
	iAng->rightThigh_z = rt_z;
	iAng->rightThigh_rot = rt_r;
	iAng->leftThigh_x = lt_x;
	iAng->leftThigh_z = lt_z;
	iAng->leftThigh_rot = lt_r;
	iAng->toraxFlex_x = tf_x;
	iAng->toraxFlex_z = tf_z;
	iAng->torax_rot = t_r;




}


void mjBiped::Animate(mjBipedUpperAction upperAction,mjBipedLowerAction lowerAction, float t_elapsed)
{
	static float ang_mov;
	switch(upperAction){
		case BIPEDU_INACTIVE:
			iAng->head_x = 0;
			iAng->head_z = 0;
			iAng->head_rot = 0;
			iAng->rightArm_x = 85;
			iAng->rightArm_z = 0;
			iAng->rightArm_rot = 0;
			iAng->leftArm_x = 85;
			iAng->leftArm_z = 0;
			iAng->leftArm_rot = 0;
			iAng->rightElbow_x = 0;
			iAng->rightElbow_rot = 0;
			iAng->leftElbow_x = 0;
			iAng->leftElbow_rot = 0;
			iAng->rightWrist_x = 0;
			iAng->leftWrist_x = 0;
			iAng->toraxFlex_x = 0;
			iAng->toraxFlex_z = 0;
			iAng->torax_rot = 0;
		break;
		case BIPEDU_WALK:

			iAng->upperFrame += 0.8 * t_elapsed;
			if (iAng->upperFrame >360) iAng->upperFrame -=360;
				ang_mov = 20.0*sin(deg_to_rad(iAng->upperFrame));		//arc mov angle


			iAng->leftArm_x = 85;
			iAng->rightArm_x = 85;

			iAng->leftElbow_x = 15;
			iAng->rightElbow_x = -15;

			iAng->toraxFlex_z = -6;

			iAng->leftArm_z=ang_mov;
			iAng->rightArm_z=-ang_mov;


		break;
		case BIPEDU_RUN:

			iAng->upperFrame += 1.8*t_elapsed;
			if (iAng->upperFrame >360) iAng->upperFrame -=360;
			ang_mov = 60.0*sin(deg_to_rad(iAng->upperFrame));		//arc mov angle

			iAng->leftArm_x = 80;
			iAng->rightArm_x = 80;

			iAng->leftElbow_x = 35;
			iAng->rightElbow_x = -35;

			iAng->rightArm_z = 90;
			iAng->leftArm_z = 90;

			iAng->toraxFlex_z = -10;

			iAng->leftArm_z=ang_mov;
			iAng->rightArm_z=-ang_mov;

		break;

		case BIPEDU_JMP_FALL:

			iAng->upperFrame += 0.8*t_elapsed;
			if (iAng->upperFrame >360) iAng->upperFrame -=360;
			ang_mov = 60*sin(deg_to_rad(iAng->upperFrame));		//arc mov angle

			iAng->leftArm_x = -70;
			iAng->rightArm_x = -70;

			iAng->leftElbow_x = 35;
			iAng->rightElbow_x = -35;

			iAng->rightArm_z = 90;
			iAng->leftArm_z = 90;

//			iAng->toraxFlex_z = 10;

			iAng->leftArm_z=ang_mov;
			iAng->rightArm_z=-ang_mov;


		break;

		case BIPEDU_PUNCH:
			iAng->upperFrame+= 1.24*t_elapsed;

			ang_mov = iAng->upperFrame*2;

			if (ang_mov >= 95.0)
			{
				ang_mov = 95.0;
				//printf("HA: %f\n", ang_mov);
			}
			iAng->rightArm_z = ang_mov;
			iAng->rightArm_x = ang_mov;
			iAng->rightArm_rot = 0;
			iAng->rightElbow_x = 5;
		break;
	}

	switch(lowerAction){
		case BIPEDL_INACTIVE:
			iAng->rightFoot_z = 0;
			iAng->rightFoot_rot = 0;
			iAng->leftFoot_z = 0;
			iAng->leftFoot_rot = 0;
			iAng->rightKnee_z = 0;
			iAng->leftKnee_z = 0;
			iAng->rightThigh_x = 0;
			iAng->rightThigh_z = 0;
			iAng->rightThigh_rot = 0;
			iAng->leftThigh_x = 0;
			iAng->leftThigh_z = 0;
			iAng->leftThigh_rot = 0;
		break;
		case BIPEDL_WALK:

			iAng->lowerFrame += 0.8*t_elapsed;
			if (iAng->lowerFrame >360) iAng->lowerFrame -=360;
			ang_mov = 15*sin(deg_to_rad(iAng->lowerFrame));		//arc mov angle

			iAng->leftThigh_z = -15;
			iAng->rightThigh_z = 15;

			iAng->leftKnee_z = -15;
			iAng->rightKnee_z = 15;

			iAng->rightThigh_z=ang_mov;
			iAng->leftThigh_z=-ang_mov;

		break;
		case BIPEDL_RUN:

			iAng->lowerFrame += 1.8*t_elapsed;
			if (iAng->lowerFrame >360) iAng->lowerFrame -=360;
			ang_mov = 40*sin(deg_to_rad(iAng->lowerFrame));		//arc mov angle

			iAng->leftThigh_z = -35;
			iAng->rightThigh_z = 35;

			iAng->leftKnee_z = -25;
			iAng->rightKnee_z = 25;

			iAng->rightThigh_z=ang_mov;
			iAng->leftThigh_z=-ang_mov;

		break;
		case BIPEDL_JMP_FALL:

			iAng->lowerFrame += 0.8*t_elapsed;
			if (iAng->lowerFrame >360) iAng->lowerFrame -=360;
			ang_mov = -15*sin(deg_to_rad(iAng->lowerFrame));		//arc mov angle

			iAng->leftThigh_z = -15;
			iAng->rightThigh_z = 15;

			iAng->leftKnee_z = -15;
			iAng->rightKnee_z = 15;

			iAng->rightThigh_z=ang_mov;
			iAng->leftThigh_z=-ang_mov;

		break;
	}
}

void mjBiped::Draw()
{


	//glScalef(10,10,10);

	push(/**/);
		//glRotatef(180,0,1,0);

		//push(/*Tronco*/);
			glTranslatef(iDef->waistPos.x, iDef->waistPos.y, iDef->waistPos.z);
			//glTranslatef(0.0, 10.0, 0.0);	///para que quede centrado en el origen en la pantalla

  			/* pelvis */
			glCallList(iDef->DLWaist);

			push(/*piernas*/);
			  push(/*left_l*/);

					glTranslatef(iDef->leftThighPos.x,iDef->leftThighPos.y,iDef->leftThighPos.z);
					glRotatef(iAng->leftThigh_rot,0.0,1.0,0.0);
					glRotatef(iAng->leftThigh_x,0.0,0.0,1.0);
					glRotatef(iAng->leftThigh_z,1.0,0.0,0.0);
					glCallList(iDef->DLLeftThigh);

					glTranslatef(iDef->leftKneePos.x,iDef->leftKneePos.y,iDef->leftKneePos.z);
					glCallList(iDef->DLLeftKnee);

					glTranslatef(iDef->leftFootPos.x,iDef->leftFootPos.y,iDef->leftFootPos.z);

					glRotatef(iAng->leftFoot_rot,0.0,1.0,0.0);
					glRotatef(iAng->leftFoot_z,1.0,0.0,0.0);
					glCallList(iDef->DLLeftFoot);

				pop(/*left_l*/);

  			//push(/*right_l*/);

					glTranslatef(iDef->rightThighPos.x,iDef->rightThighPos.y,iDef->rightThighPos.z);
					glRotatef(iAng->rightThigh_rot,0.0,1.0,0.0);
					glRotatef(iAng->rightThigh_x,0.0,0.0,1.0);
					glRotatef(iAng->rightThigh_z,1.0,0.0,0.0);
					glCallList(iDef->DLRightThigh);

					glTranslatef(iDef->rightKneePos.x,iDef->rightKneePos.y,iDef->rightKneePos.z);
					glCallList(iDef->DLRightKnee);

					glTranslatef(iDef->rightFootPos.x,iDef->rightFootPos.y,iDef->rightFootPos.z);

					glRotatef(iAng->rightFoot_rot,0.0,1.0,0.0);
					glRotatef(iAng->rightFoot_z,1.0,0.0,0.0);
					glCallList(iDef->DLRightFoot);

				//pop(/*right_l*/);
			pop(/*piernas*/);

			//push(/* Tronco sup */);
				glTranslatef(iDef->toraxPos.x,iDef->toraxPos.y,iDef->toraxPos.z);
				glRotatef(iAng->torax_rot,0.0,1.0,0.0);
				glRotatef(iAng->toraxFlex_x,0.0,0.0,1.0);
				glRotatef(iAng->toraxFlex_z,1.0,0.0,0.0);

					glCallList(iDef->DLTorax);

				push(/*Head*/);
				  glTranslatef(iDef->headPos.x,iDef->headPos.y,iDef->headPos.z);
					glRotatef(iAng->head_rot/2,0.0,1.0,0.0);
					glRotatef(iAng->head_x,0.0,0.0,1.0);
					glRotatef(iAng->head_z,1.0,0.0,0.0);
					glCallList(iDef->DLHead);
				pop(/*Head*/);

  			//push(/*Brazos*/);
		  		push(/*arm_left*/);
						glTranslatef(iDef->leftArmPos.x,iDef->leftArmPos.y,iDef->leftArmPos.z);
						glRotatef(iAng->leftArm_x,0.0,0.0,1.0);
						glRotatef(iAng->leftArm_z,0.0,1.0,0.0);

						//glRotatef(iAng->leftArm_rot,1.0,0.0,0.0);
/*						glRotatef(iAng->leftArm_rot,0.0,1.0,0.0);
						glRotatef(iAng->leftArm_x,0.0,0.0,1.0);
						glRotatef(iAng->leftArm_z,1.0,0.0,0.0);
*/
						glCallList(iDef->DLLeftArm);

						//push(/*Antebrazo*/);
							glTranslatef(iDef->leftElbowPos.x,iDef->leftElbowPos.y,iDef->leftElbowPos.z);
							glRotatef(iAng->leftElbow_rot,0.0,1.0,0.0);
							glRotatef(iAng->leftElbow_x,1.0,0.0,0.0);
							glCallList(iDef->DLLeftElbow);

							//push(/*Mano*/);
								glTranslatef(iDef->leftWristPos.x,iDef->leftWristPos.y,iDef->leftWristPos.z);
								glRotatef(iAng->leftWrist_x,1.0,0.0,0.0);
								glCallList(iDef->DLLeftWrist);

							//pop(/*Mano*/);
						//pop(/*Antebrazo*/);
		  		pop(/*arm_left*/);

					//push(/*arm_right*/);
						glTranslatef(iDef->rightArmPos.x,iDef->rightArmPos.y,iDef->rightArmPos.z);
						glRotatef(iAng->rightArm_x,0.0,0.0,-1.0);
						glRotatef(iAng->rightArm_z,0.0,-1.0,0.0);
						//glRotatef(iAng->rightArm_rot,1.0,0.0,0.0);
/*
						glRotatef(iAng->rightArm_rot,0.0,1.0,0.0);
						glRotatef(iAng->rightArm_x,0.0,0.0,1.0);
						glRotatef(iAng->rightArm_z,1.0,0.0,0.0);
*/
						glCallList(iDef->DLRightArm);

						//push(/*Antebrazo*/);
							glTranslatef(iDef->rightElbowPos.x,iDef->rightElbowPos.y,iDef->rightElbowPos.z);
							glRotatef(iAng->rightElbow_rot,0.0,1.0,0.0);
							glRotatef(iAng->rightElbow_x,1.0,0.0,0.0);
							glCallList(iDef->DLRightElbow);

							//push(/*Mano*/);
								glTranslatef(iDef->rightWristPos.x,iDef->rightWristPos.y,iDef->rightWristPos.z);
								glRotatef(iAng->rightWrist_x,1.0,0.0,0.0);
								glCallList(iDef->DLRightWrist);

							//pop(/*Mano*/);
						//pop(/*Antebrazo*/);
					//pop(/*arm_right*/);
  			//pop(/*Brazos*/);

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

}

void mjBiped::SetUpperFrame(float upperFrame)
{
	iAng->upperFrame=upperFrame;
}

void mjBiped::SetLowerFrame(float lowerFrame)
{
	iAng->lowerFrame=lowerFrame;
}

#endif