// CSE 466, Fall 2001, Final Project
// David DeTerra
// Daniel Dunham
// Jeremy Hance

#include <At89x55.h>    

#define OVERFLOW_1_INT	3
#define	SERIAL_INT		4

#define TURN_MOTOR_STEPS		700		// 90 degree motor turns
#define FORWARD_MOTOR_STEPS		500		// ~6 inches forward
#define MOTOR_SPEED		40		// in # of interrupts with an 8-bit timer

#define PROG_SIZE		1000

#define LO_DIST_THRESH 4000
#define HI_DIST_THRESH 8000

#define WEST 0
#define NORTH 1
#define EAST 2
#define SOUTH 3

#define OPEN_LIST_LENGTH 3000
#define MAP_SIDE 100
#define MAP_HEIGHT MAP_SIDE
#define MAP_WIDTH MAP_SIDE / 2

#define READY_LIGHT INT0
#define PROG_LIGHT INT1
#define RUN_LIGHT T0
*
#define SLEEP PCON |= 1

void read_sonar(void);
void load_command(void);
void run_motor(void);
void RunAI();

// Global Data / Shared Memory
bit				command_left;	// the direction command for the left motor
bit				command_right;	// the direction command for the right motor
bit				larry;			// whether or not to ignore the sonar

unsigned char next_command;	// the next command from the queue
unsigned char current;		// the location of the current command in queue
unsigned char length;			// the location of the first invalid element in queue
unsigned char xdata command_q[PROG_SIZE]; // the queue in which program commands are stored
		 char steps; // the number of steps for which to execute the current command
unsigned char incoming_cmd;

bit				loading; //state of program load, 1 = in progress

// Motor control globals
unsigned int xdata i_count;  // counts the interrupts before we pay attention to them

// Sonar control globals
bit             delay_mode;
unsigned int flight_time;	// reading from timer 1

// Path Finding Globals

typedef struct AISTRUCT1
{    
    unsigned char    x;
    unsigned char    y;
} COORD;

typedef struct AISTRUCT2
{
    char    blocked;
    char    closed;
    char	parent;
} MAP_BLOCK;

COORD         xdata open_list[OPEN_LIST_LENGTH] _at_ 0x0000;
unsigned int   open_length;
unsigned char xdata path_map[MAP_WIDTH][MAP_HEIGHT] _at_ 0x2000;

unsigned char  current_dir;
COORD          current_pos;
COORD          end_pos;


/***********************************************************************
 * Main Program Flow Control Logic
 ***********************************************************************/
void main(void)
{

	// Setup Timer 2 to be a 9600 baud rate generator
	// Sonar and Motor will be responsible for setting up timer 1
	// as either an autoreload timer or a counter and clearing out 
	// settings for timer 1 after it has been used.
	RCAP2H = 0xFF;
	RCAP2L = 0x98;
	T2CON  = 0x34;

	// Enable Serial Reception with Serial Mode 1 (8 bit UART)
	SCON = 0x50;

	// Set Timer 1 to off
	TR1 = 0;

	// Initial Values
	T2_EX = 0;
	P1_2 = 0;
	P1_3 = 0;
	
	P1_4 = 0;

	INT0 = 0;
	INT1 = 0;
	T0 = 0;
	T1 = 0;


	current = 0;
	length = 0;

	// Enable Serial Interrupts
	// Sonar and Motor will be responsible for enabling and disabling 
	// external interrupts and overflow interrupts


	while(1)
	{
		next_command = 0;		

		// enable serial interrupts
		IE |= 0x90;
		
		while(next_command == 0)
		{
			SLEEP;
		}

		// disable serial interrupts
		IE &= 0xEF;

		while(next_command != 0)
		{
			// busy wait until go switch is flipped
			while(P1_7 == 0)
			{ }
			// get the number of steps the command calls for
			steps = next_command & 0x1F;

			// set larry for sonar control
			larry		  = (bit)((next_command & 0x20) >> 5);

			for(; steps > 0; steps--)
			{
	
				// set left and right for motor control
				command_left  = (bit)((next_command & 0x80) >> 7);
				command_right = (bit)((next_command & 0x40) >> 6);

				// TODO: check sonar if !larry
				if(command_left && command_right && !larry)
				{
					read_sonar();

					if(flight_time < HI_DIST_THRESH)
					{
						RunAI();
					}
					else
					{
						// ask motor to execute a single command
						// NOTE: motor must recognize pause command
						// on own
		
						run_motor();
	
					}

				}
				else
				{
					// ask motor to execute a single command
					// NOTE: motor must recognize pause command
					// on own

					run_motor();
					
				}

				
			}

			// ask SERIAL_BOTTOM for next command
			load_command();

		}
	}
}

/***********************************************************************
 * AI CONTROL
 ***********************************************************************/

MAP_BLOCK GetBlock(COORD location)
{
	volatile COORD temp;
	volatile MAP_BLOCK retVal;
	volatile unsigned char tempNibble;
	temp.x = ((location.x) >> 1);
	temp.y = (location.y);

	// retrieve byte from byte array
	tempNibble = path_map[temp.x][temp.y];

	// if address was even, we need the upper nibble
	if(((temp.x) << 1) == location.x)
	{
		tempNibble = tempNibble >> 4;
	}
	
	// set values of blocked, closed and parent
	// from appropriate nibble
	retVal.blocked = (tempNibble & 0x08) >> 3;
	retVal.closed  = (tempNibble & 0x04) >> 2;
	retVal.parent  = (tempNibble & 0x03);

	return retVal;
}

void SetBlock(MAP_BLOCK value, COORD location)
{
	volatile COORD temp;
	volatile unsigned char tempNibble;
	volatile unsigned char tempByte;
	temp.x = ((location.x) >> 1);
	temp.y = (location.y);
	
	// work on temp nibble
	tempNibble = 0;

	// retrieve byte we're updating
	tempByte = path_map[temp.x][temp.y];
	
	// set blocked bit
	if(value.blocked)
		tempNibble |= 0x08;
	// set closed bit
	if(value.closed)
		tempNibble |= 0x04;
	// set two parent bits
	tempNibble |= value.parent;

	// if address was even, we're setting the upper nibble
	if(((temp.x) << 1) == location.x)
	{
		tempNibble = tempNibble << 4;
		tempByte = tempByte & 0x0F;
	}
	// otherwise, we're setting the lower nibble
	else
	{
		tempByte = tempByte & 0xF0;
	}

	tempByte = tempByte | tempNibble;

	path_map[temp.x][temp.y] = tempByte;
}


void ClearMap()
{
	volatile unsigned char x;
	volatile unsigned char y;

	for(x = 0; x < MAP_WIDTH; x++)
    {
		for(y = 0; y < MAP_HEIGHT; y++)
		{
			path_map[x][y] = 0;
		}
	}
}

void OpenMap()
{
    volatile MAP_BLOCK temp_block;
	volatile COORD temp_pos;

	for(temp_pos.x = 0; temp_pos.x < MAP_SIDE; temp_pos.x++)
    {
        for(temp_pos.y = 0; temp_pos.y < MAP_SIDE; temp_pos.y++)
        {
			temp_block = GetBlock(temp_pos);
			if(temp_block.closed)
			{
				temp_block.closed = 0;
            	SetBlock(temp_block, temp_pos);
			}
        }
    }
}

void MarkBlock()
{
	volatile MAP_BLOCK blocked;
	volatile COORD temp_pos;
	volatile unsigned char i;

	// initialize our blocked block
	blocked.blocked = 1;

	temp_pos = current_pos;

	for(i = 0; i < 3; i++)
	{
		switch(current_dir)
		{
			case NORTH:
				temp_pos.y ++;
				break;
			case SOUTH:
				temp_pos.y --;
				break;
			case WEST:
				temp_pos.x --;
				break;
			case EAST:
				temp_pos.x ++;
				break;
		}

		// negative numbers will wrap around to positive side
		if(temp_pos.y < MAP_SIDE &&
		   temp_pos.x < MAP_SIDE)
		{
			SetBlock(blocked, temp_pos);
		}
	}
}


char FindEnd()
{
	volatile int no_error;
	volatile char temp_dir;

	temp_dir = current_dir;
	
	end_pos = current_pos;
	no_error = 1;

	while(next_command != 0)
	{
		if(no_error && (command_left || command_right))
		{
			for(; steps > 0; steps --)
			{
				// turn right command
				if(command_left && !command_right)
				{
					if(temp_dir == 3)
						temp_dir = 0;
					else
						temp_dir ++;
				}

				// turn left command
				else if(command_right && !command_left)
				{
					if(temp_dir == 0)
						temp_dir = 3;
					else
						temp_dir --;
				}

				// move forward command
				else
				{
					switch (temp_dir)
					{
						case NORTH:
							if(end_pos.y < MAP_SIDE - 1)
							{
								end_pos.y ++;
							}
							else
							{
								no_error = 0;
							}
							break;
						case SOUTH:
							if(end_pos.y > 0)
							{
								end_pos.y --;
							}
							else
							{
								no_error = 0;
							}
							break;
						case EAST:
							if(end_pos.x < MAP_SIDE - 1)
							{
								end_pos.x ++;
							}
							else
							{
								no_error = 0;
							}
							break;
						case WEST:
							if(end_pos.x > 0)
							{
								end_pos.x --;
							}
							else
							{
								no_error = 0;
							}
							break;
					}
				}
			}

			// Ask serial for next command
			load_command();

			if(next_command)
			{
				// get the number of steps the command calls for
				steps = next_command & 0x1F;
				// set left and right for motor control
				command_left  = (bit)((next_command & 0x80) >> 7);
				command_right = (bit)((next_command & 0x40) >> 6);
			}
		}
	} 
	
	return no_error;
}


void PlaceOnOpen(COORD target_block, unsigned char parent_dir)
{
	volatile MAP_BLOCK update_block;

	update_block = GetBlock(target_block);

	if(update_block.blocked == 1 || update_block.closed == 1)
	{
		return;
	}

	if(open_length < (OPEN_LIST_LENGTH - 1))
	{
		open_list[open_length] = target_block;
		open_length++;
		update_block.closed = 1;
		update_block.parent = parent_dir;
		SetBlock(update_block, target_block);
	}
}



COORD SmallestOnOpen()
{
	volatile unsigned int i;
	volatile unsigned int pos_in_list;
	volatile unsigned int displacement;
	volatile int temp1;
	volatile int temp2;
	volatile COORD retVal;

	for(i = 0, displacement = 65000; i < open_length; i ++)
	{
		temp1 = open_list[i].x - end_pos.x;
		temp2 = open_list[i].y - end_pos.y;

		if(temp1 < 0)
			temp1 = 0 - temp1;
		if(temp2 < 0)
			temp2 = 0 - temp2;

		if(displacement > (temp1 + temp2))
		{
			pos_in_list = i;
			displacement = temp1 + temp2;
		}
	}

	retVal = open_list[pos_in_list];
	open_list[pos_in_list] = open_list[open_length - 1];
	open_length --;

	return retVal;
}

void ReversePath()
{

	volatile char		cur_dir;
	volatile char		prv_dir;
	volatile COORD		pth_pos;
	volatile MAP_BLOCK	cur_blk;

	pth_pos.x = end_pos.x;
	pth_pos.y = end_pos.y;
	cur_blk = GetBlock(pth_pos);
	cur_dir = cur_blk.parent;

	while(pth_pos.x != current_pos.x || pth_pos.y != current_pos.y)	
	{
		if(cur_dir == NORTH)
			pth_pos.y ++;
		else if(cur_dir == SOUTH)
			pth_pos.y --;
		else if(cur_dir == EAST)
			pth_pos.x ++;
		else
			pth_pos.x --;
		
		cur_blk = GetBlock(pth_pos);
		prv_dir = cur_dir;
		cur_dir = cur_blk.parent;

		cur_blk.parent = ((prv_dir + 2) & 0x03);
		SetBlock(cur_blk, pth_pos);

	} 

}

char FindPath()
{
	volatile COORD path_pos;
	volatile COORD temp_pos;
	
	OpenMap();
	
	open_length = 0;
	
	PlaceOnOpen(current_pos, SOUTH);

	while(open_length > 0)
	{
		path_pos = SmallestOnOpen();

		if(path_pos.x == end_pos.x && path_pos.y == end_pos.y)
		{
			
			ReversePath();
			return 1;
		}

		if(path_pos.x > 0)
		{
			temp_pos.x = path_pos.x - 1;
			temp_pos.y = path_pos.y;
			PlaceOnOpen(temp_pos, EAST);
		} 
		if(path_pos.x < (MAP_SIDE - 1))
		{
			temp_pos.x = path_pos.x + 1;
			temp_pos.y = path_pos.y;
			PlaceOnOpen(temp_pos, WEST);
		}
		if(path_pos.y > 0)
		{
			temp_pos.x = path_pos.x;
			temp_pos.y = path_pos.y - 1;
			PlaceOnOpen(temp_pos, NORTH);
		} 
		if(path_pos.y < (MAP_SIDE - 1))
		{
			temp_pos.x = path_pos.x;
			temp_pos.y = path_pos.y + 1;
			PlaceOnOpen(temp_pos, SOUTH);
		}
	}	

	return 0;
}


void RunAI()
{
	volatile MAP_BLOCK temp_block;
	//COORD	  temp_pos;
	
	// Clear Out the entire map in case we ran the AI
	// on a previous program
	ClearMap();

	// Set our current direction and position 
	current_dir = NORTH;
	current_pos.x = MAP_SIDE / 2;
	current_pos.y = MAP_SIDE / 2;

	if(FindEnd())
	{

		MarkBlock();

		if(FindPath() == 0)
		{
			return;
		}

		while(end_pos.x != current_pos.x || end_pos.y != current_pos.y)
		{
			//MarkBlock();

			//if(FindPath() == 0)
			//{
			//	return;
			//}

			// since we've found a block right in front of us, the first
			// move in the path will be a turn... so set front_distance
			// to a high enough value that we can make that turn
			//front_distance = HI_DIST_THRESH;
	
			//while(front_distance >= HI_DIST_THRESH && 
			//      (end_pos.x != current_pos.x || end_pos.y != current_pos.y))
			{
				temp_block = GetBlock(current_pos);

				if(current_dir == temp_block.parent)
				{
					command_left  = 1;
					command_right = 1;
				}
				else if(current_dir < temp_block.parent)
				{
					if(current_dir == WEST && temp_block.parent == SOUTH)
					{
						command_left  = 0;
						command_right = 1;
						current_dir = SOUTH;
					}
					else
					{
						command_left  = 1;
						command_right = 0;
						current_dir = current_dir + 1;
					}
				}
				else
				{
					if(current_dir == SOUTH && temp_block.parent == WEST)
					{
						command_left  = 1;
						command_right = 0;
						current_dir = WEST;
					}
					else
					{
						command_left  = 0;
						command_right = 1;
						current_dir = current_dir - 1;
					}
				}

				if(command_left == 1 && command_right == 1)
				{
					// look at sonar after every command
					read_sonar();
//					os_wait(K_SIG, 0, 0);
					
					if(flight_time < HI_DIST_THRESH)
					{
						MarkBlock();
				
						if(FindPath() == 0)
						{
							return;
						}
					}
					else
					{
						if(current_dir == NORTH)
						{
							current_pos.y += 1;
						}
						if(current_dir == SOUTH)
						{
							current_pos.y -= 1;
						}
						if(current_dir == EAST)
						{
							current_pos.x += 1;
						}
						if(current_dir == WEST)
						{
							current_pos.x -= 1;
						}
		
						// Ask motors to execute new command
						run_motor();
						//os_wait(K_SIG, 0, 0);
					}
				}
				else
				{
					// Ask motors to execute new command			
					run_motor();
					//os_wait(K_SIG, 0, 0);
				}
			}
		}
	}
}


/***********************************************************************
 * MOTOR CONTROL
 ***********************************************************************/
void run_motor(void)
{

    unsigned int motor_pulses = 0;
	unsigned int total_pulses = 0;
	//bit running = 0;
	i_count = 0; 

	// if we have a command and we aren't currently executing 
	// a command, then set the direction bits for the motors and 
	// turn on the timer to count interrupts

	// setup code for timer 1 motor control 
	TMOD &= 0x0F;
	TMOD |= 0x20;
	TH1 = 0;
	IE |= 0x08;


	T2_EX = command_left;
	P1_2 = command_right;

	if(command_left || command_right) {
		total_pulses = TURN_MOTOR_STEPS;
		if(command_left && command_right)
			total_pulses = FORWARD_MOTOR_STEPS;
	}
	else total_pulses = FORWARD_MOTOR_STEPS;

	TR1 = 1;

	while (motor_pulses < total_pulses)
	{

		SLEEP;

		// if the i_count reaches the defined wait time then send a pulse 
		// to the motor
		if(i_count >= MOTOR_SPEED) {
			if(command_left || command_right)
			{
				P1_3 ^= 1;
			}
			i_count = 0;
			motor_pulses++;
		}
	}

	TR1 = 0;

	// turn off timer 1 
	TMOD &= 0x0F;
	IE &= 0xF7;

}


void motor_top(void) interrupt OVERFLOW_1_INT
{
	// code for running motor here
	i_count++;
}



/***********************************************************************
 * SERIAL CONTROL
 ***********************************************************************/
void load_command(void)
{	
	// code for handing off commands to control logic from queue
	if( current < length) {
		next_command = command_q[current];
		current++;
	} else {
		next_command = 0;
	}
}


void serial_top(void) interrupt SERIAL_INT
{
	// code for handling serial transmissions and filling queue here
	// should load first command into next_command after the end
	// programming command is received.  Also, serial_top should disable 
	// serial interrupts before signaling CONTROL_LOGIC that the first
	// command is ready
	
	INT0 = 0;
	INT1 = 0;
	T0 = 0;
	T1 = 1;

	if (RI == 1 && (SBUF != 0)) { 	// we will check to see what was received and act accordingly
      		incoming_cmd = SBUF;
			//P2 = SBUF;

		// Case: load in progress
      	if (incoming_cmd != 32 && loading && length <= PROG_SIZE) {
			command_q[length] = incoming_cmd;
			length++;
		
			INT0 = 0;
			INT1 = 0;
			T0 = 1;
			T1 = 1;
		}

		// case: end of program
		else if (incoming_cmd == 32 && loading == 1) {
			loading = 0;
			current = 0;
			load_command();

			INT0 = 0;
			INT1 = 1;
			T0 = 0;
			T1 = 0;

		}

		// case: begin loading program
      	else if (incoming_cmd == 32 && loading == 0 && next_command == 0) {
			loading = 1;
			length = 0;

			INT0 = 0;
			INT1 = 0;
			T0 = 1;
			T1 = 0;

		}
		else
		{
			INT0 = 0;
			INT1 = 1;
			T0 = 0;
			T1 = 1;
		}
	}
   	RI = 0;	
	TI = 0;
}


/***********************************************************************
 * SONAR CONTROL
 ***********************************************************************/
void read_sonar(void)
{

		TMOD &= 0x0F;
		TMOD |= 0x10;
		TL1 = 0x00;
		TH1 = 0x00;
	
		//delay_mode = 1;
		P1_4 = 1;
		TR1 = 1;


		while(TH1 < 6)
		{ }

		while((P1_5 == 0) && (TH1 > 0))
	    { }

		TR1 = 0;
		P1_4 = 0;

		if(TH1 < 6)
		{
			flight_time = 65000;
			P1_6 = 1;
		}
		else
		{
			flight_time = TH1;
			flight_time = (flight_time << 8);
			flight_time += TL1;
			P1_6 = 0;
		}

		TMOD &= 0x0F;

		


		// code for setting up rising edge external interrupt, starting
		// timer 1 as a counter, and starting sonar reading here.
}