#pragma config(Sensor, S1,     Jednotky,       sensorEV3_Color)
#pragma config(Sensor, S2,     Desiatky,       sensorEV3_Color)
#pragma config(Sensor, S3,     Stovky,         sensorEV3_Color)
#pragma config(Motor,  motorA,          vysuvaci,      tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,          pravy,         tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          lavy,          tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorD,          pohybovy,      tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int jednotky;
int desiatky;
int stovky;
int i = 0;
int a = 0;
int cislo_0 = 23;
int cislo_1 = 18;
int cislo_2 = 18;
int cislo_3 = 18;
int cislo_4 = 12;
int cislo_5 = 12;
int cislo_6 = 12;
int cislo_7 = 8;
int cislo_8 = 8;
int cislo_9 = 8;
int cislo_258_do_prava = 6;
int cislo_369_do_prava = 11;

void stlacKlaves()
{
	setMotorSpeed(motorA, 100);
	sleep(530);
	wait1Msec(100);
	setMotorSpeed(motorA, -100);
	sleep(570);
}

void citaj_cislo()
{
	int jednotky_farba, desiatky_farba, stovky_farba;

	jednotky = 0;
	desiatky = 0;
	stovky = 0;
	jednotky_farba = 0;
	desiatky_farba = 0;
	stovky_farba = 0;

	playImmediateTone(340, 25);
	while (getColorReflected(Jednotky) > 1)
	{
 	  if (jednotky_farba==1 && getColorReflected(Jednotky) > 40)
		{
			jednotky_farba = 0;
		}
		else if (jednotky_farba==0 && getColorReflected(Jednotky) < 20)
		{
			jednotky_farba = 1;
			jednotky = jednotky + 1;
		}
 	  if (desiatky_farba==1 && getColorReflected(Desiatky) > 40)
		{
			desiatky_farba = 0;
		}
		else if (desiatky_farba==0 && getColorReflected(Desiatky) < 20)
		{
			desiatky_farba = 1;
			desiatky = desiatky + 1;
		}
 	  if (stovky_farba==1 && getColorReflected(Stovky) > 40)
		{
			stovky_farba = 0;
		}
		else if (stovky_farba==0 && getColorReflected(Stovky) < 20)
		{
			stovky_farba = 1;
			stovky = stovky + 1;
		}
		sleep(250);
	}
	playImmediateTone(440, 50);
 }

void zadaj_cislo_na_klavesnici(int cislo)
{
	if(cislo == 0)
	{
		for (i = 0; i < cislo_0; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		stlacKlaves();
		for (i = 0; i < cislo_0; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
	}
	else if(cislo == 1)
	{
		for (i = 0; i < cislo_1; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		stlacKlaves();
		for (i = 0; i < cislo_1; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
	}
	else if(cislo == 2)
	{
		for (i = 0; i < cislo_2; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_2; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	else if(cislo == 3)
	{
		for (i = 0; i < cislo_3; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_3; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	else if(cislo == 4)
	{
		for (i = 0; i < cislo_4; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		stlacKlaves();
		for (i = 0; i < cislo_4; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
	}
	else if(cislo == 5)
	{
		for (i = 0; i < cislo_5; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_5; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	else if(cislo == 6)
	{
		for (i = 0; i < cislo_6; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_6; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	else if(cislo == 7)
	{
		for (i = 0; i < cislo_7; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		stlacKlaves();
		for (i = 0; i < cislo_7; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
	}
	else if(cislo == 8)
	{
		for (i = 0; i < cislo_8; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_8; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_258_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	else if(cislo == 9)
	{
		for (i = 0; i < cislo_9; i++)
		{
			setMotorSpeed(motorD, 10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, 10);
			setMotorSpeed(motorB, 10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
		stlacKlaves();
		for (i = 0; i < cislo_9; i++)
		{
			setMotorSpeed(motorD, -10);
			sleep(100);
		}
		setMotorSpeed(motorD, 0);
		for (a = 0; a < cislo_369_do_prava; a++)
		{
			setMotorSpeed(motorC, -10);
			setMotorSpeed(motorB, -10);
			sleep(100);
		}
		setMotorSpeed(motorC, 0);
		setMotorSpeed(motorB, 0);
	}
	wait1Msec(500);
}

task main()
{
	setSoundVolume(25);

	while (getColorReflected(Jednotky) <= 40) {}
	citaj_cislo();
//	jednotky = 0;
//	desiatky = 0;
//	stovky = 1;

	zadaj_cislo_na_klavesnici(stovky);
	zadaj_cislo_na_klavesnici(desiatky);
	zadaj_cislo_na_klavesnici(jednotky);
}