#include <system.h>
#include <boostc.h>

#pragma CLOCK_FREQ 8000000

//#pragma DATA 0x2007, _HS_OSC & _CP_OFF & _WRT_OFF & _CPD_OFF & _LVP_ON & _BODEN_ON & _PWRTE_ON & _WDT_OFF

#pragma DATA 0x2007, _CP_OFF & _WDT_OFF & _BODEN_OFF & _PWRTE_ON & _HS_OSC & _LVP_OFF & _CPD_OFF

//Define the analog inputs from the sensors
#define	left_eye	_porta,0
#define	rght_eye	_porta,1
//#define			_porta,2
//#define			_porta,3
//#define			_porta,4
//#define			_porta,5

//Define control outputs for right motors
#define	motdir0a	_portb,0
#define	motdir0b	_portb,1
#define	motdir1a	_portb,2
#define	motdir1b	_portb,3
//#define			_portb,4
//#define			_portb,5
//#define			_portb,6
//#define			_portb,7


#define	FR_GREEN	0x0001	//1
#define	FR_BLUE		0x0002	//2
#define	FR_RED		0x0004	//3
#define	BLUE0		0x0008	//4
#define	BLUE1		0x0010	//5
#define	BLUE2		0x0020	//6
#define	BLUE3		0x0040	//7
#define	FL_RED		0x0080	//8
#define	FL_GREEN	0x0100	//9
#define	FL_BLUE		0x0200	//10
#define	BL_RED		0x0400	//11
#define	BL_GREEN	0x0800	//12
#define	BL_BLUE		0x1000	//13
#define	BR_GREEN	0x2000	//14
#define	BR_BLUE		0x4000	//15
#define	BR_RED		0x8000	//16


//Define the bits in the command_byte
#define	mot0dir		7
#define mot1dir		6

#define EYE_THRESHOLD 60

#define LEFT_TURN	0b10000000
#define RIGHT_TURN	0b01000000
#define BACKWARD	0b00000000
#define FORWARD		0b11000000


//Define some values for sampling different A2D channels
#define chan0	0b10000001		//select channel AN0 on PA0
#define chan1	0b10001001		//select channel AN1 on PA1
#define chan2	0b10010001		//select channel AN2 on PA2
#define chan3	0b10011001		//select channel AN3 on PA3
#define chan4	0b10100001		//select channel AN4 on PA5

typedef enum{
	ATTACK,
	SURVIVE,
	SEARCH,
	RUN,
	LINE,
	RIGHT,
	LEFT,
	OFF
} states;


//These are the global vars that would normaly go in the cblock
char counter = 0;
char temp = 0;

char command_byte = 0;		//holds motor speeds and directions
char next_command = 0;
char motorcount = 0;
char mot0speed = 0;
char mot1speed = 0;			//cycle that is being pushed out on

unsigned char currentState = SEARCH;
unsigned char lastState = OFF;
unsigned char botLastSeenOn = OFF;

unsigned char left_eye_val = 0;
unsigned char right_eye_val = 0;
unsigned char left_line_val = 0;
unsigned char right_line_val = 0;

unsigned short average = 0;

#define MAX_SAMPLES			8	
#define MAX_LINE_SAMPLES	4

unsigned char sample = 0;
unsigned char leftArray[MAX_SAMPLES];
unsigned char rightArray[MAX_SAMPLES];
unsigned char leftLineArray[MAX_SAMPLES];
unsigned char rightLineArray[MAX_SAMPLES];
	
void interrupt ( void )
{
	clear_bit( intcon, GIE );  	//Disable interrupts
	motorcount++;
	if( motorcount >= 10 )
	{
		motorcount = 0;
	}		
	
	//SetPWM on motor0
	if( test_bit( command_byte, mot0dir ) )
	{	
		//Forward on motor0
		clear_bit( portb,0 );
		clear_bit( portb,1 );	
		
		if( mot0speed > motorcount )
		{
			set_bit( portb,0 );	
		}		
	}
	else
	{
		//Backward on motor0	
		clear_bit( portb,0 );
		clear_bit( portb,1 );	

		if( mot0speed > motorcount )
		{
			set_bit( portb,1 );	
		}
	}
	
	//SetPWM on motor1	
	if( test_bit( command_byte, mot1dir ) )
	{
		//Forward on motor1
		clear_bit( portb,2 );
		clear_bit( portb,3 );	
		
		if( mot1speed > motorcount )
		{
			set_bit( portb,2 );	
		}		
	}
	else
	{
		//Backward on motor1	
		clear_bit( portb,2 );
		clear_bit( portb,3 );	

		if( mot1speed > motorcount )
		{
			set_bit( portb,3 );	
		}
	}	
	clear_bit( intcon, 2 );		//Clear the interup flag
    set_bit( intcon, GIE );     	//Enable interrupts
}

void init ( void )
{
	clear_bit(status, RP0);	//Goto bank	0	
	porta = 0b00000000;		//PORTA 0x00
	portb = 0b00000000;		//PORTB 0x00
	adcon0 = 0b01000001;	//Setup A2D on all portA pins and set osc. bits
	
	set_bit(status, RP0);	//Goto bank 1	
	trisa = 0b00011111;		//PORTA is all inputs;	
	trisb = 0b11110000;		//PORTB(0,1,2,3) outputs (4,5,6,7) inputs;
	adcon1 = 0b00001001;	//Setup A2D right justified
	option_reg = 0b10000001;//Set timer prescaler to 1/4

	clear_bit(status, RP0);	//Goto bank	0

	set_bit(intcon, 5);		//Enable the timer interupt
	clear_bit(intcon, 2);	//Clear the interup flag
	set_bit(intcon, GIE);	//Enable interrupts

}	
	
	
void main()
{
	init();
	char i = 0;
	short ledVal = 0;
	
	//clear the motor control bits
	command_byte = FORWARD;
	motorcount = 0x00;
	mot0speed = 0x00;
	mot1speed = 0x00;

	
	for( i = 0; i < 5; i++ )
	{	
		delay_ms( 250 );
		delay_ms( 250 );
		delay_ms( 250 );
		delay_ms( 250 );
		ledVal <<= 1;
//		setLeds( ledVal );		
	}
	
	while(1)
	{

	}	
}