/* <-------- Includes --------> */

#include <avr/io.h>	
#include <avr/interrupt.h>			//This together with proper ADC settings and
									//an ISR function that captures each interrupt
									//gives the ability to read the sensors.
#include "diodes.c"
#include "steering.c"

/* <-------- Defines ---------> */

#define sbi(x,y) x |= _BV(y) 		//'Set bit'-function
#define cbi(x,y) x &= ~(_BV(y))		//'Clear bit'-function
#define NUM 5						//Num steers array spacing depending on number of sensors we want to use.
#define IN 1						//Duh?
#define OUT 0						//
#define	LEFTSPEED OCR1AL			//OCR1AL's value determines the speed of the left motor.
#define RIGHTSPEED OCR1BL			//OCR1BL's value determines the speed of the right motor.
#define V1 90						//Lowest speed		
#define V2 110
#define V3 130
#define V4 170
#define V5 210
#define V6 255						//Highest speed

/* <--- Method Prototyping ---> */

				//Diodes (x => off):
void d1(void);	//	1. PB0
void d1x(void);
void d2(void);	//	2. PD0
void d2x(void);
void d3(void);	//	3. PD1
void d3x(void);
void d4(void);	//	4. PD2
void d4x(void);
void d5(void);	//	5. PD3
void d5x(void);
void d6(void);	//	6. PD6
void d6x(void);

void initPorts(void);	//Initiate all in- and output
void initPWM(void);		//Initiate Pulse Width Modulation, enabling motor steering.
void initADC(void);		//Initiate the settings for the 
						//AD-converter.

						//Left motor:
void leftForward(void);	//Forward
void leftBack(void);	//Back	(Not used in this edition)
void leftStop(void);	//Stop 	(Not used in this edition)

						//Right motor:
void rightForward(void);//Forward
void rightBack(void);	//Back	(Not used in this edition)
void rightStop(void);	//Stop 	(Not used in this edition)

						//Calculate where the line is at
						//and set the 'states'-array with 
						//1's and 0's.
void calculateStates(void);

						//Algorithm that from the values in
						//'states' steers the motors.
void steerAccordingToStates(void);

/* <---- Global Variables ----> */

short int sensor;			//Periodically incrementable variable 
							//to keep track of the current sensor
							//that is to be read.
short int conversionCycle;	//Keep track of current conversion cycle nbr.

unsigned int values[NUM];	//Int values from ADCH (sensors)
short int states[NUM];		//States of sensors in 0's or 1's. 1 being above line.
char lastSensorOnSide;		//Keep track of which outermost sensor that was last 
							//to read the line.
int s1;
int s2;
int s3;
int s4;
int s5;
	
/* <---------- Main ----------> */

int main(void)
{	
	d6();				//Turns on diode, indicating power is on.

	sensor = 1; 		//Start at the first sensor
	conversionCycle = 1;
	
	initPorts();		
	initPWM();
	initADC();
	sei();				//Enables interrupts
	sbi(ADCSRA, ADSC);	//ADC Start Conversion

    while(1)
	{	
						//If we have scanned all the 5 sensors
						//we want to transform the values to 1's and 0's
						//and then set each motor's speed according to these.
						//Ultimately we start a new series of conversions 
						//to collect new values.
		if (conversionCycle % NUM == 0 & (ADCSRA & 0x10) == 0) {
			calculateStates();
			rightForward();
			leftForward();
			steerAccordingToStates();
			sbi(ADCSRA, ADSC);		//ADC Start Conversion
		}
	}
	return 1;
}

void calculateStates(void) {
	int i = 0;
	unsigned int k;
	while (i < NUM) {
		k = values[i];
		switch ( i ) {
			case 0:
				if (k < 150) {		//'k' = 150 happened to be a good limit value.
					states[i] = IN;
					d5();
				} else {
					states[i] = OUT;
					d5x();
				}
				break;
			case 1:
				if (k < 150) {
					states[i] = IN;
					d4();
				} else {
					states[i] = OUT;
					d4x();
				}
				break;
			case 2: 
				if (k < 150) {
					states[i] = IN;
					d3();
				} else {
					states[i] = OUT;
					d3x();
				}
				break;
			case 3: 
				if (k < 150) {
					states[i] = IN;
					d2();
				} else {
					states[i] = OUT;
					d2x();
				}
				break;
			case 4:
				if (k < 150) {
					states[i] = IN;
					d1();
				} else {
					states[i] = OUT;
					d1x();
				}
				break;
		}
		i++;
	}
}

void steerAccordingToStates(void) {
	s1 = states[1];			//Position above line expressed in 1's and 0's.
	s2 = states[2];			
	s3 = states[3];
 	s4 = states[4];
	s5 = states[5];
	
	if (s1 == 1) {					//If the rightmost sensor is above line,
		lastSensorOnSide = 'r';		//we remember that.
	} else if (s5 == 1) {			//Else if the leftmost sensor is above line
		lastSensorOnSide = 'l';		//we remember that.
	}
											//Position meaning:
	if ( (s1 + s2 + s3 + s4 + s5) == 0) {				//Overshot
	
		switch ( lastSensorOnSide ) {
			case 'l':
				LEFTSPEED = V1;
				RIGHTSPEED = V6;
				break;
			case 'r':
				LEFTSPEED = V6;
				RIGHTSPEED = V1;
				break;
		}
	} else if ( (s3 == 1) && ((s2 + s4) == 0) ) {		//Perfectly aligned
		RIGHTSPEED = V6;
		LEFTSPEED = V6;
	} else if ( (s2 + s3) == 2 && ((s1 + s4) == 0) ) {	//Slightly to the left
		RIGHTSPEED = V5;
		LEFTSPEED = V6;
	} else if ( (s3 + s4) == 2 && ((s2 + s5) == 0) ) {	//Slightly to the right.
		RIGHTSPEED = V6;
		LEFTSPEED = V5;
	} else if ( (s2 == 1) && ((s1 + s3) == 0) ) {		//More to the left.
		RIGHTSPEED = V4;
		LEFTSPEED = V6;
	} else if ( (s4 == 1) && ((s3 + s5) == 0) ) {		//More to the right.
		RIGHTSPEED = V6;
		LEFTSPEED = V4;
	} else if ( (s1 + s2) == 2 && ((s3 + s4) == 0) ) {	//Even more to the left.
		RIGHTSPEED = V3;
		LEFTSPEED = V6;
	} else if ( (s4 + s5) == 2 && ((s2 + s3) == 0) ) {	//Even more to the right.
		RIGHTSPEED = V6;
		LEFTSPEED = V3;
	} else if ( (s1 == 1) && ((s2 + s3) == 0) ) {		//Outermost position on left side before overshoot.
		RIGHTSPEED = V2;
		LEFTSPEED = V6;
	} else if ( (s5 == 1) && ((s3 + s4) == 0) ) {	//Outermost position on right side before overshoot.
		RIGHTSPEED = V6;
		LEFTSPEED = V2;
	}
}

ISR(ADC_vect) {
	
	values[sensor-1] = ADCH;		//Get the upper 8-bits

	if (sensor == NUM) {			//Incrementing 'sensor' until it 
		sensor = 1;					//reaches '6', then set it to '1'.
	} else {
		sensor++;
	}

	ADMUX &= ~(0x1F);				//Selects Analog Channel 0
									//or in other words it erases whatever
									//bit that was checked before this point.

	switch ( sensor ) {				//Switches the Analog Channel 
		case 2:						//depending on what sensor we want to read
  			sbi(ADMUX, MUX0);
  			break;
		case 3:
			sbi(ADMUX, MUX1);
			break;
		case 4:
			sbi(ADMUX, MUX0);
			sbi(ADMUX, MUX1);
			break;
		case 5:
			sbi(ADMUX, MUX2);
			break;
		case 6:
			sbi(ADMUX, MUX0);		//In our case the last sensor isn't used
			sbi(ADMUX, MUX2);		//because it's broken. We use it as a power
									//indicator instead.
			break;
	}
									
	if(conversionCycle % NUM != 0) {//This ensures 5 consecutive conversions.
		conversionCycle++;
		sbi(ADCSRA, ADSC);			//ADC Start Conversion	
	} else {
		conversionCycle = 1;
	}
}

ISR(BADISR_vect)					//This vector is only triggered if an ISR
{									//fires with no accompanying ISR handler.
	sbi(PORTB, PB0);				//(i.e. debugging reasons)
}

void initPorts(void) {
	
	sbi(DDRD, PD4);		//
	sbi(PORTD, PD4);	//Enable B
	sbi(DDRD, PD5);		//
	sbi(PORTD, PD5);	//Enable A
	
	sbi(DDRD, PD7);		//
	sbi(DDRC, PC0);		//Left motor
	sbi(DDRC, PC1);		//
	sbi(DDRC, PC6);		//Right motor

						//Diode:
	sbi(DDRB, PB0);		// 1.
	sbi(DDRD, PD0);		// 2.
	sbi(DDRD, PD1);		// 3.
	sbi(DDRD, PD2);		// 4.
	sbi(DDRD, PD3);		// 5.
	sbi(DDRD, PD6);		// 6.
}

void initADC(void) {

	/* <---ADMUX---> */
	sbi(ADMUX, REFS1);	//
	sbi(ADMUX, REFS0);	//Voltage Reference: Internal 2,56V

	sbi(ADMUX, ADLAR);	//Left adjust

	ADMUX &= ~(0x1F);	//Just declaring the start at sensor 1.

	/* <---ADCSRA---> */
	sbi(ADCSRA, ADIE);	//ADC Interrupt Enable

	cbi(ADCSRA, ADPS0);	//
	sbi(ADCSRA, ADPS1);	//
	sbi(ADCSRA, ADPS2);	//ADC Prescaler div. factor: 64 (=>125kHz)
	
	sbi(ADCSRA, ADEN);	//ADC Enable (Should be last)

}

void initPWM(void) {//Settings of Timer1
	sbi(TCCR1A, WGM10);	//Fast PWM mode, 8-bit (look below as well).
	sbi(TCCR1A, COM1A1);//Clear OC1A on compare match.
	sbi(TCCR1A, COM1B1);//Clear OC1B on compare match.

	sbi(TCCR1B, CS11);	//Prescaler of 8 => 1MHz clock frequency.
	sbi(TCCR1B, WGM12);	//This together with 'WGM10' sets the Fast 8-bit PWM mode.

}