#define M_PI 3.1415926535897932384626433832795
#define fullAngle 2*M_PI

//get data
float vl = 0;
float vr = 0;
float X = 4;
float Y = 4;
float uhel = 0;
float leftEncoder = 0;
float rightEncoder = 0;


float const jednotkovyCas = 0.05; //[seconds]
const int maxMotorPower = 127;
const float a_max = 80;
const float treni_max = 1000; //1
const float speed_max = 100;
const float encodersNaMetr = 1000;
const float sirskaRobota = 1;
const float staticTreni = 0.2;

int left = 0;
int right = 0;

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

int threads = 0;
bool konec = false;

float defaultNewRobotPositionX = 0;
float defaultNewRobotPositionY = 0;
float defaultNewRobotRotation = 0;
int const numberOfSensors = 8;

CRITICAL_SECTION GlobalCriticalSection;

struct robotProperties
{
	std::string name;
	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;

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 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;
}

////////////////////////////////////////////////////////////////////////////////
void timerSimulate()
{
        int left = TrackBar1->Position;
	int right = TrackBar2->Position;

	//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, jednotkovyCas);
	vr = computeSpeed(vr, a_right, jednotkovyCas);

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

	//compute position and rotation
	if( abs(vl-vr) < 1 )
	{
		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);
	}
        Edit1->Text = IntToStr((int)vl*1000);
        Edit2->Text = IntToStr((int)vr*1000);
}