#include "stdafx.h"
#include "Yunimin3.h"
#include <conio.h>
#include <iostream>

void setConnection()
{
	gRobotName = "Yunimin";
	std::cout << "Zadejte IP serveru: ";
	//std::cin >> gConnectionAddress;
	gConnectionAddress = "localhost";
	std::cout << "Zadejte pripojovaci port: ";
	//std::cin >> gConnectionPort;
	gConnectionPort = 1111;
}


#define odchylkaEncoderu 25
#define maxMotorPower 80
#define minMotorPower -80
#define sestStupnu 70  //360stupnu je 2098 tiku

left_encoder leftEncoder;
right_encoder rightEncoder;

struct sensorsValues
{
	int c[8];
} sensorValue;


int abs(int i)
{
	if(i < 0)
		return i*(-1);
	else
		return i;
}


int blizKHodnote(int a, int b, int hodnota = 0)
{
	if( abs(a - hodnota) > abs(b - hodnota) )
		return b;
	else
		return a;
}


bool plusMinus(int original, int rovno, int error)
{
	return original-error<=rovno && original+error>=rovno;
}


int omezeniPlus(int a, int max = maxMotorPower)
{
	if(a > max)
		return max;
	else if(a < 0)
		return 0;
	else
		return a;
}


int omezeni(int a, int max = maxMotorPower, int min = minMotorPower)
{
	if(a > 127)
		return 127;
	else if(a < -127)
		return -127;
	else
		return a;
}


void waitStartButton(int pakCekej)
{
	waitForStartButton();
	wait(pakCekej);
}


bool regulaceLeftEncoderu(int hodnotaL)
{
	int errorL = hodnotaL - leftEncoder.get();
	setLeftMotor( omezeni(errorL) );
	return plusMinus( leftEncoder.get(), hodnotaL, odchylkaEncoderu );
}


bool regulaceRightEncoderu(int hodnotaR)
{
	int errorR = hodnotaR - rightEncoder.get();
	setRightMotor( omezeni(errorR) );
	return plusMinus( rightEncoder.get(), hodnotaR, odchylkaEncoderu );
}


bool regulaceEncoderu(int hodnotaL, int hodnotaR){
	int errorL = hodnotaL - leftEncoder.get();
	int errorR = hodnotaR - rightEncoder.get();
	setMotorPower( omezeni(errorL), omezeni(errorR) );
	return plusMinus( rightEncoder.get(), hodnotaR, odchylkaEncoderu ) && plusMinus( leftEncoder.get(), hodnotaL, odchylkaEncoderu );
}

void otocSe(int uhel,int stred = 0)
{
	//otoci robota o zadany uhel ve stupnich doprava
	//otaceni kolem leveho kola (stred=-127), otaceni kolem praveho kola (stred=127), otaceni kolem osy (stred=0)
	int pocatekL = leftEncoder.get();
	int pocatekR = rightEncoder.get();
	
	int encoderValue = uhel * sestStupnu / 6;
	
	if(stred == 0)
		while( !regulaceEncoderu( -encoderValue /2 + pocatekL, encoderValue /2 + pocatekR ) ){}
	else if(stred == 127)
		while( !regulaceEncoderu( encoderValue + pocatekL, 0 + pocatekR ) ){}
	else if(stred == -127)
		while( !regulaceEncoderu( 0+pocatekL, encoderValue+pocatekR ) ){}
	
	setMotorPower(0,0);
}

void jedRovne(int rozdilEncoderu, int v = 126)
{
	//rozdilEncoderu je left - right
	v = omezeni(v,126,-126);
	
	int r = leftEncoder.get() - rightEncoder.get() - rozdilEncoderu;
	setMotorPower( omezeni(v - r), omezeni(v + r) );
}

void jedRovneNaMisto(int hodnota, int v = 126)
{
	//hodnota a v musi mit stejne znamenko!!!
	int rozdilEncoderu = leftEncoder.get() - rightEncoder.get();
	int pocatekL = leftEncoder.get();
	int pocatekR = rightEncoder.get();
	
	while( !plusMinus( hodnota + pocatekL, leftEncoder.get(), odchylkaEncoderu) && !plusMinus( hodnota + pocatekR, rightEncoder.get(), odchylkaEncoderu))
	{
		int power = blizKHodnote(v, hodnota + (leftEncoder.get() - pocatekL) );
		jedRovne(rozdilEncoderu,power);
	}
	setMotorPower(0,0);
}


void run()
{
	std::cout << "Zadjete barvu robota (RGB): ";
	int R, G, B = 0;
	//std::cin >> R >> G >> B;
	R = 255; G = 255-getMyId()*16; B = 0;
	setRGBColor(R,G,B);

	std::cout << "Zadejte pocatecni souradnice robota: ";
	int X = 0, Y = 0;
	//std::cin >> X >> Y;
	setPosition(X,Y);

	bool sUp, sDown, sLeft, sRight = false;
	int leftPower, rightPower = 0;

	bool active = true;
	int id = getMyId();
	int myKey = VK_F4;
	if(id == 0)
	{
		myKey = VK_F2;
	}
	if(id == 1)
	{
		myKey = VK_F3;
	}

	while(/*!GetAsyncKeyState(VK_ESCAPE)*/ true)
	{
		if(GetAsyncKeyState(myKey))
		{
			active = true;
			setRGBColor(255,255,0);
		}
		bool otherFKey = false;
		for(int key = VK_F1; key <= VK_F24; key++)
		{
			if(GetAsyncKeyState(key))
				otherFKey = true;
		}
		if(otherFKey)
		{
			active = false;
			setRGBColor(0,255,255);
		}

		if(!active)
		{
			continue;
		}

		if(GetAsyncKeyState(VK_SPACE))
			setPosition(0,0);

		if(GetAsyncKeyState(VK_UP))
			sUp = true;
		else
			sUp = false;
		if(GetAsyncKeyState(VK_DOWN))
			sDown = true;
		else
			sDown = false;
		if(GetAsyncKeyState(VK_LEFT))
			sLeft = true;
		else
			sLeft = false;
		if(GetAsyncKeyState(VK_RIGHT))
			sRight = true;
		else
			sRight = false;

		if(sUp)
		{
			leftPower = 100;
			rightPower = 100;
		}
		if(sDown)
		{
			leftPower = -100;
			rightPower = -100;
		}
		if(sUp && sDown)
		{
			leftPower = 0;
			rightPower = 0;
		}
		if(!sUp && !sDown)
		{
			leftPower = 0;
			rightPower = 0;
		}
		if(sLeft)
		{
			leftPower = 0;
		}
		if(sRight)
		{
			rightPower = 0;
		}
		if(sLeft && !sUp && !sDown)
		{
			leftPower = -50;
			rightPower = 50;
		}
		if(sRight && !sUp && !sDown)
		{
			leftPower = 50;
			rightPower = -50;
		}

		setMotorPower(leftPower, rightPower);
		std::cout << "\r";
		std::cout << getDistanceFromRobot(0);
		Sleep(25);
	}
}