#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);
	PrMatrix *J;
	J = new PrMatrix(3,3);
	float *temp2;
	float tmp[NOJ];	
	char mesg[100];
	FILE *f1 = fopen("qdata.txt","r");
	FILE *f2 = fopen("lambda.txt","w");

	tau.zero();
	q.zero();
	dq.zero();

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

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

	int readFlag = 0;
	float tmpq[3];

	while(readFlag != -1)
	{
		readFlag = fscanf(f1,"%f %f %f\n", &tmpq[0], &tmpq[1], &tmpq[2]);

		for(i=0;i<3;i++)
			q[i] = tmpq[i]/57.3;

//		printf("q: %f, %f, %f \n", q[0],q[1],q[2]);


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

	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 = 0;
	A.operator=(A_inv->operator~());

//	A.display("A Matrix");

	// location of effective mass point
	float qt[3];
	static const float eex = 0.36328 + 0.03375, eey = -0.03889, eez = 0.0;	// with JR3 mounted on EE
//	static const float eex = 0.16328 + 0.03375, eey = -0.03889, eez = 0.0;	// with JR3 mounted on EE
	static const float d3x = 0.449, d3y = -0.012;	// origin of frame 3 from origin of frame 2
	qt[0] = q[0]; qt[1] = q[1]; qt[2] = q[2];

	//first column
	J->elementAt(0,0) = - sin(qt[0])*cos(qt[1]+qt[2])*eex 
						+ sin(qt[0])*sin(qt[1]+qt[2])*eey
						- cos(qt[0])*eez
						- sin(qt[0])*cos(qt[1])*d3x
						+ sin(qt[0])*sin(qt[1])*d3y;
	J->elementAt(1,0) = 0.0;
	J->elementAt(2,0) = cos(qt[0])*cos(qt[1]+qt[2])*eex
						- cos(qt[0])*sin(qt[1]+qt[2])*eey
						- sin(qt[0])*eez
						+ cos(qt[0])*cos(qt[1])*d3x
						- cos(qt[0])*sin(qt[1])*d3y;
	//second column
	J->elementAt(0,1) = - cos(qt[0])*sin(qt[1]+qt[2])*eex
		   				- cos(qt[0])*cos(qt[1]+qt[2])*eey
						- cos(qt[0])*sin(qt[1])*d3x
						- cos(qt[0])*cos(qt[1])*d3y;

	J->elementAt(1,1) = cos(qt[1]+qt[2])*eex
						- sin(qt[1]+qt[2])*eey
						+ cos(qt[1])*d3x
						- sin(qt[1])*d3y;

	J->elementAt(2,1) = - sin(qt[0])*sin(qt[1]+qt[2])*eex
						- sin(qt[0])*cos(qt[1]+qt[2])*eey
						- sin(qt[0])*sin(qt[1])*d3x
						- sin(qt[0])*cos(qt[1])*d3y;

	//third column
	J->elementAt(0,2) = - cos(qt[0])*sin(qt[1]+qt[2])*eex
						- cos(qt[0])*cos(qt[1]+qt[2])*eey;

	J->elementAt(1,2) = cos(qt[1]+qt[2])*eex
						- sin(qt[1]+qt[2])*eey;

	J->elementAt(2,2) = - sin(qt[0])*sin(qt[1]+qt[2])*eex
						- sin(qt[0])*cos(qt[1]+qt[2])*eey;	

//	J->display("Jacobian");

	PrMatrix Lambda(3,3), LambdaInv(3,3);

	LambdaInv = (*J)*(*A_inv)*(J->transpose());
	Lambda = LambdaInv.operator~();
//	Lambda.display("Lambda");

		fprintf(f2,"%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f \n",
					Lambda.elementAt(0,0), 
					Lambda.elementAt(0,1), 
					Lambda.elementAt(0,2), 
					Lambda.elementAt(1,1), 
					Lambda.elementAt(1,2), 
					Lambda.elementAt(2,2));
	}

	fclose(f1);
	fclose(f2);

}
#endif
