#if defined(__PCM__)
#include <16f877.h>
#fuses HS,NOWDT,NOPROTECT
#device ADC=8
#use delay(clock=4000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, parity=N)

#use fast_io(D) //pins RD2 and RD3 flip between inputs and outputs,

//***Define Default Port Constants.
#define	IO_SETA		0x3F	//AN0 - AN3 analog inputs from CDs '00111111', AN4 - unused
#define	IO_SETB		0x05	//RB0,RB1 - UART,  RB7-RB3 LEDs, RB2 - unused
#define	IO_SETC		0xB6	//RC7,RC6-UART, RC1,RC2 - heartbeat, RC3 - sound, RC4,RC5 - dips, RC0 - unused
#define	IO_SETD_I	0xFC	//RD0,RD1 - enables, RD2,RD3 - tactiles(inputs), RD4-RD7 - compass
#define 	IO_SETD_O	0xF0	//RD2 and RD3 tactiles are both outputs.
#define 	IO_SETD_R	0xF8	//RD3 is input RD2 output
#define 	IO_SETD_L	0xF4	//RD2 is input RD3 output
#define	IO_SETE		0x07	//RE0-RE2 used as analog inputs(AN5-AN7)


#define	FL	0	//NEW indices into CDs sensor arrays
#define 	FR	1
#define 	SR	2
#define 	SL	3
#define 	BR	4
#define	BL	5
#define 	BCL	6
#define	BCR	7

//portd pin value defines for individual bit manipulation 
// 
#define	NORTH	71		//RD4 - RD7 input from compass
#define	EAST	70
#define	SOUTH	69
#define	WEST	68
#define	RSENSOR	67		//RD3 - Right Tactile Sensor (input/output)
#define	LSENSOR	66		//RD2 - Left Tactile Sensor (input/output)
#define	ENB1	65		//RD1 - Enable HeartBeat1 ( bc2 output, walk)
#define	ENB2	64		//RD0 - Enable HeartBeat2


//portc defines
#define	HEARTBT1 58		//RC2 - output from master bicore
#define	HEARTBT2 57		//RC1 - output from heartbeat2 bicore
#define	TONE_PIN  PIN_C3
#define	DIP1	PIN_C4
#define	DIP2	PIN_C5

//state machine defines
#define	FWD	1
#define	REV	2
#define	LFT	3
#define	RGHT	4
#define	STP	5
#define       FUB	6


const	long	BEEP = 1175;		// frequency of tone
const	long 	DUR = 100;		// duration of tone

int	black[8], white[8], c_sensor[8], bin_sensor[8];
int	bot_step, action, black_limit, until_black, revcount;



//*******************************************************************************************
// routines to make beep,  do_delay and generate_tone are taken from the CCS Library.
// For embedded code, I like it all in one file.
//*******************************************************************************************

void do_delay(int ms_delay, int num_ms, int us_delay, int num_us)
{
 int i;

 for(i=0;i<num_ms;i++)
  delay_ms(250);
 delay_ms(ms_delay);
 for(i=0;i<num_us;i++)
  delay_us(250);
 delay_us(us_delay);
}


//  generate_tone(frequency, duration)
//  Generates wave at set frequency(Hz), for set duration(ms)
//
void generate_tone(long frequency, long duration)
{
   int32 total_delay_time;                      // in microseconds
   long total_ms_delay_time, total_us_delay_time;
   int num_us_delays, num_ms_delays, ms_delay_time, us_delay_time;
   long num_periods;

   total_delay_time = (1000000/frequency)/2-10; // calculate total delay time (10 for error)

   total_ms_delay_time = total_delay_time/1000; // total delay time of ms
   num_ms_delays = total_ms_delay_time/250;     // number of 250ms delays needed
   ms_delay_time = total_ms_delay_time%250;     // left over ms delay time needed

   total_us_delay_time = total_delay_time%1000; // total delay time of us (ms already acounted for)
   num_us_delays = total_us_delay_time/250;     // number of 250us delays needed
   us_delay_time = total_us_delay_time%250;     // left over us delay time needed

   num_periods = ((int32)duration*1000)/(1000000/frequency);

   while((num_periods--) != 0)
   {
      do_delay(ms_delay_time, num_ms_delays, us_delay_time, num_us_delays);
      output_high(TONE_PIN);
      do_delay(ms_delay_time, num_ms_delays, us_delay_time, num_us_delays);
      output_low(TONE_PIN);
   }

   return;
}



//***********************************************************************************
//These routines control the enables of the bicores Heartbeat1 and Heartbeat2
//***********************************************************************************

//all_off - turns all bicore enables to OFF
void all_enables_off()
{
	output_high(ENB1);		//turn motor enables off
	delay_cycles(1);
	output_high(ENB2);		//turn heartbeat2 enable off
	delay_cycles(1);
}


//hb2_on - turns enable for listen/walk bicore cycle to ON
void hb2_on()
{
	output_low(ENB2);		//turn heartbeat2 enable ON
	delay_cycles(1);
}


void hb2_off()
{
	output_high(ENB2);		//turn heartbeat2 enable OFF
	delay_cycles(1);
}


//motors_on - turns motor enable ON for all bc'2 that control the legs.
//	should not be enabled when hb2_on has been called.
void motors_on()
{
	output_low(ENB1);		//turn motor enables ON
	delay_cycles(1);
}

void motors_off()
{
	output_high(ENB1);		//turn motor enables OFF
	delay_cycles(1);
}



//*******************************************************************************************
//These are the routines used for controlling the IMUX - only outputs, ignores input from RC.
//*******************************************************************************************

//imx_right - SET LEFT channel of IMX low, so bot TURNS RIGHT
void imx_right()
{
	output_low(LSENSOR);		//set left channel of IMX low
	delay_cycles(1);
	output_high(RSENSOR);		//make sure right channel is high
	delay_cycles(1);
}


//imx_left -  SET RIGHT channel of IMX low, so bot TURNS LEFT
void imx_left()
{
	output_low(RSENSOR);		//set right channel of IMX low
	delay_cycles(1);
	output_high(LSENSOR);		//make sure left channel is high
	delay_cycles(1);
}


//reverse - sets both pins low on IMX to put bot into reverse
void imx_reverse()
{
	output_low(LSENSOR);		//set left channel of IMX low
	delay_cycles(1);
	output_low(RSENSOR);		//set right channel of IMX low
	delay_cycles(1);

}

//set IMX pins high to move bot forward
void imx_forward()
{
	output_high(LSENSOR);		//set left channel of IMX high
	delay_cycles(1);
	output_high(RSENSOR);		//set right channel of IMX high
	delay_cycles(1);
}



//***************************************************************************************
//sensor_read() - Turns the LEDs on /off and samples the cds array.
// 
// note: this could be broken down into smaller subgroups.
//***************************************************************************************

void sensor_read()	//Front Center Sensor, FCL and FCR, sampled twice
{

	// test Cd cell A and B - AN0,AN1
	output_high(PIN_B7);
	delay_ms(60);
	set_adc_channel(0);
	delay_us(10);
	c_sensor[FR]=read_adc();
	set_adc_channel(1);
	delay_us(10);
	c_sensor[FL]=read_adc();
	output_low(PIN_B7);
	delay_cycles(1);

	// test Cd cell 2 - AN2
	output_high(PIN_B6);
	delay_ms(60);
    	set_adc_channel(2);
	delay_us(10);
	c_sensor[SR]=read_adc();
	output_low(PIN_B6);
	delay_cycles(1);

	// test Cd cell 3 - AN3
	output_high(PIN_B5);
	delay_ms(60);
	set_adc_channel(3);
	delay_us(10);
	c_sensor[SL]=read_adc();
	output_low(PIN_B5);
	delay_cycles(1);

	// test Cd cell D and F - AN5,AN7
	output_high(PIN_B3);
	delay_ms(60);
	set_adc_channel(5);
	delay_us(10);
	c_sensor[BR]=read_adc();
	set_adc_channel(7);
	delay_us(10);
	c_sensor[BCR]=read_adc();
	output_low(PIN_B3);
	delay_cycles(1);

	// test Cd cell E and F - AN6,AN7
	output_high(PIN_B4);
	delay_ms(60);
	set_adc_channel(6);
	delay_us(10);
	c_sensor[BL]=read_adc();
	set_adc_channel(7);
	delay_us(10);
	c_sensor[BCL]=read_adc();
	output_low(PIN_B4);
	delay_cycles(1);

} //end read_sensor



//****************************************************************************************
// usefull routines for debugging and analysis.
//****************************************************************************************

void do_balance()	//do white and then black balance
{
	int i;

	generate_tone(BEEP, DUR);	// signal start 100ms beep
	delay_ms(2000);		//wait 2 second
	sensor_read();
	printf("WHITE\n\r");
	for(i=0; i<8; i++)
	{
		white[i]=c_sensor[i];
		printf("%U \n\r", white[i]);
	}
	generate_tone(BEEP, DUR);	// 100ms beep
	delay_ms(250);		//wait 1/2 second
	generate_tone(BEEP, DUR);	// do another 100ms beep
	delay_ms(2000);		//wait 2 second

	sensor_read();
	printf("BLACK\n\r");
	for(i=0; i<8; i++)
	{
		black[i]=c_sensor[i];
		printf("%U \n\r", black[i]);
	}
	generate_tone(BEEP, DUR);	// signal done 100ms beep
	delay_ms(2000);

}


void prtbin_sensors()
{
	//debug stuff - print out binary values of sensors

	printf("      %U", bin_sensor[FL] );
	printf("   %U\n\r", bin_sensor[FR] );
	printf("  %U", bin_sensor[SL] );
	printf("           %U\n\r", bin_sensor[SR] );
	printf("    %U", bin_sensor[BL] );
	printf("  %U", bin_sensor[BCL] );
	printf(" %U", bin_sensor[BCR] );
	printf("  %U\n\r", bin_sensor[BR] );

	printf("\n\r");
	printf("\n\r");

}


void prtdec_sensors()
{
	//debug stuff - print out decimal values of sensors

	printf("      %U", c_sensor[FL] );
	printf("   %U\n\r", c_sensor[FR] );
	printf("  %U", c_sensor[SL] );
	printf("           %U\n\r", c_sensor[SR] );
	printf("    %U", c_sensor[BL] );
	printf("  %U", c_sensor[BCL] );
	printf(" %U", c_sensor[BCR] );
	printf("  %U\n\r", c_sensor[BR] );

	printf("\n\r");
	printf("\n\r");

}


//****************************************************************************************
// The robots action, and bot_step, is determined here based on the sensor data.
// 
//****************************************************************************************

void update_action()	//takes sensor data and determines next state(action)
{                       //sets the global variable 'action' and 'bot_step'.
	int i;



	//apply threshold filter 
	for(i=0; i<8; i++)
	{
		bin_sensor[i]=((c_sensor[i] >= black_limit) ? 1 : 0);

	}
	//prtdec_sensors();

	//determine action
	if( !(bin_sensor[FL]) && !(bin_sensor[FR]) )	//do both sensors see white?
	{
		if( bin_sensor[SL] || bin_sensor[SR] )	//check side sensors for black
		{
			if(bin_sensor[SL] && !(bin_sensor[SR]) )
			{
				if(bin_sensor[BL])
				{
					bot_step=2;
				}
				else
				{
					bot_step=3;
				}
				action=LFT;
			}
			else if(bin_sensor[SR] && !(bin_sensor[SL]) )
			{
				if(bin_sensor[BR])
				{
					bot_step=2;
				}
				else
				{
					bot_step=3;
				}
				action=RGHT;
			}
			else // SL and SR both see black
			{
				if(bin_sensor[BL] && !(bin_sensor[BR]) )
				{
					bot_step=2;
					action=LFT;
				}
				else if(bin_sensor[BR] && !(bin_sensor[BL]) )
				{
					bot_step=2;
					action=RGHT;
				}
				else
				{
					action=STP;
				}
			}
		}	// check back sensors for black
		else if( bin_sensor[BL] || bin_sensor[BCL] || bin_sensor[BCR] || bin_sensor[BR] )
		{
			if( bin_sensor[BL] && !(bin_sensor[BR]) )
			{
				bot_step=3;
				action=LFT;
			}
			else if( bin_sensor[BR] && !(bin_sensor[BL]) )
			{
				bot_step=3;
				action=RGHT;
			}
			else if( c_sensor[BR] > c_sensor[BL] )
			{
				bot_step=2;
				action=RGHT;
			}
			else if( c_sensor[BL] > c_sensor[BR] )
			{
				bot_step=2;
				action=LFT;
			}
		}
		else //all sensors see white
		{
			bot_step=1;
			action=REV;
		}
	}
	else if( bin_sensor[FL] && !(bin_sensor[FR]) )
	{
		if( bin_sensor[BL])
		{
			bot_step=1;
			action=FWD;
		}
		else if(bin_sensor[BR] || bin_sensor[BCR] )
		{
			bot_step=1;
			action=LFT;
		}
		else if( bin_sensor[SL] )
		{
			bot_step=1;
			action=FWD;
		}
		else  //only FL is on the black
		{
			bot_step=1;
			action=FWD;
		}
	}
	else if( bin_sensor[FR] && !(bin_sensor[FL]) )
	{
		if( bin_sensor[BR] )
		{
			bot_step=1;
			action=FWD;
		}
		else if(bin_sensor[BL] || bin_sensor[BCL])
		{
			bot_step=1;
			action=RGHT;
		}
		else if( bin_sensor[SR] )
		{
			bot_step=1;
			action=FWD;
		}
		else // only FR is on the black
		{
			bot_step=1;
			action=FWD;
		}
	}
	else	//FR and FL are both 1
	{
		bot_step=1;
		action=FWD;
	}
}



//****************************************************************************************
//State Machine routines for the five possible actions
//****************************************************************************************

void forward()
{
	int n, c_act,bstep;

	bstep = bot_step;			//record current bot_step
	c_act = action;			//record current action
	revcount = 0;			//reset reverse count

	imx_forward();			//set IMX to forward movement

	for(n=0; n<bstep; n++)
	{
		//first half of heart beat
		while(!input(HEARTBT1))	//wait for heartbeat to go high
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use sensor info to set actions and bot_step
		if(c_act != action)		//check for a change in action
		{
			return;		//if action has changed then exit
		}
		//second half of heart beat
		while(input(HEARTBT1))	//wait for heartbeat to go low
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check change in action
		{
			return;		//exit if action changed
		}
	}
}


void right()
{
	int n, c_act, bstep;

	bstep = bot_step;			//record current bot_step
	c_act = action;			//record current action
	revcount = 0;			//reset reverse count

	imx_right();				//set IMX to turn right
	for(n=0; n<bstep; n++)
	{
		//first half of heart beat
		while(!input(HEARTBT1))	//wait for heartbeat to go high
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check change in action
		{
			return;		//exit if action changed
		}
		//second half of heart beat
		while(input(HEARTBT1))	//wait for heartbeat to go low
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check change in action
		{
			return;		//exit if action changed
		}
	}
}


void left()
{
	int n, c_act, bstep;

	bstep = bot_step;			//record current bot_step
	c_act = action;			//record current action
	revcount = 0;			//reset reverse count

	imx_left();				//set IMX to turn left
	for(n=0; n<bstep; n++)
	{
		//first half of heart beat
		while(!input(HEARTBT1))	//wait for heartbeat to go high
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check for a change in action
		{
			return;		//if action has changed then exit
		}
		//second half of heart beat
		while(input(HEARTBT1))	//wait for heartbeat to go low
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check for a change in action
		{
			return;		//if action has changed then exit
		}
	}
}

void reverse()
{
	int n, c_act, bstep;

	bstep = bot_step;			//record current bot_step
	c_act = action;			//record current action

	imx_reverse();			//set IMX to reverse
	for(n=0; n<bstep; n++)
	{
		//first half of heart beat
		while(!input(HEARTBT1))	//wait for heartbeat to go high
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check for a change in action
		{
			return;		//if action has changed then exit
		}
		//second half of heart beat
		while(input(HEARTBT1))	//wait for heartbeat to go low
		{
			delay_us(5);
		}
		sensor_read();		//get sensor values
		update_action();		//use zone info to set actions and bot_step
		if(c_act != action)		//check for a change in action
		{
			return;		//if action has changed then exit
		}
	}
	revcount++;		//each natural completion of reverse count
	if(revcount==3)
	{
		revcount=0;	//reset revcount
		action=FUB;	//action = Forward Until Black
	}
}


// this one is for when it has been in reverse for awhile and is
// lost. So it goes forward until it finds a sensor that is black.
//
void f_until_b()
{
	int i;

	imx_forward();		//set IMX to forward movement
	until_black=1;

	while(until_black)
	{
		sensor_read();
		//apply threshold filter 
		for(i=0; i<8; i++)
		{
			bin_sensor[i]=((c_sensor[i] >= black_limit) ? 1 : 0);

		}
		//prtdec_sensors();

		if( bin_sensor[FL] || bin_sensor[FR])
		{
			until_black=0;
			action=STP;
		}
		else if( bin_sensor[SL] || bin_sensor[SR] )
		{
			until_black=0;
			action=STP;
		}
		else if( bin_sensor[BL] || bin_sensor[BCL] || bin_sensor[BCR] || bin_sensor[BR] )
		{
			until_black=0;
			action=STP;
		}

	}

}

void stop()
{
	motors_off();
	delay_ms(2000);
	sensor_read();
	update_action();
	motors_on();
}



void main()
{
	int n,i;

	set_tris_b(IO_SETB);		//setup default io direction
	port_b_pullups(TRUE);
	set_tris_c(IO_SETC);
	delay_cycles(1);
	set_tris_d(IO_SETD_O);	//set tactile tris bits for outputs to IMX
	all_enables_off();		//make sure everything is off in portd

	setup_adc_ports (ALL_ANALOG);  	// set analog inputs
	setup_adc (ADC_CLOCK_INTERNAL);        // sets analog clock to internal

	if( (input(DIP1)) && (input(DIP2)) )	//check dip switch settings
	{
		set_tris_d(IO_SETD_I);	//set tactile tris bits for inputs to IMX
		motors_on();		//turn motor enables on
		delay_ms(2);		//wait 2 ms for motors to settle
		while(1){}			//total BEAM mode
	}

	delay_ms(2000);		//wait 2 second
	//do_balance();		//get black and white balance
	black_limit = 220;		//preset for now
	motors_on();		//turn motor enables on
	delay_ms(2);		//wait 2 ms for motors to settle

	action=FWD;		//default action and step count
	bot_step=2;
	while(1)	// track line forever
	{
		switch(action)
		{
			case FWD:	forward();
				break;

			case REV:	reverse();
				break;

			case LFT:	left();
				break;

			case RGHT: right();
				 break;

			case STP:	stop();
				break;

			case FUB:	f_until_b();
				break;
		}

	} //end line forever


} //end main