#include <AVR/io.h>
#include <AVR/interrupt.h>
#include <util/delay.h>

#define P4 PC0
#define P3 PC1
#define P2 PC2
#define P1 PC3
#define L1 PC4
#define L2 PC5
#define L3 20U
#define L4 21U
#define L_PWM PD6
#define P_PWM PB2
#define PM PD0
#define LM PD5
int8_t Tablica_czujnikow[8] = { P4,P3,P2,P1,L1,L2,L3,L4 };
int8_t Tablica_wag[8] = { 10,7,3,1,-1,-3,-7,10 };
int8_t analog2digital(int8_t Pin);
float wartosc_odczytana(int8_t tab_czuj[8], int8_t tab_wag[8])
{
	int8_t odczyt = 0;
	int8_t il_czuj = 0;
	for (int8_t i = 0; i < 8; i++)
	{
		if (analog2digital(tab_czuj[i]) < 150)
		{
			il_czuj++;
			odczyt += tab_wag[i];
		}
	}
	if (il_czuj = 0) return 0;
	else
	{
		return odczyt / il_czuj;
	}
}
volatile int16_t var_kierunkowe = 130;
ISR(TIMER0_COMPA_vect) // LEWY PWM // tutaj mozecie wrzucic pida dla lewgo pwm
{

}
ISR(TIMER2_OVF_vect) // PRAWY PWM // tutaj mozecie wrzucic pida dla prawego pwm
{

}
int main(void)
{
	Serial.begin(9600);
	DDRC &= ~(1 << P4);
	DDRC &= ~(1 << P3);
	DDRC &= ~(1 << P2);
	DDRC &= ~(1 << P1);
	DDRC &= ~(1 << L1);
	DDRC &= ~(1 << L2);
	DDRC &= ~(1 << L3);
	DDRC &= ~(1 << L4);
	// Silnik 
	DDRD |= (1 << L_PWM); // lewy pwm
	DDRB |= (1 << P_PWM); // prawy pwm
	DDRB |= (1 << PM); // prawy silnik przod
	DDRD |= (1 << LM); // lewy silnik przod
	// ADC 
	ADCSRA |= ((1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0));
	ADMUX |= (1 << REFS0); // poczytaj note 
	ADCSRA |= (1 << ADEN);
	ADCSRA |= (1 << ADSC);
	//PWM
	TCCR0A |= (1 << COM0A1) | (1 << WGM01) | (1 << WGM00);
	TIMSK0 |= (1 << TOIE0);
	TCCR2A |= (1 << COM2B1) | (1 << WGM21) | (1 << WGM20);
	TIMSK2 |= (1 << TOIE2);
	OCR2B = 0; //prawy pwm
	OCR0A = 0; //lewy pwm
	sei();
	TCCR0B |= (1 << CS00);
	TCCR2B |= (1 << CS20);

	while (1)
	{
		float kierunek = wartosc_odczytana(Tablica_czujnikow, Tablica_wag);
		if (kierunek<5 && kierunek>-5)
		{
			PORTB|=(1 << PM);
			PORTD |= (1 << LM);
			OCR0A = var_kierunkowe;
			OCR2B = var_kierunkowe;

		}
		if (kierunek > 5)
		{
			PORTB |= (1 << PM);
			PORTD |= (0 << LM);
			OCR0A = var_kierunkowe+30;
			OCR2B = 30;
		}
		if (kierunek < -5)
		{
			PORTB |= (0 << PM);
			PORTD |= (1 << LM);
			OCR0A = 30;
			OCR2B = var_kierunkowe + 30;
		}


	}
}
int8_t analog2digital(int8_t Pin)
{
	ADMUX &= 0xF0;
	ADMUX |= Pin;
	ADCSRA |= (1 << ADSC);
	ADCSRA |= (1 << ADSC);
	while (ADCSRA & (1 << ADSC));
	return ADCW;
}