#define _USE_MATH_DEFINES
#include <cmath>

#include <windows.h>
#include <AR/gsub.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/ar.h>

#include <iostream>
#include <vector>
#include <map>
#include <assert.h>

#  include <GL/gl.h>
#  include <GL/glut.h>


#include "Roomba.h"

#include "RVOSimulator.h"

int hRes, vRes, hLen, vLen;
int running;
std::vector<Roomba> roombas;

//// RVO STUFF //////
float RVO_TIME_STEP =  (float)(0.20f);
/////////////////////
float lastFrameUpdate = (float)GetTickCount();


RVO::RVOSimulator* rvosim;

void rvoInit();
void init(char* cParamName);
void destroy();

void keyEvent(unsigned char key, int x, int y);
void mainLoop();

void getCameraOrientation(ARMarkerInfo* markerInfo, double& theta);
void getCameraPosition(ARMarkerInfo* markerInfo, double& x, double& y);

int main(int argc, char **argv)
{
	running = 1;
	char *ports[4] = {
		TEXT("\\\\.\\COM7"), 
		TEXT("\\\\.\\COM3"), 
		TEXT("\\\\.\\COM5"), 
		TEXT("\\\\.\\COM9")};
	char *patterns[4] = {
		"Data/patt.hiro", 
		"Data/patt.kanji", 
		"Data/patt.sample1", 
		"Data/patt.sample2"};
	
	for(int i = 0; i < 4; ++i)
	{
		Roomba roomba(ports[i], patterns[i]);
		roombas.push_back(roomba);
		//roomba.drive(0,10);
		Sleep(200);
	}

/*
	Roomba roomba3(ports[0], "Data/patt.sample1");
	roombas.push_back(roomba3);
	roomba3.drive(100,10);
	Sleep(200);

	Roomba roomba2(ports[3], "Data/patt.sample2");
	roombas.push_back(roomba2);
	roomba2.drive(100,10);
	Sleep(200);

	Roomba roomba1(ports[7], "Data/patt.kanji");
	roombas.push_back(roomba1);
	roomba1.drive(100,10);
	Sleep(200);
*/

	hLen = 1024;
	vLen = 768;

	rvoInit();

 	init("Data/camera_para.dat");

	return 0;
}

void rvoInit()
{

//#define OBSTACLE_BOX
//#define OBSTACLE_PLUS
#define WAYPOINT_DIS 150
#define WALL_X_POS 1500
#define WALL_Y_POS 1000
#define GOAL_WALL_OFFSET 50

	RVO::RVOSimulator* rvosim = RVO::RVOSimulator::Instance();
	rvosim->setTimeStep(RVO_TIME_STEP);
	rvosim->setAgentDefaults(16, 1000000.0f, 10, 240.0f, 400.0f, 200.0f, 300.0f, 2000.0f);

	// outer walls
	rvosim->addObstacle(RVO::Vector2(-WALL_X_POS, WALL_Y_POS),  RVO::Vector2(WALL_X_POS, WALL_Y_POS));
	rvosim->addObstacle(RVO::Vector2(WALL_X_POS, WALL_Y_POS),   RVO::Vector2(WALL_X_POS, -WALL_Y_POS));
	rvosim->addObstacle(RVO::Vector2(WALL_X_POS, -WALL_Y_POS),  RVO::Vector2(-WALL_X_POS, -WALL_Y_POS));
	rvosim->addObstacle(RVO::Vector2(-WALL_X_POS, -WALL_Y_POS), RVO::Vector2(-WALL_X_POS, WALL_Y_POS));

	//rectangle
#ifdef OBSTACLE_BOX
#define CIRCLE_RESOLUTION M_PI / 6
#define RADIUS 300.0f
	for(double theta=0; theta<2*M_PI; theta += CIRCLE_RESOLUTION)
	{
		rvosim->addObstacle(RVO::Vector2(cos(theta)*RADIUS, sin(theta)*RADIUS),  RVO::Vector2(cos(theta+CIRCLE_RESOLUTION)*RADIUS, sin(theta+CIRCLE_RESOLUTION)*RADIUS));
	}
/*
#define BOX_X_POS 300
#define BOX_Y_POS 300
	rvosim->addObstacle(RVO::Vector2(-BOX_X_POS, BOX_Y_POS),  RVO::Vector2(BOX_X_POS, BOX_Y_POS));
	rvosim->addObstacle(RVO::Vector2(BOX_X_POS, BOX_Y_POS),   RVO::Vector2(BOX_X_POS, -BOX_Y_POS));
	rvosim->addObstacle(RVO::Vector2(BOX_X_POS, -BOX_Y_POS),  RVO::Vector2(-BOX_X_POS, -BOX_Y_POS));
	rvosim->addObstacle(RVO::Vector2(-BOX_X_POS, -BOX_Y_POS), RVO::Vector2(-BOX_X_POS, BOX_Y_POS));
*/
#endif

	//walls with openings
#ifdef OBSTACLE_PLUS
#define PLUS_X_POS 400
#define PLUS_Y_POS 400
	rvosim->addObstacle(RVO::Vector2(0, PLUS_Y_POS),  RVO::Vector2(0, 0));
	rvosim->addObstacle(RVO::Vector2(PLUS_X_POS, 0),  RVO::Vector2(0, 0));
	rvosim->addObstacle(RVO::Vector2(0, -PLUS_Y_POS),  RVO::Vector2(0, 0));
	rvosim->addObstacle(RVO::Vector2(-PLUS_X_POS, 0),  RVO::Vector2(0, 0));

#define INNER_X_POS 800
#define OUTER_X_POS 1300
#define INNER_Y_POS 800
#define OUTER_Y_POS 1100
	rvosim->addObstacle(RVO::Vector2(0, OUTER_Y_POS),   RVO::Vector2(0, INNER_Y_POS));
	rvosim->addObstacle(RVO::Vector2(OUTER_X_POS, 0),  RVO::Vector2(INNER_X_POS, 0));
	rvosim->addObstacle(RVO::Vector2(0, -OUTER_Y_POS),   RVO::Vector2(0, -INNER_Y_POS));
	rvosim->addObstacle(RVO::Vector2(-OUTER_X_POS, 0),  RVO::Vector2(-INNER_X_POS, 0));
#endif

	for(int i = -(WALL_X_POS-WAYPOINT_DIS); i <= (WALL_X_POS-WAYPOINT_DIS); i += WAYPOINT_DIS)
	{
		for(int j = -(WALL_Y_POS-WAYPOINT_DIS); j <= (WALL_Y_POS-WAYPOINT_DIS); j += WAYPOINT_DIS)
		{
			//rvosim->addRoadmapVertex(RVO::Vector2(i, j));
		}
	}
	rvosim->setRoadmapAutomatic(true);


	rvosim->addGoal(RVO::Vector2(-(WALL_X_POS-GOAL_WALL_OFFSET),(WALL_Y_POS-GOAL_WALL_OFFSET)));
	rvosim->addGoal(RVO::Vector2((WALL_X_POS-GOAL_WALL_OFFSET),(WALL_Y_POS-GOAL_WALL_OFFSET)));
	rvosim->addGoal(RVO::Vector2((WALL_X_POS-GOAL_WALL_OFFSET),-(WALL_Y_POS-GOAL_WALL_OFFSET)));
	rvosim->addGoal(RVO::Vector2(-(WALL_X_POS-GOAL_WALL_OFFSET),-(WALL_Y_POS-GOAL_WALL_OFFSET)));
	
	/*
	rvosim->addGoal(RVO::Vector2(-900,800));
	rvosim->addGoal(RVO::Vector2(900,800));
	rvosim->addGoal(RVO::Vector2(900,-800));
	rvosim->addGoal(RVO::Vector2(-900,-800));
	*/


	for(unsigned  int i = 0; i < roombas.size(); ++i)
	{
		rvosim->addAgent(RVO::Vector2(0,0), rand()%4);
	}

    // TODO: Add obstacles here.

	rvosim->setTimeStep(RVO_TIME_STEP);
	rvosim->initSimulation();
}

void keyEvent( unsigned char key, int x, int y)
{
    if( key == 0x1b )
	{
		for(unsigned int i=0; i<roombas.size(); i++)
			roombas[i].drive(0,0);

        destroy();

        exit(0);
    }
}

struct AgentData
{
	double x, y;
	double theta;
	bool stale;
};

std::map<int, AgentData> oldData;

void updateRVOFromCamera(std::map<int, AgentData> &currentData, ARMarkerInfo* detectedMarkerInfo, int detectedMarkerCount)
{
	for (unsigned int i = 0; i < roombas.size(); i++)
	{
		int k = -1;

		for (int j = 0; j < detectedMarkerCount; j++ )
		{
			if (roombas.at(i).getPatternID() == detectedMarkerInfo[j].id)
			{
				if (k == -1)
				{
					k = j;
				}
		        else
				{
					if (detectedMarkerInfo[k].cf < detectedMarkerInfo[j].cf)
					{
						k = j;
					}
				}
			}
		}

		if( k != -1 )
		{
			//double x, y, theta;

			AgentData data;

			getCameraPosition(&(detectedMarkerInfo[k]), data.x, data.y);
			getCameraOrientation(&(detectedMarkerInfo[k]), data.theta);
			printf("cam_tht[%i]:\t%.2f\n", i, data.theta);
			//std::cout << "cam_tht[" << i << "]:\t" << data.theta << std::endl;
			printf("cam_pos[%i]:\t%.2f %.2f\n", i, data.x, data.y);
			//std::cout << "cam_pos[" << i << "]:" << data.x << " " << data.y << std::endl;

			data.stale = false;

			currentData[i] = data;

			//std::cout << i << " " << detectedMarkerInfo[k].cf << " " << detectedMarkerInfo[k].id << " " << x << " " << y << " " << theta << std::endl;
			rvosim->setAgentPosition(i, RVO::Vector2((float)data.x, (float)data.y));
			rvosim->setAgentOrientation(i, (float)data.theta);

			if(rvosim->getAgentReachedGoal(i))
			{
				for(int u = 0; u < 1000; ++u) std::cout<< rvosim->getAgentGoal(i);
				
				int lastGoal = rvosim->getAgentGoal(i);
				int nextGoal = lastGoal;// = (lastGoal + 2)%4;
				while(lastGoal == (nextGoal = rand()%4))
				{ std::cout << nextGoal; }
				rvosim->setAgentGoal(i, nextGoal);
				
				for(int u = 0; u < 1000; ++u) std::cout<< rvosim->getAgentGoal(i);
			}
		}
		else
		{
			currentData[i] = oldData[i];
			currentData[i].stale = true;

			rvosim->setAgentPosition(i, RVO::Vector2((float)currentData[i].x, (float)currentData[i].y));
			rvosim->setAgentVelocity(i, RVO::Vector2(0,0));
			rvosim->setAgentOrientation(i, (float)currentData[i].theta);
		}
	}

	oldData = currentData;
}


void updateRoomba(std::map<int, AgentData> &currentData)
{
	for (unsigned int i = 0; i < roombas.size(); i++)
	{
		if( ( currentData.find(i)!= currentData.end() ) &&
		    (!currentData.find(i)->second.stale)  )
		{
			std::map<int, AgentData>::iterator curData = currentData.find(i);

			RVO::Vector2 newVelocity = rvosim->getAgentVelocity(i);
			double speed = sqrt(newVelocity.x() * newVelocity.x() + newVelocity.y() * newVelocity.y());
			//std::cout << "rvo_vel[" << i << "]:" << newVelocity.x() << " " << newVelocity.y() << std::endl;

			double rvoTheta = atan2(newVelocity.y(), newVelocity.x());
			printf("rvo_tht[%i]:\t%.2f\n", i, rvoTheta);

			RVO::Vector2 v;
			v = rvosim->getAgentPosition(i);
			//std::cout << "rvo_pos[" << i << "]:" << v.x() << " " << v.y() << std::endl;
			v = rvosim->getGoalPosition(i);
			//std::cout << "rvo_gol[" << i << "]:" << v.x() << " " << v.y() << std::endl;

			//std::cout << "goal["<<i<<"]:\t"<< rvosim->getAgentGoal(i) << std::endl;

			//std::cout << "cam_theta[" << i << "]:\t" << curData->second.theta << std::endl;
			//std::cout << "rvo_theta[" << i << "]:\t" << rvoTheta << std::endl;

			double thetaDiff = rvoTheta - curData->second.theta;
			if(thetaDiff >= M_PI+0.2)
				thetaDiff -= 2*M_PI;
			if(thetaDiff <= -M_PI)
				thetaDiff += 2*M_PI;

			printf("rvo_diff[%i]:\t%.2f\n", i, thetaDiff);

			short turnSpeed = 150;
			//short turnSpeed = (short)(speed);
			short slowArc = 1400;
			short fastArc = 100;
			short fastestArc = 10;
			char turnDir = thetaDiff < 0 ? -1 : 1;

			if(fabs(thetaDiff) >=  M_PI/4)
			{
				//roombas[i].driveWheels(turnDir*turnSpeed, -turnDir*turnSpeed);
				roombas[i].drive((short)(speed), turnDir * fastestArc);
			}
			else if(fabs(thetaDiff) < M_PI/8)
			{
				roombas[i].drive((short)(speed), turnDir * slowArc);
			}
			else
			{
				roombas[i].drive((short)(speed), turnDir * fastArc);
			}
		}
		else
		{
			//roombas[i].drive((short)(250), 1500);
			//roombas[i].driveWheels(0, 0);
			//roombas[i].driveWheels(250,150);
			roombas[i].driveWheels(-75, -75);
		}
	}
}


void draw(std::map<int, AgentData> &currentData)
{
	/*
	for (unsigned int i = 0; i < roombas.size(); i++)
	{
		RVO::Vector2 rvoPos = rvosim->getAgentPosition(i);
		RVO::Vector2 rvoVelocity = rvosim->getAgentVelocity(i);
		argLineSeg(rvoPos.x(), rvoPos.y(), rvoPos.x()+rvoVelocity.x(), rvoPos.y()+rvoVelocity.y(), 100, 100);
	}
	*/
}

void mainLoop(void)
{
	float startTick = GetTickCount();
	lastFrameUpdate = startTick;

	//std::cout << std::endl;


	rvosim = RVO::RVOSimulator::Instance();
	std::map<int, AgentData> currentData;

	ARUint8* imageData;
    ARMarkerInfo* detectedMarkerInfo;
	int detectedMarkerCount;

	if ((imageData = (ARUint8*) arVideoGetImage()) == 0)
	{
        arUtilSleep(2);

        return;
    }

	argDrawMode2D();
    argDispImage(imageData, 0, 0);
	arVideoCapNext();

	if (arDetectMarker(imageData, 100, &detectedMarkerInfo, &detectedMarkerCount) < 0)
	{
		destroy();

		exit(1);
	}

	
	updateRVOFromCamera(currentData, detectedMarkerInfo, detectedMarkerCount);

	static bool firstTime = true;
	if(firstTime)
	{
		// pick opposite goal
		for(int i = 0; i < 4; ++i)
		{
			RVO::Vector2 pos = rvosim->getAgentPosition(i);
			int goal;// = rvosim->getAgentGoal(i);
			double dist = -1;
			for(int j = 0; j < 4; ++j)
			{
				RVO::Vector2 g = rvosim->getGoalPosition(j);
				
				RVO::Vector2 diff = pos - g;
				double diffSq = diff * diff;
				if(diffSq > dist)
				{
					goal = j;
					dist = diffSq;
				}
			}

			rvosim->setAgentGoal(i, goal);
		}

		firstTime = false;
	}

	rvosim->doStep();
	updateRoomba(currentData);

	draw(currentData);

    // TODO: execute RVO instructions

	


    //Sleep((DWORD)(1000*RVO_TIME_STEP));  // sleep for the RVO time step.

	argSwapBuffers();

	float endTick = GetTickCount();

	if((endTick - startTick) < RVO_TIME_STEP*1000)
	{
		Sleep(RVO_TIME_STEP*1000 - (endTick - startTick));
	}
}


void init(char* cParamName)
{	
	ARParam cParam, wParam;
	
	if (arVideoOpen("") < 0)
	{
		std::cerr << "Cannot open video" << std::endl;

		exit(1);
	}

    if (arVideoInqSize(&hRes, &vRes) < 0)
	{
		std::cerr << "Cannot get video size" << std::endl;

		exit(1);
	}

	if (arParamLoad(cParamName, 1, &wParam) < 0)
	{
		std::cerr << "Cannot load parameters" << std::endl;

        exit(1);
    }

	arParamChangeSize(&wParam, hRes, vRes, &cParam);
    arInitCparam(&cParam);
    arParamDisp(&cParam);
	argInit(&cParam, 1.0, 0, 0, 0, 0);

	for (unsigned int i = 0; i < roombas.size(); i++)
	{
		int patternID;

		if ((patternID = arLoadPatt(roombas.at(i).getPattern())) < 0)
		{
			std::cout << "Pattern ID: " << patternID << std::endl;

			std::cerr << "Cannot load pattern" << std::endl;

			std::cout << patternID << std::endl;

			exit(1);
		}

		roombas.at(i).setPatternID(patternID);
	}

	arVideoCapStart();
	argMainLoop(NULL, keyEvent, mainLoop);
}

void destroy()
{
	arVideoCapStop();
    arVideoClose();
    argCleanup();
}

void getCameraOrientation(ARMarkerInfo* markerInfo, double& theta)
{
	int dir = markerInfo->dir;

	if (dir == 1)
	{
		dir = 3;
	}
	else if (dir == 3)
	{
		dir = 1;
	}

	double x = markerInfo->vertex[(0 + dir) % 4][0] - markerInfo->vertex[(2 + dir) % 4][0] + markerInfo->vertex[(1 + dir) % 4][0] - markerInfo->vertex[(3 + dir) % 4][0];
	double y = markerInfo->vertex[(0 + dir) % 4][1] - markerInfo->vertex[(2 + dir) % 4][1] + markerInfo->vertex[(1 + dir) % 4][1] - markerInfo->vertex[(3 + dir) % 4][1];

	theta = atan2(y, -x);
}

void getCameraPosition(ARMarkerInfo* markerInfo, double& x, double& y)
{
	x = 3 * (hRes / 2.0 - markerInfo->pos[0]);
	y = 3 * (markerInfo->pos[1] - vRes / 2.0);
}