// header files

#include <stdio.h>

#include "SmallSocket.h"

#include <conio.h>

#include <GL/glut.h>

#include "program.h"



using namespace std;


// function prototypes

void display(void);
void reshape(int width, int height);
void init();
void idle();
//void keyboard(unsigned char key, int x, int y);

void keyboard_down(unsigned char key, int x, int y);
void keyboard_up(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);

float move=0;
const int WIN_WID = 1270; 
const int WIN_HT = 600;

DWORD WINAPI ThreadFunc( LPVOID lpParam ) 
{ 
	SmallSocket server(SmallSocket::SERVER);
	string s("Hi\n");

	char recvbuf[128];

	while (1)
	{
		server.receive(recvbuf,128);

		//quits the program

		if (recvbuf[0] == 'q')
		{
			server.sendout("q");
			exit(0);
			break;
		}

		// toggle showing the path history of the robot

		else if (recvbuf[0] == 'v')
		{
			bool& history = Program::Get().robot->show_Hist;
			history = !history;
		}

		// Get sonar data (distance, theta)

		else if (recvbuf[0] == 's')
		{
			char s [128];
			double T;
			double D;

			sscanf(recvbuf, "%s %lf %lf", s, &D, &T);
			Program::Get().robot->setSonarStatus(D, T);
		}

		// clear the path history of the robot

		else if (recvbuf[0] == 'h')
		{
			Program::Get().robot->x_hist.clear();
			Program::Get().robot->y_hist.clear();
			Program::Get().robot->th_hist.clear();
		}

		// toggle showing the particles of the robot

		else if (recvbuf[0] == 'p')
		{
			bool& draw = Program::Get().robot->draw_particles;
			draw = !draw;
		}

		//move the robot based on data that the client sends  "translate"

		else if (recvbuf[0] == 't')
		{
			char s [128];
			double X;
			double Y;
			double T;

			sscanf(recvbuf, "%s %lf %lf %lf", s, &X, &Y, &T);
			Program::Get().robot->setCoordinates(X,Y,T);
		}

		//move the robot based on data that the client sends  "delta"

		else if (recvbuf[0] == 'd')
		{
			char s [128];
			double X;
			double Y;
			double T;

			sscanf(recvbuf, "%s %lf %lf %lf", s, &X, &Y, &T);
			Program::Get().robot->deltaCoordinates(X,Y,T);
		}

		else if (recvbuf[0] == '7')
		{
			Program::Get().robot->UpdateParticles();
		}
		//change the sensor value of the robot based on client data

		else if (recvbuf[0] == 'y')
		{
			char s[128];
			double L;
			double C;
			double R;
			double cam;

			sscanf(recvbuf, "%s %lf %lf %lf %lf", s, &L,&C,&R, &cam);
			Program::Get().robot->setIR(L,C,R);
			Program::Get().robot->cam_red = cam;
			Program::Get().robot->is_red = 0.0;
		}

	    //Updates and moves the particles from set point to new point

		else if (recvbuf[0] == 'u' )
		{
			Program::Get().robot->UpdateParticles();
		}

		//Updates and moves the particles from set point to new point

		else if (recvbuf[0] == 'm' )
		{
			Program::Get().robot->MoveParticles();
		}

		//Updates the probabilities of each particle

		else if (recvbuf[0] == 'n' )
		{
			Program::Get().robot->UpdateParticleProbabilities();
		}

		//Updates and copies particles based on particle probabilities

		else if (recvbuf[0] == 'b' )
		{
			Program::Get().robot->ResampleParticles();
		}

		//logging the position of the robot

		else if (recvbuf[0] == 'L')
		{
			char s [128];
			double x,y,th;
			
			sscanf(recvbuf, "%s %lf %lf %lf", &s, &x, &y, &th);
			//cout << "Received the following values:  " << x << " " << y << " " << th << endl;

			Program::Get().log->logPos(x,y,th);

		}
		
		//logging position of the robot when it stores an image

		else if (recvbuf[0] == 'I') 
		{
			char s [128];
			double x,y,th;
			int frame, red;

			sscanf(recvbuf, "%s %lf %lf %lf %d %d", &s, &x, &y, &th, &frame, &red);
			Program::Get().log->log_image(x,y,th,frame);
			
			
			if (red == 1)
			{
				Program::Get().robot->is_red = 1;
			}
			else
			{
				Program::Get().robot->is_red = 3;
			}
		}

		//emulates the previous path logged by reading in the log file

		else if (recvbuf[0] == 'E')
		{
			if (Program::Get().log->inlog == false)
			{
				Program::Get().log->inlog = true;
			}
			else 
			{
				Program::Get().log->inlog = false;
			}
		}

		//case when server receives message that

		// will not be used for the map

		else
		{
			;
		}

		// here is the response (it's required!)

		server.sendout(s.c_str());
		
	}

	WSACleanup(); 

	return 1;
} 

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

	// initialize glut 

	glutInit(&argc, argv);

	// set window size

	glutInitWindowSize(WIN_WID,WIN_HT);

	// establish glut display parameters

	glutInitDisplayMode(GLUT_DOUBLE   | GLUT_RGB | GLUT_DEPTH);

	// create window

	glutCreateWindow("Feature Finding Tool");

	// register callback functions

	glutDisplayFunc(display);
	glutReshapeFunc(reshape); 
	glutKeyboardFunc(keyboard_down);
	glutKeyboardUpFunc(keyboard_up);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutIdleFunc(idle);

	// initalize opengl parameters

	if ( !Program::Get().Init(WIN_WID, WIN_HT) ){
		cout << "Error initializing OpenGL" << endl;
		return 0;
	}

	// start the server in another thread

    DWORD dwThreadId;
	DWORD dwThrdParam = 42.0;

	HANDLE hThread = CreateThread( 
        NULL,                        // default security attributes 

        0,                           // use default stack size  

        ThreadFunc,                  // thread function 

        &dwThrdParam,                // argument to thread function 

        0,                           // use default creation flags 

        &dwThreadId);                // returns the thread identifier 

 
	// Check the return value for success. 

	 
	if (hThread == NULL) 
	{
		printf(  "CreateThread failed.\n" ); 
	}
	else 
	{
		printf( "Server Thread succeeded!\n");
		// CloseHandle( hThread );

	}

	// Load the drawables that will be used

	//Program::Get().LoadRobot();


	// loop until something happens

	glutMainLoop();
	return 0;           
}


void reshape(int width, int height)
{
	Program::Get().Reshape(width, height);
}


void display()
{
	Program::Get().Render();	
}


static int last_tick = glutGet( GLUT_ELAPSED_TIME );

void idle()
{
	int new_time = glutGet( GLUT_ELAPSED_TIME );
	float dt = (new_time-last_tick)*0.001f;

	Program::Get().Timestep( dt );

	last_tick = new_time;
}


void keyboard_down( unsigned char key, int x, int y)
{
	//key = tolower(key);	//undo case sensitivity

	Program::Get().KeyEvent( key, true );
}


void keyboard_up(unsigned char key, int x, int y)
{
	//key = tolower(key);  //undo case sensitivity

	Program::Get().KeyEvent( key, false );
}


void mouse(int button, int button_state, int x, int y)
{
	Program::Get().MouseClick(button, button_state, x, y);
}


void motion(int x, int y)
{
	Program::Get().MouseMove(x, y);
}