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


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

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

#if 1
PrRobot* robot;
PrTreeRobot *simRobot;

#define NOJ		5

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

	printf("\nAsimo Arm Dynamic Controller Test\n\n\n");
	printf("\nJoint limits: \n");
	simRobot = new PrTreeRobot(NOJ,"./asimo.prm");

	printf("Enter joint angles in degrees> \n");
	read(1,mesg,sizeof(mesg));
	sscanf(mesg,"%f %f %f %f", &tmp[0], &tmp[1], &tmp[2], &tmp[3]);
	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] = 0.0/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");

	robot = simRobot->robot();

	// calculating inverse of mass matrix
//	float A_inv[NOJ][NOJ];
	PrMatrix *A_inv;
	PrMatrix A = PrMatrix(NOJ,NOJ);

	int j,k=0,l;

	A_inv = new PrMatrix(NOJ,NOJ);
	//A = 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++)
		{

	//		printf("%f ", temp2[j]);
			A_inv->elementAt(j,k) = temp2[j];
		}
//	printf("\n");
		k++;
	}

/*
	float a[5];
	for(i=0;i<5;i++)
		a[i] = A_inv->elementAt(i,i);
	A_inv->zero();
	for(i=0;i<5;i++)
		A_inv->elementAt(i,i) = a[i];
*/

	printf("\n");
	A_inv->display("A inverse");
	A.operator=(A_inv->operator~());
	// display A matrix
/*	printf("A: \n");
	printf("[\n ");
	for(i=0;i<NOJ;i++)
	{
		for(j=0;j<NOJ;j++)
			printf("%8f ",A.elementAt(i,j));

		printf(";\n");
	}
	printf(" ]\n");
*/
//	q.display("q:");
	printf("\n");
	for(i=0;i<5;i++)
		printf("q[%d]: %f, \n", i, q[i]*57.3);
	printf("\n");
	A.display("A matrix:");

/*
	A.elementAt(0,3) = 0.0;

	(*A_inv) = A.operator~();

	A_inv->display("A_inv:");
	A.operator=(A_inv->operator~());
	A.display("A:");
//	A.operator=(A_inv->operator~());
*/



}
#endif
