/*
 * minisumo.c
 *
 * Created: 14.04.2021 10:33:41
 * Author : vstrasil
 */ 
#define F_CPU 32000000UL
#include <avr/io.h>
#include <util/delay.h>
#include "Header/HID.h"
#include "Header/Motors.h"
#include "Header/Sensors.h"

typedef enum {
	default_state,
	flag1,
	choice,
	flag2,
	countdown,
	search,
	search2,
	sturn_L,
	sturn_R,
	turn_L,
	turn_R,
	straight,
	back,
	timer2,
	timer3,
	turn_2,
	turn_3,
	straight_2,
	straight_3,
	low_battery,
		} states;
states state = default_state;

int MIN = 8192;
int MAX = 0;

uint8_t speed = 120;
int distance = 900;

int LINE_value = 0;
int LINE_raw = 0;

long time_prev = 0;

uint8_t select_prog = 0;


int main(void)
{
	hid_init();
	clock_init();
	motors_init();
	sensors_init();
	
    while (1) 
    {	
		//1450 černá x bílá na ringu
		
		if (state != default_state)
		{
			if ((!button(STOP)))
			{
				state = default_state;
			}
			
		}
		
		switch (state)
		{
			//kalibrace
			case default_state:
			motorL_stop();
			motorR_stop();
			
					if (sensor_ch0(LINE)>1350)
					{
						led_on(LED1);
					} 
					else
					{
						led_off(LED1);
					}
					if (sensor_ch0(LINE)>1400)
					{
						led_on(LED2);
					}
					else
					{
						led_off(LED2);
					}
					if (sensor_ch0(LINE)>1450)
					{
						led_on(LED3);
					}
					else
					{
						led_off(LED3);
					}
					if (sensor_ch0(LINE)>1500)
					{
						led_on(LED4);
					}
					else
					{
						led_off(LED4);
					}
					if (sensor_ch0(LINE)>1550)
					{
						led_on(LED5);
					}
					else
					{
						led_off(LED5);
					}
					
					if (button(START))
					{
						_delay_ms(200);
						state = flag1;
					}
			break;
			
			case flag1:
			if (!button(START))
			{
				state = choice;
			}
			break;
			
			case choice:
			led_off(ALL);
			
			select_prog = dip();
			if (select_prog==1)
			{
				led_on(LED1);
			}
			else
			{
				led_off(LED1);	
			}
			if (select_prog==2)
			{
				led_on(LED2);
			}
			else
			{
				led_off(LED2);
			}
			if (select_prog==3)
			{
				led_on(LED3);
			}
			else
			{
				led_off(LED3);
			}
			if (select_prog==4)
			{
				led_on(LED4);
			}
			else
			{
				led_off(LED4);
			}
			if (select_prog==5)
			{
				led_on(LED5);
			}
			else
			{
				led_off(LED5);
			}
			
			
			if (button(START))
			{
				if (is_battery_ok()==1)
				{
					_delay_ms(200);
					state = flag2;
				}
				else
				{
					state = low_battery;
				}
			}
			break;
			
			case flag2:
			if (!button(START))
			{
				state = countdown;
			}
			break;
			

			case countdown:
			led_off(ALL);
			led_on(ALL);
			_delay_ms(300);
			led_off(ALL);
			_delay_ms(500);
			led_on(ALL);
			_delay_ms(500);
			led_off(ALL);
			_delay_ms(500);
			led_on(ALL);
			_delay_ms(500);
			led_off(ALL);
			_delay_ms(500);
			led_on(ALL);
			_delay_ms(500);
			led_off(ALL);
			_delay_ms(500);
			led_on(ALL);
			_delay_ms(500);
			led_off(ALL);
			_delay_ms(500);
			if (select_prog == 0)
			{
				state = search;
			}
			else if (select_prog == 1)
			{
				state = search2;
			}
			else if (select_prog == 2)
			{
				state = timer2;
			}
			else if (select_prog == 3)
			{
				state = timer3;
			}
			break;
			
			//hledani
			case search:
			led_off(ALL);
			led_on(LED1);
			motorR_forward(speed);
			motorL_backward(speed);
			
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch1(DIS2)>distance)
			{
				state = turn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = turn_L;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			break;
			
			case search2:
			led_off(ALL);
			led_on(LED1);
			motorR_backward(speed);
			motorL_forward(speed);
			
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch1(DIS2)>distance)
			{
				state = turn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = turn_L;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			break;
			
			//vidi pravy
			case sturn_R:
			led_off(ALL);
			led_on(LED2);
			
			motorR_backward(speed);
			motorL_forward(speed);
			
			if (sensor_ch1(DIS2)>distance)
			{
				state = turn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = turn_L;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}	
			break;
			
			//vidi pravy predni
			case turn_R:
			led_off(ALL);
			led_on(LED3);
			
			motorR_stop();
			motorL_forward(speed);
			
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = straight;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}
			break;
			
			//vidi levy predni
			case turn_L:
			led_off(ALL);
			led_on(LED4);
			
			motorR_forward(speed);
			motorL_stop();
			
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch1(DIS2)>distance)
			{
				state = straight;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}
			break;
			
			//vidi levy
			case sturn_L:
			led_off(ALL);
			led_on(LED5);
			
			motorR_forward(speed);
			motorL_backward(speed);
			
			if (sensor_ch1(DIS2)>distance)
			{
				state = turn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = turn_L;
			}
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}
			break;
			
			//vidi oba predni
			case straight:
			led_off(ALL);
			led_on(ALL);
			
			motorR_forward(speed);
			motorL_forward(speed);
			
			if (sensor_ch1(DIS1)>distance)
			{
				state = sturn_R;
			}
			if (sensor_ch1(DIS2)>distance)
			{
				state = turn_R;
			}
			if (sensor_ch1(DIS3)>distance)
			{
				state = turn_L;
			}
			if (sensor_ch1(DIS4)>distance)
			{
				state = sturn_L;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}
			break;
			
			//strategie 2 //levá
			case timer2:
			timer_init();
			time_prev=timer_act();
			
			state = turn_2;
			break;
			
			case turn_2:
			led_off(ALL);
			led_on(LED1);		
			
			motorR_forward(speed);
			motorL_backward(speed);
			
			if ((timer_act()-time_prev)>270)
			{
				state = straight_2;
			}
			break;
			
			
			case straight_2:
			led_off(ALL);
			led_on(LED2);
			
			motorR_forward(speed);
			motorL_forward(speed);
			
			if ((timer_act()-time_prev)>1000)
			{
				state = search;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}

			break;
			
			//strategie 3 //pravá
			case timer3:
			timer_init();
			time_prev=timer_act();
			
			state = turn_3;
			break;
			
			case turn_3:
			led_off(ALL);
			led_on(LED1);
			
			motorR_backward(speed);
			motorL_forward(speed);
			
			if ((timer_act()-time_prev)>270)
			{
				state = straight_3;
			}
			break;
			
			
			case straight_3:
			led_off(ALL);
			led_on(LED2);
			
			motorR_forward(speed);
			motorL_forward(speed);
			
			if ((timer_act()-time_prev)>1000)
			{
				state = search2;
			}
			if (sensor_ch0(LINE)<1450)
			{
				state = back;
			}

			break;

			
			//cara
			case back:
			motorR_backward(speed);
			motorL_backward(speed);
			_delay_ms(50);
			state = search;
			break;
			
			//low battery
			case low_battery:
			led_off(ALL);	
			_delay_ms(200);
			led_on(ALL);
			_delay_ms(200);
			break;
			
		}
		
    }
}

