#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

uint8_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;
	}
}

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);
	while (1)
	{
		for (int i = 0; i < 8; i++)
		{
			Serial.print("Czujnik: " + i);
			Serial.println(analog2digital(Tablica_czujnikow[i]));
			delay(200);
		}
	}
}
int8_t analog2digital(int8_t Pin)
{
	ADMUX &= 0xF0;
	ADMUX |= Pin;
	ADCSRA |= (1 << ADSC);
	ADCSRA |= (1 << ADSC);
	while (ADCSRA & (1 << ADSC));
	return ADCW;
}