#include <GL/glut.h>
#include <iostream>
#include <math.h>
#include <string>
#include <iomanip>
#include "nr.h"
#include "geometry.h"


using namespace std;

#define PI 3.14159

// function prototypes
void display(void);
void reshape(int width, int height);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void init(void);
void computeJacobian();
void computePseudoInverse();
void computeGlobals();
bool move(Tuple dState, int depth);

// window
int width=400;
int height=400;

// world space
int screenWidth=60;
int screenHeight=60;

// the links
const int numLinks=2;
const int stateSize=2; // this cannot be changed!  throughout
// the program we assume we the system
// exists in a plane and we are changing
// the position -- no orientation change

double theta[numLinks]={0,45};
double len[numLinks]={10,10};
double globalTheta[numLinks];
Tuple globalPosition[numLinks+1];

// the Jacobian
Vec_DP w(numLinks);
Mat_DP u(stateSize,numLinks),v(numLinks,numLinks);
double J[stateSize][numLinks], Jplus[numLinks][stateSize];

// movement
bool moving=false;
bool destinationSet=false;
Tuple start;
Tuple destination;

int 
main(int argc, char **argv)
{

	// set up window
	glutInitWindowSize(width,height);
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
	glutCreateWindow("Simple Inverse Kinematics Engine");

	// register callback functions
	glutDisplayFunc(display);
	glutReshapeFunc(reshape); 
	glutMouseFunc(mouse);
	glutMotionFunc(motion);

	// initalize opengl parameters
	init();

	// loop until something happens
	glutMainLoop();
	return 0;           
}

void init()
{
	// initialize viewing system
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(-screenWidth/2,screenWidth/2,-screenHeight/2,screenHeight/2);
	glMatrixMode(GL_MODELVIEW);


	// initialize background color to black
	glClearColor(0.0,0.0,0.0,0.0);

	// set line width
	glLineWidth(3.0);

	// compute global angle and positions
	computeGlobals();

	// initialize destination
	//for (int i=0;i<3;i++)
		//destination[i]=globalPosition[numLinks][i];
	


}


void reshape(int newWidth, int newHeight)
{
	width=newWidth;
	height=newHeight;
	glViewport(0,0,width,height);
}

void display()
{


	// clear buffers
	glClear(GL_COLOR_BUFFER_BIT );

	//load identity
	glLoadIdentity();


	// draw links
	glColor3f(0,0,1);
	glBegin(GL_LINE_STRIP);
	for (int i=0; i<=numLinks; i++) {
		glVertex2d(globalPosition[i][0],globalPosition[i][1]);

	}
	glEnd();

	// draw joints
	glColor3f(1,0,0);
	glPointSize(5);
	glBegin(GL_POINTS);
	for (int i=0; i<=numLinks; i++)
		glVertex2d(globalPosition[i][0],globalPosition[i][1]);
	glEnd();

	// draw destination
	if (destinationSet)
	{
		glPointSize(5);
		glColor3f(0,1,0);
	}
	else
	{
		glPointSize(8);
		glColor3f(1,1,1);
	}
	glBegin(GL_POINTS);
	glVertex2f(destination[0],destination[1]);
	glEnd();





	glutSwapBuffers();


}


void mouse(int button, int state, int x, int y)
{
	if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
	{
		start[0]=x;
		start[1]=y;
		moving=true;
	}
	else 
	{
		moving=false;
		destinationSet=false;
	}
	glutPostRedisplay();
}

void motion(int x, int y)
{

	Tuple dPosition;  // change in position
	if (moving==true) {
		// compute change in mouse position
		double dx = (double) (x-start[0]) * (double) screenWidth / (double) width;
		double dy = (double) (start[1]-y) * (double) screenHeight / (double) height;
		Tuple end=globalPosition[numLinks];
		
		// set destination and change in position
		if (!destinationSet) {
			destination[0]=end[0]+dx;
			destination[1]=end[1]+dy;
			dPosition[0]=dx;
			dPosition[1]=dy;
			destinationSet=true;
		}
		else {
			destination[0]+=dx;
			destination[1]+=dy;
			dPosition[0] = destination[0]-end[0];
			dPosition[1] = destination[1]-end[1];
		}
		// remember current mouse position
		start[0]=x;
		start[1]=y;

		// move
		if (!move(dPosition,0))
		{
			cout << "Unable to reach destination." << endl;
			destinationSet=false;
			return;
		}
		else
		{
			computeGlobals();
		}

		glutPostRedisplay();
	}
}


void computeJacobian()
{

	Tuple z(0,0,1);
	for (int i=0; i<numLinks; i++) // build column i
	{
		// compute the vector from link i to the end effector


		// find velocity vector


		// put velocity into ith column of Jacobian


	}
}


void computePseudoInverse()
{	
	// copy Jacobian into u -- for the svd routine	
	for (int i=0;i<numLinks;i++) 
	{
		u[0][i]=J[0][i];
		u[1][i]=J[1][i];
	}

	//compute svd
	NR::svdcmp(u,w,v);

	// the svd returns u, w, v which are used as follows
	// to compute the inverse

	// first compute Winv 
	// Winv is a numlinks x numlinks matrix
	// The off-diagonal elements are 0
	// The diagonal element Winv[i][i] = 1/w[i] except
	// when w[i]<epsilon; in which case Winv[i][i]=0
	double Winv[numLinks][numLinks];


	// now compute pseudo inverse
	// which is v * Winv * uT
// compute Winv
	double Winv[numLinks][numLinks];
	for (int i=0;i<numLinks;i++)
	{
		for (int j=0;j<numLinks;j++) 
		{
			if (i==j && w[i]>.00001)
				Winv[i][j]=1.0/w[i];
			else
				Winv[i][j]=0;
		}
	}


	// now compute pseudo inverse
	// which is v * Winv * uT
	double tmp[numLinks][numLinks];
	for (int i=0;i<numLinks;i++)
	{
		for (int j=0;j<numLinks;j++) 
		{
			tmp[i][j]=0;
			for (int k=0;k<numLinks;k++) 
			{
				tmp[i][j] += v[i][k]*Winv[k][j];
				double val=tmp[i][j];
			}
		}
	}

	for (int i=0;i<numLinks;i++)
	{
		for (int j=0;j<stateSize;j++)
		{
			Jplus[i][j]=0;
			for (int k=0;k<numLinks;k++) 
			{
				Jplus[i][j] += tmp[i][k]*u[j][k];
				double val0 = u[j][k];
				double val = Jplus[i][j];
			}
		}
	}
 

}

bool move(Tuple dState,int depth)
{

	if (depth>10) {
		//cout << "Can't get to target." << endl;
		return false;
	}

	if (stateSize != 2)
		cout << "Can't compute change in orientation." << endl;
	
	// now compute the change in angles 


	// update angles and positions


	// check residual

	// if too much error try halving distance with another recursive
	// depth and return the result

	// else return true


	// for now we'll just return false

	return false;
}

/*
*	computeGlobals
*		This routine computes the global angles
*		and positions of the links
*/

void computeGlobals()
{


}