#include <stdio.h>
#include <string.h>
#include <stdlib.h>


#include "PrGlobalDefn.h"
#include "matrix/PrMatrix.h"
#include "matrix/PrVector.h"
#include "matrix/PrVector6.h"
#include "matrix/PrVector3.h"
#include "matrix/PrTransform.h"
#include "matrix/PrQuaternion.h"
#include "robot/PrRobot.h"
#include "robot/PrLink.h"
#include "robot/PrJointIntegrator.h"
#include "robot/PrJointState.h"
#include "PrTreeRobot.h"

#include <unistd.h>
#include <sys/sched.h>

#if 1
PrRobot* robot;
PrTreeRobot *simRobot;

#define NOJ		3

#define GRAVITY_VECTOR	0
#define ROTATION_MATRIX	0
#define A_MATRIX		1

void main(void)
{
	int i;
	PrVector tau(NOJ), temp(NOJ),q(NOJ), dq(NOJ),g(NOJ);
	float *temp2;
	float tmp[NOJ];	
	char mesg[100];
	tau.zero();
	q.zero();
	dq.zero();

	printf("\nSEA 3DOF Dynamic Engine Test\n\n\n");
	printf("\nJoint limits: \n");
	simRobot = new PrTreeRobot(NOJ,"./sea3dof_right1KgEEMass.prm");
//	simRobot = new PrTreeRobot(NOJ,"./sea3dof_right.prm");

	printf("Number of joints: %d\n", simRobot->noj());

/*
	printf("Enter joint angles in degrees> \n");

	read(1,mesg,sizeof(mesg));
	sscanf(mesg,"%f %f %f %f", &tmp[0], &tmp[1], &tmp[2]);
	q[0] = tmp[0]/57.3;
	q[1] = tmp[1]/57.3;
	q[2] = tmp[2]/57.3;
	
	simRobot->jointPos(q);
	simRobot->jointVel(dq);
	g = simRobot->computedTorque(tau);

	for(i=0;i<NOJ;i++)
		printf("Joint %d: %5.4f degrees  \n", i, q[i]*57.3);

	printf("\nGravity compensation torques: \n");
//	for(i=0;i<NOJ;i++)
//		printf("Joint %d: %5.4f N.m \n", i, g[i]);
	g.display("Gravity vector");
	printf("\n");
*/

#if GRAVITY_VECTOR
	printf("GRAVITY VECTORS \n");

	q[0] = -10.0/57.3;
	q[1] = -10.0/57.3;
	q[2] = 5.0/57.3;
	for(int i =0;i<10;i++)
	{
		q[1]+= 10.0/57.3;
		simRobot->jointPos(q);
		simRobot->jointVel(dq);
		g = simRobot->computedTorque(tau);
		printf("[%5.2f, %5.2f, %5.2f]: %7.4f, %7.4f %7.4f \n", 
						q[0]*57.3, q[1]*57.3, q[2]*57.3, g[0], g[1], g[2]);
	}

	printf("\n");
	q[0] = -75.33/57.3;
	q[1] = -10.0/57.3;
	q[2] = 7.55/57.3;
	for(int i =0;i<10;i++)
	{
		q[1]+= 10.0/57.3;
		simRobot->jointPos(q);
		simRobot->jointVel(dq);
		g = simRobot->computedTorque(tau);
		printf("[%5.2f, %5.2f, %5.2f]: %7.4f, %7.4f %7.4f \n", 
						q[0]*57.3, q[1]*57.3, q[2]*57.3, g[0], g[1], g[2]);
	}
	printf("\n");
	q[0] = -75.33/57.3;
	q[1] = 2.68/57.3;
	q[2] = -10.0/57.3;
	for(int i =0;i<10;i++)
	{
		q[2]+= 10.0/57.3;
		simRobot->jointPos(q);
		simRobot->jointVel(dq);
		g = simRobot->computedTorque(tau);
		printf("[%5.2f, %5.2f, %5.2f]: %7.4f, %7.4f %7.4f \n", 
						q[0]*57.3, q[1]*57.3, q[2]*57.3, g[0], g[1], g[2]);
	}
#endif


#if ROTATION_MATRIX
//	TESTING ROTATION MATRICES
	PrMatrix3 N_R_1, N_R_2, N_R_3;

	PrRobot *robot = simRobot->robot();

	N_R_1 = robot->link(0)->globalTransform().rotation().matrix();
	N_R_2 = robot->link(1)->globalTransform().rotation().matrix();
	N_R_3 = robot->link(2)->globalTransform().rotation().matrix();

	PrVector3 x;

	x = robot->object()->globalTransform().translation();
	printf("x: %f, y: %f, z: %f\n", x[0], x[1], x[2]);

	printf("Rotation matrix N->1: \n");

	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
				printf("%f ", N_R_1.elementAt(i,j));
		printf("\n");
	}

	printf("\n");
	printf("Rotation matrix N->2: \n");

	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
				printf("%f ", N_R_2.elementAt(i,j));
		printf("\n");
	}

	printf("\n");
	printf("Rotation matrix N->3: \n");

	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
				printf("%f ", N_R_3.elementAt(i,j));
		printf("\n");
	}
#endif	

#if A_MATRIX
	printf("Enter joint angles in degrees> \n");
/*
	read(1,mesg,sizeof(mesg));
	sscanf(mesg,"%f %f %f %f", &tmp[0], &tmp[1], &tmp[2]);
	q[0] = tmp[0]/57.3;
	q[1] = tmp[1]/57.3;
	q[2] = tmp[2]/57.3;
	robot = simRobot->robot();
	simRobot->jointPos(q);
	simRobot->jointVel(dq);
	g = simRobot->computedTorque(tau);
*/
// calculating inverse of mass matrix
	PrMatrix *A_inv;
	PrMatrix A = PrMatrix(NOJ,NOJ);

	// WAIST INERTIA CALCULATION
	// KEEP [WA] and {EL] constant while varying [SH]
	
	q[0] = -70/57.3;
	q[1] = -150.0/57.3;
	q[2] = 00.0/57.3;
//	q[2] = 0.0/57.3;

	int varyingIndex = 1;
	float increment = 10.0;
	for(int l = 0; l<25; l++)
	{
		q[varyingIndex] += increment/57.3;
		q[2] += increment/57.3;
//		q[0] += (70.0/25.0)/57.3;
//		q[1] += 10.0/57.3;
	robot = simRobot->robot();
	simRobot->jointPos(q);
	simRobot->jointVel(dq);
	g = simRobot->computedTorque(tau);
	int j,k=0,l;

	A_inv = new PrMatrix(NOJ,NOJ);
	for(i=0;i<NOJ;i++)
	{
		// set torque to the desired form
		temp.zero();
		temp[i] = 1.0;
		temp += g;

		// write torque values to controller
		simRobot->torque(temp);
		// do forward dynamics calc to get acceleration
		robot->fwdDynamics();
		// get acceleration info
		temp2 = robot->jointAcceleration();
		// put accel. info into matrix form
		for(j=0;j<NOJ;j++)
			A_inv->elementAt(j,k) = temp2[j];
		k++;
	}

	int jointIndex = 2;
	A.operator=(A_inv->operator~());
	printf("q = [%6.2f, %6.2f, %6.2f]: %6.4f \n", q[0]*57.3, q[1]*57.3, q[2]*57.3,
					A.elementAt(jointIndex,jointIndex));
//	printf("G: %f, %f, %f \n", g[0], g[1], g[2]);
//	A.display("A matrix");
	}


#endif


}
#endif
