#define F_CPU 8000000UL
#include <avr/io.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include <util/delay.h>

unsigned char DOLE = 12;
unsigned char NAHORE = 20;	
unsigned char linka;
unsigned char cisloPrikazu;
unsigned char id;
unsigned char ks;
unsigned char p = 18;
unsigned char dioda;
unsigned char citacTimeru1;
signed char poziceMotoru1;
signed char poziceMotoru2;
unsigned char reletivniPozicePocatkuX = 0;
unsigned char relativniPozicePocatkuY = 0;
unsigned short x;
unsigned short y;
signed char zx;
signed char zy;
signed short dx;
signed short dy;
unsigned char x1;
unsigned char x2;
unsigned char y11;
unsigned char y2;
unsigned short xp;
unsigned short yp;
unsigned short citacTimeru2;
unsigned char XJeVetsiNezY;
unsigned short hodnota;
unsigned char pole[] = {0b00001000, 0b00001001, 0b00000001, 0b00000101, 0b00000100, 0b00000110, 0b00000010, 0b00001010};
// send char
void uart_putc(unsigned char data )
{
	while ( !( UCSR0A & (1<<UDRE0)) )
	;
	UDR0 = data;
	
}
// get char
unsigned char uart_getc( void )
{
	/* Wait for data to be received */
	while ( !(UCSR0A & (1<<RXC0)) )
	;
	/* Get and return received data from buffer */
	return UDR0;
}
/*
// Přerušení USART
ISR (USART_RXC_vect) {
	unsigned char znak;
	znak = UDR;
	uart_putc(znak);
	
}
*/
ISR (TIMER0_OVF_vect){
	dioda +=1;
	if (dioda > 20)
	{
		PORTD ^= 0x80;
		dioda = 0;
	}
	
	
}
ISR (TIMER1_COMPA_vect){
	
	
	citacTimeru1 += 1;
	citacTimeru1 = citacTimeru1 % 200;
	if (citacTimeru1 < p) {
		PORTC |= (1 << 5);
	}
	else
	{
		PORTC &= (~(1 << 5)); 
	}
	
}
ISR (TIMER2_OVF_vect)
{
	citacTimeru2 +=1; //1/(f*1000000/(2^8*1024[preddelicka]))*1000*citac [ms]
	if (citacTimeru2 >= 6)
	{
		
		citacTimeru2 = 0;
	}
}
signed short ABS(signed short x){
	if (x<0)
	{
		return -x;
	}
	else
	{
		return x;
	}
}
void pozice (void) {
	
	
	if (p != NAHORE)
	{
		p = NAHORE;
		_delay_ms(100);
	}
	while ( (!(PINB & (1<<PINB4))) || (!(PINB & (1<<PINB5))) ) {
		
		if (!(PINB & (1<<PINB5)))
		{
			poziceMotoru1 -= 1;
			poziceMotoru1 = (poziceMotoru1 + 8) % 8;
			PORTC &= 0b11110000;
			PORTC |= pole[poziceMotoru1];
		}
		if (!(PINB & (1<<PINB4)))
		{
			poziceMotoru2 -= 1;
			poziceMotoru2 = (poziceMotoru2 + 8) % 8;
			PORTB = pole[poziceMotoru2];
		}
		_delay_ms(20);
	}
	
	x = 0;
	y = 0;
	
	while (((x < reletivniPozicePocatkuX) || (y < relativniPozicePocatkuY))) {
		
		if (x < reletivniPozicePocatkuX)
		{
			poziceMotoru1 += 1;
			poziceMotoru1 = (poziceMotoru1 + 8) % 8;
			PORTC &= 0b11110000;
			PORTC |= pole[poziceMotoru1];
			x += 1;
		}
		if (y < relativniPozicePocatkuY)
		{
			poziceMotoru2 += 1;
			poziceMotoru2 = (poziceMotoru2 + 8) % 8;
			PORTB = pole[poziceMotoru2];
			y += 1;
		}
		_delay_ms(10);
	}
	x = 0;
	y = 0;
	
}
void prejezd (void) {
	
	if (p != NAHORE)
	{
		p = NAHORE;
		_delay_ms(100);
	}
	xp = x2 | (x1 << 8);
	yp = y2 | (y11 << 8);
	if (y < yp)
	{
		zy = 1;
		}
		else
		{
		zy = -1;
	}
	if (x < xp)
	{
		zx = 1;
		}
		else
		{
		zx = -1;
	}
	while (((x != xp) || (y != yp))) {
		if (x != xp)
		{
			poziceMotoru1 += zx;
			poziceMotoru1 = (poziceMotoru1 + 8) % 8;
			PORTC &= 0b11110000;
			PORTC |= pole[poziceMotoru1];
			x += zx;
		}
		if (y != yp)
		{
			poziceMotoru2 += zy;
			poziceMotoru2 = (poziceMotoru2 + 8) % 8;
			PORTB = pole[poziceMotoru2];
			y += zy;
		}
		_delay_ms(8);
	}
}
void tisk(void) {
	unsigned short ax = 0;
	unsigned short ay = 0;
	
	if (p != DOLE)
	{
		p = DOLE;
		_delay_ms(100);
	}
	
	xp = x2 | (x1 << 8);
	yp = y2 | (y11 << 8);
	
	if (y < yp)
	{
		zy = 1;
	}
	else
	{
		zy = -1;
	}
	if (x < xp)
	{
		zx = 1;
	}
	else
	{
		zx = -1;
	}
	
	dx = xp - x;
	dy = yp - y;
	dx = ABS(dx);
	dy = ABS(dy);
	if (dx > dy)
	{
		XJeVetsiNezY = 1;
		hodnota = dx;
		TIMSK2 = (1<<TOIE2); //povolení přerušení od timeru 2
		while ((dx - ax > 0) || (dy - ay > 0))
		{
			if (dy * ax > ay * dx)
			{
				poziceMotoru2 += zy;
				poziceMotoru2 = (poziceMotoru2 + 8) % 8;
				PORTB = pole[poziceMotoru2];
				y += zy;
			}
		}
	}
	else
	{
		XJeVetsiNezY = 0;
		hodnota = dy;
		TIMSK2 = (1<<TOIE2); //povolení přerušení od timeru 2
		while ((dx - ax > 0) || (dy - ay > 0))
		{
			if (dy * ax > ay * dx)
			{
				poziceMotoru1 += zx;
				poziceMotoru1 = (poziceMotoru1 + 8) % 8;
				PORTC &= 0b11110000;
				PORTC |= pole[poziceMotoru1];
				x += zx;
			}
		}
	}
	TIMSK2 = 0; //zakázání přerušení od přetečení timeru 2
	
}



int main(void)
{
	
	
	
	DDRD = 0x80;
	PORTD = 0x00;
	DDRB = (1<<PB0)|(1<<PB1)|(1<<PB2)|(1<<PB3);
	DDRC = (1<<PC0)|(1<<PC1)|(1<<PC2)|(1<<PC3)|(1<<PC5);
	
	/***** inicializace uart  *****/
	UCSR0A = 0x00;
	UBRR0H = 0x00;
	UBRR0L = (unsigned char) 51;   // 9600 baud
	UCSR0B |= (1<<TXEN0) | (1<<RXEN0);
	UCSR0C |= (1<<UMSEL00) | (1<<UCSZ00) | (1<<UCSZ01);  // ramec dat:  8 datovych, 1 stop bit, bez parity
	
	TIMSK0 = (1<<TOIE0); //povolení přerušení od přetečení timeru 0
	TCCR0B |= (1<<CS00) | (1<<CS02); //předělička timeru 0 na 1024
	
	TIMSK1 = (1<<OCIE1A); //povolení přerušení od timeru 1 (CTC)
	TCCR1B |= (1<<CS11) | (1<<WGM12); //předělicka na 8 a nastavebí CTC módu
	OCR1AL = 99; // 460 při 18,xx MHz, 199 při 8 MHz (1. 8 bitů)
	OCR1AH = 0; //(zbytek bitů)
	
	TCCR2B = (1<<CS22); //předdělicka timeru 2 na 64
		
	
	
	PORTD = 0x80;
	
	_delay_ms(2000);
	
	sei();
		
	while(1)
	{
		linka = uart_getc();
		switch (linka) {
		case 237:
			uart_putc(237);
			break;
		case 239:
			cisloPrikazu = uart_getc();
			id = uart_getc();
			switch (id) {
			case 1:
				ks = uart_getc();
				if (239 ^ cisloPrikazu ^ id ^ ks)
				{
					break;
				}
				ks = 239 ^ cisloPrikazu ^ 10;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(10);
				uart_putc(ks);
				
				pozice();
				
				ks = 239 ^ cisloPrikazu ^ 11;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(11);
				uart_putc(ks);
				
				break;
			case 2:
				x1 = uart_getc();
				x2 = uart_getc();
				y11 = uart_getc();
				y2 = uart_getc();
				ks = uart_getc();
				if (239 ^ cisloPrikazu ^ id ^ x1 ^ x2 ^ y11 ^ y2 ^ ks)
				{
					break;
				}
				ks = 239 ^ cisloPrikazu ^ 10;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(10);
				uart_putc(ks);
				
				prejezd();
				
				ks = 239 ^ cisloPrikazu ^ 11;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(11);
				uart_putc(ks);
				
				break;
			case 3:
				x1 = uart_getc();
				x2 = uart_getc();
				y11 = uart_getc();
				y2 = uart_getc();
				ks = uart_getc();
				if (239 ^ cisloPrikazu ^ id ^ x1 ^ x2 ^ y11 ^ y2 ^ ks)
				{
					
					break;
				}
				ks = 239 ^ cisloPrikazu ^ 10;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(10);
				uart_putc(ks);
				
				tisk();
				
				ks = 239 ^ cisloPrikazu ^ 11;
				uart_putc(239);
				uart_putc(cisloPrikazu);
				uart_putc(11);
				uart_putc(ks);
			case 4:
				p = uart_getc();
				NAHORE = p;
				DOLE = p;
				break;
			case 5:
				PORTB = pole[uart_getc()];
				break;
			case 6:
				PORTC = pole[uart_getc()];
				break;
			}
			break;
		}
		
	}
	return 0;
}