#include <iostream>
#include <sstream>
#include <string>
#include <time.h>
#include "connectionToServer.h"

int g_req_speed_left = 0;
int g_req_speed_right = 0;
bool g_led_status = false;

void doNothing()
{
}

std::string IntToStr(int a)
{
	std::ostringstream pomoc;
	pomoc << a;
	return pomoc.str();
}

int StrToInt(std::string text)
{
	return atoi(text.c_str());
}

double StrToFloat(std::string text)
{
	return strtod(text.c_str(),NULL);
}

std::string FloatToStr(float a)
{
	std::ostringstream pomoc;
	pomoc << a;
	return pomoc.str();
}

std::string CharToString(char a)
{
	std::string s(1, a);
	return s;
}

void setLeftMotor(int leftMotor)
{
	sendMessageToServer("setLeftMotor", IntToStr(leftMotor) );
	g_req_speed_left = leftMotor;
}

void setRightMotor(int rightMotor)
{
	sendMessageToServer("setRightMotor", IntToStr(rightMotor));
	g_req_speed_right = rightMotor;
}

void stopLeftMotor()
{
	setLeftMotor(0);
	sendMessageToServer("stopLeftMotor","0");
}

void stopRightMotor()
{
	setRightMotor(0);
	sendMessageToServer("stopRightMotor","0");
}

void setMotorPower(int leftMotor, int rightMotor)
{
	setLeftMotor(leftMotor);
	setRightMotor(rightMotor);
}

int getSensorValue(int sensor)
{
	return StrToInt( sendMessageToServer("getSensorValue",IntToStr(sensor)) );
}

void wait(unsigned int microseconds)
{
	microseconds /= (1000000/CLOCKS_PER_SEC);
	unsigned int start = clock();
	while(clock()-start < microseconds)
	{
		doNothing();
	}
}

class stopwatch
{
private:
	int startTime;
	int stopTime;
	bool run;
public:
	stopwatch()
	{
		startTime = clock();
		run = true;
	}
	void start()
	{
		if(!run)
			startTime = clock();
		run = true;
	}
	void stop()
	{
		if(run)
		{
			stopTime = clock() - startTime;
			run = false;
		}
	}
	void clear()
	{
		startTime = clock();
	}
	int getTime()
	{
		if(run)
			return (clock()-startTime)*(1000000/CLOCKS_PER_SEC);
		else
			return stopTime*(1000000/CLOCKS_PER_SEC);
	}
};

void waitForStartButton()
{
	while( sendMessageToServer("isStartButtonPressed","") == "0" )
	{
		doNothing();
	}
}

bool isStartButtonPressed()
{
	if( sendMessageToServer("isStartButtonPressed","") == "0" )
		return false;
	else
		return true;
}

void setLeftServo(int value)
{
	sendMessageToServer("setLeftServo",IntToStr(value));
}

void setRightServo(int value)
{
	sendMessageToServer("setRightServo",IntToStr(value));
}

void setLed()
{
	sendMessageToServer("led","1");
	g_led_status = true;
}

void clearLed()
{
	sendMessageToServer("led","0");
	g_led_status = false;
}

void toggleLed()
{
	if(g_led_status)
		clearLed();
	else
		setLed();
}

class left_encoder
{
private:
	int value;
	bool run;
public:
	left_encoder()
	{
		if(connectionIsInitialize())
			value = StrToInt( sendMessageToServer("encoder","left") );
		else
			value = 0;
		run = true;
	}
	void start()
	{
		if(!run)
			value = StrToInt( sendMessageToServer("encoder","left") );
		run = true;
	}
	void stop()
	{
		if(run)
		{
			value = StrToInt( sendMessageToServer("encoder","left") ) - value;
			run = false;
		}
	}
	void clear()
	{
		value = StrToInt( sendMessageToServer("encoder","left") );
	}
	int get()
	{
		if(run)
			return StrToInt( sendMessageToServer("encoder","left") ) - value;
		else
			return value;
	}
};

class right_encoder
{
private:
	int value;
	bool run;
public:
	right_encoder()
	{
		if(connectionIsInitialize())
			value = StrToInt( sendMessageToServer("encoder","left") );
		else
			value = 0;
		run = true;
	}
	void start()
	{
		if(!run)
			value = StrToInt( sendMessageToServer("encoder","right") );
		run = true;
	}
	void stop()
	{
		if(run)
		{
			value = StrToInt( sendMessageToServer("encoder","right") ) - value;
			run = false;
		}
	}
	void clear()
	{
		value = StrToInt( sendMessageToServer("encoder","right") );
	}
	int get()
	{
		if(run)
			return StrToInt( sendMessageToServer("encoder","right") ) - value;
		else
			return value;
	}
};

class rs232_t
{
public:
	void send(std::string const & text)
	{
		sendMessageToServer("rs232",text);
	}
	void sendCharacter(char pismeno)
	{
		sendMessageToServer("rs232",CharToString(pismeno));
	}
	void sendNumber(int number,int lengh = 0)
	{
		std::string buffer = "";
		int mezer = lengh - IntToStr(number).size();
		for(int i=0;i<mezer;++i)
		{
			buffer += " ";
		}
		buffer += IntToStr(number);
		sendMessageToServer("rs232",buffer);
	}
	char get()
	{
		std::string pomoc = "";
		while( (pomoc = sendMessageToServer("rs232get","")) == "" )
		{
			doNothing();
		}
		return pomoc[0];
	}
	bool peek(char & ch)
	{
		std::string pomoc = sendMessageToServer("rs232get","");
		if( pomoc != "" )
		{
			ch = pomoc[0];
			return true;
		}
		else
		{
			return false;
		}
	}
};
rs232_t rs232;

//i2c a dalsi veci, ktere nejsou v referencni prirucce ted neresim//

//implementace imaginarnich funkci

void setRColor(int red)
{
	sendMessageToServer("Rcolor",IntToStr(red));
}

void setGColor(int green)
{
	sendMessageToServer("Gcolor",IntToStr(green));
}

void setBColor(int blue)
{
	sendMessageToServer("Bcolor",IntToStr(blue));
}

void setRGBColor(int R, int G, int B)
{
	setRColor(R);
	setGColor(G);
	setBColor(B);
}

int getNumberOfRobots()
{
	return StrToInt( sendMessageToServer("numberOfRobots","") );
}

float getDistanceFromRobot(int idRobota)
{
	return StrToFloat( sendMessageToServer("distanceFromRobot", IntToStr(idRobota) ) );
}

float getAngleFromRobot(int idRobota)
{
	return StrToFloat( sendMessageToServer("angleFromRobot", IntToStr(idRobota) ) );
}

int getMyId()
{
	return StrToInt( sendMessageToServer("getMyId","") );
}

bool amIAlive()
{
	if( sendMessageToServer("amIAlive","") == "1" )
		return true;
	else
		return false;
}

bool setPosition(float X, float Y)
{
	sendMessageToServer("positionX",FloatToStr(X));
	sendMessageToServer("positionY",FloatToStr(Y));
	return true;
}

bool setRotation(float rot)
{
	sendMessageToServer("rotation",FloatToStr(rot));
	return true;
}