/* 
 * Robotics 3D Map Tool
 * program.cpp
 *
 * Developer: Kamil A. Wnuk
 * Advisor: Professor Zach Dodds
 * Harvey Mudd College
 *
 * Updated: 5/20/04
 */

#include <windows.h>

#define GLUT_DISABLE_ATEXIT_HACK 

#include <GL/glut.h>


#include <iostream>

#include <fstream>

#include <vector>

#include "program.h"



using namespace std;


Program& Program::Get()
{
	static Program p;
	return p;
}


Program::Program():
	window_wid(0), window_ht(0),
	view_zoom(1.0), view_dx(0), view_dy(0),
	controlState(CONTROL_NONE)
{
	view_dx = 0.0;
	view_dy = 0.0;
	view_th = 0.0;
	view_zoom = 1.0;
}


Program::~Program()
{
	DrawableList::iterator lastDrawable = all_Drawables.end();
	for (DrawableList::iterator i = all_Drawables.begin();
			i != lastDrawable; )
	{
		delete (*i);
		i = all_Drawables.erase(i);
	}
}


bool Program::Init( const int & wid, const int & ht )
{
	cout << "Init OpenGL" << endl;

	window_wid = wid;
	window_ht = ht;

	// initialize viewing system

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D( 0, window_wid, 0, window_ht);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	// enable depth buffering

	glEnable(GL_DEPTH_TEST);
	//	glEnable(GL_BLEND);

	//	glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);


	// initialize background color to black

	glClearColor(0.5,0.5,0.5,0);	

	// shading model

	// glShadeModel(GL_SMOOTH); // Goraud Shading

	// glShadeModel(GL_FLAT); // takes the first vertex's color


	LoadMap();
	LoadRobot();
	LoadLog();

	cout << "Init Finished" << endl;
	return true;
}


void Program::Render( )
{
	glLoadIdentity();

	// Control zoom

	// glPixelZoom (view_zoom, view_zoom);


	// Move the view around


	glViewport(0,0,window_wid,window_ht); // this should never change... all 

	// changes to our window's view of the global coordinate system

	// should be done in the following...

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

    // here is where we set our window's view of the global coordinate system,

	// which (at start) should have the origin in the center of the window,

	// with the robot's origin at this origin and 1 pixel/centimeter

	// 

	// certainly, changing the view will change all of these parameters!

	//

	// see the key event handling method to get a sense of how we can change things...


    // center at the origin (plus our user-specified view_dx and view_dy

    double lower_left_x = -window_wid/2;
	double lower_left_y = -window_ht/2;
	double upper_right_x = lower_left_x + window_wid;
	double upper_right_y = lower_left_y + window_ht; // these should be computed once per window resize, eventually...


	gluOrtho2D(lower_left_x,upper_right_x,lower_left_y,upper_right_y);
	glTranslatef(view_dx, view_dy, 0.0);
	glRotatef(view_th,0.0,0.0,1.0);
    glScalef(view_zoom,view_zoom,1.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();



	// clear buffers

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// draw all drawable items

	
	DrawableList::iterator lastDrawable = all_Drawables.end();
	for (DrawableList::iterator i = all_Drawables.begin();
			i != lastDrawable; ++i )
	{
		(*i)->Draw();
	}
	

	glutSwapBuffers();
}


void Program::Reshape(const int & wid, const int & ht)
{
	window_wid = wid;
	window_ht = ht;

	glViewport(0,0,window_wid,window_ht);
	//glMatrixMode(GL_PROJECTION);

	//glLoadIdentity();

	//gluOrtho2D(0.0, window_wid + view_dx, 0.0, window_ht + view_dy);

	//glMatrixMode(GL_MODELVIEW);

	//glLoadIdentity();

}


/*
 * LoadRobot
 */
void Program::LoadRobot()
{
	robot = new Robot();
	all_Drawables.push_back(robot);
}

void Program::LoadMap()
{
	map = new Map();
	all_Drawables.push_back(map);
}

void Program::LoadLog()
{
	log = new Log();
}

 
//time intervals

void Program::Timestep( float dt )
{
	glutPostRedisplay();

	//redraws path taken logged in log_in.txt

	if(log->inlog==true)
	{
		double x,y,th;
		log->getNextPos(&x,&y,&th);
		if (x < -100000000000)
		{
			log->inlog = false;
		}
		else
		{
			robot->setCoordinates(x,y,th);
		}
	}
}


void Program::KeyEvent( char key, bool key_down )
{
	//key = tolower(key);

	cout << "KEY " << key << ((key_down)?" down":" up") << endl;
	
	if (key_down) {

		// move robot up

		if (key == 'i') {
			robot->deltaCoordinates(0,10,0);
		}
		
		// move robot down

		if (key == 'k') {
			robot->deltaCoordinates(0,-10,0);
		}
		
		// move robot to left

		if (key == 'j') {
			robot->deltaCoordinates(-10,0,0);
		}
		
		// move robot to right

		if (key == 'l') {
			robot->deltaCoordinates(10,0,0);
		}

		// rotate robot counterclockwise

		if (key == 'o') {
			robot->deltaCoordinates(0,0,10);
		}

		// rotate robot clockwise

		if (key == 'u') {
			robot->deltaCoordinates(0,0,-10);	
		}

		// move map up

		if (key == 'e') {
			view_dx += 0; view_dy += 10; view_th += 0; view_zoom += 0;
		}
		
		// move map down

		if (key == 'd') {
			view_dx += 0; view_dy -= 10; view_th += 0; view_zoom += 0;
		}

		// move map to left

		if (key == 's') {
			view_dx -= 10; view_dy += 0; view_th += 0; view_zoom += 0;
		}

		// move map to right

		if (key == 'f') {
			view_dx += 10; view_dy += 0; view_th += 0; view_zoom += 0;
		}

		// rotate map counterclockwise

		if (key == 'w') {
			view_dx -= 0; view_dy += 0; view_th += 10; view_zoom += 0;
		}

		// rotate map clockwise

		if (key == 'r') {
			view_dx += 0; view_dy += 0; view_th -= 10; view_zoom += 0;
		}

		// zoom in

		if (key == 'z') {
			view_dx += 0; view_dy += 0; view_th += 0; view_zoom *= 1.1;
		}
		
		// zoom out

		if (key == 'x') {
			view_dx += 0; view_dy += 0; view_th += 0; view_zoom /= 1.1;
		}

		// show the robot IR sensors

		if (key == 'c') {
			robot->show_IR = !robot->show_IR;
		}

		// show the robot's sonar sensor

		if (key == 'a') {
			robot->show_sonar = !robot->show_sonar;
		}

	    // show the robot's sonar sensor

		if (key == 'A') {
			//robot->computeSonarDistance();

			//cout << "son_dist is " << robot->son_dist << endl;

			robot->show_particle_sonar = !robot->show_particle_sonar;
		}

		// draw particles 

		if (key == 'p') {
			robot->draw_particles = !robot->draw_particles;
		}

		// show the path taken by the robot

		if (key == 'v') {
			robot->show_Hist = !robot->show_Hist;
		}

        // clear the history of the robot (the path)

		if (key == 'h') {
			robot->x_hist.clear();
			robot->y_hist.clear();
			robot->th_hist.clear();
		}

		// updates the particle position when pushed

		if (key == 'm') {
			Program::Get().robot->MoveParticles();
		}

		// updates particle probability

		if (key == 'n') {
			Program::Get().robot->UpdateParticleProbabilities();
		}

		// updates the particles based on probability

		if (key == 'b') {
			Program::Get().robot->ResampleParticles();
		}

		/*
		//allows the logging to take place
		if (key == '1') {
			log->outlog = !log->outlog;
		}

		//emulates log stored in folder
		if (key == '2') {
			log->inlog = !log->inlog;
		}
		*/

		// clear particles and set N of them at the robot's current pose...

		if (key == '1') {
			int N = 100; // getNumParticlesFromUser();

			Program::Get().robot->setupParticles(N,Robot::AT_POSE);
		}

		// clear particles and set N of them at the robot's current position...

		// this time with random orientation

		if (key == '2') {
			int N = 500; // getNumParticlesFromUser();

			Program::Get().robot->setupParticles(N,Robot::AT_POSITION);
		}

		// clear particles and set N of them all over the place (but in bounds)

		if (key == '3') {
			int N = 500; // getNumParticlesFromUser();

			Program::Get().robot->setupParticles(N,Robot::ANYWHERE);
		}

		// clear particles and set N of them all over the place (but in bounds)

		// sampled by pulling back from the environment!

		if (key == '4') {
			int N = 500; // getNumParticlesFromUser();

			Program::Get().robot->setupParticles(N,Robot::WALL_RELATIVE);
		}

	
		// quit program

		if (key == 'q') {
			cout << "Exit" << endl;
			exit(0);
		}
	}
}


void Program::MouseClick (int button, int button_state, int x, int y)
{
	;
	/*
	prevMousePos_x = x;
	prevMousePos_y = y;

	// Print the mouse position (x,y) when the left button is clicked
	// The mouse origin is in the upper left corner, positive y is down,
	// positive x is right
	if (button == GLUT_LEFT_BUTTON && button_state == 0)
	{
		// Allocate space for one pixel of data
		unsigned char* requested_pixel = new byte[4];

		// read just one pixel by specifying a 1x1 pixel rectangle
		glReadPixels(x, (window_ht-1)-y, 1, 1, GL_RGBA, 
			GL_UNSIGNED_BYTE, requested_pixel);
		
		if (requested_pixel != NULL)
			cout << " mousePos = ( " << x << ", " << y 
				<< " )  color = ( " 
				<< (int)requested_pixel[0] << ", " << (int)requested_pixel[1] << ", "
				<< (int)requested_pixel[2] << ", " << (int)requested_pixel[3] << 
				" )" << endl;	

		delete[] requested_pixel;
	}
	// right button controls the point you are looking at in X and Y
	else if (button == GLUT_RIGHT_BUTTON)
	{
		if( button_state == GLUT_DOWN )
		{
			controlState = CONTROL_TRANSLATE;
		}
		else
		{
			controlState = CONTROL_NONE;
		}
	}*/
	//// middle button controls the point you are looking at in X and Y

	//else if (button == GLUT_MIDDLE_BUTTON)

	//{

	//	if( button_state == GLUT_DOWN )

	//	{

	//		controlState = CONTROL_TRANSLATE;

	//	}

	//	else

	//	{

	//		controlState = CONTROL_NONE;

	//	}

	//}

}


void Program::MouseMove (int x, int y)
{
	/*
	const float SENSITIVITY = 0.004f;
	const double VIEWSCALE = 60.0f;

	int delta_x = x - prevMousePos_x;
	int delta_y = y - prevMousePos_y;
	prevMousePos_x = x;
	prevMousePos_y = y;

	if( controlState == CONTROL_TRANSLATE )
	{
		MoveView( delta_x * SENSITIVITY * VIEWSCALE,
			-delta_y * SENSITIVITY * VIEWSCALE);	//Moving mouse up is negative
													// so reverse it
	}*/
}