/* Example 2.3.1.robot2d.c: 2D three segments arm transformation */
// by Jim X. Chen; Feb, 2001

#include <windows.h> 
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <GL/glut.h>


#define ESC 27
#define SPACE 32
int Height=400, Width=400;

float O[3] = {0.0, 0.0, 0.0}, A[3] = {0.0, 0.0, 0.0}, 
		B[3] = {0.0, 0.0, 0.0}, C[3] = {0.0, 0.0, 0.0};

float ang, alpha, beta, gama, aalpha=.1, abeta=.3, agama=0.7;


float matrixStack[5][4][4]; // five layers of the stack
int stackPointer=0;


void clearMatrix(float mat[4][4]) {
	int i, j;

	for (i=0; i<4; i++)
    for (j=0; j<4; j++)
		mat[i][j] = 0.;
}


void myIdentity(float mat[4][4]) {
	int i;

	clearMatrix(mat);

	for (i=0; i<4; i++)
		mat[i][i] = 1.;
}


void myLoadIdentity(void) {
	myIdentity(matrixStack[stackPointer]);

}


void myMultMatrix (float mat[4][4]) {
	int i, j, k, tmp=stackPointer+1;

		clearMatrix(matrixStack[tmp]); // temp location

		for (i=0; i<4; i++)
			for (j=0; j<4; j++) 
			for (k=0; k<4; k++) 
			matrixStack[tmp][i][j] += matrixStack[stackPointer][i][k] * mat[k][j];

			

		for (i=0; i<4; i++)
			for (j=0; j<4; j++) {
			matrixStack[stackPointer][i][j] = matrixStack[tmp][i][j];
		 }

}


void myPushMatrix(void) {
	int i, j, tmp=stackPointer+1;

	clearMatrix(matrixStack[tmp]); // temp location

	for (i=0; i<4; i++)
	for (j=0; j<4; j++) 
		matrixStack[tmp][i][j] = matrixStack[stackPointer][i][j];

	stackPointer++;

}


void myPopMatrix(void) {

	stackPointer--;

}


void myTranslate(float x, float y, float z) {
	float mat[4][4];

	myIdentity(mat);
	mat[0][3] = x;
	mat[1][3] = y;
	mat[2][3] = z;

	myMultMatrix(mat);
}


void myRotateX(float angle) {
	float mat[4][4];

	myIdentity(mat);

	mat[1][1] = cos(angle);
	mat[1][2] = -sin(angle);
	mat[2][1] = sin(angle);
	mat[2][2] = cos(angle);

	myMultMatrix(mat);
}


void myRotateY(float angle) {
	float mat[4][4];

	myIdentity(mat);

	mat[0][0] = cos(angle);
	mat[0][2] = sin(angle);
	mat[2][0] = -sin(angle);
	mat[2][2] = cos(angle);

	myMultMatrix(mat);
}


void myRotateZ(float angle) {
	float mat[4][4];

	myIdentity(mat);

	mat[0][0] = cos(angle);
	mat[0][1] = -sin(angle);
	mat[1][0] = sin(angle);
	mat[1][1] = cos(angle);

	myMultMatrix(mat);
}


void myRotate(float angle, float x, float y, float z) {
	float a, b, r;

	if (x==0 && y==0) { myRotateZ(angle); return; }
	if (x==0 && z==0) { myRotateY(angle); return; }
	if (z==0 && y==0) { myRotateX(angle); return; }

	r = sqrt(x*x + y*y + z*z);
	a = acos(y/r);
	b = atan(z/x);

	myRotateY(-b);
	myRotateZ(-a);
	myRotateY(angle);
	myRotateZ(a);
	myRotateY(b);
}



myTransform(float vertex[3]) {
// multiply current coordinates by the matrix

	float hv[4], homoVertex[4];
	int i, j;

	for (i=0; i<4; i++)  hv[i] = 0.;
	
	for (i=0; i<3; i++)  homoVertex[i] = vertex[i];	 
	homoVertex[3] = 1.; // extend into homogeneous coord
	
	for (i=0; i<4; i++) 
	for (j=0; j<4; j++) 
		hv[i] += matrixStack[stackPointer][i][j] * homoVertex[j];
		
	for (i=0; i<3; i++) // map homo into 3D coordinates
		vertex[i] = hv[i]/hv[3];	
}


void drawArm(float *e1, float *e2) {
	int i;
	float End1[3], End2[3];

	for (i=0;i<3; i++) {
		End1[i] = e1[i];
		End2[i] = e2[i];
	}
	myTransform(End1); // multiply the point with the matrix on the stack
	myTransform(End2);

	glBegin(GL_LINES);
		glVertex3fv(End1);
		glVertex3fv(End2);
	glEnd();
}

 void drawRobot(float *A, float *B, float*C,
					  float alpha, float beta, float gama)
{

	myPushMatrix();

		glColor3f(1, 0, 0);
		myRotateZ (alpha);
		// R_z(alpha) is on top of the matrix stack
		drawArm (O, A);

		glColor3f(0, 1, 0);
		myTranslate (A[0], A[1], 0.0);
		myRotateZ (beta);
		myTranslate (-A[0], -A[1], 0.0);
		// R_z(alpha)T(A)R_z(beta)T(-A) is on top of the matrix stack
		drawArm (A, B);

		glColor3f(0, 0, 1);
		myTranslate (B[0], B[1], 0.0);
		myRotateZ (gama);
		myTranslate (-B[0], -B[1], 0.0);
		// R_z(alpha)T(A)R_z(beta)T(-A) is on top of the matrix stack
		drawArm (B, C);

 	myPopMatrix();
}



void drawColorCoord(float xlen, float ylen, float zlen)
{
	float O[3] = {0., 0., 0.}, X[3] = {0., 0., 0.}, Y[3] = {0., 0., 0.}, Z[3] = {0., 0., 0.};

	X[0] = xlen; Y[1] = ylen; Z[2] = zlen;

	glColor3f(1,0,0);
	drawArm(O, X);
	glColor3f(0,1,0);
	drawArm(O, Y);
	glColor3f(0,0,1);
	drawArm(O, Z);
}


void drawSolar(float earthDistance, float earthAngle, float moonDistance, float moonAngle)
{
	float O[3] = {0., 0., 0.}, V[3] = {0., 0., 0.};
	float tiltAngle=1;

	// Global coordinates 
	glLineWidth(3);
	drawColorCoord(Width/4, Width/4, Width/4);

   myPushMatrix();
		myRotate(earthAngle, 0.0, 1.0, 0.0); // rotating around the "sun"; proceed angle
		myRotate(tiltAngle, 0.0, 0.0, 1.0); // tilt angle		
		
		V[1] = earthDistance; 
		glColor3f(1., 1., 1.);
		drawArm(O,V); // the line from the origin to the center of the earth

		myTranslate(0., earthDistance, 0.0);

		glLineWidth(2);
		drawColorCoord(Width/6, Width/6, Width/6);

		myRotate(moonAngle, 0.0, 1.0, 0.); // rotating around the "earth"
		myTranslate(moonDistance, 0., 0.);
		glLineWidth(1);
		drawColorCoord(Width/8, Width/8, Width/8);
	myPopMatrix();
 }


void display(void)
{ 

	if (rand() % 10000 == 0) aalpha = -aalpha;
	if (rand() % 1000 == 0) abeta = -abeta;
	if (rand() % 100 == 0) agama = -agama;
	
	alpha+= aalpha/10;
	beta+= abeta/10;
	gama+= agama/10;

	ang=ang+0.01; 

	glClear(GL_COLOR_BUFFER_BIT);

	myPushMatrix();
		myTranslate(Width/4.,0.,0.);
		drawRobot(A, B, C, alpha, beta, gama);
	myPopMatrix();
	myPushMatrix();
		myTranslate(-Width/4.,0.,0.);
		drawSolar(Width/4, ang, Width/12, ang*2);
	myPopMatrix();

	glutSwapBuffers();
}



void Reshape(int w, int h)
{
    glClearColor (0.0, 0.0, 0.0, 1.0);

	//initialize robot arm end pointions
	A[0] = (float) w/7; 
	B[0] = (float) w/5; 
	C[0] = (float) w/4; 

	Width = w; Height = h; 

    glViewport (0, 0, Width, Height);

	// hardware set to use projection transformation matrix stack
    glMatrixMode (GL_PROJECTION);
    glLoadIdentity ();
    glOrtho(-Width/2, Width/2, -Height/2, Height/2, -Width/2, Width/2); 

	// hardware set to use model transformation matrix stack
	glMatrixMode (GL_MODELVIEW);
	// initialize the current top of matrix stack to identity
    glLoadIdentity ();
}



void Key(unsigned char key, int x, int y)
{ 
    switch (key) {
      case ESC:
			exit(0);
	  case SPACE: 
			glutIdleFunc(NULL);
			glRotatef(10, 1, 1, 1); 
			display();
			break; 
	  default: 
			glutIdleFunc(display); 
    }
}


int main(int argc, char **argv)
{
 	myLoadIdentity(); // load my own matrix stack

	glutInit(&argc, argv);
 	glutInitDisplayMode(GLUT_DOUBLE);
    glutInitWindowSize(Width, Height);
    glutCreateWindow("Example: press SPACE & another key");

    glutKeyboardFunc(Key);
    glutReshapeFunc(Reshape); 
    glutDisplayFunc(display);
    glutIdleFunc(display);

    glutMainLoop();
}