/*
 * ACM/SIGGRAPH University of Michigan Student Chapter
 * OpenGL Workshop
 *
 * OpenGL Matrix Stacks
 *
 * This program draws an articulated, human-like robot arm using viewing transformations
 * and the OpenGL matrix stack.
 *
 */

#include <iostream>
#include <cmath>
#include <GL/glut.h>

using namespace std;

/* Constants */

const double PI = acos(-1.0);

/* Global Variables */

GLfloat rho = 2.5;
GLfloat phi = 45 * (PI/180);
GLfloat theta = 45 * (PI/180);

GLfloat arm_length = 5;
GLfloat elbow_angle = 180;
GLfloat vertical_wrist_angle = 0;
GLfloat horizontal_wrist_angle = 0;
GLfloat claw_open_angle = 0;

/* Function Prototypes */

bool initOpenGL();

// GLUT Callback Functions:
void reshape(int new_width, int new_height);
void display();
void key(unsigned char key, int x, int y);

// Some drawing functions:
void drawBox(float length, float width, float height);

void drawArm();
void drawForearm();
void drawHand();
void drawFinger();

// Utility functions:
void incr_float(float &val, float incr_amount, float min, float max);

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

	glutInit(&argc, argv);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	
	glutCreateWindow("Drawing 3D Objects in OpenGL");

	glutReshapeFunc(reshape);
	glutDisplayFunc(display);
	glutKeyboardFunc(key);

	initOpenGL();

	glutMainLoop();

}

bool initOpenGL(){

	cout << "OpenGL Vender:\t\t" << glGetString(GL_VENDOR) << endl;
	cout << "OpenGL Renderer:\t" << glGetString(GL_RENDERER) << endl;
	cout << "OpenGL Version:\t\t" << glGetString(GL_VERSION) << endl;

	glEnable(GL_DEPTH_TEST);

	// Call our reshape function to setup the viewport and projection matrix:
	// (Actually we don't need to do this, the GLUT does it for us)
	// reshape(glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT));

	return true;

}

void reshape(int new_width, int new_height){
	
	glViewport(0, 0, new_width, new_height);
	
	// Setup the projeciton matrix:
	glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
	gluPerspective( 60.0,	// Field of view in degrees
					// Calculate the aspect ratio from the width and height of the window:
					static_cast<float>(new_width)/static_cast<float>(new_height),	// Aspect ratio
                    1.0,	// Position of near clipping plane
					10.0);	// Position of far clipping plane

}

void display(){
	
	// Set our clear color:
	glClearColor(0.0, 0.0, 0.0, 0.0);
	
	// Clear the previously drawn scene:
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Draw the new scene:

	glMatrixMode(GL_MODELVIEW);

	glPushMatrix();
	
	// Setup the model view matrix:
	glLoadIdentity();
	gluLookAt(// Position the camera's eye based on current angles and distance:
			  -rho*sin(theta)*sin(phi), rho*cos(theta), -rho*sin(theta)*cos(phi),
			  0.0, 0.0, 0.0,  // Set the center of the scene to be at (0,0,0)
			  0.0, 1.0, 0.0);  // Set the up vector of the camera to the positive Y direction
	
	drawArm();
	
	glutSwapBuffers();

}

void key(unsigned char key, int x, int y){

	switch(key){

		// Left and right rotation:
		case '4':

			phi = fmod(phi - 0.1, 2*PI);

			break;

		case '6':

			phi = fmod(phi + 0.1, 2*PI);

			break;

		// Up and down rotation:
		case '2':

			theta += 0.1;
			theta = (theta > (PI-0.1))? PI - 0.1 : theta;

			break;

		case '8':

			theta -= 0.1;
			theta = (theta < 0.1)? 0.1 : theta;

			break;

		// Zoom in and out:
		case '1':

			rho -= 0.1;
			rho = (rho < 0)? 0 : rho;

			break;

		case '5':

			rho += 0.1;

			break;

		case 'q':

			incr_float(elbow_angle, 2, 0, 180);

			break;

		case 'w':

			incr_float(elbow_angle, -2, 0, 180);

			break;

		case 'a':

			incr_float(vertical_wrist_angle, 0.5, -90, 90);

			break;

		case 's':

			incr_float(vertical_wrist_angle, -0.5, -90, 90);

			break;

		case 'z':

			incr_float(horizontal_wrist_angle, 0.5, -90, 90);

			break;

		case 'x':

			incr_float(horizontal_wrist_angle, -0.5, -90, 90);

			break;

		case 'd':

			incr_float(claw_open_angle, 0.5, 0, 90);

			break;

		case 'f':

			incr_float(claw_open_angle, -0.5, 0, 90);

			break;

	}

	glutPostRedisplay();

}

void drawBox(float x_size, float y_size, float z_size){
// Draws of a box a the origin with the specified lenght, width and height:

	glPushMatrix();

	glScalef(x_size, y_size, z_size);

	glutSolidCube(1.0);

	glPopMatrix();

}

void drawArm(){

	glPushMatrix();

	// Draw the upper arm.  It should be half the length of the entire arm:
    glPushMatrix();

	// Translate half way down the upper arm:
	glTranslatef(arm_length/4, 0.0, 0.0);

	glColor3f(1.0, 0.0, 0.0);
	drawBox(arm_length/2, arm_length/8, arm_length/8);

	glPopMatrix();

	drawForearm();

	glPopMatrix();

}

void drawForearm(){

	glPushMatrix();

	// Translate to the end of the upper arm:
	glTranslatef(arm_length/2, 0.0, 0.0);
	
	// Apply the elbow angle:
	glRotatef(180 - elbow_angle, 0.0, 0.0, 1.0);

	// Draw the forearm (again, half the length of the entire arm):
	glPushMatrix();

	// Translate half way down the forearm:
	glTranslatef(arm_length/4, 0.0, 0.0);

	glColor3f(0.0, 1.0, 0.0);
	drawBox(arm_length/2, arm_length/8, arm_length/8);

	glPopMatrix();

	drawHand();

	glPopMatrix();

}

void drawHand(){

	glPushMatrix();

	// Translate to the end of the forearm:
	glTranslatef(arm_length/2, 0.0, 0.0);

	// Apply the wrist angles:
	glRotatef(vertical_wrist_angle, 0.0, 0.0, 1.0);
	glRotatef(horizontal_wrist_angle, 0.0, 1.0, 0.0);

	// Draw the hand:

	// First draw the upper claw finger:
	glPushMatrix();

	glRotatef(-75, 1.0, 0.0, 0.0);
	//glRotatef(claw_open_angle/2, 0.0, 0.0, 1.0);
	glColor3f(0.0, 0.0, 1.0);
	glutSolidTeapot(arm_length/16);
	//drawFinger();

	glPopMatrix();

	// Then draw the lower claw finger:
	glPushMatrix();

	glRotatef(75, 1.0, 0.0, 0.0); // Flip it vertically
	//glRotatef(claw_open_angle/2, 0.0, 0.0, -1.0);
	glColor3f(0.0, 1.0, 1.0);
	glutSolidTeapot(arm_length/16);
	//drawFinger();

	glPopMatrix();

	glPopMatrix();

}

void drawFinger(){

	glPushMatrix();

	glRotatef(45, 0.0, 0.0, 1.0);

	//glPushMatrix();

	glTranslatef(arm_length/16, 0.0, 0.0);
	//drawBox(arm_length/8, arm_length/16, arm_length/16);

	glutSolidCube(arm_length/16);

	//glPopMatrix();

	//glTranslatef(arm_length/8, 0.0, 0.0);
	////glRotatef(90 ,0.0, 0.0, 1.0);

	//glPushMatrix();

	//glTranslatef(arm_length/16, 0.0, 0.0);
	//drawBox(arm_length/8, arm_length/16, arm_length/16);

	//glPopMatrix();

	glPopMatrix();

}

void incr_float(float &val, float incr_amount, float min, float max){

	val += incr_amount;
	val = (val > max)? max : val; 
	val = (val < min)? min : val;

}