#include <string>
#include <math.h>
#include "cs_holder.h"

std::string prepisHodnoty(int threadId,std::string type, std::string value)
{
	cs_holder csh(GlobalCriticalSection);
	if(type == "setLeftMotor")
	{
		robots[threadId].leftMotorPower = StrToInt(value);
		robots[threadId].lockLeftServo = true;
		return "1";
	}
	else if(type == "setRightMotor")
	{
		robots[threadId].rightMotorPower = StrToInt(value);
		robots[threadId].lockRightServo = true;
		return "1";
	}
	else if(type == "stopLeftMotor")
	{
		robots[threadId].leftMotorPower = 0;
		robots[threadId].lockLeftServo = false;
		return "1";
	}
	else if(type == "stopRightMotor")
	{
		robots[threadId].rightMotorPower = 0;
		robots[threadId].lockLeftServo = false;
		return "1";
	}
	else if(type == "getSensorValue")
	{
		return IntToStr( robots[threadId].sensorValues[StrToInt(value)] );
	}
	else if(type == "isStartButtonPressed")
	{
		if( robots[threadId].startButton )
		{
			return "1";
		}
		else
		{
			return "0";
		}
	}
	else if(type == "setLeftServo")
	{
		robots[threadId].leftServoValue = StrToInt(value);
		return "1";
	}
	else if(type == "setRightServo")
	{
		robots[threadId].rightServoValue = StrToInt(value);
		return "1";
	}
	else if(type == "led")
	{
		if( robots[threadId].led )
		{
			return "1";
		}
		else
		{
			return "0";
		}
	}
	else if(type == "encoder")
	{
		if(value == "left")
		{
			return IntToStr( robots[threadId].leftEncoderValue );
		}
		else if(value == "right")
		{
			return IntToStr( robots[threadId].rightEncoderValue );
		}
		return "0";
	}
	else if(type == "rs232get")
	{
		while( robots[threadId].rs232.empty() )
		{
			Sleep(1);
		}
		std::string znakRS232;
		znakRS232.push_back( robots[threadId].rs232.front() );
		robots[threadId].rs232.pop();
		return znakRS232;
	}
	else if(type == "finish")
	{
		robots[threadId].active = false;
		return "1";
	}
	else if(type == "connect")
	{
		robotProperties newRobot;
		newRobot.name = value;
		newRobot.active = true;
		newRobot.led = false;
		newRobot.startButton = false;
		newRobot.lockLeftServo = false;
		newRobot.lockRightServo = false;
		newRobot.leftEncoderValue = 0;
		newRobot.rightEncoderValue = 0;
		newRobot.leftMotorPower = 0;
		newRobot.rightMotorPower = 0;
		newRobot.leftMotorSpeed = 0;
		newRobot.rightMotorSpeed = 0;
		newRobot.leftServoValue = 0;
		newRobot.rightServoValue = 0;
		newRobot.positionX = defaultNewRobotPositionX;
		newRobot.positionY = defaultNewRobotPositionY/*+threadId*500*/; //to do (pomer vzdalenosti)
		newRobot.rotation = defaultNewRobotRotation;
		for(int i=0;i<numberOfSensors;i++)
		{
			newRobot.sensorValues[i] = 0;
		}
		robots[threadId] = newRobot;
		//robots.push_back(newRobot);
		return "1";
	}
	else if(type == "distanceFromRobot")
	{
		if(StrToInt(value) >= robots[threadId].distances.size())
		{
			return "-1";
		}
		else
		{
			return FloatToStr( robots[threadId].distances[StrToInt(value)] );
		}
	}
	else if(type == "numberOfRobots")
	{
		return IntToStr( (int)robots[threadId].distances.size() );
	}
	else if(type == "getMyId")
	{
		return IntToStr(threadId);
	}
	else if(type == "angleFromRobot")
	{
		if(threadId == StrToInt(value))
			return "0";
		else if(StrToInt(value) > robots.size())
			return "-1";
		else
		{
			float angle = atan( (robots[StrToInt(value)].positionX-robots[threadId].positionX)/(robots[StrToInt(value)].positionY-robots[threadId].positionY) );
			if(robots[threadId].positionX == robots[StrToInt(value)].positionX)
			{
				if(robots[threadId].positionY < robots[StrToInt(value)].positionY)
					angle = 2*M_PI;
				else
					angle = 0;
			}
			else if(robots[threadId].positionX < robots[StrToInt(value)].positionX)
			{
				if(robots[threadId].positionY < robots[StrToInt(value)].positionY)
					angle = M_PI - angle;
				else
					angle = -angle;
			}
			else if(robots[threadId].positionX > robots[StrToInt(value)].positionX)
			{
				if(angle >= 0)
					angle = -angle;
				else if(angle < 0)
					angle = -M_PI/2 - (M_PI/2+angle);
			}
			float otoceni = modulo(robots[threadId].rotation+M_PI/2, 2*M_PI);
			float output = angle - otoceni;
			output = modulo(output, 2*M_PI);
			if(output > M_PI)
				output -= 2*M_PI;
			else if(output < -M_PI)
				output += 2*M_PI;
			return FloatToStr(output*180/M_PI);
		}
	}
	else if(type == "amIAlive")
	{
		if(robots[threadId].active)
			return "1";
		else
			return "0";
	}
	else if(type == "Rcolor")
	{
		robots[threadId].Rcolor = StrToInt(value);
		return "1";
	}
	else if(type == "Gcolor")
	{
		robots[threadId].Gcolor = StrToInt(value);
		return "1";
	}
	else if(type == "Bcolor")
	{
		robots[threadId].Bcolor = StrToInt(value);
		return "1";
	}
	else if(type == "positionX")
	{
		robots[threadId].positionX = StrToFloat(value);
		return "1";
	}
	else if(type == "positionY")
	{
		robots[threadId].positionY = StrToFloat(value);
		return "1";
	}
	else if(type == "rotation")
	{
		robots[threadId].rotation = StrToFloat(value);
		return "1";
	}
	else
	{
		return "1";
	}
}
