#include <stdio.h>
#define ROZMIAR_KOLEJKI 	100
#define GORNA				1
#define DOLNA				2
#define LEWA				4
#define PRAWA				8
#define MOTOR_RIGHT			1
#define MOTOR_LEFT			2
#define WHEEL_DIAMETER		82
#define WHEEL_DISTANCE		130
#define PI					3.14159

typedef enum kierunek
{
	gora, dol, prawo, lewo
} kierunek;

typedef struct punkt
{
	int x, y;
} punkt;

struct punkt newPunkt(int nX, int nY)
{
	struct punkt result;
	result.x = nX;
	result.y = nY;

	return result;                  
}

int poczatek = 0, koniec = 0, n = 0;
int mapka[5][5];
int sciany[5][5] = {7, 9, 5, 3, 9, 5, 8, 12, 5, 8, 12, 6, 0, 8, 14, 12, 13, 14, 6, 9, 14, 6, 3, 3, 10};
punkt kolejka[ROZMIAR_KOLEJKI];
punkt start = {0,4};
punkt stop = {1,3};
kierunek aktualny = gora;

void dodaj(int y, int x)
{
	if(n == ROZMIAR_KOLEJKI)
		return;

	kolejka[koniec] = newPunkt(x, y);

	koniec++;
	n++;

	if(koniec >= ROZMIAR_KOLEJKI)
		koniec = 0;
}

punkt usun()
{
	if(n == 0)
		return newPunkt(-1, -1);

	punkt wartosc = kolejka[poczatek];
	
	poczatek++;
	n--;
	
	if(poczatek == ROZMIAR_KOLEJKI)
		poczatek = 0;
	
	return wartosc;
}

void wyswietl_mapke()
{	
	int x=0, y=0;

	for(; y<5; y++)
	{
		x = 0;
		
		for(; x<5; x++)
		{
			if(mapka[y][x] == 0x7FFFFFFF)
				printf("#     |");
			else
			{
				printf("%d   ", mapka[y][x]);

				if(mapka[y][x] < 10)
					printf(" ");

				if(x != 4)
					printf("|");
			}
		}
		
		printf("\n");
	}
}

void wyswietl_sciany()
{	
	int x=0, y=0;

	for(; y<5; y++)
	{
		x = 0;

		for(; x<5; x++)
		{
			printf("%d   ", sciany[y][x]);

			if(sciany[y][x] < 10)
				printf("  ");

			if(x != 4)
				printf("|");
		}
	
		printf("\n");
	}
}

void oblicz_labirynt()
{
	punkt teraz;
	dodaj(stop.y, stop.x);
	mapka[stop.y][stop.x] = 0;

	while (n != 0) 
	{
		teraz = usun();

		if((sciany[teraz.y][teraz.x] & GORNA) == 0)
		{
			if (mapka[teraz.y - 1][teraz.x] > mapka[teraz.y][teraz.x] + 1)
			{
				mapka[teraz.y - 1][teraz.x] = mapka[teraz.y][teraz.x] + 1;
				dodaj (teraz.y - 1, teraz.x);
			}
		}

		if((sciany[teraz.y][teraz.x] & DOLNA) == 0)
		{
			if (mapka[teraz.y + 1][teraz.x] > mapka[teraz.y][teraz.x] + 1)
			{
				mapka[teraz.y + 1][teraz.x] = mapka[teraz.y][teraz.x] + 1;
				dodaj (teraz.y + 1, teraz.x);
			}
		}

		if((sciany[teraz.y][teraz.x] & PRAWA) == 0)
		{
			if (mapka[teraz.y][teraz.x + 1] > mapka[teraz.y][teraz.x] + 1)
			{
				mapka[teraz.y][teraz.x + 1] = mapka[teraz.y][teraz.x] + 1;
				dodaj (teraz.y, teraz.x + 1);
			}
		}

		if((sciany[teraz.y][teraz.x] & LEWA) == 0)
		{
			if (mapka[teraz.y][teraz.x - 1] > mapka[teraz.y][teraz.x] + 1)
			{
				mapka[teraz.y][teraz.x - 1] = mapka[teraz.y][teraz.x] + 1;
				dodaj (teraz.y, teraz.x -1);
			}
		}
	}

	printf("koniec\n");
}

int rotateEncoders(int angle)
{
	return WHEEL_DISTANCE*angle*25/(9*WHEEL_DIAMETER);
}
 
void rotate(int angle, int speed) // angle>0 - w prawo
{
	int encoders = rotateEncoders(angle);
	
	mrp(MOTOR_RIGHT, speed, encoders);
	mrp(MOTOR_LEFT, speed, -encoders);
	bmd(MOTOR_RIGHT);
	bmd(MOTOR_LEFT);
	mav(MOTOR_RIGHT, 0);
	mav(MOTOR_LEFT, 0);
	
}
int distanceEncoders(int distance)
{
	float encoders = (float)distance*1000.0/(PI*(float)WHEEL_DIAMETER);
	return (int)encoders;
}

void driveStraight(int distance, int speed) // jazda na okreslony dystans 
{
	int encoders = distanceEncoders(distance);
	
	mrp(MOTOR_RIGHT, speed, encoders);
	mrp(MOTOR_LEFT, speed, encoders);
	bmd(MOTOR_RIGHT);
	bmd(MOTOR_LEFT);
}

void dobicie()
{
	driveStraight(-150, 400);
}

void jazda(kierunek obrotu) // jazda do gory/na dol/na prawo/na lewo
{
	if (aktualny == obrotu) 
		driveStraight(300, 500);
	
	else if ((aktualny == gora && obrotu == prawo) || (aktualny == prawo && obrotu == dol) || (aktualny == dol && obrotu == lewo) || (aktualny == lewo && obrotu == gora))   // w prawo
	{ 
		rotate(90, 300);
		driveStraight(300,500);
		if(aktualny == gora)
			aktualny = prawo;
		else if (aktualny == prawo)
			aktualny = dol;
		else if (aktualny == dol)
			aktualny = lewo;
		else
			aktualny = gora;
	}
	else // w lewo
	{
		rotate(-90, 300);
		driveStraight(300, 500);
		
		if(aktualny == gora)
			aktualny = lewo;
		else if (aktualny == lewo)
			aktualny = dol;
		else if (aktualny == dol)
			aktualny = prawo;
		else
			aktualny = gora;
	}

}

void jazda_labirynt()
{
	punkt now = start;
	int wykonane = 0;
	
	while (mapka[now.y][now.x] != 0)
	{
		wykonane = 0;
		
		// sprawdzanie gory
		if (((now.y-1) < 5) && ((now.y-1) >= 0) && ((now.x) < 5) && ((now.x) >= 0) && (wykonane == 0)) 
		{
			if ((mapka[now.y - 1][now.x] == mapka[now.y][now.x] - 1) && ((sciany[now.y][now.x] & GORNA) == 0))
			{
				jazda(gora);
				now = newPunkt(now.x, now.y-1);
				wykonane = 1;
			}
		}
			// sprawdzanie dolu
		if (((now.y+1) < 5) && ((now.y+1) >= 0) && ((now.x) < 5) && ((now.x) >= 0) && (wykonane == 0)) 
		{
			if ((mapka[now.y + 1][now.x] == mapka[now.y][now.x] - 1) && ((sciany[now.y][now.x] & DOLNA) == 0))
			{
				jazda(dol);
				now = newPunkt(now.x, now.y+1);
				wykonane = 1;
			}	
		}
			// sprawdzanie prawej
		if (((now.y) < 5) && ((now.y) >= 0) && ((now.x+1) < 5) && ((now.x+1) >= 0) && (wykonane == 0)) 
		{
			if ((mapka[now.y][now.x + 1] == mapka[now.y][now.x] - 1) && ((sciany[now.y][now.x] & PRAWA) == 0))
			{
				jazda(prawo);
				now = newPunkt(now.x+1, now.y);
				wykonane = 1;
			}	
		}
			// sprawdzanie lewej
		if (((now.y) < 5) && ((now.y) >= 0) && ((now.x-1) < 5) && ((now.x-1) >= 0) && (wykonane == 0)) 
		{
			if ((mapka[now.y][now.x - 1] == mapka[now.y][now.x] - 1) && ((sciany[now.y][now.x] & LEWA) == 0))
			{
				jazda(lewo);
				now = newPunkt(now.x-1, now.y);
				wykonane = 1;
			}	
		}
		
		if (wykonane == 0)
			printf("Blad!");
		
		printf("(%d, %d)\n", now.y, now.x); 

	}
}


int main()
{
	set_each_analog_state(0,0,0,0,0,0,0,0);

	int i=0,j=0;

	for(; i < 5; i++)
	{
		j=0;

		for(; j < 5;j++)
		mapka[i][j] = 0x7FFFFFFF;
	}

	oblicz_labirynt();
	wyswietl_mapke();
	jazda_labirynt();
	
	
}