/* Example 2.8.robot3d.c: 3D three segments arm transformation */
// by Jim X. Chen; September, 2000

#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, A, B, C;
float alpha, beta, gama, aalpha=.4, abeta=.9, agama=1.4;


int depth=5; 


static float vdata[4][3] = {
	{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, 
	{-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}
};


void normalize(float v[3]) {
	float d = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);

	if (d == 0) {
		printf("zero length vector");
		return;
	}
	v[0] /= d;	v[1] /= d;	v[2] /= d;
}

void drawtriangle(float *v1, float *v2, float *v3)
{
	glBegin(GL_TRIANGLES);
		glVertex3fv(v1);
		glVertex3fv(v2);
		glVertex3fv(v3);
	glEnd();
}


void subdivideCylinder(float *v1, float *v2, int depth)
{
	float v11[3], v22[3], v00[3] = {0, 0, 0}, v12[3];
	float v01[3], v02[3]; 
	int i;


	if (depth == 0) {
		glColor3f(v1[0]*v1[0], v1[1]*v1[1], v1[2]*v1[2]);

		for (i=0; i<3; i++) {
			v02[i] = v22[i] = v2[i];
			v01[i] = v11[i] = v1[i];
			v02[i] = v22[i] = v2[i];
		}
		// the height of the cone along z axis
		v01[2] = v02[2] = 1;

		drawtriangle(v11, v22, v00); // draw sphere at the cylinder's bottom

		glBegin(GL_POLYGON); // draw the side rectangles of the cylinder
			glVertex3fv(v11);
			glVertex3fv(v22);
			glVertex3fv(v02);
			glVertex3fv(v01);
		glEnd();

		return;
	}
		
	v12[0] = v1[0]+v2[0]; 
	v12[1] = v1[1]+v2[1];
	v12[2] = v1[2]+v2[2];


	normalize(v12);

	subdivideCylinder(v1, v12, depth - 1);
	subdivideCylinder(v12, v2, depth - 1);
}


void drawCylinder(void)
// draw a unit cylinder with bottom in xy plane

{

	subdivideCylinder(vdata[0], vdata[1], depth);
	subdivideCylinder(vdata[1], vdata[2], depth);
	subdivideCylinder(vdata[2], vdata[3], depth);
	subdivideCylinder(vdata[3], vdata[0], depth);	
}

drawArm(float End1, float End2) {

	float scale;
	
	scale = End2-End1;

	glPushMatrix();
		// the cylinder lies in the z axis; rotate it to lie in the x axis
		glRotatef(90.0, 0.0, 1.0, 0.0);
		glScalef(scale/5.0, scale/5.0, scale); 	
		drawCylinder();	
	glPopMatrix();
}

static void drawRobot(float alpha, float beta, float gama)
{
	float O, A, B, C;
	O = 0.0; A = (float) 1.5*Width/8;  
	B = (float) 1.5*Width/4.2; C = (float) 1.5*Width/3; 

	// the robot arm is rotating around y axis
	glRotatef (1.0, 0.0, 1.0, 0.0);
	glPushMatrix();

		glRotatef (alpha, 0.0, 0.0, 1.0);
		// R_z(alpha) is on top of the matrix stack
		drawArm (O, A);

		glTranslatef (A, 0.0, 0.0);
		glRotatef (beta, 0.0, 0.0, 1.0);
		// R_z(alpha)T_x(A)R_z(beta) is on top of the matrix stack
		drawArm (A, B);

		glTranslatef (B-A, 0.0, 0.0);
		glRotatef (gama, 0.0, 0.0, 1.0);
		// R_z(alpha)T_x(A)R_z(beta)T_x(B)R_z(gama) is on top of the matrix stack
		drawArm (B, C);

 	glPopMatrix();
}




void display(void)
{ 

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


	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	drawRobot(alpha, beta, gama);

	glutSwapBuffers();
}



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


	//initialize robot arm end pointions

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



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


int main(int argc, char **argv)
{
    glutInit(&argc, argv);
 	glutInitDisplayMode(GLUT_DOUBLE);
    glutInitWindowSize(Width, Height);
    glutCreateWindow("Example 2.8.robot3d.c: press SPACE & another key");

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

    glutMainLoop();
}