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


using namespace std;

#define PI 3.14159

// function prototypes
void display(void);
void reshape(int width, int height);
void keyboard(unsigned char key, int x, int y);
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=3;
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); 
	glutKeyboardFunc(keyboard);
	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();


}


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 keyboard(unsigned char key, int x, int y)
{

	switch (key) 
	{
	

	default:
		break;
	}

	glutPostRedisplay();


}

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

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()
{
	
}

void computePseudoInverse()
{
}


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;
	
	computeJacobian();
	computePseudoInverse();
	
	// compute change in angle

	// update positions


	// check residual

	// if too much error try halving distance

	return true;
}

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

void computeGlobals()
{

	// compute global angles
	globalTheta[0]=theta[0];
	for (int i=1;i<numLinks;i++)
		globalTheta[i]=globalTheta[i-1]+theta[i];

	// compute global positions
	globalPosition[0] = Tuple(0,0,0);
	for (int i=1;i<=numLinks;i++) {
		double angle = globalTheta[i-1]*PI/180.0;
		globalPosition[i] = globalPosition[i-1] + 
			len[i-1]*Tuple(cos(angle),sin(angle),0);
	}
}