// 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		50

#define HI_DIST_THRESH 8000

#define SLEEP PCON |= 1

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

// 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 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 data 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


/***********************************************************************
 * 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;

	// create all the bottom half processes
	//os_create_task(MOTOR_BOTTOM);
	//os_create_task(SERIAL_BOTTOM);
	//os_create_task(SONAR_BOTTOM);

	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)
					{

						// will pause one extra time at destination before 
						// acknowledging done
						command_left = command_right = 0;
					}


				}


				run_motor();
					
			}

			// ask SERIAL_BOTTOM for next command
			load_command();

		}
	}
}

/***********************************************************************
 * 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.
}