#include <iostream>
#include <string>
#include <time.h>
#include <math.h>
#include <vector>
#include <queue>
#include <stdio.h>
#include <stdlib.h>
#include <map>
#include "cs_holder.h"

int const startX = 0;
int const startY = 0;

int threads = 0;

float defaultNewRobotPositionX = 0;
float defaultNewRobotPositionY = 0;
float defaultNewRobotRotation = 0;

#define M_PI 3.1415926535897932384626433832795
#define fullAngle 2*M_PI

CRITICAL_SECTION GlobalCriticalSection;
bool konec = false;

int const numberOfSensors = 8;

struct robotProperties
{
	std::string name;
	int Rcolor;
	int Gcolor;
	int Bcolor;
	bool active;
	float positionX;
	float positionY;
	float rotation;
	int leftMotorPower;
	int rightMotorPower;
	float leftMotorSpeed;
	float rightMotorSpeed;
	bool led;
	bool startButton;
	float leftEncoderValue;	//to do: prevod na float
	float rightEncoderValue;	//to do: prevod na float
	int leftServoValue;
	int rightServoValue;
	bool lockLeftServo;
	bool lockRightServo;
	int sensorValues[numberOfSensors];
	std::queue<char> rs232;
	std::map<int,float> distances;
};

std::map<int,robotProperties> robots;

void waitForConnectRobot()
{
	while(robots.size() == 0)
	{
		Sleep(1);
	}
	return;
}

float getDistance(float X1, float Y1, float X2, float Y2)
{
	float x = abs(X2-X1);
	float y = abs(Y1-Y2);
	return sqrt(x*x+y*y);
}

void posun(float & pocX, float & pocY, float delka, float uhel)
{
	pocX += delka * cos(uhel);
	pocY += delka * sin(uhel);
}

void rotaceKolemKola(float & pocX, float & pocY, float & uhel, float delkaOblouku, float sirskaRobota, int smer)	//smer=1 doprava, smer=-1 doleva
{
	float alpha = (delkaOblouku*fullAngle)/(2*M_PI*sirskaRobota) * smer;
	float delkaPosunu = sqrt( 2*sirskaRobota*sirskaRobota - 2*sirskaRobota*sirskaRobota*cos(alpha) )/2;
	if(delkaOblouku<0)
		delkaPosunu *= -1;
	uhel += alpha/2;
	posun(pocX, pocY, delkaPosunu, uhel);
	uhel += alpha/2;
}

void pohyb(float & pocX,float & pocY, float & uhel, float drahaL, float drahaR, float sirskaRobota)
{
	{
		float r = (sirskaRobota * drahaL)/(drahaL - drahaR);
		float alpha = drahaL/r; //v radianech
		r = r - sirskaRobota/2;
		float delkaOblouku = 2* r * sin(alpha);
		float delkaPosunu = sqrt( 2*r*r - 2*r*r*cos(alpha) );
		if(delkaOblouku<0)
			delkaPosunu *= -1;
		uhel += alpha/2;
		posun(pocX,pocY,delkaPosunu,uhel);
		uhel += alpha/2;
	}
}

float computeAcceleration(int power, int maxMotorPower, float a_max, float treni_max, float speed, float speed_max)
{
	float a = power * a_max / maxMotorPower - speed * treni_max / speed_max;
	return a;
}

float computeSpeed(float speed, float a, float staticTreni, float jednotkovyCas)
{
	float newSpeed = speed + a*jednotkovyCas;
	if(newSpeed < staticTreni && newSpeed > 0)
		newSpeed = 0;
	if(newSpeed > -staticTreni && newSpeed < 0)
		newSpeed = 0;
	return newSpeed;
}

float computeEncoderValue(float encoderValue, float speed, float encodersNaMetr, float jednotkovyCas)
{
	return encoderValue + speed * jednotkovyCas * encodersNaMetr;
}

DWORD WINAPI computeSimulation(LPVOID lpParam)
{
	//<begin>constant definitions
	float const jednotkovyCas = 0.05; //[seconds]
	const int maxMotorPower = 127;
	const float a_max = 25;
	const float treni_max = 250;
	const float speed_max = 150;
	const float encodersNaMetr = 1000;
	const float sirskaRobota = 1;
	const float staticTreni = 0.2;
	//<end>constant definitions
	while(!konec)
	{
		Sleep((int)(jednotkovyCas*1000)); //cekani na dalsi krok simulace (real-time)
		waitForConnectRobot();
		
		cs_holder csh(GlobalCriticalSection);
		for(std::map<int, robotProperties>::iterator it = robots.begin(); it!=robots.end(); it++)
		{
			robotProperties & robot = it->second;
			if(robot.active)
			{
				//get data
				int left = robot.leftMotorPower;
				int right = robot.rightMotorPower;
				float vl = robot.leftMotorSpeed;
				float vr = robot.rightMotorSpeed;
				float X = robot.positionX;
				float Y = robot.positionY;
				float uhel = robot.rotation;
				float leftEncoder = robot.leftEncoderValue;
				float rightEncoder = robot.rightEncoderValue;

				//compute acceleration
				float a_left = computeAcceleration(left, maxMotorPower, a_max, treni_max, vl, speed_max);
				float a_right = computeAcceleration(right, maxMotorPower, a_max, treni_max, vr, speed_max);

				//compute speeds
				vl = computeSpeed(vl, a_left, staticTreni, jednotkovyCas);
				vr = computeSpeed(vr, a_right, staticTreni, jednotkovyCas);

				//compute encoders
				leftEncoder = computeEncoderValue(leftEncoder, vl, encodersNaMetr, jednotkovyCas);
				rightEncoder = computeEncoderValue(rightEncoder, vr, encodersNaMetr, jednotkovyCas);

				//compute position and rotation
				if(vl == vr)
				{
					posun(X,Y,vl*jednotkovyCas,uhel);
				}
				else if(vl == 0)
				{
					rotaceKolemKola(X, Y, uhel, vr*jednotkovyCas, sirskaRobota, -1);
				}
				else if(vr == 0)
				{
					rotaceKolemKola(X, Y, uhel, vl*jednotkovyCas, sirskaRobota, 1);
				}
				else
				{
					pohyb(X, Y, uhel, vl*jednotkovyCas, vr*jednotkovyCas, sirskaRobota);
				}

				robot.leftMotorSpeed = vl;
				robot.rightMotorSpeed = vr;
				robot.positionX = X;
				robot.positionY = Y;
				robot.rotation = uhel;
				robot.leftEncoderValue = leftEncoder;
				robot.rightEncoderValue = rightEncoder;
			}
		}
		//compute distances
		for(unsigned int i=0; i<robots.size(); i++)
		{
			robots[i].distances[i] = 0;	//vzdalenost od sebe sama
			for(unsigned int j=i+1; j<robots.size(); j++)
			{
				float distance = getDistance(robots[i].positionX, robots[i].positionY, robots[j].positionX, robots[j].positionY);
				robots[i].distances[j] = distance;
				robots[j].distances[i] = distance;
				if(distance < sirskaRobota)
				{
					if(i!=0)
					{
						robots[i].active = false;	//VIP robot, ktery je pronasledovan ostatnimi je nesmrtelny
						robots[i].positionX = 1000000;
						robots[i].positionY = 1000000;
					}
					robots[j].active = false;
					robots[j].positionX = 1000000;
					robots[j].positionY = 1000000;
				}
			}
		}
	}
	return 0;
}