/* NumericInvKin.C 
 * Numeric Inverse Kinematics
 *  v = J.theta-dot 
 *  JT.v = JT.J.theta-dot
 *  (JT.J)inv.JT.v = theta-dot*/

#include "glut.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "matrix.h"

#define  PI  3.1415926535
#define  TRUE 1
#define  FALSE 0
#define  EPSILON  0.001
#define  RAND  ((rand()*1.0)/(RAND_MAX))

typedef struct xyz_struct {
	float	x,y,z;
} xyz_td;

typedef struct rgb_struct {
	float	r,g,b;
} rgb_td;

void display_numericinvkin();

float	theta1,theta2,theta3,theta1Rad,theta2Rad,theta3Rad;
xyz_td	goal;
float	L1,L2,L3;

extern void	InvrsMatrix(float **,int);

// ---------------------------------------------------------------------
// INITIALIZE NUMERICINVKIN
// ---------------------------------------------------------------------
void initialize_numericinvkin(float gx,float gy,float gz,float t1,float t2,float t3)
{

	goal.x = gx; goal.y = gy; goal.z = gz;
	
	theta1 = t1; theta1Rad = theta1*PI/180;
	theta2 = t2; theta2Rad = theta2*PI/180;
	theta3 = t3; theta3Rad = theta3*PI/180;
	
	L1 = 5.0;
	L2 = 5.0;
	L3 = 5.0;
	
}

// ---------------------------------------------------------------------
// REINITIALIZE NUMERICINVKIN
// ---------------------------------------------------------------------
void reinitialize_numericinvkin()
{
}

// ---------------------------------------------------------------------
// SET GOAL
// ---------------------------------------------------------------------
void set_goal(float x,float y,float z)
{
	xyz_td	joint0,joint1,joint2,endeffector;
	xyz_td	v,v0,v1,v2,axis;
	float	len0,len1,len2;
	float	**J,**JT,**JTJ,**JTJinv;
	int		i,j,k,n,m;
	float	step_size = 1.0;
	float 	vv[3],vvv[3],thetaDot[3],len;
	float	**A;
	int		*rowswaps,val;
	float	*b;

	// J is 2x3	
	J = (float **)malloc(sizeof(float *)*2);
	for (i=0; i<2; i++) J[i] = (float *)malloc(sizeof(float)*3);
	// JT is 3x2
	JT = (float **)malloc(sizeof(float *)*3);
	for (i=0; i<3; i++) JT[i] = (float *)malloc(sizeof(float)*2);
	// JTJ is 3x3
	JTJ = (float **)malloc(sizeof(float *)*3);
	for (i=0; i<3; i++) JTJ[i] = (float *)malloc(sizeof(float)*3);
	// JTJ inverse is 3x3
	JTJinv = (float **)malloc(sizeof(float *)*3);
	for (i=0; i<3; i++) JTJinv[i] = (float *)malloc(sizeof(float)*3);
	
	goal.x = x;
	goal.y = y;
	goal.z = z;
	
	joint0.x = 0.0; joint0.y = 0.0; joint0.z = 0.0;
	
	joint1.x = -L1*sin(theta1Rad); joint1.y = L1*cos(theta1Rad); joint1.z = 0.0;
	
	joint2.x = -L1*sin(theta1Rad)-L2*sin(theta1Rad+theta2Rad);
	joint2.y = L1*cos(theta1Rad)+L2*cos(theta1Rad+theta2Rad);
	joint2.z = 0.0;
	
	endeffector.x = -L1*sin(theta1Rad)-L2*sin(theta1Rad+theta2Rad)-L3*sin(theta1Rad+theta2Rad+theta3Rad);
	endeffector.y = L1*cos(theta1Rad)+L2*cos(theta1Rad+theta2Rad)+L3*cos(theta1Rad+theta2Rad+theta3Rad);
	endeffector.z = 0.0;
	
	// printf("joint0: %f %f %f\n",joint0.x,joint0.y,joint0.z);
	// printf("joint1: %f %f %f\n",joint1.x,joint1.y,joint1.z);
	// printf("joint2: %f %f %f\n",joint2.x,joint2.y,joint2.z);
	printf("ef: %f %f %f\n",endeffector.x,endeffector.y,endeffector.z);

	v0.x = endeffector.x - joint0.x;
	v0.y = endeffector.y - joint0.y;
	v0.z = endeffector.z - joint0.z;
	len0 = sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
	
	v1.x = endeffector.x - joint1.x;
	v1.y = endeffector.y - joint1.y;
	v1.z = endeffector.z - joint1.z;
	len1 = sqrt(v1.x*v1.x+v1.y*v1.y+v1.z*v1.z);
	
	v2.x = endeffector.x - joint2.x;
	v2.y = endeffector.y - joint2.y;
	v2.z = endeffector.z - joint2.z;
	len2 = sqrt(v2.x*v2.x+v2.y*v2.y+v2.z*v2.z);
	
	if (len0>len1) len = len0; else len = len1;
	if (len2>len) len = len2;
	
	v0.x /= len; v0.y /= len; v0.z /= len;
	v1.x /= len; v1.y /= len; v1.z /= len;
	v2.x /= len; v2.y /= len; v2.z /= len;
	
	v.x = goal.x - endeffector.x;
	v.y = goal.y - endeffector.y;
	v.z = goal.z - endeffector.z;
	len = sqrt(v.x*v.x+v.y*v.y+v.z*v.z);
	vv[0] = v.x*step_size/len;
	vv[1] = v.y*step_size/len;
	vv[2] = v.z*step_size/len;
	
	axis.x = 0.0; axis.y = 0.0; axis.z = 1.0;
	
	// printf("v0: %f %f %f\n",v0.x,v0.y,v0.z);
	// printf("v1: %f %f %f\n",v1.x,v1.y,v1.z);
	// printf("v2: %f %f %f\n",v2.x,v2.y,v2.z);
	// printf("axis: %f %f %f\n",axis.x,axis.y,axis.z);
	printf("v: %f %f\n",vv[0],vv[1]);
	printf("\n");
	
	n=3; m=2;

	// J is d-v/d-theta-dot, 2x3
	// form column by column
	J[0][0] = axis.y*v0.z - axis.z*v0.y;
	J[1][0] = axis.z*v0.x - axis.x*v0.z;
	// J[0][2] = axis.x*v0.y - axis.y*v0.x;
	
	J[0][1] = axis.y*v1.z - axis.z*v1.y;
	J[1][1] = axis.z*v1.x - axis.x*v1.z;
	// J[1][2] = axis.x*v1.y - axis.y*v1.x;
	
	J[0][2] = axis.y*v2.z - axis.z*v2.y;
	J[1][2] = axis.z*v2.x - axis.x*v2.z;
	// J[2][2] = axis.x*v2.y - axis.y*v2.x;

	printf("\n");
	printf("J\n");
	matrixNxM_print(J,m,n);
	printf("\n");


	// JT is 3x2
	JT[0][0] = J[0][0]; JT[0][1] = J[1][0];
	JT[1][0] = J[0][1]; JT[1][1] = J[1][1];
	JT[2][0] = J[0][2]; JT[2][1] = J[1][2];
	
	printf("\n");
	printf("JT\n");
	matrixNxM_print(JT,n,m);
	printf("\n");

	// form JT*J, 3x3
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			JTJ[i][j] = 0.0;
			for (k=0; k<m; k++) JTJ[i][j] += JT[i][k]*J[k][j];
		}
	}

	printf("\n");
	printf("JTJ\n");
	matrixNxM_print(JTJ,n,n);
	// len = matrix3x3_determinant(JTJ);
	// printf("determinant: %f\n",len);
	printf("\n");
	
	// copy JTJ into A
	A = (float **)malloc(sizeof(float *)*n);
	for (i=0; i<n; i++) A[i] = (float *)malloc(sizeof(float)*n);

	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			A[i][j] = JTJ[i][j];
		}
	}

	// compute LU decomposition of A
	rowswaps = (int *)malloc(sizeof(int)*n);
	b = (float *)malloc(sizeof(float)*n);


	matrixNxM_multiply_vector(b,JT,vv,n,m);
	printf("JT.v = ");
	for (i=0; i<n; i++) printf("%f ",b[i]);
	printf("\n");
	LUdecomp(A,n,rowswaps,&val);
	LUsubstitute(A,n,rowswaps,b);
	printf("x = ");
	for (i=0; i<n; i++) printf("%f ",b[i]);
	printf("\n");
	thetaDot[0] = b[0];
	thetaDot[1] = b[1];
	thetaDot[2] = b[2];
	matrix3x3_multiply_vector(vvv,JTJ,b);
	printf("JT.J.b = ");
	for (i=0; i<n; i++) printf("%f ",vvv[i]);
	printf("\n");

	theta1 += thetaDot[0];
	theta2 += thetaDot[1];
	theta3 += thetaDot[2];

	theta1Rad = theta1*PI/180;
	theta2Rad = theta2*PI/180;
	theta3Rad = theta3*PI/180;
}

// ---------------------------------------------------------------------
// DISPLAY PARTICLES
// ---------------------------------------------------------------------
void display_numericinvkin()
{
	// base
	glPushMatrix();
	glTranslatef(0.0,-1.0,0.0);
	draw_cube();
	glPopMatrix();
	
	// first segment
	glPushMatrix();
	glRotatef(theta1,0.0,0.0,1.0);
	glScalef(0.25,L1/2,0.25);
	glTranslatef(0.0,1.0,0.0);
	draw_cube();
	glPopMatrix();
	
	// second segment
	glPushMatrix();
	glTranslatef(-L1*sin(theta1Rad),L1*cos(theta1Rad),0.0);
	glRotatef(theta1+theta2,0.0,0.0,1.0);
	glScalef(0.25,L2/2,0.25);
	glTranslatef(0.0,1.0,0.0);
	draw_cube();
	glPopMatrix();
	
	// third segment
	glPushMatrix();
	glTranslatef(-L1*sin(theta1Rad)-L2*sin(theta1Rad+theta2Rad),L1*cos(theta1Rad)+L2*cos(theta1Rad+theta2Rad),0.0);
	glRotatef(theta1+theta2+theta3,0.0,0.0,1.0);
	glScalef(0.25,L3/2,0.25);
	glTranslatef(0.0,1.0,0.0);
	draw_cube();
	glPopMatrix();

	// goal
	glPushMatrix();
	glTranslatef(goal.x,goal.y,goal.z);
	draw_tetrahedron();
	glPopMatrix();

}