#include <time.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "system.h"
#include "project.h"
#include "LCD.h"
#include "LEDs.h"
#include "recognize.h"

int main()
{
	printf("System Start!\n\n");

	volatile int * audio_ptr = (int *)AUDIO_BASE;	// audio port address
	unsigned int * pio = (unsigned int *)PIO_0_BASE;	// pio port address
	unsigned int * green_LED_ptr = (unsigned int *)GREEN_LEDS_BASE;	// green LED address
	unsigned int * red_LED_ptr = (unsigned int *)RED_LEDS_BASE;	// red LED address
	unsigned int * SvnSegU = (unsigned int *)HEX7_HEX4_BASE;	// upper seven segment address
	unsigned int * SvnSegL = (unsigned int *)HEX3_HEX0_BASE;	// lower seven segment address
	unsigned int * Switch = (unsigned int *)SLIDER_SWITCHES_BASE;	// slide switch address

	char * displayUpper = calloc(LCD_BUFFER_SIZE, sizeof(char));	// string buffer for LCD upper row
	char * displayLower = calloc(LCD_BUFFER_SIZE, sizeof(char));	// string buffer for LCD lower row

	int system_on = 0;	// flag for the power of the system
	int word;	// flag for detection of a word
	int idleland = 0;
	int left_buffer, right_buffer;	// buffer for the two channels of the CoDec
	int key_up = 0, key_down = 0, key_off = 0;	// flag for the three pushbuttons
	int lengthCount = 0, blankCount = 0;	// counters for the length of the current word or blank
	int peak, valley;	// The maximum positive/negative values from the CoDec

	int currentSpeed = SPEED_0;
	int currentSwitch;

	int speed;
	char * speedInfo;

	// Turn off the seven segments and the LEDs
	*SvnSegU = OFF;
	*SvnSegL = OFF;
	*green_LED_ptr = OFF;
	*red_LED_ptr = OFF;

	*pio = currentSpeed;

	speedInfo = calloc(40, sizeof(char));

	// The main while loop
	while(1)
	{
		// Check if there was any change in the switches
		if (*Switch != currentSwitch){
			currentSwitch = *Switch;
			// Check the slide switch to see if the system is turned on
			if (currentSwitch%2){
				system_on = 1;
				*SvnSegU = OFF;
				*SvnSegL = HEX_ON;

				// Check 2nd switch to see which control mode is turned on
				if (currentSwitch/2%2){
					// Idle/Land Mode is turned on
					idleland = 1;
					showLCD(LCD_ON2_U, displayLower, 'u');
				}
				else{
					// Up/Down Mode is turned on
					idleland = 0;
					showLCD(LCD_ON_U, displayLower, 'u');
				}
			}
			else {
				system_on = 0;
				*SvnSegU = OFF;
				*SvnSegL = HEX_OFF;
				*green_LED_ptr = OFF;
				*red_LED_ptr = OFF;

				showLCD(LCD_OFF_U, displayUpper, 'u');
				showLCD(LCD_OFF_L, displayLower, 'l');

				currentSpeed = SPEED_0;
				*pio = currentSpeed;
			}
		}

		// Store the values from the Audio CoDec
		left_buffer = *(audio_ptr + 2);
		right_buffer = *(audio_ptr + 3);

		if (system_on)
		{
			if (!word){
				// If no word was detect, then try to detect the beginning of a word
				if (fabs(left_buffer)>NOISE_THRESHOLD){
					// Value passed the threshold level
					word = 1;
					lengthCount = 1;
					peak = valley = left_buffer;
				}
			}

			if (word){
				// If a word was detected, then check new data
				if (fabs(left_buffer)>NOISE_THRESHOLD){
					// If new data is still above the noise threshold, then increment the word length, reset the blank count and update the peak and valley
					lengthCount++;
					blankCount = 0;
					if (peak < left_buffer){
						peak = left_buffer;
					}
					else if (valley > left_buffer){
						valley = left_buffer;
					}
				}
				else {
					// If new data is below the noise threshold, then increment the blank count
					blankCount++;
					if (blankCount >= BLANK_LENGTH_THRESHOLD){
						/* If blank count has go over the threshold, then the current word is finished
						 * try to match the word, and reset the flags and counters
						 */

						// Debugging Outputs
						if (lengthCount > 1000)
							printf("Length: %d, Peak: %d, Valley: %d\n", lengthCount, peak, valley);

						// Command Matching
						int command = matchCommand(lengthCount, peak, valley, idleland);
						if (command == STATUS_UP){
							speed = 7;
							sprintf(speedInfo, "SPEED %d", speed);
							showLCD(speedInfo, displayLower, 'l');
							*SvnSegL = HEX_UP;

							currentSpeed = SPEED_7;
							*pio = currentSpeed;

							wait(WAIT_TIME);

							speed = 6;
							sprintf(speedInfo, "SPEED %d", speed);
							showLCD(speedInfo, displayLower, 'l');
							*SvnSegL = HEX_ON;

							currentSpeed = SPEED_6;
							*pio = currentSpeed;
						}
						else if(command == STATUS_DOWN){
							speed = 5;
							sprintf(speedInfo, "SPEED %d", speed);
							showLCD(speedInfo, displayLower, 'l');
							*SvnSegL = HEX_DOWN;

							currentSpeed = SPEED_5;
							*pio = currentSpeed;

							wait(WAIT_TIME);

							*SvnSegL = HEX_ON;
						}
						else if(command == STATUS_IDLE) {
							speed = 1;
							sprintf(speedInfo, "SPEED %d", speed);
							showLCD(speedInfo, displayLower, 'l');
							*SvnSegL = HEX_IDLE;

							currentSpeed = SPEED_1;
							*pio = currentSpeed;

							wait(WAIT_TIME);

							*SvnSegL = HEX_ON;
						}
						else if(command == STATUS_LAND) {
							showLCD(LCD_LAND_L, displayLower, 'l');
							*SvnSegL = HEX_LAND;

							while((speed = speedGear(&currentSpeed, 'd')) >= 0){
								sprintf(speedInfo, "SPEED %d", speed);
								showLCD(speedInfo, displayLower, 'l');
								*pio = currentSpeed;
								wait(WAIT_TIME);
							}

							*SvnSegL = HEX_ON;
						}

						// Reseting flags and counters
						word = 0;
						lengthCount = 0;
						blankCount = 0;
					}
				}
			}
		}

		// LEDs to show the amplitude of the data
		showLEDs(left_buffer, red_LED_ptr);
		showLEDs(right_buffer, green_LED_ptr);

		// Pass the sound data back into the CoDec and output through line out
		*(audio_ptr + 2) = left_buffer;
		*(audio_ptr + 3) = right_buffer;

		// Check for emergency safety pushbuttons
		check_KEYs (&key_up, &key_down, &key_off);
		if (key_up){
			// Increment Speed
			speed = speedGear(&currentSpeed, 'i');
			printf("speedx: %X\n", currentSpeed);
			sprintf(speedInfo, "SPEED %d", speed);
			showLCD(speedInfo, displayLower, 'l');
			*pio = currentSpeed;
			*SvnSegU = HEX_SAFE;
			*SvnSegL = HEX_UP;
			key_up = 0;
			wait(WAIT_TIME);
			*SvnSegU = OFF;
			*SvnSegL = OFF;
		}
		else if (key_down){
			// Decrement speed
			speed = speedGear(&currentSpeed, 'd');
			sprintf(speedInfo, "SPEED %d", speed);
			showLCD(speedInfo, displayLower, 'l');
			*pio = currentSpeed;
			*SvnSegU = HEX_SAFE;
			*SvnSegL = HEX_DOWN;
			key_down = 0;
			wait(WAIT_TIME);
			*SvnSegU = OFF;
			*SvnSegL = OFF;
		}
		else if (key_off){
			// Emergency shut down
			*red_LED_ptr = OFF;
			*green_LED_ptr = OFF;
			*SvnSegU = HEX_SAFE;
			*SvnSegL = HEX_LAND;
			while((speed = speedGear(&currentSpeed, 'd')) >= 0){
				sprintf(speedInfo, "SPEED %d", speed);
				showLCD(speedInfo, displayLower, 'l');
				*pio = currentSpeed;
				wait(WAIT_TIME);
			}
			while(1);
		}
	}
	free(speedInfo);
	return 0;
}

// Make the program stall for a few seconds
void wait ( int seconds )
{
	clock_t endwait;
	endwait = clock () + seconds * 3/4 * CLOCKS_PER_SEC/100;
	clock_t c;
	while ((c = clock()) < endwait){
	}
}

// Check the key to see if any button was pushed
void check_KEYs(int * KEY1, int * KEY2, int * KEY3)
{
	volatile int * KEY_ptr = (int *) PUSHBUTTONS_BASE;		// pushbutton KEY address
	int KEY_value;

	KEY_value = *(KEY_ptr); 				// read the pushbutton KEY values);
	while (*KEY_ptr);							// wait for pushbutton KEY release

	if (KEY_value == 0x2)					// check KEY1
	{
		*KEY1 = 1;
	}
	else if (KEY_value == 0x4)				// check KEY2
	{
		*KEY2 = 1;
	}
	else if (KEY_value == 0x8)				// check KEY3
	{
		*KEY3 = 1;
	}
}

// Change the speed of the helicopter
int speedGear(int * speed, char inc){
	int gear = -1;

	switch(*speed){
	case SPEED_0:
		if(inc == 'i'){
			*speed = SPEED_1;
			gear = 1;
		}
		else gear = -1;
		break;

	case SPEED_1:
		if(inc == 'i'){
			*speed = SPEED_2;
			gear = 2;
		}
		else if(inc == 'd'){
			*speed = SPEED_0;
			gear = 0;
		}
		break;
	case SPEED_2:
		if(inc == 'i'){
			*speed = SPEED_3;
			gear = 3;
		}
		else if(inc == 'd'){
			*speed = SPEED_1;
			gear = 1;
		}
		break;
	case SPEED_3:
		if(inc == 'i'){
			*speed = SPEED_4;
			gear = 4;
		}
		else if(inc == 'd'){
			*speed = SPEED_2;
			gear = 2;
		}
		break;
	case SPEED_4:
		if(inc == 'i'){
			*speed = SPEED_5;
			gear = 5;
		}
		else if(inc == 'd'){
			*speed = SPEED_3;
			gear = 3;
		}
		break;
	case SPEED_5:
		if(inc == 'i'){
			*speed = SPEED_6;
			gear = 6;
		}
		else if(inc == 'd'){
			*speed = SPEED_4;
			gear = 4;
		}
		break;
	case SPEED_6:
		if(inc == 'i'){
			*speed = SPEED_7;
			gear = 7;
		}
		else if(inc == 'd'){
			*speed = SPEED_5;
			gear = 5;
		}
		break;
	case SPEED_7:
		if(inc == 'd'){
			*speed = SPEED_6;
			gear = 6;
		}
		else gear = 7;
		break;
	default:;
	}
	return gear;
}
