#include <avr/io.h>
#include <avr/interrupt.h>
#define F_CPU 16000000UL // 16 MHz
#include <util/delay.h>

volatile uint8_t count;
int running;
int ready;

void main()
{
	TCCR0|=(1<<CS02) | (CS01);  //Prescaler igång

	TIMSK|=(1<<TOIE0); //Overflow-interrupt igång

	sei(); // Sätter igång globala interrupts

	TCNT0 = 0;

	count = 0; 

	//Initierar variabler
	running = 0; 
	ready = 0;


	
	DDRC = 0x43; //Port C(0, 1 och 6) får riktning output
    DDRD = 0xFF; //Port D(alla) får riktning output

	
	
	display_init(); //Initierar LCD-skärmen

	

	//Evighetsloopen
	while(1){ 

		//Om ingen av sensorerna ger utslag skrivs "Ready" ut
		if ((PINA == 0xFF) && (running == 0)){ 
			if(ready == 0) {
				writeReady();
				ready = 1;
			}
		
		//Om startsensorn ger utslag startas klockan och displayen rensas
		}else if(((PINA == 0xF7) && (running == 0)) || ((PINA == 0xF3) && (running == 0)))  {
			startClock();
			display_clear();
			running = 1;

		//Om andra slutsensorn ger utslag och running = 1 så visas hastigheten
		//och sedan återgår programmet till grundläge.
		}else if(((PINA == 0xEF) && (running == 1)) || ((PINA == 0xCF) && (running == 1))){
			showSpeed();
			_delay_ms(1000);
			display_clear();
			running = 0;
			ready = 0;
		}
	
	
		
	}


}

// 3.3 = avståndet mellan sensorerna, count är timerns tick, ger hastigheten i cm/sek
void showSpeed(){
	double temp = count;
	double v = 3.3 / (temp/30); 
	
	//Delar upp hastigheten i ensiffriga variabler
	int num1 = v / 10;
	int temp2 = (int) v;
	int num2 = (temp2 % 10);
	int dec = (v - (num1*10) - num2)*10;

	writeSpeed(num1, num2, dec);


	
	
}

//Skriver ut hastigheten som skickas in på displayen
void writeSpeed(int num1, int num2, int dec) {
	//Konverterar till char
	char cnum1 = (char) (((int) '0') +num1);
	char cnum2 = (char) (((int) '0') +num2);
	char cdec = (char) (((int) '0') +dec);

	display_writeCh('S');
	display_writeCh('p');
	display_writeCh('e');
	display_writeCh('e');
	display_writeCh('d');
	display_writeCh(':');
	display_writeCh(' ');
	display_writeCh(cnum1);
    display_writeCh(cnum2);
	display_writeCh('.');
    display_writeCh(cdec);
	display_writeCh(' ');
	display_writeCh('c');
	display_writeCh('m');
	display_writeCh('/');
	display_writeCh('s');
}


//Nollställer timern
void startClock(){
	count = 0;
}

//Detta utförs vid varje interupt
ISR(TIMER0_OVF_vect)
{
	count++;
}




void writeReady(){
	display_writeCh('R');
	display_writeCh('E');
	display_writeCh('A');
	display_writeCh('D');
	display_writeCh('Y');
}



//Används för att ge en pin(char pin) på porten (char port) 
//ett speciellt värde(char state)
void set_pin(char port, char pin, char state){


	char set = 1 << pin;
 

	if(port == 'A'){
		set &= PORTA;
	if(set && !state){ // 1 -> 0
		PORTA ^= set;
	}
	if(set == 0 && state){ // 0 -> 1
		set = 1 << pin;
		PORTA ^= set;
	}
	}

	else if(port == 'B'){
		set &= PORTB;
	if(set && !state){ // 1 -> 0
		PORTB ^= set;
	}
	if(set == 0 && state){ // 0 -> 1
		set = 1 << pin;
		PORTB ^= set;
	}
	}
	else if(port == 'C'){
		set &= PORTC;
		if(set && !state){ // 1 -> 0
		PORTC ^= set;
	}
	if(set == 0 && state){ // 0 -> 1
		set = 1 << pin;
		PORTC ^= set;
	}
	}
	else if(port == 'D'){
		set &= PORTD;
	if(set && !state){ // 1 -> 0
		PORTD ^= set;
	}
	if(set == 0 && state){ // 0 -> 1
		set = 1 << pin;
		PORTD ^= set;
	}
	}
}

//Anropas när displayen skall ta emot ett kommando
void display_cmd(char val) { 
    PORTD=val;
    _delay_ms(3);
    set_pin('C', PC1, 0); //RW -> 0
    set_pin('C', PC0, 0); //RS -> 0
    _delay_ms(3);
    set_pin('C', PC6, 1); //E -> 1
    _delay_ms(3);
    set_pin('C', PC6, 0); //E -> 0
}

//Anropas när displayen skall skriva ut ett tecken
void display_writeCh(char val) { 
    PORTD=val;
    set_pin('C', PC1, 0); //RW -> 0
    set_pin('C', PC0, 1); //RS -> 1
    _delay_ms(3);
    set_pin('C', PC6, 1); //E -> 1
    _delay_ms(3);
    set_pin('C', PC6, 0); //E -> 0
}


void display_clear() {
    display_cmd(0x01); //Skickar in kommandot som rensar displayen
    display_cmd(0x38); //Functions set
}

void display_init() {
    display_clear();
    display_cmd(0x0F); //Skickar in kommandot som sätter på displayen
    display_cmd(0x06); //Entry mode set
}