#include <stdio.h>
#include <string.h>
#include <math.h>
#include "GL/glut.h"

bool if_leftdown = false;
float angle = 45.0f;
float angle1 = 45.0f;
float angle2 = 45.0f;
float old_x, old_y;

enum CONTROL_LEVEL
{
	BASE = 0,
	JOINT1,
	JOINT2
}  level;

void processMenuEvents(int option)
{
	level = (CONTROL_LEVEL) option;
}

void createGLUTMenus()
{
	int menu;

	menu = glutCreateMenu(processMenuEvents);
	glutAddMenuEntry("Base",0);
	glutAddMenuEntry("Joint1",1);
	glutAddMenuEntry("Joint2",2);
	glutAttachMenu(GLUT_RIGHT_BUTTON);
}

void myMouseCB(int button, int state, int x, int y)
{ 	
	if( button==GLUT_LEFT_BUTTON )
	{
		if ( state==GLUT_DOWN )
			if_leftdown = true;
		else
			if_leftdown = false;
		old_x = x;
		old_y = y;
	}
}

void myMotionCB(int x, int y)
{
	if ( if_leftdown )
	{
		int inc_x = x - old_x;
		printf( "%d\n", inc_x );
		int inc_y = y - old_y;
		switch ( level )
		{
		case BASE:
			angle += inc_x / 7.5f;
			break;
		case JOINT1:
			angle1 += inc_x / 7.5f;
			break;
		case JOINT2:
			angle2 += inc_x / 7.5f;
			break;

		}
		old_x = x;
		old_y = y;
		glutPostRedisplay();
	}
}

void myKeyboardCB(unsigned char key, int x, int y)
{
	switch (key) {
		case 'q':
		case 27:
			exit(0); 
			break;
	}
}
void output(float x, float y, char *string)
{
  int len, i;
  glRasterPos2f(x, y);
  len = (int) strlen(string);
  for (i = 0; i < len; i++)
  {
    glutBitmapCharacter(GLUT_BITMAP_8_BY_13, string[i]);
  }
}

int width, height;

void drawCoord()
{
	glPushMatrix();
	glScalef( 10, 10, 1 );
	glColor3f( 0.1,0.1,0.1);
	for ( float i = -.9; i <= .91; i+= .1 )
	{
		glBegin( GL_LINES );
		glVertex2f(-1, i);
		glVertex2f(1, i);
		glVertex2f(i, -1);
		glVertex2f(i, 1);
		glEnd();
	}

	glBegin( GL_LINES );
	glColor3f( 1, 0, 0 );
	glVertex2f( 0.0f, 0.0f);
	glVertex2f( 0.0f, 1);

	glVertex2f( -0.02f, 0.95f );
	glVertex2f( 0.0f, 1 );
	glVertex2f( 0.02f, 0.95f );
	glVertex2f( 0.0f, 1 );

	glVertex2f( 0.0f, 0.0f );
	glVertex2f( 1, 0.0f );

	glVertex2f( 0.95f, -0.02f  );
	glVertex2f( 1, 0.0f );
	glVertex2f( 0.95f, 0.02f  );
	glVertex2f( 1, 0.0f );
	glEnd();
	output( 0.03, 0.9, "Y" );
	output( 0.9, 0.03, "X" );
	output( 0.0, 0.03, "(0,0)" );

	glPopMatrix();

	glColor3f( 0.7,0.7,0.7);
}

void drawunitsquare()
{
	float size = 1;
	float startx = -.5, starty = -.5;
	glBegin( GL_LINE_LOOP );
	glVertex2f( startx, starty );
	glVertex2f( startx, starty + size );
	glVertex2f( startx + size, starty + size );
	glVertex2f( startx + size, starty );
	glEnd();
}

void drawobject()
{
	glPushMatrix();
	glTranslatef( 0, 1.5, 1 );
	glScalef( 1, 3, 1 );
	drawunitsquare();
	glPopMatrix();
}

void display(void)  {
	glClear(GL_COLOR_BUFFER_BIT);
	glLoadIdentity();
	drawCoord();

	drawobject();
	

	glPushMatrix();
	glTranslatef( 0, 3, 0 );
	glRotatef( -30, 0, 0, 1 );
	drawobject();
	glPopMatrix();


	glTranslatef( 0, 3, 0 );
	glRotatef( angle, 0, 0, 1 );
	drawobject();

	
	glTranslatef( 0, 3, 0 );
	glRotatef( angle1, 0, 0, 1 );
	drawobject();

	glPushMatrix();
	glTranslatef( 0, 3, 0 );
	glRotatef( -30, 0, 0, 1 );
	drawobject();
	glPopMatrix();

	glTranslatef( 0, 3, 0 );
	glRotatef( angle2, 0, 0, 1 );
	drawobject();


	glutSwapBuffers();
}

void myReshapeCB( int w, int h )
{
	width = w;
	height = h;
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION); /* switch matrix mode */
	glLoadIdentity();
	if (w <= h)
	gluOrtho2D(-10.0, 10.0, -10.0 * (GLfloat) h / (GLfloat) w,
	10.0 * (GLfloat) h / (GLfloat) w);
	else gluOrtho2D(-10.0 * (GLfloat) w / (GLfloat) h, 10.0 *
	(GLfloat) w / (GLfloat) h, -10.0, 10.0);
	glMatrixMode(GL_MODELVIEW); /* return to modelview mode */
}

void myinit()
{
	glClearColor(0.0f,0.0f,0.0f,0.0f);   // set the background colour 

	glMatrixMode(GL_PROJECTION) ;
	glLoadIdentity() ;
	glMatrixMode(GL_MODELVIEW) ;
	glLoadIdentity();

	glShadeModel(GL_SMOOTH);
}

int main(int argc, char** argv)
{
  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowPosition (0, 0);
  glutInitWindowSize(640,640);
  glutCreateWindow(argv[0]);

  // register callbacks
  glutReshapeFunc (myReshapeCB);
  glutKeyboardFunc(myKeyboardCB );
  glutMouseFunc(myMouseCB) ;
  glutMotionFunc(myMotionCB) ;
  glutDisplayFunc(display);

  myinit() ; 	// initialize
  createGLUTMenus();
  glutMainLoop();	// start the main  loop
  return 0;         	// never reached
}