#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <stdlib.h>

#define sbi(port, bit) port |= _BV(bit)
#define cbi(port, bit) port &= ~_BV(bit)

#define ms 1450
#define jobb ms
#define bal (2*ms)

void ioinit (void);

void Haladj(long time);
// A robot max. sebeséggel halad elore a megadott time eltelteig

void Haladji(int Balseb, int Jobbseb, long time);
// A bal kerek "balseb", a jobb pedig "jobbseb" sebesseggel forog "time" ideig, ahol a sebesseg
// -20..20 lehet !!!

void Tolass(long time);
// A robot tolat max. sebeséggel "time" ideig

void Jobbra(long time);
// A robot folyamatosan jobbra forog "time" ideig

void Balra(long time);
// A robot folyamatosan balra forog "time" ideig

void Allj(void);
// A robot megáll

uint16_t Korulnez(void);
// Megadja, hogy az infraszenzor melyik pozicioban lat el legmesszebre
// A visszateresi ertek: 120 es 500 kozott van

void Radar(uint16_t pos, uint16_t ido);
// Pos = 0..180

void Csap(void);                                            //figyeli a ket csápot (mechanikus erzekelot)

uint16_t getADC(uint8_t channel);

#define Dataout PD3                                       // Az infraszenzor alatti szervo portja
#define Sbal PD4                                           // A baloldali mechanikus erzekelo portja
#define Sjobb PD5                                          // A jobboldali mechanikus erzekelo portja


//------------------------------------------------------------------------------------------------------
//														MAIN
//------------------------------------------------------------------------------------------------------
long _time;

int main (void)
{
	long delta;
	uint16_t d1, d2, tav;
	
    ioinit();

	for(;;)
	{
		Radar(90,ms/50);
		if (getADC(0) > 400)
		{
			Tolass(100);
			Allj();
			tav = Korulnez();
			delta = abs(tav - 90);
			if (tav < 90) Jobbra(delta); else Balra(delta);
		}
		else
		{
			Radar(90+30,ms/50);
			d1 = getADC(0);
			Radar(90-30,ms/50);
			d2 = getADC(0);
			if (d1 < d2) Haladji(20, 5, 50); else Haladji(5, 20, 50);
		}
	}
    return (0);
}

//------------------------------------------------------------------------------------------------------
// 													END MAIN
//------------------------------------------------------------------------------------------------------

void ioinit (void)
{
    TCCR1A = (1<<WGM11) | (1<<COM1A1) | (1<<COM1B1);
	TCCR1B = (1<<WGM13) | (1<<WGM12)  | (1<<CS12);

    /* set PWM value to 0 */
    OCR1A = 0;
    OCR1B = 0;	

	DDRB |= (1<<PB5);
	DDRB |= (1<<PB6);
	
	ICR1 = 777;                                              // 40 Hz * 1024/777 = 50 Hz
	
	DDRD = 0;
	DDRD |= (1<<PD3);
	
}

#define CHANNELOFFSET 4

/*
ADCSR: AD Control and Status Register
7: ADEN - ADC Enable
6: ADSC - ADC Start Conversion
5: ADFR - ADC Free Running select
4: ADIF
3: ADIE
2: ADPS2 - prescaler;000=2;001=2;010=4;011=8;100=16;101=32;110=64;111=128;
1: ADPS1 
0: ADPS0

refs1 reference; 11=internal;01=external;00=AREF
refs0
*/

uint16_t getADC(uint8_t channel)
{
	uint8_t i;
	uint16_t result=0;
	ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS0); // prescaler=32
// setzen auf 8 (1) und ADC aktivieren (1)
	ADMUX = CHANNELOFFSET+channel; // Kanal waehlen
	ADMUX |= (1<<REFS1) | (1<<REFS0); // interne Referenzspannung nutzen
/* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
	ADCSRA |= (1<<ADSC); // eine ADC-Wandlung
	while(!(ADCSRA & (1<<ADIF))); // auf Abschluss der Konvertierung warten (ADIF-result = 0;
/* Eigentliche Messung - Mittelwert aus 4 aufeinanderfolgenden Wandlungen */
	for(i=0;i<4;i++)
	{
		ADCSRA |= (1<<ADSC); // eine Wandlung "single conversion"
		while(!(ADCSRA & (1<<ADIF))); // auf Abschluss der Konvertierung warten (ADIF-result += ADC; // Wandlungsergebnisse aufaddieren
	}
	ADCSRA &= ~(1<<ADEN); // ADC deaktivieren (2)
	result >>= 2; // Summe durch vier teilen = arithm. Mittelwert
	return result;	
}

int csapol = 0;

void Csap(void)
{
	if (csapol) return;
	csapol = 1;
	if (bit_is_clear(PIND,Sbal))
	{
		Tolass(100);
		Jobbra(50);
	}
	else
	if (bit_is_clear(PIND,Sjobb))
	{
		Tolass(100);
		Balra(50);
		csapol = 0;		
	}
	csapol = 0;					
}

void Indulj(void)                                          // PWM-et bekapcsolja
{
	ICR1 = 777;
}

#define CsapotFigyel for (_time = 0; _time < time*ms; _time++) Csap();

void Haladj(long time)
{
	Indulj();
	OCR1A = 75;
	OCR1B = 35;
	CsapotFigyel
}

void Haladji(int Balseb, int Jobbseb, long time)
{
	Indulj();
	Balseb %= 21;
	Jobbseb %= 21;
	OCR1A = 55 + Balseb;
	OCR1B = 55 - Jobbseb;
	CsapotFigyel
}

void Tolass(long time)
{
	Indulj();
	OCR1A = 35;
	OCR1B = 75;
	CsapotFigyel
}

void Jobbra(long time)
{
	Indulj();
	OCR1A = 75;
	OCR1B = 75;
	CsapotFigyel
}

void Balra(long time)
{
	Indulj();
	OCR1A = 35;
	OCR1B = 35;
	CsapotFigyel
}

void Allj(void)
{
	ICR1 = 10;
}

uint16_t Korulnez(void)
{
   uint16_t p,w,minsv,minsvi;
   Radar(0,ms/30);

   minsv = 1000;
   minsvi = 0;

   for (p = 0; p <= 180; p += 8)
   {
		Radar(p,ms/100);
		w = getADC(0);
		if (w < minsv)
		{
			minsv = w;
			minsvi = p;
		}
	}
	Radar(90,ms/30);
	cbi(PORTD,Dataout);
    return minsvi;
}

void Radar(uint16_t pos, uint16_t ido)
{
	long p2,_time2;
	pos = (long)jobb+(pos/180.0)*(bal-jobb);
	p2 = 20*ms - pos;
	for (_time = 0; _time < ido; _time++)
	{
		sbi(PORTD, Dataout); for (_time2 = 0; _time2 < pos; _time2++);
		cbi(PORTD, Dataout); for (_time2 = 0; _time2 < p2; _time2++);
		Csap();
	}
}
