/* Example 2.4.robot2d.c: 2D 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[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 alpha, beta, gama, aalpha=.1, abeta=.3, agama=0.7;


void drawArm(float *End1, float *End2) {

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

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

	glPushMatrix();

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

		glColor3f(0, 1, 0);
		glTranslatef (A[0], A[1], 0.0);
		glRotatef (beta, 0.0, 0.0, 1.0);
		glTranslatef (-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);
		glTranslatef (B[0], B[1], 0.0);
		glRotatef (gama, 0.0, 0.0, 1.0);
		glTranslatef (-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);

 	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);
	drawRobot(A, B, C, alpha, beta, gama);

	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, -1.0, 1.0); 

	// 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); display();
			break; 
	  default: 
			glutIdleFunc(display); 
    }
}


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

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

    glutMainLoop();
}