#define F_CPU 8000000UL
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <stdio.h>
#include "mirf.h"
#include "mirf.c"
#include "spi.h"
#include "spi.c"
#include <stdlib.h>

// adres nadawania clie1 = 63:6c:69:65:31 zrodlo: http://dolcevie.com/js/converter.html
uint8_t tx_address[5] = { 0x63, 0x6c, 0x69, 0x65, 0x31 };

// adres odbioru serv1 = 73:65:72:76:31 zrodlo: http://dolcevie.com/js/converter.html
uint8_t rx_address[5] = { 0x73, 0x65, 0x72, 0x76, 0x31 };

void init_pwm()
{
	DDRD |= (1<<PD4)|(1<<PD5);

	/*TCCR1A*/
	TCCR1A |=(1<<5)|(1<<COM1A1)|(1<<WGM12)|(1<<WGM11);
	TCCR1A &= ~(1<<COM1A0);
	TCCR1B |= (1<<WGM13);

	/*TCCR1B*/

	TCCR1B |= (1<<7)|(0<<CS12 | 1<<CS11 | 1<<CS10);

	/*NADANIE START. WARTOSCI*/
	OCR1A=0;
	OCR1B=0;
	ICR1=255;
}

void init_engines()
{
	DDRA |= (1<<PA0)|(1<<PA1);
	DDRD |=(1<<PD7);
	DDRC |=(1<<PC0);
}

void watch_dog_engines()
{
	OCR1A=0;
	OCR1B=0;

	//PORTA = PORTA & (0b11111100);
	//PORTD = PORTD & (0b01111111);
	//PORTC = PORTC & (0b11111110);
}

void steer_pwm(int adc_from_pilot)
{
	int max_pwm = 980;
	float pwm_for_left=90+((980-adc_from_pilot)/6);
	float pwm_for_right=90+(adc_from_pilot/6);

	OCR1A = pwm_for_left;
	OCR1B = pwm_for_right;
}

void go_forward()
{
	PORTA |=(1<<PA0);
	PORTD |=(1<<PD7);
	PORTC &=~(1<<PC0);
	PORTA &=~(1<<PA1);
}

void go_backward()
{
	PORTA |=(1<<PA1);
	PORTD &=~(1<<PD7);
	PORTC |=(1<<PC0);
	PORTA &=~(1<<PA0);
}

steer_engines(char engine1, char engine2)
{
	if(engine1 == '1' && engine2 == '0')
		go_forward();
	else if(engine1 == '0' && engine2 == '1')
		go_backward();
	else
		watch_dog_engines();
}

int mainloop()
{
	int connection_lost=0;

	PORTD &= ~(1<<PD6);
	_delay_ms(10);

	watch_dog_engines();

	while(1)
	{
			char to_print[14];
			char temp[12];
			PORTD |=(1<<PD6);

			while(!mirf_data_ready())
			{
				_delay_us(20);
				connection_lost++;

				if(connection_lost==0)
					mainloop();
			}

			mirf_get_data(to_print);
			connection_lost=0;



			for(int i=2;i<14;i++)
				temp[i-2]=to_print[i];

			int digital_value=atoi(temp);
			steer_pwm(digital_value);
			steer_engines(to_print[0], to_print[1]);

			_delay_ms(10);
			PORTD &= ~(1<<PD6);
			_delay_ms(10);
	}

    return 0;
}

int main()
{
	DDRD |= (1<<PD6);

	mirf_init();
	_delay_ms(50);
	sei();

	mirf_set_RADDR(rx_address);
	mirf_set_TADDR(tx_address);

	mirf_config();

	init_pwm();
	init_engines();

	mainloop();
}