#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <string.h>

#define bitSet(var, mask)   ((var) |= (1 << (mask)))
#define bitClear(var, mask)   ((var) &= ~(1 << (mask)))

#define F_CPU 14745600UL // 14.7456 MHz

#define LEVY_SENSOR_H_PIN	PINB
#define LEVY_SENSOR_H		0
#define LEVY_SENSOR_Z_PIN	PIND
#define LEVY_SENSOR_Z		7

#define PRAVY_SENSOR_H_PIN	PIND
#define PRAVY_SENSOR_H		6
#define PRAVY_SENSOR_Z_PIN	PIND
#define PRAVY_SENSOR_Z		5

#define LEVY_MOTOR_RELE_PORT	PORTD
#define LEVY_MOTOR_RELE_PIN	PIND
#define LEVY_MOTOR_RELE		2

#define PRAVY_MOTOR_RELE_PORT	PORTD
#define PRAVY_MOTOR_RELE_PIN	PIND
#define PRAVY_MOTOR_RELE	3

#define CURSPEED_WEIGHT		15 // vaha soucasne rychlosti
#define AVGSPEED_WEIGHT		32750 // vaha prumerne rychlosti
#define AVGSPEED_WEIGHT_M	32750 // nasobitel

#define DATA_REGISTER_EMPTY (1<<UDRE)
#define FRAMING_ERROR (1<<FE)
#define DATA_OVERRUN (1<<DOR)

#define RX_BUFFER_SIZE 15


#include <util/delay.h>
void m_delay_ms(unsigned int ms) {
	unsigned int i;
	for ( i=0; i<ms; i++) { _delay_ms(1); }
}
void m_delay_s(unsigned int s) {
	unsigned int i, i2;
	for ( i=0; i<s; i++) { for ( i2=0; i2<100; i2++) { _delay_ms(10); } }
}

volatile unsigned char rs232enter = 0;
volatile unsigned char rx_buffer[RX_BUFFER_SIZE];   // buffer prijatych znaku na RS232 

unsigned int E_a, E_b, E_aWAS, E_bWAS;
int E_llll=0, E_pppp=0, E_llllNEW=0, E_ppppNEW=0;



long int SP1=0, SP1conv=0, SP2=0, SP2conv=0, speedDiffP_sum=0, speedDiffL_sum=0;
int speedDiffL=0, speedDiffP=0;


int spA1=0, spA2=0, spB1=0, spB2=0, accelA=0, accelB=0, wTimeA=1, wTimeB=1;


unsigned int pulseL=0, pulsecounterL=0, pulseLfix=0, pulsecounterLfix=0, finalSpeedL=0, pulseP=0, pulsecounterP=0, finalSpeedP=0, finalSpeedLastL=0;

unsigned int speedLcounted1=0, speedLcounted2=0, speedLcounted3=0, speedLcounted4=0, speedLdir=0, speedLdirTest=0;
unsigned int speedPcounted1=0, speedPcounted2=0, speedPcounted3=0, speedPcounted4=0, speedPdir=0, speedPdirTest=0;

int speedLset=0;
int speedPset=0;

int pwmDtosetDest=0, pwmDtoset=0, pwm1counter=0, pwm2counter=0, pwm3counter=0, PWMD1=0, PWMD2=0, PWMD3=0;

unsigned long int longac=0, longacDOit=0, longac3=0, longac3DOit=0, doshutdown=0, longac10=0, longac10DOit=0;

int S_a, S_b, S_llll=0, S_pppp=0, StestL=0, StestLWAS=0, StestLSleep=0, StestP=0, StestPWAS=0, StestPSleep=0;

// mereni doby behu
unsigned int hodiny=0,minuty=0,sekundy=0;


// odeslani znaku na USART
// ceka na vyprazneni TX bufferu a zapise do nej novy znak  
void putchar1(char c) {
	while (!(UCSRA & DATA_REGISTER_EMPTY));
	UDR=c;
}

// odesle na terminal znaky konce radku a navratu na zacatek
void lfcr(void) {
	putchar1('\n');    //send lf - 10
	//putchar1('\r');    //send cr - 13
}

// vypsani znakoveho retezce na terminal z flash pameti  
void write(char *sss){
	char k;
	while ((k=pgm_read_byte(sss++))) {
		putchar1(k);
	}
}

// vypsani znakoveho retezce na terminal  
void writestr(char *sss){
	char k;
	while ((k=(*sss++))) {
		putchar1(k);
	}
}

// test ASCII znaku zda je argument cislo od 0-9
unsigned char is_numeric(unsigned char arg)  {
	if ((arg>='0') && (arg<='9')) return 1;
	return 0;
}


ISR(TIMER0_OVF_vect) { // zavola se pri preteceni interniho 8bit citace
	TCNT0=160; // 14.7456MHz/256/96=600 preruseni/sec - pocita s nastavenim interniho oscilatoru na 	
		// 14.7456MHz. Pokud tak neni, je nutshne prepocitat
		 // 256 (velikost counteru) - 250 (nas delitel) = 6 (pri kazdem preteceni	
			// zaciname od 6 a ne od 0)

	// pokud nulove otacky a chci nulu, tak vypnout uplne pwm
//	if (S_llll<50 && finalSpeedL==0) { SP1=0; }

//	if (S_llll>finalSpeedL) { speedDiffL=S_llll-finalSpeedL; } else { speedDiffL=finalSpeedL-S_llll; }
//	speedDiffL*=2;

//	if (S_llll>finalSpeedL) { SP1+=speedDiffL; }
//	if (S_llll<finalSpeedL) { SP1-=speedDiffL; }

//	if (finalSpeedL>finalSpeedLastL) { SP1-=(finalSpeedL-finalSpeedLastL)*1000; } else { SP1+=(finalSpeedLastL-finalSpeedL)*1000; }
	
//	if (SP1<0) { SP1=0; }
//	if (SP1>511000) { SP1=511000; }
	
//	if (SP1<500) { SP1conv=0; } else { SP1conv=SP1; SP1conv/=1000; }

//	finalSpeedLastL=finalSpeedL;

//	OCR1A=SP1conv;
//OCR1A=330;


// smer otaceni

if ( E_a && bit_is_clear(LEVY_MOTOR_RELE_PIN,LEVY_MOTOR_RELE) && (finalSpeedL<20) && (OCR1A<10) ) { bitSet(LEVY_MOTOR_RELE_PORT,LEVY_MOTOR_RELE); }
if ( !E_a && bit_is_set(LEVY_MOTOR_RELE_PIN,LEVY_MOTOR_RELE) && (finalSpeedL<20) && (OCR1A<10) ) { bitClear(LEVY_MOTOR_RELE_PORT,LEVY_MOTOR_RELE); }

if ( E_b && bit_is_clear(PRAVY_MOTOR_RELE_PIN,PRAVY_MOTOR_RELE) && (finalSpeedP<20) && (OCR1B<10) ) { bitSet(PRAVY_MOTOR_RELE_PORT,PRAVY_MOTOR_RELE); }
if ( !E_b && bit_is_set(PRAVY_MOTOR_RELE_PIN,PRAVY_MOTOR_RELE) && (finalSpeedP<20) && (OCR1B<10) ) { bitClear(PRAVY_MOTOR_RELE_PORT,PRAVY_MOTOR_RELE); }


//levy motor

// pokud nulove otacky a chci nulu, tak vypnout uplne pwm
	if (S_llll<30 && finalSpeedL==0) {
		SP1=0;
	}
	else {
		if (S_llll>finalSpeedL) { speedDiffL=S_llll-finalSpeedL; } else { speedDiffL=finalSpeedL-S_llll; }
		if (speedDiffL<=10) bitSet (PORTC, 1); else bitClear (PORTC, 1);
		if (S_llll>finalSpeedL) { speedDiffL_sum+=speedDiffL; SP1=((CURSPEED_WEIGHT*speedDiffL_sum)+(AVGSPEED_WEIGHT*speedDiffL)); }
		if (S_llll<finalSpeedL) { speedDiffL_sum-=speedDiffL; SP1=((CURSPEED_WEIGHT*speedDiffL_sum)-(AVGSPEED_WEIGHT*speedDiffL)); }
	}

	if (SP1<0) { SP1=0; }
	if (SP1>5110000) { SP1=5110000; }

	SP1conv=SP1; SP1conv/=10000;


	if (SP1<500) { SP1conv=0; } else { SP1conv=SP1; SP1conv/=10000; }

	OCR1A=SP1conv;


//pravy motor

// pokud nulove otacky a chci nulu, tak vypnout uplne pwm
	if (S_pppp<30 && finalSpeedP==0) {
		SP2=0;
	}
	else {
		if (S_pppp>finalSpeedP) { speedDiffP=S_pppp-finalSpeedP; } else { speedDiffP=finalSpeedP-S_pppp; }
		if (speedDiffP<=10) bitSet (PORTC, 2); else bitClear (PORTC, 2);
		if (S_pppp>finalSpeedP) { speedDiffP_sum+=speedDiffP; SP2=((CURSPEED_WEIGHT*speedDiffP_sum)+(AVGSPEED_WEIGHT*speedDiffP)); }
		if (S_pppp<finalSpeedP) { speedDiffP_sum-=speedDiffP; SP2=((CURSPEED_WEIGHT*speedDiffP_sum)-(AVGSPEED_WEIGHT*speedDiffP)); }
	}

	if (SP2<0) { SP2=0; }
	if (SP2>5110000) { SP2=5110000; }

	SP2conv=SP2; SP2conv/=10000;


	if (SP2<500) { SP2conv=0; } else { SP2conv=SP2; SP2conv/=10000; }

	OCR1B=SP2conv;

}

ISR(TIMER2_OVF_vect) { // zavola se pri preteceni interniho 8bit citace
	TCNT2=254; // 14.7456MHz/256/2=28800 preruseni/sec - pocita s nastavenim interniho oscilatoru na 	
		// 14.7456MHz. Pokud tak neni, je nutne prepocitat
		 // 1024 (velikost counteru) - 9 (nas delitel) = 6 (pri kazdem preteceni 		
			// zaciname od 6 a ne od 0)

	longac++;
	// longac ma byt 600 pro poslani 1x/vterinu, 120 pro 5x/vterinu
	if (longac>=28800) {
		longac=0;
		longacDOit=1;
		longac3DOit=1;
		longac10++;
	}

	if (pwm1counter<=PWMD1) { bitSet (PORTC, 3); } else { bitClear (PORTC, 3); }
	if (pwm1counter>=256) { pwm1counter=0; }
	pwm1counter++;

	if (pwm2counter<=PWMD2) { bitSet (PORTC, 4); } else { bitClear (PORTC, 4); }
	if (pwm2counter>=256) { pwm2counter=0; }
	pwm2counter++;

	if (pwm3counter<=PWMD3) { bitSet (PORTC, 5); } else { bitClear (PORTC, 5); }
	if (pwm3counter>=256) { pwm3counter=0; }
	pwm3counter++;

// levy
		pulsecounterL++;

//		if (bit_is_set(LEVY_SENSOR_Z_PIN, LEVY_SENSOR_Z) && ! speedLcounted1) {
//			pulseL++;
//			speedLcounted3 = 0;
//			speedLcounted1 = 1;
//		}
//
//		if (bit_is_set(LEVY_SENSOR_H_PIN, LEVY_SENSOR_H) && ! speedLcounted2) {
//			pulseL++;
//			speedLcounted4 = 0;
//			speedLcounted2 = 1;
//		}
//
//		if (bit_is_clear(LEVY_SENSOR_Z_PIN, LEVY_SENSOR_Z) && ! speedLcounted3) {
//			pulseL++;
//			speedLcounted1 = 0;
//			speedLcounted3 = 1;
//		}
//
//		if (bit_is_clear(LEVY_SENSOR_H_PIN, LEVY_SENSOR_H) && ! speedLcounted4) {
//			pulseL++;
//			speedLcounted2 = 0;
//			speedLcounted4 = 1;
//		}


		if (bit_is_set(LEVY_SENSOR_Z_PIN, LEVY_SENSOR_Z) && ! speedLcounted1) {
			pulseL++;
			speedLcounted3 = 0;
			speedLcounted1 = 1;
		}

		if (bit_is_set(LEVY_SENSOR_H_PIN, LEVY_SENSOR_H) && ! speedLcounted2) {
			pulseL++;
			speedLcounted4 = 0;
			speedLcounted2 = 1;
		}

		if (bit_is_clear(LEVY_SENSOR_Z_PIN, LEVY_SENSOR_Z) && ! speedLcounted3) {
			pulseL++;
			speedLcounted1 = 0;
			speedLcounted3 = 1;
		}

		if (bit_is_clear(LEVY_SENSOR_H_PIN, LEVY_SENSOR_H) && ! speedLcounted4) {
			pulseL++;
			speedLcounted2 = 0;
			speedLcounted4 = 1;
		}



		if (speedLcounted1 && speedLcounted2) {
			if (!speedLdirTest) speedLdir = 1; else speedLdir = 0;
		}

		if (speedLcounted1 && ! speedLcounted2) {
			speedLdirTest=0;
		}

		if (! speedLcounted1 && speedLcounted2) {
			speedLdirTest=1;
		}

		if (pulseL>=12 || pulsecounterL>28800) {
			finalSpeedL = (pulseL*(28800000/pulsecounterL))/1000;
			pulsecounterL = 0;
			pulseL = 0;
		}

// pravy
		pulsecounterP++;

		if (bit_is_set(PRAVY_SENSOR_H_PIN, PRAVY_SENSOR_H) && ! speedPcounted1) {
			pulseP++;
			speedPcounted3 = 0;
			speedPcounted1 = 1;
		}

		if (bit_is_set(PRAVY_SENSOR_Z_PIN, PRAVY_SENSOR_Z) && ! speedPcounted2) {
			pulseP++;
			speedPcounted4 = 0;
			speedPcounted2 = 1;
		}

		if (bit_is_clear(PRAVY_SENSOR_H_PIN, PRAVY_SENSOR_H) && ! speedPcounted3) {
			pulseP++;
			speedPcounted1 = 0;
			speedPcounted3 = 1;
		}

		if (bit_is_clear(PRAVY_SENSOR_Z_PIN, PRAVY_SENSOR_Z) && ! speedPcounted4) {
			pulseP++;
			speedPcounted2 = 0;
			speedPcounted4 = 1;
		}

		if (speedPcounted1 && speedPcounted2) {
			if (!speedPdirTest) speedPdir = 1; else speedPdir = 0;
		}

		if (speedPcounted1 && ! speedPcounted2) {
			speedPdirTest=0;
		}

		if (! speedPcounted1 && speedPcounted2) {
			speedPdirTest=1;
		}

		if (pulseP>=12 || pulsecounterP>28800) {
			finalSpeedP = (pulseP*(28800000/pulsecounterP))/1000;
			pulsecounterP = 0;
			pulseP = 0;
		}

}

// preruseni po prichodu noveho znaku na USART
// novy byte zapise do pozice 0, ostatni posune o 1 vyse = FIFO fornta
// po prichodu \n nastavi priznak enteru - je zpracovan v main()
ISR(USART_RXC_vect) {
	unsigned char status,data,i;
	status=UCSRA;
	data=UDR;
	if ((status & (FRAMING_ERROR | DATA_OVERRUN))==0) {
		if((data=='\n')) { rs232enter=1; }
		if(data>=32 && data<=126) {
			i=RX_BUFFER_SIZE-1;
			while(i>0) {
				rx_buffer[i]=rx_buffer[i-1];
				i--;
			}
			rx_buffer[0]=data;
		}
	}
}
// otestuje, zdali se cmd nachazi na count pozici bufferu
// a zda-li je za cmd pouze numericky argument
unsigned char cmdtest(unsigned char cmd, unsigned char count) {
	if(cmd!=rx_buffer[count]) return 0;
	while(count--) {
		if(!is_numeric(rx_buffer[count])) {
			write(PSTR("ERROR: NUMBER EXPECTED AS AN ARGUMENT"));
			lfcr();
			return 0;
		}
	}
	return 1;
}

// nastavi pozadovanou rychlost motoru ve stupnich za sekundu
void speedset(void) {
	char stConv[4];
	write(PSTR("SETTING ENGINE SPEEDS: LEFT "));

// cteni smeru, rychlosti
	E_a = (int) (rx_buffer[9]-48);
	E_b = (int) (rx_buffer[8]-48);

	S_llll = ( ( (int) (rx_buffer[7]-48) ) * 1000 );
	S_llll += ( ( (int) (rx_buffer[6]-48) ) * 100 );
	S_llll += ( ( (int) (rx_buffer[5]-48) ) * 10 );
	S_llll += ( (int) (rx_buffer[4]-48) );
	S_pppp = ( ( (int) (rx_buffer[3]-48) ) * 1000 );
	S_pppp += ( ( (int) (rx_buffer[2]-48) ) * 100 );
	S_pppp += ( ( (int) (rx_buffer[1]-48) ) * 10 );
	S_pppp += ( (int) (rx_buffer[0]-48) );

	itoa(S_llll, stConv, 10);
	writestr(stConv);
	write(PSTR(" / RIGHT "));
	itoa(S_pppp, stConv, 10);
	writestr(stConv);
	lfcr();
}

// nastavi pozadovane pwm pro led
void pwmledset(void) {
	char stConv[4];
	write(PSTR("SETTING LED "));

	pwmDtoset = ( ( (int) (rx_buffer[2]-48) ) * 100 );
	pwmDtoset += ( ( (int) (rx_buffer[1]-48) ) * 10 );
	pwmDtoset += ( ( (int) (rx_buffer[0]-48) ) );
	if (pwmDtoset > 255) pwmDtoset = 255;

	pwmDtosetDest = ( ( (int) (rx_buffer[3]-48) ) );
	if (pwmDtosetDest == 1) { write(PSTR("1 PWM: ")); PWMD1 = pwmDtoset; }
	else if (pwmDtosetDest == 2) { write(PSTR("2 PWM: ")); PWMD2 = pwmDtoset; }
	else if (pwmDtosetDest == 3) { write(PSTR("3 PWM: "));  PWMD3 = pwmDtoset; }
	else write(PSTR(": UNKNOWN LED: "));


	itoa(pwmDtoset, stConv, 10);
	writestr(stConv);
	lfcr();
}

// zobrazeni casu
void display_uptime(void){
	char stConv[4];



	write(PSTR("Uptime: "));
	sekundy++;
	if (sekundy == 60) {
		if (minuty == 59) {
			hodiny++;
			minuty = 0;
		}
		else {
			minuty++;
		}
		sekundy = 0;
	}

	itoa(hodiny, stConv, 10);
	if (hodiny < 10) write(PSTR("0"));
	writestr(stConv);

	itoa(minuty, stConv, 10);
	write(PSTR(":"));
	if (minuty < 10) write(PSTR("0"));
	writestr(stConv);

	itoa(sekundy, stConv, 10);
	write(PSTR(":"));
	if (sekundy < 10) write(PSTR("0"));
	writestr(stConv);

	lfcr();
}

// nastavi uptime
void setuptime(void){

	write(PSTR("SETTING UPTIME"));
	lfcr();

	hodiny=(10*(rx_buffer[5]-48))+(rx_buffer[4]-48);
	minuty=(10*(rx_buffer[3]-48))+(rx_buffer[2]-48);
	sekundy=(10*(rx_buffer[1]-48))+(rx_buffer[0]-48);

}



int main () {

	char stConv[4];

	UCSRA=0x00;
	UCSRB=0x98;
	UCSRC=0x86;
	UBRRH=0x00;
	UBRRL=7;  // speed 115200

	// Vstupni piny
	bitClear (DDRD, 5);
	bitClear (DDRD, 6);
	bitClear (DDRD, 7);
	bitClear (DDRB, 0);

	// Vystupni piny
	bitSet (DDRC, 0);
	bitSet (DDRC, 1);
	bitSet (DDRC, 2);
	bitSet (DDRC, 3);
	bitSet (DDRC, 4);
	bitSet (DDRC, 5);

	bitSet (DDRD, 2);
	bitSet (DDRD, 3);
	bitSet (DDRB, 1);
	bitSet (DDRB, 2);

	bitSet(TCCR0, CS02); // Clock prescaler (/ 256)
	bitSet(TIMSK, TOIE0); // povoleni preruseni od casovace

	bitSet(TCCR2, CS22); // Clock prescaler (/ 256)
	bitSet(TCCR2, CS21); // Clock prescaler (/ 256)
	bitSet(TIMSK, TOIE2); // povoleni preruseni od casovace

	OCR1A = 0;
	OCR1B = 0;
	// nastavi oc1a a oc1b fast 9bit pwm (az do cisla 511) viz. datasheet
	TCCR1A = 0xA2;
	TCCR1B = 0x09;

	sei();


bitSet (PORTC, 0);
m_delay_ms(100);
m_delay_ms(100);
m_delay_ms(100);
m_delay_ms(100);
m_delay_ms(100);
bitClear (PORTC, 0);
int ala=0;
	while(1) {

		if (longacDOit) {
if (ala>10) { S_llll=0; ala=0; } else { S_llll=200; }
ala++;
display_uptime();
if(! bit_is_set(PINC,0)) {
bitSet (PORTC, 0);
} else {
bitClear (PORTC, 0);
}
			itoa(OCR1A, stConv, 10);
			write(PSTR("PWM_LEFT: "));
			writestr(stConv);
			lfcr();

			itoa(OCR1B, stConv, 10);
			write(PSTR("PWM_RIGHT: "));
			writestr(stConv);
			lfcr();

			itoa(finalSpeedL, stConv, 10);
			write(PSTR("SPEED_LEFT: "));
			if(finalSpeedL) { if (!speedLdir) write(PSTR("+")); else write(PSTR("-")); }
			writestr(stConv);
			lfcr();

			itoa(finalSpeedP, stConv, 10);
			write(PSTR("SPEED_RIGHT: "));
			if(finalSpeedP) { if (!speedPdir) write(PSTR("+")); else write(PSTR("-")); }
			writestr(stConv);
			lfcr();
			longacDOit=0;
		}
		if(rs232enter) {
			unsigned char i=RX_BUFFER_SIZE;
			if(cmdtest('S',10)) speedset();
			else if(cmdtest('L',4)) pwmledset();
			else if(cmdtest('U',6)) setuptime(); // nastavi uptime - uzitecne napr. pri ladeni casovani
			else { write(PSTR("ERROR: COMMAND UNKNOWN")); lfcr(); }
			while (i--) rx_buffer[i]=0;
			rs232enter=0;
		}
	}
	return 0;

}