#include <glew.h>
#include <glut.h>
#include <iostream>
//#include <math_3d.h>
#include <stdio.h>
#include <math.h>
using namespace std;


#define TRIANLGE_SIZE 15.f 
#define AXES_SIZE 25.f 

class robot{
public:
	float x, y, dir;//robot position and orientation angle
	float tx, ty, nx, ny;
	float size;
	robot(){}
	~robot(){}
	robot(float win_w, float win_h)
	{
		/*!!!
		Zero of y is supposed to be at the window bottom
		!!! */
		size = TRIANLGE_SIZE;
		x = 0.5f*win_w;
		y = 0.3f*win_h;
		dir = 0.f;
		get_tan_norm();
	}


	void paint(){
		glPushMatrix();
			glTranslatef(x, y, 0.f);

			glBegin(GL_LINES);
				glColor3f(1.0f, 1.0f, 0.0f);
				glVertex2f(0.f, 0.f);
				glVertex2f(tx*5.f*size, ty*5.f*size);
				glColor3f(0.0f, 1.0f, 1.0f);
				glVertex2f(0.f, 0.f);
				glVertex2f(nx*5.f*size, ny*5.f*size);
			glEnd();

			glPushMatrix();
				glRotatef(dir, 0.f, 0.f, -1.f); // ? ...-1.f);
				glColor3f(1.0f, 0.0f, 0.0f);
				glBegin(GL_TRIANGLES);
					float wx, wy;
					wx = 0.f;
					wy = 1.f*size;
					glVertex2f(wx, wy);

					wx = -0.5f*size;
					wy = -0.5f*size;
					glVertex2f(wx, wy);

					wx = 0.5f*size;
					wy = -0.5f*size;
					glVertex2f(wx, wy);
				glEnd();

				glColor3f(1.0f, 1.0f, 1.0f);
				glBegin(GL_LINES);
					glVertex2f(0.f, 0.f);
					wx = 0.f;
					wy = AXES_SIZE;
					glVertex2f(wx, wy);

					glVertex2f(0.f, 0.f);
					wx = AXES_SIZE;
					wy = 0.f;
					glVertex2f(wx, wy);

					glVertex2f(0.f, 0.f);
					wx = -AXES_SIZE;
					wy = 0.f;
					glVertex2f(wx, wy);

				glEnd();
			glPopMatrix();

		glPopMatrix();
	}


	void paint_mouse_control(int mousex, int mousey){
		/* !!!
		mousey must be already transformed from the top-zero  y-coordinate
		of mouse to the bottom-zero y-coordinate
		!!!*/


		glColor3f(1.0f, 1.0f, 1.0f);
		float wx, wy;
		//glBegin(GL_POINTS);
		//	glVertex2f(mousex, mousey); //just to show the difference 
		//			// between the mouse y-zero and the bottom y-zero
		//glEnd();

		glBegin(GL_LINES);
		glVertex2f(mousex, mousey);
		glVertex2f(x, y);
		glEnd();

		glFlush();

	}

#define PI 3.14159f
	void get_tan_norm(){
		tx = sin(2.f*PI*dir / 360.f); ty = cos(2.f*PI*dir / 360.f);
		nx = ty; ny = -tx;
	}

	void adjust_robot(int number_steps_ahead,
		int number_turn_steps,
		float one_step_ahead,
		float one_turn_step){

		float deltax = tx*number_steps_ahead* one_step_ahead,
			deltay = ty*number_steps_ahead* one_step_ahead,
			deltadir = number_turn_steps* one_turn_step;

		x += deltax;
		y += deltay;
		dir += deltadir;

		get_tan_norm();
	}
};

GLint win_x = 500;
GLint win_y = 700;

robot vr;

void RenderScene(void)
{
	// Clear the window with current clearing color
	glClear(GL_COLOR_BUFFER_BIT);

	vr.paint();

	glFlush();
	//glutSwapBuffers();
}

// Setup the rendering state

void SetupRC(void)
{    // Set clear color to blue
	glClearColor(0.0f, 0.0f, 1.0f, 1.0f);
}
///////////////////////////////////////////////////////////
// Called by GLUT library when the window has changed size

void ChangeSize(GLsizei w, GLsizei h)
{
	GLfloat aspectRatio;
	// Prevent a divide by zero
	if (h == 0)
		h = 1;
	// Set Viewport to window dimensions
	glViewport(0, 0, w, h);
	robot wvr(w, h);
	wvr.dir = vr.dir;
	wvr.get_tan_norm();
	vr = wvr;
	// Reset coordinate system
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	win_x = w;
	win_y = h;
	glOrtho(0.0, win_x, 0.0, win_y, 1.0, -1.0);

	//cout << "w=" << w << "  h=" << h << endl;

}

///////////////////////////////////////////////////////////
#define TURN_THRESHOLD 50.f
#define AHEAD_THRESHOLD 25.f
#define CONTROLS 4
void SpecialKeys(int key, int x, int y)
{

	if (key == GLUT_KEY_F1)//Rotate camera around scene center in Oyz plane
	{
		/* !!!
		transform from the top-zero  y-coordinate
		of mouse to the bottom-zero y-coordinate
		!!!*/
		int y1 = win_y - y;

		vr.paint_mouse_control(x, y1);

		//cout << "x=" << x << "  y1=" << y1 << endl;

		float wx = x - vr.x, wy = y1 - vr.y,

			control_turn = wx*vr.nx + wy*vr.ny,
			absct = control_turn>0.f ? control_turn : -control_turn,
			control_ahead = wx*vr.tx + wy*vr.ty,
			absca = control_ahead>0.f ? control_ahead : -control_ahead;

		//clamping controls
		if (absct > TURN_THRESHOLD)control_turn =
			control_turn > 0.f ? TURN_THRESHOLD : -TURN_THRESHOLD;
		if (absca > AHEAD_THRESHOLD)control_ahead =
			control_ahead > 0.f ? AHEAD_THRESHOLD : 0.f/*-AHEAD_THRESHOLD*/;
		float deltaT = TURN_THRESHOLD / CONTROLS,
			deltaA = AHEAD_THRESHOLD / CONTROLS;
		int dct = control_turn / deltaT,
			dca = control_ahead / deltaA;

		vr.adjust_robot(dca, dct, deltaA, deltaT);

		glutPostRedisplay(); return;
	}

}

void mouse(int button, int state, int x, int y){
	if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN)
	{
		/* !!!
		transform from the top-zero  y-coordinate
		of mouse to the bottom-zero y-coordinate
		!!!*/
		int y1 = win_y - y;

		vr.paint_mouse_control(x, y1);

		//cout << "x=" << x << "  y1=" << y1 << endl;

		float wx = x - vr.x, wy = y1 - vr.y,

			control_turn = wx*vr.nx + wy*vr.ny,
			absct = control_turn>0.f ? control_turn : -control_turn,
			control_ahead = wx*vr.tx + wy*vr.ty,
			absca = control_ahead>0.f ? control_ahead : -control_ahead;

		//clamping controls
		if (absct > TURN_THRESHOLD)control_turn =
			control_turn > 0.f ? TURN_THRESHOLD : -TURN_THRESHOLD;
		if (absca > AHEAD_THRESHOLD)control_ahead =
			control_ahead > 0.f ? AHEAD_THRESHOLD : 0.f/*-AHEAD_THRESHOLD*/;
		float deltaT = TURN_THRESHOLD / CONTROLS,
			deltaA = AHEAD_THRESHOLD / CONTROLS;
		int dct = control_turn / deltaT,
			dca = control_ahead / deltaA;

		vr.adjust_robot(dca, dct, deltaA, deltaT);

		glutPostRedisplay(); return;

	}
	else{
		//using mouse for selection
	}
		return;

}

void main(int argc, char *argv[])
{
	glutInitWindowSize(win_x, win_y);
	glutInitWindowPosition(140, 140);
	glutInitDisplayMode(GLUT_RGB | GLUT_SINGLE);
	glutInit(&argc, argv);
	glutCreateWindow("Bounce");
	glutDisplayFunc(RenderScene);
	glutReshapeFunc(ChangeSize);
	//glutSpecialFunc(SpecialKeys);
	glutMouseFunc(mouse);

	SetupRC();
	glutMainLoop();
}