#include <lbt.h>
#include <al_common.h>
#include <al_swtimer.h>
#include <al_smd.h>
#include <al_uart.h>
#include <avr/interrupt.h>
#include <stdio.h>

#define CMD_SCAN            50
#define CMD_INIT			51
#define CMD_SET_SAMPLES     70
#define CMD_SET_SPEED       71

char send[16];
uint8_t pos_data = 0;
uint8_t com_data[2] = {0, 0};

uint16_t speed = 160;
uint16_t samples = 20;
uint16_t shift = 1600;
uint16_t turns = 20;

uint16_t readAD(uint8_t channel)
{
	ADMUX = 0b01000000;	
	ADMUX |= channel;
	ADCSRA |= 0b01000000;  
	while((ADCSRA & 0b00010000) == 0);
	ADCSRA |= 0b00010000;
	return ADCW;
}

void mechanics_init(void)
{
	smd_init();
	smd_set_step(SMD_STEP_EIGHTH);
    smd_set_current(100);
    smd_set_speed(speed);
    smd_move(-100);
    while(smd_get_flags() & SMD_IS_RUNNING == SMD_IS_RUNNING);
    if(PIND & 0b00100000)
    {
    	smd_move(100);
    	while(smd_get_flags() & SMD_IS_RUNNING == SMD_IS_RUNNING);
    }
	smd_move(-15000);
	while(smd_get_flags() & SMD_IS_RUNNING == SMD_IS_RUNNING);
	smd_init();
	smd_set_step(SMD_STEP_EIGHTH);
    smd_set_current(100);
    smd_set_speed(speed);
}

void scan(void)
{
	uint16_t i; 
	for (i=0;i<samples;i++)
    {
		smd_move((int32_t)i*shift);
		while(smd_get_flags() & SMD_IS_RUNNING == SMD_IS_RUNNING);
		sprintf(send, "%d;%d\n", i, readAD(6));
		uart_puts(send);
    }
	smd_move(-15000);
	while(smd_get_flags() & SMD_IS_ON_BEG_LSW != SMD_IS_ON_BEG_LSW);
}

void read_uart(void)
{
	while(!uart_rx_fifo_empty())	
	{
		com_data[pos_data] = uart_getc();
		if(pos_data == 1)
		{		
			switch(com_data[0])
			{
			case CMD_SCAN:
				pos_data = 0;
				scan();
				return;
			case CMD_INIT:
				pos_data = 0;
				mechanics_init();
				return;
			case CMD_SET_SAMPLES:
				samples = (uint16_t)com_data[1];
				shift = 10680/(samples-1); //200 - kroky na otáčku, 8 - osminový krok
				break;
			case CMD_SET_SPEED:
				speed = (uint16_t)com_data[1]*8; //10 - konstantní násobitel, 8 - osminový krok
				smd_set_speed(speed);
				break;
			default:
				break;
			}	
		}
		pos_data = !pos_data;	
	}
}

int main(void)
{
    swtimer_init();
	uart_init();
    sei();

	ADMUX = 0b00000000;
	ADCSRA = 0b10000111;  

    swtimer_delay(0,1000);

	while(1)
	{
		read_uart();
	}
    
    return 0;
}

