#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		6

#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,NOJ);
	float *temp2;
	float tmp[NOJ];	
	char mesg[100];
	tau.zero();
	q.zero();
	dq.zero();

	printf("\nPUMA 560 Dynamic Engine Test\n\n\n");
	printf("\nJoint limits: \n");
	simRobot = new PrTreeRobot(NOJ,"./pumaarm.prm");

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

#if A_MATRIX
	printf("Enter joint angles in degrees (0-4)> \n");

	read(1,mesg,sizeof(mesg));
	sscanf(mesg,"%f %f %f %f %f ", &tmp[0], &tmp[1], &tmp[2], &tmp[3], &tmp[4]);
	printf("Enter direction vector xhat, yhat, zhat> \n");
	q[0] = tmp[0]/57.3;
	q[1] = tmp[1]/57.3;
	q[2] = tmp[2]/57.3;
	q[3] = tmp[3]/57.3;
	q[4] = tmp[4]/57.3;
	q[5] = 0.0;

	PrVector x(3);
	read(1,mesg,sizeof(mesg));
	sscanf(mesg,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);
	x[0] = tmp[0];
	x[1] = tmp[1];
	x[2] = tmp[2];

	robot = simRobot->robot();
	simRobot->jointPos(q);
	simRobot->jointVel(dq);
	g = simRobot->computedTorque(tau);

	// find normal direction of impact point (outward)
	// impact vector in [5] frame: [0 0 -1]
	PrMatrix3 N_R_2;
	PrVector3 normal, localNormal;
	localNormal[0] = 0.0; localNormal[1] = 1.0; localNormal[2] = 0.0;
	N_R_2 = robot->lastLink()->globalTransform().rotation().matrix();
	normal = N_R_2*localNormal;
	normal.display("Impact normal vector [global]");

	printf("\n \n");
	N_R_2.display("Rotation matrix");
	printf("\n \n");
	
// 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] = -140.0/57.3;
//	q[2] = 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 = 0;
	A.operator=(A_inv->operator~());

	A.display("A Matrix");

	// Calculate revised A matrix when considering mini effective inertia
	PrMatrix A2(NOJ,NOJ), A2_inv(NOJ,NOJ);

	A2 = A;
	A2.elementAt(0,0) = A.elementAt(0,0) + 62.61*62.61*0.0002;
	A2.elementAt(1,1) = A.elementAt(1,1) + 107.36*107.36*0.0002;
	A2.elementAt(2,2) = A.elementAt(2,2) + 53.69*53.69*0.0002;
	A2.elementAt(3,3) = A.elementAt(2,2) + 76.01*76.01*0.000033;
	A2.elementAt(4,4) = A.elementAt(2,2) + 71.91*71.91*0.000033;
	A2.elementAt(5,5) = A.elementAt(2,2) + 76.63*76.63*0.000033;

	printf("\n");
	A2.display("Modified A Matrix (with motor effective inertia");

	A2_inv = A2.operator~();

#if 1
	
	// calculate Jacobian; This only calculates up to the origin of the last link
	PrLink *link = robot->lastLink();

	PrVector3 &globalP = link->globalTransform().translation();
	while(link->parent() != NULL)
	{
		PrVector6 &Jj = link->globalJacobian(globalP);
		for(int j=0;j<3;j++)
			J->elementAt(j,link->id()) = Jj.elementAt(j);

//	printf("\n");
//		Jj.display();
		link = link->parent();
	}
	printf("\n");
	J->display("Jacobian");
#endif

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

	LambdaInv = (*J)*(*A_inv)*(J->transpose());

	LambdaInv.display("Lambda Inverse");

//  PrVector x(3);
//	x[0] = 1.0; x[1] = 0.0; x[2] = 0.0;	
//	x[0] = 1.0; x[1] = -1.0; x[2] = 0.0;	
//	x[0] = 1.0; x[1] = 1.0; x[2] = 0.0;	

	float mtemp, meff;

	PrVector temp1(3);
	temp1 = LambdaInv*x;
//
//	temp1.display();
	mtemp = x[0]*temp1[0] + x[1]*temp1[1] + x[2]*temp1[2]; 

	meff = 1.0/mtemp;

	printf("\n \n");
	printf("Joint angles: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f \n", 
				q[0]*57.3, q[1]*57.3,q[2]*57.3, q[3]*57.3, q[4]*57.3, q[5]*57.3);
	printf("For direction of: %3.1f %3.1f %3.1f \n", x[0], x[1], x[2]);
	printf("\n \n");
	printf("With no motor reflected inertia \n");
	printf("Effective mass is %f kg \n", meff);

	LambdaInv = (*J)*(A2_inv)*(J->transpose());

	LambdaInv.display("Lambda Inverse");

	temp1 = LambdaInv*x;

//	temp1.display();
	mtemp = x[0]*temp1[0] + x[1]*temp1[1] + x[2]*temp1[2]; 

	meff = 1.0/mtemp;

	printf("\n");
	printf("With mini motor reflected inertia \n");
	printf("Effective mass is %f kg \n", meff);

	PrVector normal_(3);
	normal_[0] = normal[0];
	normal_[1] = normal[1];
	normal_[2] = normal[2];
	
	LambdaInv = (*J)*(*A_inv)*(J->transpose());
	temp1 = LambdaInv*normal_;
	mtemp = normal[0]*temp1[0] + normal[1]*temp1[1] + normal[2]*temp1[2]; 

	meff = 1.0/mtemp;

	printf("\n");
	printf("Joint angles: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f \n", 
				q[0]*57.3, q[1]*57.3,q[2]*57.3, q[3]*57.3, q[4]*57.3, q[5]*57.3);
	printf("For direction of: %4.2f %4.2f %4.2f \n", normal[0], normal[1], normal[2]);
	printf("\n \n");
	printf("With no motor reflected inertia \n");
	printf("Effective mass is %f kg \n", meff);

	LambdaInv = (*J)*(A2_inv)*(J->transpose());

//	LambdaInv.display("Lambda Inverse");

	temp1 = LambdaInv*normal_;

//	temp1.display();
	mtemp = normal[0]*temp1[0] + normal[1]*temp1[1] + normal[2]*temp1[2]; 

	meff = 1.0/mtemp;

	printf("\n");
	printf("With mini motor reflected inertia \n");
	printf("Effective mass is %f kg \n", meff);

	PrVector3 loc;
	loc = robot->object()->globalTransform().translation();

	printf("\n\n");
	loc.display("Origin of [5]");
	printf("\n");
#endif


}
#endif
