#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "lcd.h"
#include "ObAvoid.h"
#include "MotorDriver.h"

//Globals
int LWheelDir;		//1 = Forward  0 = Reverse
int RWheelDir;		//1 = Forward  0 = Reverse
int LSpeed;			//0 to 120 in increments of 30  (0,30,60,90,120)
int RSpeed; 		//0 to 120 in increments of 30	(0,30,60,90,120)

char name[21] = "Obstacle Avoidance";

char mspeed[14] = "Motors= ";  //Text for the motor speed
char LIR[6] = "LIR= ";		//Text for Left IR
char RIR[6] = "RIR= ";		//Text for Right IR
char LSO[6] = "LSO= ";		//Text for Left Sonar
char RSO[6] = "RSO= ";		//Text for Right Sonar

char LeftIR[4];		//Sensor string output
char RightIR[4];	//Sensor string output;
char LeftSonar[4];	//Sensor string output
char RightSonar[4];	//Sensor string output
char Bump[4];

int LeftIRData;		//Sensor data used for behavior
int RightIRData;	//Sensor data used for behavior
int LeftSonarData;	//Sensor data used for behavior
int RightSonarData;	//Sensor data used for behavior

int BumpData;		//Sensor data use for the bump switch

int LeftSonarArray[10] = {0,0,0,0,0,0,0,0,0,0};		//10 point averaging filter
int RightSonarArray[10] = {0,0,0,0,0,0,0,0,0,0};	//10 point averaging filter

int Behavior0;		//Variable used to determine behavior = All sensors clear
int Behavior1;		//Variable used to determine behavior = Both sonar or both IR detect directly ahead
int Behavior2;		//Variable used to determine behavior = Left sonar or IR detect => turn right
int Behavior3;		//Variable used to determine behavior = Right sonar or IR detect => turn left
int Behavior4;		//Variable used to determine behavior
int Behavior5;		//Variable used to determine behavior
int Behavior6;		//Variable used to determine behavior
int Behavior7;		//Variable used to determine behavior

int ADdata;			//Used to stablize UART data
int ADhigh;			//Used to read UART data
int TempSonarLeft;	//Used to calculate average
int TempSonarRight;	//Used to calculate average
int t;				//Used for for-loops and delays
int s;				//Used for sensor arrays
int r;				//Used for initial behavior delay
unsigned int tempC;			//Used for bumper routine


int main(void)
{
	DDRA = 0xFF;  //PortA is all outputs.  Used for the LCD.
	DDRB = 0xFF;  //PortB is all outputs.  Used for the reset pin.
	DDRC = 0x00;
//	DDRD = 0x00;  //PortD is all inputs.   Used for external interrupts  PortD[0] = Bumper
	DDRF = 0x00;  //PortF is all inputs.   Used for the A/D converter.
	PORTB = 0x00; //Initialize to 0
	PORTF = 0x00; //Initialize to 0

	s = 0;				//Initialize sensor array variable
	TempSonarLeft = 0;	//Initialize sensor temp value
	TempSonarRight = 0; //Initialize sensor temp value
	LWheelDir = 1;		//Initialize left direction variable
	RWheelDir = 1;		//Initialzie right direction variable
	LSpeed = 0;			//Initialize left speed variable
	RSpeed = 0;			//Initialize right speed variable

//	Interupt_Init(); //Turns on the bumper interrupt

	AD_Init();		//Turns on AD converter

	USART_Init();	//Turns on UART
	_delay_ms(15);

	LCD_init();		//Turns on LCD

	LCD_text_setup();	//Sets up text on LCD for sensors

	Motor_Init();	//Initializes motor driver

	for(r=0; r<10; r++)
	{
		Sensor_Reading();
	}

	while(1)
	{
		Sensor_Reading();

		if(BumpData > 200 )
		{
			Bumper_routine();
		}

		if(LeftIRData <= 180 && RightIRData <= 180 && LeftSonarData >= 108 && RightSonarData >= 108)
		{
			Behavior0 = 1;		//All sensors are clear
		}						//Move forward
		else
		{
			Behavior0 = 0;
		}

		if( (LeftSonarData < 108 && RightSonarData < 108) ) //|| (LeftIRData > 180 && RightIRData >180) )
		{
			Behavior1 = 1;		//Both sonar detect something up ahead //////OR both IR detect something up ahead
		}						//Turn around
		else
		{
			Behavior1 = 0;
		}

		if(LeftIRData > 180 ) //|| LeftSonarData < 110)
		{
			Behavior2 = 1;		//Something on the left side of the robot
		}						//Turn right
		else
		{
			Behavior2 = 0;
		}

		if(RightIRData > 180 ) //|| LeftSonarData < 110)
		{
			Behavior3 = 1;		//Something on the right side of the robot
		}						//Turn left
		else
		{
			Behavior3 = 0;
		}

		Behavior_Arbitrate();
	}

	

} //End of main

void Behavior_Arbitrate()
{
		if(Behavior0 == 1 && Behavior1 == 0 && Behavior2 == 0 && Behavior3 == 0)  
		{														//If the path is clear, do this behavior
			if(LWheelDir == 1 && RWheelDir == 1)				//Move forward
			{
				if(LSpeed < 120 && RSpeed < 120)
				{
					LSpeed = LSpeed + 30;
					RSpeed = RSpeed + 30;
					Motor_forward(RSpeed, LSpeed);
					delay150ms();
				}
				else
				{
					Motor_forward(120, 120);
					delay150ms();
				}
			}
			else
			{
				Motor_stop_left();
				Motor_stop_right();
				LWheelDir = 1;
				RWheelDir = 1;
				LSpeed = 0;
				RSpeed = 0;
				delay150ms();
			}
		}

		if(Behavior0 == 0 && Behavior1 == 1)		
		{															//Both sonars detect
			if(LWheelDir == 1 && RWheelDir == 0)					//Spin around going right
			{
				if(LSpeed < 90 && RSpeed < 90)
				{
					LSpeed = LSpeed + 30;
					RSpeed = RSpeed + 30;
					LWheelDir = 1;
					RWheelDir = 0;
					Motor_spin_right(LSpeed, RSpeed);
					delay150ms();
				}
				else
				{
					Motor_spin_right(90, 90);
					delay150ms();
				}

			}
			else
			{
				Motor_stop_left();
				Motor_stop_right();
				LWheelDir = 1;
				RWheelDir = 0;
				LSpeed = 0;
				RSpeed = 0;
				delay150ms();
			}
		}

		if(Behavior2 == 1 && Behavior3 == 0 && Behavior1 == 0)		//Something on the left side of the robot
		{											//Turn right
			if(LWheelDir == 1 && RWheelDir == 0)
			{
				if(LSpeed < 120)
				{
					LSpeed = LSpeed + 30;
					RSpeed = 0;
					LWheelDir = 1;
					RWheelDir = 0;
					Motor_turn_right(LSpeed);
					delay150ms();
				}
				else
				{
					RSpeed = 0;
					LWheelDir = 1;
					RWheelDir = 0;
					Motor_turn_right(120);
					delay150ms();
				}
			}
			else
			{
				if(LWheelDir == 0)
				{
					Motor_stop_left();
				}
				Motor_stop_right();
				LWheelDir = 1;
				RWheelDir = 0;
				LSpeed = 0;
				RSpeed = 0;
				delay150ms();
			}
		}

		if(Behavior3 == 1 && Behavior2 == 0 && Behavior1 == 0)		//Something on the right side of the robot
		{											//Turn left
			if(LWheelDir == 0 && RWheelDir == 1)
			{
				if(RSpeed < 120)
				{
					LSpeed = 0;
					RSpeed = RSpeed + 30;
					LWheelDir = 0;
					RWheelDir = 1;
					Motor_turn_left(RSpeed);
					delay150ms();
				}
				else
				{
					LSpeed = 0;
					LWheelDir = 0;
					RWheelDir = 1;
					Motor_turn_left(120);
					delay150ms();
				}
			}
			else
			{
				if(RWheelDir == 0)
				{
					Motor_stop_right();
				}
				Motor_stop_left();
				LWheelDir = 0;
				RWheelDir = 1;
				LSpeed = 0;
				RSpeed = 0;
				delay150ms();
			}
		}

		if(Behavior2 == 1 && Behavior3 == 1 && Behavior1 ==0)			//Both IR are triggered but no sonar
		{
			if(LWheelDir == 1 && RWheelDir == 0)					//Spin around going right
			{
				if(LSpeed < 90 && RSpeed < 90)
				{
					LSpeed = LSpeed + 30;
					RSpeed = RSpeed + 30;
					LWheelDir = 1;
					RWheelDir = 0;
					Motor_spin_right(LSpeed, RSpeed);
					delay150ms();
				}
				else
				{
					Motor_spin_right(90, 90);
					delay150ms();
				}

			}
			else
			{
				Motor_stop_left();
				Motor_stop_right();
				LWheelDir = 1;
				RWheelDir = 0;
				LSpeed = 0;
				RSpeed = 0;
				delay150ms();
			}
		}
}

void Sensor_Reading()		//Sensor Loop  Used to read sensors and store data.
{
		//Bumper Switch attempt
		ADMUX = 0b01100100;
		_delay_ms(5);
		ADhigh = ADCH;
		//ADhigh = ADhigh + 100;
		ADdata = ADhigh;
		BumpData = ADdata; //((BumpData + ADdata) / 2);				//Stores bump data into global variable
		sprintf(Bump, "%d", BumpData);
		_delay_ms(5);
		LCD_moveTo(1,3);
		_delay_ms(5);
		LCD_string(Bump);
		_delay_ms(5);
		
		//Left IR Sensor Code = Red Wire = PortF[3]
		ADMUX = 0b01100011;
		_delay_ms(5);
		ADhigh = ADCH;
		ADhigh = ADhigh + 100;
		ADdata = ADhigh;
		LeftIRData = ADdata;				//Stores LeftIR data into global variable
		sprintf(LeftIR, "%d", LeftIRData);
		_delay_ms(5);
		LCD_moveTo(2,5);
		_delay_ms(5);
		LCD_string(LeftIR);
		_delay_ms(5);

		//Right IR Sensor Code = Orange Wire = PortF[2]
		ADMUX = 0b01100010;
		_delay_ms(5);
		ADhigh = ADCH;
		ADhigh = ADhigh + 100;
		ADdata = ADhigh;
		RightIRData = ADdata;				//Stores RightIR data into global variable
		sprintf(RightIR, "%d", RightIRData);
		_delay_ms(5);
		LCD_moveTo(2,15);
		_delay_ms(5);
		LCD_string(RightIR);
		_delay_ms(5);

		//Left Sonar Sensor Code = Purple Wire = PortF[1]
		ADMUX = 0b01100001;
		_delay_ms(5);
		ADhigh = ADCH;
		ADdata = ADhigh + 100;
		LeftSonarArray[s] = ADdata;			//Stores LeftSonar data into global LeftSonarArray
		sprintf(LeftSonar, "%d", LeftSonarData);
		_delay_ms(5);
		LCD_moveTo(3,5);
		_delay_ms(5);
		LCD_string(LeftSonar);
		_delay_ms(5);

		//Right Sonar Sensor Code = Blue Wire = PortF[0]
		ADMUX = 0b01100000;
		_delay_ms(5);
		ADhigh = ADCH;
		ADdata = ADhigh + 100;
		RightSonarArray[s] = ADdata;		//Stores RightSonar data into global RightSonarArray
		sprintf(RightSonar, "%d", RightSonarData);
		_delay_ms(5);
		LCD_moveTo(3,15);
		_delay_ms(5);
		LCD_string(RightSonar);
		_delay_ms(5);

		s++;
		if(s >= 10)			//If s > array length, reset to 0
		{
			s=0;
		}
		
		TempSonarLeft = 0;		//Reset temp variables to 0
		TempSonarRight = 0;

		for(t=0; t<10; t++)		//Add the array values into the temp variable
		{
			TempSonarLeft = TempSonarLeft + LeftSonarArray[t];
			TempSonarRight = TempSonarRight + RightSonarArray[t];
		}
	
		LeftSonarData = (TempSonarLeft / 10);		//Average out the array data
		RightSonarData = (TempSonarRight /10);
}

void Bumper_routine()
{
	Motor_stop_left();
	Motor_stop_right();
	LWheelDir = 1;
	RWheelDir = 1;
	LSpeed = 0;
	RSpeed = 0;
	delay150ms();
	
	LCD_clear();
	_delay_ms(1);
	LCD_moveTo(1,3);
	_delay_ms(1);
	LCD_string("Oops!!!!!");

	Motor_reverse(30,30);
	delay150ms();
	Motor_reverse(60,60);
	delay150ms();
	Motor_reverse(90,90);
	delay150ms();
	Motor_reverse(120,120);
	delay150ms();
	delay500ms();
	delay500ms();
	delay500ms();
	delay500ms();

	Motor_stop_left();
	Motor_stop_right();
	delay150ms();
	
	Motor_spin_left(30,30);
	delay150ms();
	Motor_spin_left(60,60);
	delay150ms();
	Motor_spin_left(90,90);
	delay150ms();
	Motor_spin_left(120,120);
	delay500ms();
	delay500ms();
	delay500ms();
	delay500ms();
	delay500ms();
	delay500ms();

	Motor_stop_left();
	Motor_stop_right();

	delay150ms();

	LCD_clear();
	_delay_ms(1);
	LCD_moveTo(0,0);
	_delay_ms(1);
	LCD_text_setup();
}

void delay150ms()
{
	for(t = 0; t<10; t++)
	{
		_delay_ms(15);
	}
}

void delay500ms()
{
	for(t = 0; t<100; t++)		//Delays for 500 mseconds = 0.5 seconds
	{
		_delay_ms(5);
	}
}

void delay100ms()
{
	for(t = 0; t<20; t++)		//Delays for 100 mseconds = 0.1 seconds
	{
		_delay_ms(5);
	}
}

void AD_Init()
{	
	//AD Initialization Code
	ADMUX  = 0b01100000;
	ADCSRA = 0b11100111;
}

void USART_Init()
{
	//UART Initialization Code
	UCSR0A = 0x00;
	UCSR0B = 0x08;
	UCSR0C = 0x06;

	UBRR0H = 0x00;
	UBRR0L = 0x5F;
}

void LCD_text_setup()
{
	//LCD Initial Text
	_delay_ms(1);
	LCD_string(name);
	_delay_ms(1);
	LCD_moveTo(2,0);
	_delay_ms(1);
	LCD_string(LIR);
	_delay_ms(1);
	LCD_moveTo(2,10);
	_delay_ms(1);
	LCD_string(RIR);
	_delay_ms(1);
	LCD_moveTo(3,0);
	_delay_ms(1);
	LCD_string(LSO);
	_delay_ms(1);
	LCD_moveTo(3,10);
	_delay_ms(1);
	LCD_string(RSO);
}