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

#define MODE1BUTTON PB1
#define MODE2BUTTON PB4
#define PLUSBUTTON PB5
#define MINUSBUTTON PB6
#define MAXDISTANS 8
// The ammount of ticks it takes for the pulse to travel one meter and back
#define ONEMETER 735/50

// The number of pulses to send
volatile uint8_t pulseCounter;
// If the button is pressed
volatile uint8_t isPressed;
// Flag to indicate if sending
volatile uint8_t sending;
// The number of recived pulses
volatile uint8_t recivedPulses;
// The time when send cycle starts
volatile uint16_t startTime;
// The time when send cyxle stops 
volatile uint16_t stopTime;
// The differense between startTime and stopTime
volatile uint16_t distans;
// The distans where indication start
volatile uint16_t alarmDistans;
// The number of diods to indicate alarmdistans
volatile uint16_t alarmSeting;
// The maximum distanse to messure (Not used!!!)
volatile uint16_t maxDistans;

/* No return signal */
SIGNAL (SIG_OVERFLOW1) 
{
	stopTime = 0xffff;
	sending = 0;
}

/* Generate pulse */
SIGNAL (SIG_OUTPUT_COMPARE2) 
{
	if (!(PINB & 0x10)) {
		if (pulseCounter == 6) {		
			// Turn off timer2 
			TCCR2 = TCCR2 & 0xf8;	
			TCNT2 = 0x00;
			// Turn on overflow1 interrupt
			TIMSK = TIMSK | 0x04;
			pulseCounter = 0;
		}
		else {	
			pulseCounter++;
		}
	}		
}

/* Detect returning pulse */
SIGNAL (SIG_COMPARATOR) {
//	recivedPulses++;
//	if (recivedPulses == 3) {
	    stopTime = TCNT1;
		// Turn of timer1
		TIMSK = TIMSK & 0xef;
		// Turn of analog comparator
	    //ACSR = ACSR & 0xfd;
	    sending = 0;
//	    recivedPulses = 0;
//	}
}

// Just taking up time :)
void Sleep(void) {
	uint16_t nisse1 = 0xffff;
	while(nisse1) {
		nisse1--;
	}
}

// Calculate the distanse the pulse has traveled 
void calculateDistans(void) {
	distans = stopTime - startTime;
}
/* Light diods to display distans */
void displayDistans(void) 
{
	// If alarmMode
	if (PINB & 0x02) {
		switch (alarmSeting) {
			case 0 : 
				PORTA = 0x00;
				alarmDistans = 0;
				break;
			case 1 : 
				PORTA = 0x01;
				alarmDistans = ONEMETER;
				break;
			case 2 : 
				PORTA = 0x03;
				alarmDistans = 2*ONEMETER;
				break;
			case 3 : 
				PORTA = 0x07;
				alarmDistans = 3*ONEMETER;
				break;
			case 4 : 
				PORTA = 0x0f;
				alarmDistans = 4*ONEMETER;
				break;
			case 5 : 
				PORTA = 0x1f;
				alarmDistans = 5*ONEMETER;
				break;
			case 6 : 
				PORTA = 0x3f;
				alarmDistans = 6*ONEMETER;
				break;
			case 7 : 
				PORTA = 0x7f;
				alarmDistans = 7*ONEMETER;
				break;
			case 8 : 
				PORTA = 0xff;
				alarmDistans = 8*ONEMETER;
				break;
			default :
				PORTA = PORTA;
				break;
		}
	}
	// If taking messurments
	else {	
		// Less than a meter
		if (distans< (ONEMETER+alarmDistans)) {
			PORTA = 0xff;
		}
		// 1 to 2 meter
		else if (distans > (ONEMETER+alarmDistans+1) && distans < (2*ONEMETER+alarmDistans)) {
			PORTA = 0x7f;
		}
		// 2 to 3 meter
		else if (distans > (2*ONEMETER+alarmDistans+1) && distans < (3*ONEMETER+alarmDistans)) {
			PORTA = 0x3f;
		}
		// 3 to 4 meter
		else if (distans >(3*ONEMETER+alarmDistans+1) && distans < (4*ONEMETER+alarmDistans)) {
			PORTA = 0x1f;
		}
		// 4 to 5 meter
		else if (distans > (4*ONEMETER+alarmDistans+1) && distans < (5*ONEMETER+alarmDistans)) {
			PORTA = 0x0f;
		}
		// 5 to 6 meter
		else if (distans > (5*ONEMETER+alarmDistans+1) && distans < (6*ONEMETER+alarmDistans)) {
			PORTA = 0x07;
		}
		// 6 to 7 meter
		else if (distans > (6*ONEMETER+alarmDistans+1) && distans < (7*ONEMETER+alarmDistans)) {
			PORTA = 0x03;
		}
		// 7 to 8 meter
		else if (distans > (7*ONEMETER+alarmDistans+1) && distans < (8*ONEMETER+alarmDistans)) {
			PORTA = 0x01;
		}
		// More than 8 meter
		else if (distans > (8*ONEMETER+alarmDistans+1)) {
			PORTA = 0x00;
		}
		else {
			PORTA = PORTA;
		}
	}		
}

/* Button Pressed, PINB2 -> alarm, PINB5 -> max */
void buttonPressed(void) 
{
	// Turn of interrupts
	cli();	
	displayDistans();
	while (PINB & 0x02) {
		// MODE1BUTTON (AlarmDistans) Pressed
		if (PINB & 0x02) {
			if ((PINB & 0x40) && (!isPressed)) {
				if (alarmSeting < MAXDISTANS) {
					alarmSeting++;
				}
				Sleep();
				Sleep();
				isPressed=1;
			}
			else if ((PINB & 0x20) && (!isPressed)) {
				if (alarmSeting > 0) {
					alarmSeting--;
				}
				Sleep();
				Sleep();
				isPressed = 1;
			}
			else if (!(PINB & 0x60)) {
				Sleep();
				Sleep();
				isPressed = 0;
			}
			else {
				displayDistans();
			}
		}
		/*
		// Code to set the max distans (Not Used!!!)
		else if (PINB5 == 1) {
			if ((PINB6 == 1) && (isPressed == 0)) {
				if (maxDistans < MAXDISTANS) {
					maxDistans++;
				}
				isPressed=1;
			}
			else if ((PINB7 == 1) && (isPressed == 0)) {
				if (maxDistans != 0) {
					maxDistans--;
				}
				isPressed = 1;
			}
			else if (isPressed == 1 && ((PINB6 == 1) || (PINB7 == 1))) {
				isPressed = 0;
			}	
			else {
			}
		}	
		*/
	}
	// Restart pulseGenerator
	sending = 0;
	// Start interrupt again
	sei ();
}

/* Init registers and ports */
void ioInit (void) 
{
	distans = 10000;
	alarmDistans = 1;
	alarmSeting = 0;
	isPressed=0;
    // Turn off AD-converter
	ADCSRA = ADCSRA & 0x7f;;
	// Set portA for output, lighting diods 
	DDRA = DDRA | 0xff;
	// Set PD7 for output
	DDRD = DDRD | 0x80;
	// Set analog comp. to rigger on rising edge
	ACSR = ACSR | 0x03;
	// Start analog comparator interrupt
	ACSR = ACSR | 0x08;
	// Enable output compare 2 interrupt 
	TIMSK = TIMSK | 0x80;	
    /* enable interrupts */
    sei ();
}

/* Start sendPulse rutin */
void sendPulseInit(void) 
{
	/* PulseGenerator counter */
	TCCR2 = TCCR2 | 0x19;
	OCR2 = 102; // 8MHz / 40kHz / 2	(102 is better)
	pulseCounter = 0;
	// Zero Counter1
	TCNT1 = 0x0000;
	// Set prescale 64 and start Counter1
	TCCR1B = TCCR1B | 0x03;
	// Save value from 16 bit counter
	startTime = TCNT1;
	sending = 1;
}

int main(void) {
	ioInit();
    while (1) {
		if (sending == 0) {
			sendPulseInit();
		}
		if (PINB & 0x02) {
			buttonPressed();
		}
		calculateDistans();
		displayDistans();
    }  
    return (0);
}