#include "yunimin3.h"


#define odchylkaEncoderu 20
#define maxMotorPower 126
#define minMotorPower -126
#define sestStupnu 35  //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 min(int a, int b)
{
	if(a > b)
		return b;
	else
		return a;
}


int max(int a, int b)
{
	if(a < b)
		return b;
	else
		return a;
}


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


sensorsValues kalibraceBarev(int v = 60)
{
	int min[6];
	for(int i = 0; i < 6; i++)
		min[i] = 512;

	int max[6];
	for(int i = 0; i < 6; i++)
		max[i] = -512;
	
	setMotorPower(v, -v);
	for(int i = 0; i < 3000; i++)
	{
		for(int j = 0; j < 6; j++)
		{
			int r = getSensorValue(j);
			if(min[j] > r) min[j] = r;
			if(max[j] < r) max[j] = r;
		}
	wait(1000);
	}
	setMotorPower(0,0);
	sensorsValues sensor;
	for(int i=0;i<6;i++)
		sensor.c[i] = (max[i] + min[i]) / 2;
	
	return sensor;
}


void sendControl()
{
	//hlaska
	rs232.send("~");
	rs232.wait();
	
	//sensory
	for(int i=0;i<8;i++)
	{
		rs232.sendNumber(getSensorValue(i));
		rs232.send(" ");
		rs232.wait();
	}
	
	//encodery
	rs232.sendNumber(leftEncoder.get());
	rs232.send(" ");
	rs232.wait();
	rs232.sendNumber(rightEncoder.get());
	rs232.send(" ");
	rs232.wait();
	
	//start button
	if(isStartButtonPressed()) rs232.sendNumber(1);
	else rs232.sendNumber(0);
	rs232.send(" ");
	rs232.wait();
	
	//motor power
	rs232.sendNumber(g_req_speed_left);
	rs232.send(" ");
	rs232.wait();
	rs232.sendNumber(g_req_speed_right);
	rs232.send(" ");
	rs232.wait();
	
	//led status
	if(g_led_status) rs232.sendNumber(1);
	else rs232.sendNumber(0);
	rs232.send(" ");
	rs232.wait();
	
	rs232.send("\r\n");
	rs232.wait();
}


int whichSensor(int value)
{
	//zjisti, ktery sensor je nejbliz zadanemu cislu
	int odchylka=1024;
	int sensor=0;
	
	for(int i=0;i<8;i++)
		if( odchylka > abs(value) - abs(getSensorValue(i)) )
			sensor=i;
	
	return sensor;
}


void jedPoCare45(int smer = 1, int v = 100, int k = 12)
{
	//drzi se na care pomoci sensoru 4 a 5
	int r = getSensorValue(4) - getSensorValue(5);
	setMotorPower( omezeniPlus(v - r/k)*smer, omezeniPlus(v + r/k)*smer );
}


void obaKrajniNaCare(bool smer, sensorsValues barva, int vz = 127, int nula = 0)
{
	//predpoklada, ze se cara rozdvojuje a vyda se bud doprava (smer=1), nebo doleva (smer=0)
	while(getSensorValue(smer)>barva.c[smer])
	{
		if(smer)
			setMotorPower(vz,nula);
		else
			setMotorPower(nula,vz);
	}
	setMotorPower(0,0);	
}


int jedPoCare01(sensorsValues barva, int vz = 127, int nula = 0)
{
	//drzi si caru mezi krajnimi sensory
	
	//pokud je jeden ze sensoru na care, otoci se aby nevyjel a vrati 0
	//pokud jsou oba sensory na cerne, vrati 1, pokud zadny na cerne, vrati -1
	
	if(getSensorValue(0)>barva.c[0])
	{
		setMotorPower(nula,vz);
		return 0;
	}
	if(getSensorValue(1)>barva.c[1])
	{
		setMotorPower(vz,nula);
		return 0;
	}
	if(getSensorValue(0)>barva.c[0] && getSensorValue(1)>barva.c[1])
	{
		return 1;
	}
	if( getSensorValue(0)<barva.c[0] && getSensorValue(1)<barva.c[1] )
	{
		return -1;
	}
	return 0;
}


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)
{
	v = abs(v);
	if( hodnota < 0 )
		v = -v;
	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 objedPrekazku(int sirskaPrekazky, int delkaPrekazky, int v = 60)
{
	int kolikrat=0;
	do{
		jedRovneNaMisto(-250,-v);
		otocSe(90);
		jedRovneNaMisto(sirskaPrekazky+250,v);
		otocSe(-90);
		jedRovneNaMisto(delkaPrekazky+500,v);
		kolikrat++;
	}while(isStartButtonPressed());
	otocSe(-90);
	for(int i=0;i<kolikrat;i++)
		jedRovneNaMisto(sirskaPrekazky + 250 + 100, v);
	otocSe(90);
}