#include <string>
#include <map>
#include <queue>

int const numberOfSensors = 8;

class robotProperties
{
public:
        int drawingPolomer;
        int id;
	std::string name;
	bool active;
        int Rcolor;
        int Gcolor;
        int Bcolor;
	float positionX;
	float positionY;
	float rotation;
	int leftMotorPower;
	int rightMotorPower;
	int leftMotorSpeed;
	int rightMotorSpeed;
	bool led;
	bool startButton;
	int leftEncoderValue;
	int rightEncoderValue;
	int leftServoValue;
	int rightServoValue;
	bool lockLeftServo;
	bool lockRightServo;
	int sensorValues[numberOfSensors];
	std::queue<char> rs232;
};

std::map<int,robotProperties> robots;

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

void convertReceiveData(float * actualData, std::string receiveText)
{
        std::string prvek;
        std::string buffer;
        int pozice = 0;
        for(int i=0; i<receiveText.size(); i++)
        {
                if(receiveText[i] == ';')
                {
                        prvek = buffer;
                        buffer = "";
                        actualData[pozice] = StrToFloat(prvek);
                        pozice++;
                }
                else
                {
                        buffer.push_back(receiveText[i]);
                }
        }
}


std::string getLineFromStream(std::string & data, char oddelovac)
{
        if(data.find(oddelovac) == -1)
        {
                return "";
        }
        std::string output;
        std::string outputStream;
        bool zaOddelovacem = false;
        for(int i=0; i<data.size(); i++)
        {
                if(data[i] == oddelovac && (!zaOddelovacem))
                {
                        zaOddelovacem = true;
                }
                else if(!zaOddelovacem)
                {
                        output.push_back(data[i]);
                }
                else if(zaOddelovacem)
                {
                        outputStream.push_back(data[i]);
                }
        }
        data = outputStream;
        return output;
}

void nactiDoRobota(robotProperties & robot, float * actual)
{
        robot.id = actual[0];
        robot.Rcolor = actual[1];
        robot.Gcolor = actual[2];
        robot.Bcolor = actual[3];
        robot.positionX = actual[4];
        robot.positionY = actual[5];
        robot.rotation = actual[6];
        robot.leftMotorSpeed = actual[7];
        robot.rightMotorSpeed = actual[8];
        robot.leftMotorPower = actual[9];
        robot.rightMotorPower = actual[10];
        robot.leftEncoderValue = actual[11];
        robot.rightEncoderValue = actual[12];
        robot.leftServoValue = actual[13];
        robot.rightServoValue = actual[14];
        for(int i=0;i<numberOfSensors;i++)
        {
                robot.sensorValues[i] = actual[15+i];
        }
        robot.led = actual[14+numberOfSensors+1];
        robot.startButton = actual[14+numberOfSensors+2];

        robot.drawingPolomer = 1;      //to do const
}
