/*
					Accelerometer

This file is a part of a project created on 

	Lund Institute of Technology

by

	Mattias Kindborg
	Rikard Ekström

*/

#include <avr/io.h>				// To get access to the names of the PORTS, REGISTERS on the AVR
#include <avr/wdt.h>			// To be able to handle the Watchdog Timer
#include <avr/interrupt.h>		// To get access to various commands, for example sei()
#include <avr/signal.h>			// To get access to interrupt handling
#include <math.h>				// Accessing mathematics
#include <avr/delay.h>			// Delay function
#include <string.h>
#include <avr/pgmspace.h>
#include <inttypes.h>

//-----------------------------------------------------------------------------------------------------------
// DEFINITIONS

/*   GENERAL   */
#define bit_write(register, pinNr, value) 		(value ? (register |= 1<<pinNr) : (register &= ~(1<<pinNr)))
#define CPUFREQ									16000000		// The frequence of the CPU
#define output									1				// I usually get confused...do you?
#define input									0				// I guess you do...
#define LCD_RS_write_diplay_data				1				// Write display data to the LCD
#define LCD_RS_commands							0				// Give the LCD commands
#define LCD_RW_write							0				// Data direction pin, write to LCD
#define LCD_RW_read								1				// Data direction pin, read to LCD
#define g										9.8156			// m/s2
#define zeroG									0.49112			// Zero g in our accelerometer 0.49101347
#define oneG									0.13309			// 0.12546532 0.13589 One  g in our accelerometer 0.125892864

/*   PORTA   */
// All pins on port A are LCD data

/*   PORTB   */
#define LCD_RS									0				// LCD RS (Register Select)
#define LCD_RW									1				// LCD R/W (Data direction pin)
#define buttonPressed							2				// 
#define menueButton1							3				// Button 1 (Bestäm användning)
#define menueButton2							4				// Button 2 (Bestäm användning)
#define menueButton3							5				// Button 3 (Bestäm användning)

/*   PORTC   */

/*   PORTD   */
#define LCD_Enable								5				// LCD Enable Pin
#define ICP										6				// Input Capture Pin

/*   STORED STRINGS   */
const char welcome1[] PROGMEM = "    Acc v1.0    ";
const char welcome2[] PROGMEM = "Dig Project 2004";
const char welcome3[] PROGMEM = "Mattias   Rikard";
const char welcome4[] PROGMEM = "Kindborg Ekström";
PGM_P welcomePointer[4] PROGMEM = {welcome1, welcome2, welcome3, welcome4};

const char menue1[] PROGMEM = "    0-201 m     ";
const char menue2[] PROGMEM = "    0-402 m     ";
const char menue3[] PROGMEM = "   0-100 km/h   ";
const char menue4[] PROGMEM = "   100-0 km/h   ";
const char menue5[] PROGMEM = "  Max. G-force  ";
const char menue6[] PROGMEM = " Set trig value ";
const char menue7[] PROGMEM = "     Start!     ";
const char menue8[] PROGMEM = "t:   .   s     ";
const char menue9[] PROGMEM = "max G:  .      ";

PGM_P menue[9] PROGMEM = {menue1, menue2, menue3, menue4, menue5, menue6, menue7, menue8, menue9};



/*   GLOBAL VARIABLES   */
unsigned int T1;			// Accelerometer: Period time 
unsigned int T2;			// Accelerometer: Duty time
unsigned int T2_last;
unsigned int T1_current;
unsigned int T2_current;
int menueIndex;				// Menue index
unsigned long int time;	// Time
double acc;				// Acceleration
double speed;				// Velocity
double distance;			// Distance
double triggValue;			// Trigger value for starting any calculation
double maxG;				// Max G-force
unsigned long int startSampling;

//-----------------------------------------------------------------------------------------------------------

/*   INITIATE THE LCD   */
void initLCD(void) {
	int i;
	for(i=0; i<256; i++) {
		_delay_loop_1(0xff);
	}
	
	// Set control pins as output
	bit_write(DDRB, LCD_RW, output);
	bit_write(DDRB, LCD_RS, output);
	bit_write(DDRD, LCD_Enable, output);
	
	// Set data pins as output
	DDRA = 0xff;
	
	// Prepare for commands
	bit_write(PORTB, LCD_RS, LCD_RS_commands);
	bit_write(PORTB, LCD_RW, LCD_RW_write);
	for(i=0; i<48; i++) {
		_delay_loop_1(0xff);
	}
	
	// Display clear
	PORTA = 0x01;
	bit_write(PORTD, LCD_Enable, 1);
	bit_write(PORTD, LCD_Enable, 0);
	for(i=0; i<42; i++) {
		_delay_loop_1(0xff);
	}
	
	// Display, cursor home??
	PORTA = 0x02;
	bit_write(PORTD, LCD_Enable, 1);
	bit_write(PORTD, LCD_Enable, 0);
	for(i=0; i<42; i++) {
		_delay_loop_1(0xff);
	}
	
	// Entry mode set
	PORTA = 0x06;
	bit_write(PORTD, LCD_Enable, 1);
	bit_write(PORTD, LCD_Enable, 0);
	for(i=0; i<17; i++) {
		_delay_loop_1(0xff);
	}
	
	// Display on
	PORTA = 0x0c;		// 0x0f=cursor & position
	bit_write(PORTD, LCD_Enable, 1);
	bit_write(PORTD, LCD_Enable, 0);
	for(i=0; i<17; i++) {
		_delay_loop_1(0xff);
	}
	
	// Display, cursor shift???
	
	// Function set
	PORTA = 0x38;	// or 0x30 / 0x38
	bit_write(PORTD, LCD_Enable, 1);
	bit_write(PORTD, LCD_Enable, 0);
	for(i=0; i<17; i++) {
		_delay_loop_1(0xff);
	}
}

/*   PRINT A CHAR ON LCD   */
void LCD_putChar(unsigned int address, unsigned int character) {
	// Init
	bit_write(DDRB, LCD_RW, output);
	bit_write(PORTB, LCD_RW, LCD_RW_write);
	bit_write(DDRB, LCD_RS, output);
		
	// DD RAM write
	bit_write(PORTB, LCD_RS, LCD_RS_commands);
	bit_write(PORTD, LCD_Enable, 1);
	PORTA = 128 + address;
	bit_write(PORTD, LCD_Enable, 0);
	_delay_loop_1(0xff);
	
	// char write
	bit_write(PORTB, LCD_RS, LCD_RS_write_diplay_data);
	bit_write(PORTD, LCD_Enable, 1);
	PORTA = character;
	bit_write(PORTD, LCD_Enable, 0);
	_delay_loop_1(0xff);
}

/*   PRINT A STRING ON THE LCD   */
void LCD_putString(unsigned int address, char string[]) {
	int j;
	j = strlen(string);
	int i;
	for (i=0; i<j; i++) {
		LCD_putChar(address+i, string[i]);
	}
}

/*void welcome(void) {
	char buf[16];
    strcpy_P (buf, welcomePointer[0]);
	LCD_putString(0, buf);
	strcpy_P (buf, welcomePointer[1]);
	_delay_loop_2(0xffff);
	strcpy_P (buf, misc[0]);
	LCD_putString(0, buf);
	strcpy_P (buf, misc[1]);
	LCD_putString(64, buf);
}*/

//-----------------------------------------------------------------------------------------------------------

// ACCELEROMETER

/*   INITIATE THE INPUT CAPTURE PIN   */
void initICP(void) {
	bit_write(DDRD, ICP, input);	// Set the pin as a input
	bit_write(PORTD, ICP, 1);		// Pull up enable
	TCCR1B = 0x41;					// Input Capture Edge Select, Clock Select
	bit_write(DDRD, 4, output);		// ??? Test time in interrupt
}

/*   DISPLAY RESULTS AFTER MESSUREMENT   */
void displayResults(void) {
	char buf[20];
	unsigned long int timeInt;
	switch(menueIndex) {
	case 1:			// 0-201 m
		strcpy_P (buf, menue[7]);
		LCD_putString(64, buf);			// "t:   .   s     "
		timeInt = time / 160000;
		if (timeInt/1000 != 0) {					
			LCD_putChar(67, timeInt/1000 + 48);			// x
		}
		LCD_putChar(68, fmod(timeInt, 1000)/100 + 48);		// xx
		LCD_putChar(70, fmod(timeInt, 100)/10 + 48);		// xx.x
		LCD_putChar(71, fmod(timeInt, 10) + 48);			// xx.xx
		break;
	case 2:			// 0-402 m
		strcpy_P (buf, menue[7]);
		LCD_putString(64, buf);			// "t:   .   s     "
		timeInt = time / 160000;
		if (timeInt/1000 != 0) {					
			LCD_putChar(67, timeInt/1000 + 48);			// x
		}
		LCD_putChar(68, fmod(timeInt, 1000)/100 + 48);		// xx
		LCD_putChar(70, fmod(timeInt, 100)/10 + 48);		// xx.x
		LCD_putChar(71, fmod(timeInt, 10) + 48);			// xx.xx
		break;
	case 3:			// 0-100 km/h
		strcpy_P (buf, menue[7]);
		LCD_putString(64, buf);			// "t:   .   s     "
		timeInt = time / 160000;
		if (timeInt/1000 != 0) {					
			LCD_putChar(67, timeInt/1000 + 48);			// x
		}
		LCD_putChar(68, fmod(timeInt, 1000)/100 + 48);		// xx
		LCD_putChar(70, fmod(timeInt, 100)/10 + 48);		// xx.x
		LCD_putChar(71, fmod(timeInt, 10) + 48);			// xx.xx
		break;
	case 4:			// 100-0 km/h
		strcpy_P (buf, menue[7]);
		LCD_putString(64, buf);			// "t:   .   s     "
		unsigned long int timeInt;
		timeInt = time / 160000;
		if (timeInt/1000 != 0) {					
			LCD_putChar(67, timeInt/1000 + 48);			// x
		}
		LCD_putChar(68, fmod(timeInt, 1000)/100 + 48);		// xx
		LCD_putChar(70, fmod(timeInt, 100)/10 + 48);		// xx.x
		LCD_putChar(71, fmod(timeInt, 10) + 48);			// xx.xx
		break;
	case 5:			// Max G-force
		strcpy_P (buf, menue[8]);
		LCD_putString(64, buf);			// "max G:  .      "
		double tempG1;
		tempG1 = fabs(maxG * 100 / g);
		int tempG2;
		tempG2 = tempG1;
		LCD_putChar(71, tempG2/100 + 48);				// x
		LCD_putChar(73, fmod(tempG2, 100)/10 + 48);		// x.x
		LCD_putChar(74, fmod(tempG2, 10) + 48);			// x.xx
		break;
	}
}

/*   T1 AND T2 INTERRUPTS   */
INTERRUPT(SIG_INPUT_CAPTURE1) {
	bit_write(PORTD, 4, 1);		// ??? Test time in interrupt
	if ((TCCR1B & 0x40) == 0x40) {		// Rising edge
		T2_current = ICR1;				// Save T2
		bit_write(TCCR1B, 6, 0);		// Trig on falling edge next time	
		if (T2_current < T2_last) {	// Wrap
			T2 = (65535 - T2_last) + T2_current;
		} else {						// No wrap
			T2 = T2_current - T2_last;
		}
		T2_last = T2_current;
		if (startSampling < 2) {		// Counts two rising edges before sampling acc
			startSampling += 1;
		}
		
		double T1_double;
		T1_double = T1;
		double T2_double;
		T2_double = T2;
		acc = - g * (T1_double/T2_double - zeroG) / oneG;	// In m/s^2
		
		switch(menueIndex) {
		case 1:						// 0-201 m
			if (distance >= 201) {
				bit_write(TIMSK, 5, 0);		// Disable ICP
				displayResults();
			}
			break;
		case 2:						// 0-402 m
			if (distance >= 402) {
				bit_write(TIMSK, 5, 0);		// Disable ICP
				displayResults();
			}
			break;
		case 3:						// 0-100 km/h
			if (speed >= 27.77778) { // = 100 km/h
				bit_write(TIMSK, 5, 0);		// Disable ICP
				displayResults();
			}
			break;
		case 4:
			if (speed <= 0.2777778) {		// 100-0 km/h
				bit_write(TIMSK, 5, 0);		// Disable ICP
				displayResults();
			}
			break;
		case 5:
			if ((fabs(acc) > maxG) && (fabs(acc) <= 2*g)) {	// Save largest G
				maxG = fabs(acc);
			}
			if ((time/16000000) > 9) {
				bit_write(TIMSK, 5, 0);		// Disable ICP
				displayResults();
			}
			break;
		}
	} else {							// Falling edge
		T1_current = ICR1;					// Save T1
		bit_write(TCCR1B, 6, 1);			// Trig on rising edge next time
		if (T1_current < T2_current) {		// Wrap
			T1 = (65535 - T2_current) + T1_current;
		} else {							// No wrap
			T1 = T1_current - T2_current;
		}
		
		// Calculations
		double T2_double;
		T2_double = T2;
		if((fabs(acc) > triggValue || time != 0) && (startSampling == 2)) {
			speed += (acc * (T2_double/16000000));		// In m/s
			distance += (fabs(speed) + fabs(acc) * (T2_double/16000000) / 2) * (T2_double/16000000);
			time += T2;										// Large counter value
		}
	}
	bit_write(PORTD, 4, 0);		// ??? Test time in interrupt
}


//-----------------------------------------------------------------------------------------------------------

// MENUE

/*   INITIATE THE MENUE   */
void initMenue(void) {
	bit_write(DDRB, buttonPressed, input);		// Define INT2 as input
	bit_write(DDRB, menueButton1, input);		// Define menueButton1 as input
	bit_write(DDRB, menueButton2, input);		// Define menueButton2 as input
	bit_write(DDRB, menueButton3, input);		// Define menueButton3 as input
	bit_write(PORTB, buttonPressed, 1);			// Activate pull up
	bit_write(PORTB, menueButton1, 1);			// Activate pull up
	bit_write(PORTB, menueButton2, 1);			// Activate pull up
	bit_write(PORTB, menueButton3, 1);			// Activate pull up
	
	//bit_write(GICR, 6, 1);						// Enable external interrupt 0 (on pin PD2)
	GICR |= 0x20;
	MCUCSR |= 0x40;								// 
	menueIndex = 1;
	char buf[20];
	strcpy_P (buf, menue[0]);
	LCD_putString(0, buf);
}

// LCD
/*   MENUE BUTTON PRESSED   */
INTERRUPT(SIG_INTERRUPT2) {
	///bit_write(PORTD, 7, 1);
	char pressedButtonIndex;
	pressedButtonIndex = (PINB & 0x38)>>3;
	char buf[20];
	if (pressedButtonIndex == 4) { 		// Backward in menue	
		switch(menueIndex) {
		case 1: 
			menueIndex = 6;
			strcpy_P (buf, menue[5]);
			break;
		case 2:
			menueIndex = 1;
			strcpy_P (buf, menue[0]);
			break;
		case 3:
			menueIndex = 2;
			strcpy_P (buf, menue[1]);
			break;
		case 4:
			menueIndex = 3;
			strcpy_P (buf, menue[2]);
			break;
		case 5:
			menueIndex = 4;
			strcpy_P (buf, menue[3]);
			break;
		case 6:
			menueIndex = 5;
			strcpy_P (buf, menue[4]);
			break;
		}
		LCD_putString(0, buf);
	} else if (pressedButtonIndex == 2) {			// Forward in menue
		switch(menueIndex) {
		case 1: 
			menueIndex = 2;
			strcpy_P (buf, menue[1]);
			break;
		case 2:
			menueIndex = 3;
			strcpy_P (buf, menue[2]);
			break;
		case 3:
			menueIndex = 4;
			strcpy_P (buf, menue[3]);
			break;
		case 4:
			menueIndex = 5;
			strcpy_P (buf, menue[4]);
			break;
		case 5:
			menueIndex = 6;
			strcpy_P (buf, menue[5]);
			break;
		case 6:
			menueIndex = 1;
			strcpy_P (buf, menue[0]);
			break;
		}
		LCD_putString(0, buf);
	} else if (pressedButtonIndex == 1) {			// Select
		startSampling = 0;
		time = 0;					
		speed = 0;
		distance = 0;
		maxG = 0;
		switch(menueIndex) {
		case 1: 		// 0-201 m
			strcpy_P (buf, menue[6]);
			LCD_putString(64, buf);
			TIMSK |= 0x20;				// Enable ICP
			break;
		case 2:			// 0-402
			strcpy_P (buf, menue[6]);
			LCD_putString(64, buf);
			TIMSK |= 0x20;				// Enable ICP
			break;
			break;
		case 3:			// 0-100 km/h
			strcpy_P (buf, menue[6]);
			LCD_putString(64, buf);
			TIMSK |= 0x20;				// Enable ICP
			break;
		case 4:			// 100-0 km/h
			strcpy_P (buf, menue[6]);
			LCD_putString(64, buf);
			speed = 27.77778;			// Startspeed = 100 km/h
			TIMSK |= 0x20;				// Enable ICP
			break;
		case 5:			// Max. G-force
			strcpy_P (buf, menue[6]);
			LCD_putString(64, buf);
			TIMSK |= 0x20;				// Enable ICP
			break;
		case 6:			// Set trig value
			break;
		}
	}
	//bit_write(PORTD, 7, 0);
}


//-----------------------------------------------------------------------------------------------------------
// MAIN

int main(void) {
/* Initieringar */
	initLCD();
	initMenue();
	/*bit_write(DDRC, 2, input);
	bit_write(DDRC, 3, input);
	bit_write(DDRC, 4, input);
	bit_write(DDRC, 5, input);
	bit_write(PORTC, 2, input);
	bit_write(PORTC, 3, input);
	bit_write(PORTC, 4, input);
	bit_write(PORTC, 5, input);*/
	//welcome();
	initICP();
	//TIMSK = 0x20;					
	//triggValue = 0.1;
	//acc = 0.0;
	//T1 = 18473;
	//T2 = 37700;
/* Testutskrift */
	//char buf[16];
	//strcpy_P (buf, menue[0]);
	//LCD_putString(0, buf);

	sei();
	while(1) {
	}
}