//********************/
// Include Statements
//********************/
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <string.h>
#include <util/delay.h>

// serial communication library
#include "uart.h"
#include "uart.c"

#define begin {
#define end }

//************************* CAMERA Stuff *********************/
 
//********************/
// Port Definitions
//********************/
// PIN ASSIGNMENTS - LIST WHAT PINS YOU ARE USING
// PORTA - A.1 ADC - all A ports are set as inputs atm
// PORTB - B.2(compare), B.3(compare), B.4(cam clock)
// PORTC - Camera Port
// PORTD - D.7(Timer2 PWM)

//************************* Distance Sensor ******************/

// 20MHz clock
// Vref for ADC set to internal 2.56V
// ADC data output left adjusted
// Use ADC0 (PORTA0) for distance sensor output
// ADC uses single-end mode
// ADC clock prescaler set to 128

// program blinks led and displays ADC output onto terminal
// ADC read/convert control uses task loop

//*************************************************************/

// Port A is the ADC 

#define CAM_PORT		PORTC
#define CAM_DDR  		DDRC
#define CAM_PIN			PINC

#define TURN_PORT		PORTD

#define CAM_VOUT_PORT	PORTB // this is the compare port
#define CAM_VOUT_DDR	DDRB

//inputs
#define CAM_START_BIT 	2	// note that for PORTB, bit 2 and 3 are reserved for comparator
#define CAM_SIN_BIT	  	3
#define CAM_LOAD_BIT	4
#define CAM_RESET_BIT	5
#define CAM_XCK_BIT		6

//outputs
#define CAM_READ_BIT	7

// uses CAM_VOUT_PORT port
#define CAM_VOUT_BIT	2

#define TURN_IN1_BIT	3
#define TURN_IN2_BIT	4
#define TURN_SB_BIT		5


//macros
#define cbi(a,b)	a = ( a & ~(1<<b) )	//clear bit at b in a
#define sbi(a,b)	a = ( a | (1<<b) )	//set bit at b in a
#define check(a,b)	((a & (1<<b)) >> b) //see if b is set in a
#define absolute(a,b)	a = (b>0) ? b : -b;		


//last Y line to read before starting algorithm
#define lastLine		36

//DC Motor PWM driver values
#define normalDrive 48
#define jumpDrive 80
#define numJumps 1
#define stopDrive 0

//Method Declarations
void init(void);
void initUART(void);
void camResetAR(void);
void camStartAR(void);
void camReset(void);	
void setPixel(unsigned char, unsigned char, unsigned char);
char getPixel(unsigned char, unsigned char);

//********************/
// Variables
//********************/

volatile char XCK;
volatile char halfXCK;
volatile char counter;
volatile char dDelay;

//128 x 123 picture array - 1 bit per pixel
char picture[1968];

// UART file descriptor
// putchar and getchar are in uart.c
FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);

//cmd line variables
char cmd[4];
char currPix;

// array for registers

unsigned char darkLabBench[8]= {0x80, 0x15, 0x00, 0xf0, 0x01, 0x00, 0x01, 0x22};

unsigned char whiteFloor[8]= {0x80, 0x15, 0x00, 0x30, 0x01, 0x00, 0x01, 0x22};

unsigned char posE[8]= {0x80, 0x15, 0x00, 0x63, 0x01, 0x00, 0x01, 0x22};

//Distance Sensor Code

//init variables
unsigned char haltFlag;		//flag that notifies image processing code that car has stopped due to distance sensor
unsigned char jumpStartCount;

//********************/
// ISR
//********************/

ISR(TIMER0_COMPA_vect){
	
	if(counter==0){
		halfXCK = halfXCK^0x1; // shifted camera clock .5MHz
		counter++;

	}
	else { 
		dDelay++;

		XCK = XCK^0x1; // camera clock .5MHz
		
		cbi(CAM_PORT, CAM_XCK_BIT);
		CAM_PORT = CAM_PORT | (XCK << CAM_XCK_BIT);
		
		counter=0;
	}
}
//*/


//***************************************************/
// Camera Functions
//**************************************************/

//******* Reset***********//
// Reset is read at the posedge of XCK
#define wait0	0
#define	wait1	1
#define low		2
#define done_high 3

void camReset(void) {
	char resetState;
	
	if(XCK==1){
		resetState = wait0; // wait for XCK to be 0
		//fprintf(stdout,"reset wait0\n\r");
	}
	else if(halfXCK==0){
		resetState = wait1; // wait for halfXCK to be 1
		//fprintf(stdout,"reset wait1\n\r");
	}
	else{ // XCK is 0 and halfXCK is 1. Reset goes low
		CAM_PORT = CAM_PORT & ~(1<<CAM_RESET_BIT);
		resetState = low;
		//fprintf(stdout,"reset low\n\r");
	}
	
	while(resetState!= done_high){
	
		switch(resetState)
		{
			case wait0:
				if(XCK==0) {
					resetState=wait1;
					//fprintf(stdout,"reset wait1\n\r");
				}
			break;
			
			case wait1:
				if(halfXCK==1) {
					// set reset signals low at posedge of halfXCK
					CAM_PORT = CAM_PORT & ~(1<<CAM_RESET_BIT);
					resetState=low;
					//fprintf(stdout,"reset low camport = %x\n\r",CAM_PIN);

				}
			break;
			
			case low:
				if(halfXCK==0) {
					// set reset high at negedge of halfXCK
					 //fprintf(stdout,"reset done high camport = %x\n\r",CAM_PIN);
					 CAM_PORT = CAM_PORT | (1<<CAM_RESET_BIT);
					 resetState=done_high;
				}
			break;
			
			case done_high:
			break;			
		}
	}
}

//********* Set Registers ******************************//
// 2. Set Registers 
// SIN is read at posedge of XCK and read is read at negedge of XCK
// SIN is updated at negedge of XCK and read is 1 at negedge of halfXCK and 0 at posedge of halfXCK

#define Reg_waitXCK0			0
#define Reg_waitXCK1			1
#define Reg_waithalf1			2
#define Reg_waithalf0			3
#define Reg_waithalf0_done		4
#define Reg_done				5

void setCamReg(unsigned char regVal[8]){
	unsigned char indCount;
	unsigned char regCount;
	unsigned char regState;
	indCount = 0;
	regCount = 0;
	
	if(XCK == 0){
		regState = Reg_waitXCK0;
	}
	else{
		regState = Reg_waitXCK1;
	}
	
	while(regState!=Reg_done){
		switch(regState)
		{
			case Reg_waitXCK0:
				if (XCK==1){ // posedge of XCK
					if(indCount==11){ // indCount is incremented too far. it means that we have to assert load.
						regState = Reg_waithalf1; // this state waits for the negedge of halfXCK
					}else{
						regState = Reg_waitXCK1; // this stage waits for the negedge of XCK
					}
				 }
			break;
			
			case Reg_waitXCK1:
				
				if (XCK==0) { // neg edge of XCK
					if(indCount==0 && regCount>0){
					// this condition exist to have the correct timing to wait for load to be 0
						// change SIN
						if((0x1<<2) & regCount) sbi(CAM_PORT,CAM_SIN_BIT);
						else cbi(CAM_PORT,CAM_SIN_BIT);
						
						indCount++;
						regState = Reg_waithalf0;
					
					} else{
					
						if(indCount<=2){
							if((0x1<<(2-indCount)) & regCount) sbi(CAM_PORT,CAM_SIN_BIT);
							else cbi(CAM_PORT,CAM_SIN_BIT);
						}
						else{ // (indCount>2)
							if((0x1<<(7-indCount+3)) & regVal[regCount]) sbi(CAM_PORT,CAM_SIN_BIT);
							else cbi(CAM_PORT,CAM_SIN_BIT);
						}
						
						indCount++;
						regState = Reg_waitXCK0;
						
					}
				}
			break;
			
			case Reg_waithalf0:
				
				if(halfXCK==1) { // posedge of halfXCK
					cbi(CAM_PORT,CAM_LOAD_BIT);
					regState = Reg_waitXCK0;
				}
			break;
			
			case Reg_waithalf1:
				if(halfXCK==0){ // negedge of halfXCK
				
					if(regCount==7){
						// ************************load to 0
						sbi(CAM_PORT,CAM_LOAD_BIT);
						cbi(CAM_PORT,CAM_SIN_BIT);
						regState = Reg_waithalf0_done;
						//fprintf(stdout,"Reg_done\n\r");
					}
					else {
						regCount++;
						indCount=0;
						// ****************************load to 0
						sbi(CAM_PORT,CAM_LOAD_BIT);
						regState = Reg_waitXCK1;
						//fprintf(stdout,"Reg_wait0: next reg\n\r");
					}
				}
			break;
			
			case Reg_waithalf0_done:
				if(halfXCK==1){
					cbi(CAM_PORT,CAM_LOAD_BIT);
					regState = Reg_done;
				}
			break;
			
			case Reg_done:
				//fprintf(stdout,"Reg_done\n\r");
			break;
		}
		
	}
}

//************ Store and get Pixel *******************************************//
//store a pixel in our picture array
//ASSUMING read goes low on x,y pixel 127,127 - MAY NEED TO DBL CHECK
//data ASSUMED to to 1 or 0 ONLY
//NEED TO MINIMIZE THIS TO AROUND 40 INSTRUCTIONS - CAN WE DO ARRAY LOOKUPS??????
void storePixel(unsigned char data, unsigned char x, unsigned char y){
	unsigned char aByte;
	int aPtr;
	
	//128 pixels in every row. 16 entries per row
	aPtr = ((int)y << 4) + ((int)x >> 3); //16*y + x/8
	aByte = picture[aPtr];
	
	cbi(aByte, (x & 0x03)); //(x & 0x03) -> x mod 8
	aByte = (aByte | (data << (x & 0x03)));// 8 pixels per byte loc in array
	picture[aPtr] = aByte;
}

//return the pixel value stored in picture array
char getPixel(unsigned char x, unsigned char y){
	unsigned char aByte;
	int aPtr;
	
	aPtr = ((int)y << 4) + ((int)x >> 3);
	aByte = picture[aPtr];

	//mask the byte based on x mod 8, then shift result to 1's place
	return (aByte & (1 << (x & 0x03)) ) >> (x & 0x03); 
}

//************************* Read Data ***********************************//
// 4. Read Data
// PICTURE is stored in a local array
#define read_waitread1	0
#define read_waitread0	1
#define read_waitXCK1	2
#define read_waitXCK0	3
#define read_done		4
	

void camRead(void){
	char readState;
	unsigned char xCount;
	unsigned char yCount;

	xCount = 0; // x is spot in the row
	yCount = 0; // y is the row number

	cbi(PORTD, PORTD2);
	
	if(CAM_PIN&(1<<CAM_READ_BIT)){ // read = 1 
		readState = read_waitread1; // wait for read to go low
	} else { // read = 0
		readState = read_waitread0;	// wait for posedge of read
	}

	while(readState != read_done){
		switch(readState){
			case read_waitread1:
				if((CAM_PIN & (0x1<<CAM_READ_BIT))==0){ //read = 0 
					readState = read_waitread0;
				}
			break;
				
			case read_waitread0:
				if(CAM_PIN & (0x1<<CAM_READ_BIT)) { // read = 1 
					readState = read_waitXCK1;
				}
			break;
			
			case read_waitXCK1:	
				if(XCK==0){ // read here
					storePixel((ACSR & (0x1<<ACO)) >> ACO ,xCount,yCount);
					
					if(xCount==127){
						xCount =0;
						yCount++;
					} else{
						xCount++;
					}
					readState = read_waitXCK0;
				}
			break;
			
			case read_waitXCK0:
				
				if (xCount==127 && yCount==lastLine){ // this doesn't need to wait for XCK
					readState = read_done;
					sbi(PORTD, PORTD2);
				} else if(XCK==1) {
					readState = read_waitXCK1;
				}
			break;
			
			case read_done:		
			break;
		}
	}
}

//************************** START **************************//
// 3. Initiate Start

#define start_wait0	0
#define	start_wait1	1
#define start_high	2
#define start_done_low 3

void initStart(void) {
	char startState;
	
	if(XCK==1){ // need to wait for XCK to be 0
		startState = start_wait0;
	}
	else if(halfXCK==0){ // need to wait for posedge
		startState = start_wait1;
	}
	else{ // XCK is 0 and halfXCK is 1
		sbi(CAM_PORT,CAM_START_BIT);
		startState = start_high;
	}
	
	while(startState!= start_done_low){
	
		switch(startState)
		{
			case start_wait0:
				if(XCK==0) {
					startState= start_wait1;
				}
			break;
			
			case start_wait1:
				if(halfXCK==1) {
					// set start signal high
					sbi(CAM_PORT,CAM_START_BIT);
					startState=start_high;
				}
			break;
			
			case start_high:
				if(halfXCK==0) {
					 cbi(CAM_PORT,CAM_START_BIT);
					 startState=start_done_low;
					// immediately read from Camera after start is done. This could be changed if our camera is working correctly
					 camRead();
				}
			break;
			
			case start_done_low:
				
			break;			
		}
	}
}
//***********************************************************************//
// End of Camera Functions
//**********************************************************************//


//***********************************************************************//
// Algorithm
//**********************************************************************//

// find left lane
// 1. initialize left lane center at 0 <- left edge of line
// 2. start from left side of line, search through pixels
// 3. when we detect a 0 (possible lane), increment a tracker. Once we have
//	  3 consecutive 0's, set the left lane marker (LM) to 1. For every 1
//	  detected between consecutive 0's, decrement the tracker by 2.
//	  tracker always between 0-3
// 4. Outside lane detected, mark location of trigger as "outside edge".
//	  Offset will be ok if right lane is working under same algo.
// 5. Look for inside lane - basically look for 3 consecutive 1's. Decrement
//	  tracker by 1 when a 1 is seen, increment by 1 when a 0 is seen. When tracker
//    equals 0, set LM to 1, set done to 1 to stop loop, and mark loc as "inside edge"
// 6. Find center of left lane based on "outside edge" and "inside edge"
// similar steps for right lane

//MAY WANT TO DO THIS FOR MULTIPLE LINES FOR BETTER ACCURACY
// Find center based on centers of left and right lanes. Compare curr center with curr center
// of the car. If the difference is great enough (<4 pixels off), turn left or right.   

#define trackerMax	3

//Method returns the "center" of the left lane given a specific line number
int leftLaneLoc(int lineOffset){
	int leftOutEdge = 0;
	int leftInsideEdge = 0;
	char LM = 0;
	char tracker = 0;
	char done = 0;
	char currBit;

	//left lane 
	//start from left side of line, try to find a line
	//if no line found, center left lane at left side of line
	
	int i= 0;	//pixel index
	while (done==0 && i<128){		
		currBit = getPixel(i,lineOffset);

		//tracker counts number of consecutive 0's - up to 3
		if (currBit==0) { // 0 detected
			tracker = (tracker < trackerMax)? tracker+1 : trackerMax;
		} else { // 1 detected - decrement tracker value
			tracker = (tracker > 0)? tracker-1 : 0;
		}
			
		if (LM==0 && tracker == trackerMax){ //outside edge detected
			LM = 1;
			leftOutEdge =  i;
			//fprintf(stdout,"leftOutEdge = %d\n\r",leftOutEdge);
		}
		if (LM==1 && (tracker == 0 || i == 127)){ //inside edge detected
			done = 1;
			leftInsideEdge =  i;
			//fprintf(stdout,"leftInsideEdge = %d\n\r",leftInsideEdge);			
		}
		i++;
	}
	//By now - either found the inside and outside edges on the lane
	//or didn't find a lane at all

	//fprintf(stdout,"leftInsideEdge = %d, leftOutEdge = %d\n\r", leftInsideEdge,leftOutEdge);
	return ( leftInsideEdge + leftOutEdge ) >> 1; 
}

//Method returns the "center" of the right lane given a specific line number
int rightLaneLoc(int lineOffset){
	int rightOutEdge = 127;
	int rightInsideEdge = 127;
	char RM = 0;
	char tracker = 0;
	char done=0;
	char currBit;

	//right lane 
	//LOOK for outside edge first - REVERSE of LEFT LANE METHOD
	int i = 127; //pixel index
	while (done==0 && i>=0){
		currBit = getPixel(i,lineOffset);
			
		//tracker counts number of consecutive 0's - up to 3
		if (currBit==0) { // 0 detected
			tracker = (tracker < trackerMax)? tracker+1 : trackerMax;
		} else { // 1 detected - decrement tracker value
			tracker = (tracker > 0)? tracker-1 : 0;
		}
			
		if (RM==0 && tracker == trackerMax){ //outside edge detected
			RM = 1;
			rightOutEdge = i;
		}
		if (RM==1 && (tracker == 0 || i == 0)){ //inside edge detected
			done = 1;
			rightInsideEdge = i;
		}
		i--;
	}

//fprintf(stdout,"inside = %d outside = %d\n\r", rightInsideEdge, rightOutEdge);
	//By now - either found the inside and outside edges on the lane
	//or didn't find a lane at all
	return ( rightInsideEdge + rightOutEdge ) >> 1; //average 
}


// algorithm to determine proper turning conditions
// simple version - take 2 lines
// requirements: line_1_s < line_2_s < line_3_s and they are relative close together

#define line_1_s			15
#define line_2_s			25
#define line_3_s			35
#define min_pix_distance_s	35


#define carCenter		63 	// 
#define turnLTH			10  // L and R thresholds. These need to be adjusted accordingly. they should be smaller if we are detecting a further line
#define turnRTH			20

#define invalid			0	// 000
#define turnL			1	// 001
#define turnR			2	// 010
#define goStraight		4	// 100

char turningAlgoSimple(){
	int line1_L = leftLaneLoc(line_1_s);
	int line1_R = rightLaneLoc(line_1_s);
	int line2_L = leftLaneLoc(line_2_s);
	int line2_R = rightLaneLoc(line_2_s);
	int line3_L = leftLaneLoc(line_3_s);
	int line3_R = rightLaneLoc(line_3_s);	
	int line1_C;

	int dist3_L, dist3_R;

	absolute(dist3_L, (line1_L - line3_L));
	absolute(dist3_R, (line1_R - line3_R));

//fprintf(stdout, "line1_L = %d  line1_R = %d\n\r", line1_L,line1_R);
		
	// find center of line 1, if 2 lanes present
	// center accordingly and figure out proper turn
	//
	// if 1 lane detected, interpolate behavior from
	// other sample lines - basically, need 2 more lines
	// 		1)sample line3, if it only has 1 lane, find
	//		  the vector direction from 3->1 and act
	//		  accordingly. If 2 lanes, goto step 2
	//		2)sample line2, if a single lane, find the
	//		  the vector direction from 2->1 and act
	//		  accordingly. If 2 lanes, the info from all
	//		  3 lines is enough to interpolate the lane's
	//		  behavior
	
	if ((line1_R-line1_L) >= min_pix_distance_s ){ //2 distinct lanes for line1
		line1_C = (line1_R+line1_L) >> 1;

//fprintf(stdout,"Line1 2 Lanes\n\r");

	} else { //1 lane for line1 - follow the above algorithm
	
//fprintf(stdout, "Line1 1 Lane\n\r");		

		//sample line3
		if (((line3_R-line3_L) >= min_pix_distance_s )){ //2 lanes for line 3
			
			//sample line2
			if (((line2_R-line2_L) >= min_pix_distance_s )){ //2 lanes for line 2
				//check if we've detected the left lane
				
				//if it could be either right lane or left lane, check which lane line 1's lane is closest to
				if ((line1_L >= line2_L) && (line2_L >= line3_L) && (line1_R <= line2_R) && (line2_R <= line3_R)) {
					if (dist3_L < dist3_R) {
						line1_R = 127;
					} else {
						line1_L = 0;
					}			
				}
				else if ((line1_L >= line2_L) && (line2_L >= line3_L)) {
					line1_R = 127;
				
				} else if ((line1_R <= line2_R) && (line2_R <= line3_R)){ //check right lane
					line1_L = 0;
				
				} else { //ERROR - TURN LED ON		// this case can exist, but if our algorithm is correct, it should never occur
				//	PORTD = PORTD & ~(1<<PORTD2);
					//fprintf(stdout, "ERROR - ALGO WRONG");
					return invalid;
				}
				
			} else { //1 lane for line 1, 1 lane for line 2, 2 lanes for line 3 - do instruction (2)
				//check if we've detected the left lane
				if (line1_L >= line3_L){
					line1_R = 127; 
				} else if (line1_R <= line3_R) { //check right lane
					line1_L = 0; 
				} else { //ERROR - TURN LED ON
				//	PORTD = PORTD & ~(1<<PORTD2);
					//fprintf(stdout, "ERROR - ALGO WRONG");
					return invalid;
				}			
			}	
			
		} else { // 1 lane for line 3, 1 lane for line 1 - do instruction (1)
			//check if we've detected the left lane
			if (line1_L >= line3_L){
				line1_R = 127; //set right lane to edge of pic			
			} else if (line1_R <= line3_R) { //check right lane
				line1_L = 0; //set left lane to edge
			} else { //ERROR - TURN LED ON
				//PORTD = PORTD & ~(1<<PORTD2);
				//fprintf(stdout, "ERROR - ALGO WRONG");
				return invalid;
			}
		}
		
		//line1_L and line1_R are set to their proper values now
		line1_C = (line1_R+line1_L) >> 1;
	}

	//Center for Line 1 calculated correctly - figure out the turning conditions

	if (line1_C < carCenter-turnLTH){// turn left
		return turnL;
	}
	else if(line1_C > carCenter+turnRTH){// turn right
		return turnR;
	} else{// go straight
		return goStraight;
	}
}

// determines turning
void turningTask(void){
		char turnData;
		
		// lane centering algorithm
		camRead();
		turnData = turningAlgoSimple();
		
		// determine turn
		if(turnData == turnL){
			// turn wheels left
			cbi(TURN_PORT,TURN_IN1_BIT);
			sbi(TURN_PORT,TURN_IN2_BIT);

			//PORTD = PORTD ^ 1<<PORTD2;
//			fprintf(stdout,"turn left\n\r");
		} else if (turnData == turnR){
			// turn wheels right
			cbi(TURN_PORT,TURN_IN2_BIT);
			sbi(TURN_PORT,TURN_IN1_BIT);

			//PORTD = PORTD ^ 1<<PORTD2;
//			fprintf(stdout,"turn right\n\r");
		} else if (turnData == goStraight){
			// go straight
			cbi(TURN_PORT,TURN_IN1_BIT);
			cbi(TURN_PORT,TURN_IN2_BIT);

			//PORTD = PORTD ^ 1<<PORTD2;
//			fprintf(stdout,"go Straight\n\r");
		} else {
			// invalid data
			// stop the car for now. 
			//cbi(TURN_PORT,TURN_IN1_BIT);
			//cbi(TURN_PORT,TURN_IN2_BIT);
//			fprintf(stdout,"invalid data\n\r");
		}

}


//*********************************************//
// Adjust Lighting - take out this section later.
//********************************************//


// used to change lighting settings
int blackCount(int lineNum){ // counts up the number of black pixels in a line
	int bkcount = 0;
	for (unsigned char i=0; i<128; i++){
		if(getPixel(i,(unsigned char) lineNum)==0){ // 0 is black. 
			bkcount++;
		}
	}
	return bkcount;
}

void adjustLighting(unsigned char regArray[8]){
	int bkcount;

	bkcount = blackCount(30);
	if(bkcount>32){ // too dark. need to increase exposure time
		if(regArray[3] > 0xf0){
			regArray[3] = 0xff;
			// need to change regArray[2]. something I don't want to do and a case that might not happen.
		} else if(regArray[3] == 0x80){
			regArray[3] = 0xf0;
		} else if (regArray[3] ==0x00){
			regArray[3] = 0x30;

		} else{
			regArray[3] = regArray[3] + 0x10;
		}
		setCamReg(regArray);
		//	fprintf(stdout,"lighting: too dark \n\r");
	} else if(bkcount<22){ // too bright. need to decrease exposure time
		if(regArray[3]>0x01){
			regArray[3] = regArray[3] - 0x10;
			setCamReg(regArray);
		}
		//	fprintf(stdout,"lighting: too bright \n\r");
	} else {
		//	fprintf(stdout,"lighting is fine\n\r");
	}
		
}



//***************************************//
// distance sensor
//***************************************//

//distance sensor and PWM speed control
void speedTask(void){

	//start another conversion
	ADCSRA |= (1<<ADSC);

	//give time for conversion to finish before reading it
//	_delay_us(90);

	dDelay = 0;
	while(1){ //wait 4 XCK cycles
		if(dDelay == 7) break;
	}

	//stop car if too close to an object
	if(ADCH>60) begin
		OCR2A = stopDrive;
		haltFlag = 1;
	//	jumpStartCount = 0;
	end
/*	else if(jumpStartCount > 0) begin
		OCR2A = jumpDrive;
		haltFlag = 0;
		jumpStartCount--;
	end
*/	else if(OCR2A<10) begin
		OCR2A = jumpDrive;
		haltFlag = 0;
	//	jumpStartCount = numJumps;
	end
	else begin
		OCR2A = normalDrive;
		haltFlag = 0;
	end

	// print conversions onto terminal
	//printf("%u\n\r", ADCH);

}

//***************************************//
// Testing
//***************************************//
//testing terminal
void puttyTestTask(void){
	fprintf(stdout, ">") ;
	fscanf(stdin, "%s", cmd) ;

	if (cmd[0] == 'p') {
		camRead();
	//	PORTD = PORTD ^ 1<<PORTD2; // LED TOGGLE
	}
	//should make a 128 by 123 char representation
	// of the picture on the putty output
	if (cmd[0] == 'd') {
		fprintf(stdout, "\n\r");
		for(unsigned char y=0; y<lastLine; y++){
			for(unsigned char x=0; x<128;x++){
				currPix = getPixel(x,y); //1 or 0

				if (currPix){
					fprintf(stdout,"X");						
				} else {
					fprintf(stdout,"_");						
				}

			}
			
			fprintf(stdout,"\n\r");
		}
	}
	// detect lanes and determine turning
	if(cmd[0] == 'c'){ 
		turningTask();
		fprintf(stdout,"\n\r");
	}
	// adjust lighting
	if(cmd[0] == 'a'){
		camRead();
		adjustLighting(posE);
		fprintf(stdout,"exposure time = %x\n\r",posE[3]);
	}
	// display current lighting settings
	if(cmd[0] == 't'){
		fprintf(stdout,"exposure time = %x\n\r",posE[3]);
	}
}

//**********************************************************/
// Main
//***********************************************************/
int main(void){
	init();
	camReset();
	
	//PORTD = PORTD ^ 1<<PORTD2; // LED TOGGLE
	setCamReg(posE);
	//PORTD = PORTD ^ 1<<PORTD2; // LED TOGGLE
	initStart();

/*	for(int i =0; i<6; i++){
		camRead();
		adjustLighting(posE);
		setCamReg(posE);
	}
*/

	//OCR2A = 48;//FOR TESTING

	while(1){


		speedTask();

		if(!haltFlag){
			turningTask();
		} else {
			//straighten wheels if halted	
			cbi(TURN_PORT,TURN_IN1_BIT);
			cbi(TURN_PORT,TURN_IN2_BIT);
		}

		
//puttyTestTask();

	}


} //end main


//****************************************************/
// Init
//******************************************************/

void initUART(void){
	//init the UART -- trt_uart_init() is in trtUart.c
	uart_init();
	stdout = stdin = stderr = &uart_str;
	fprintf(stdout,"\n\r Starting \n\r\n\r");
}

void init(void){
	// sets up the timers
	counter = 0;
	XCK = 0;
	halfXCK = 0;
	
	// pins driven as OUTPUTS, all except INPUT to MCU -> READ pin
	CAM_DDR = 0xff & ~(0x01<<CAM_READ_BIT);
	
	// set reset signals high
	CAM_PORT = 0x00;
	sbi(CAM_PORT, CAM_RESET_BIT); //Reset bit active low
	sbi(CAM_PORT, CAM_READ_BIT);  //enable pull up resistor
		
	// change this if we change port B
	CAM_VOUT_DDR = 0x01; // PORTB is all inputs for now. we know for certain that B.2 and B.3 have to be inputs
	CAM_VOUT_PORT = 0xff;

	//set up timer 0 for 0.5 us ticks -- 20 MHz CLOCK
	TIMSK0 = 0x02;		// turn on timer 0 cmp match ISR 
	OCR0A = 199;//124;  	 	// set the compare reg to 10 time ticks
	TCCR0A = 0x02; 		// turn on clear-on-match
	TCCR0B = 0x01;		// clock prescaled by 8		

	//initialize picture array to 0 values
	for (int i=0; i < 2048; i++){
		picture[i] = 0;
	}
	
	//************* DISTANCE SENSOR INITIALIZE*****************
	
	//set up PWM and HBridge output ports
  	DDRD = 0b11111100;	// PORT D.3 through D.7 are outputs

	//initialize Hbridge control ports
	//PORTD3 = IN1	: Right turn if this high and IN2 low.
	//PORTD4 = IN2	: Left turn if this high and IN1 low.
	//PORTD5 = SB
	//PORTD6 = PWM
	PORTD = 0b01100100;	//set PWM and standby(SB) high

	//set up PORTA as input
	DDRA = 0;
	
	//set up timer 2 for fast PWM
	//TCCR2B = (1<<CS22) | (1<<CS20);
	TCCR2B = 5;						//clock prescaler 128, with 20MHz crystal this means a 610.35Hz PWM
	OCR2A = stopDrive; 
	//TCCR2A = (1<<COM2A1) | (1<<WGM21) | (1<<WGM20);
	TCCR2A = 0b10000011;		//non-inverting mode, TOP set to 0xff
	
	//set up the ADC
	//ADMUX = (1<<REFS1) | (1<<REFS0) | (ADLAR);
	ADMUX = 0b11100001;		//bits 7:6 set for internal 2.56V Vref; bit 5 set for left adjusted data output
							//A0 BUSTED - USING A1
	//ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);
	ADCSRA = 0b10000111;	//bit 7 set to turn on ADC; bits 2:0 all set for prescaler of 128	
	
	//start a conversion
	ADCSRA |= (1<<ADSC);

	
	//************* DISTANCE SENSOR INITIALIZE END*************

	//setup timer 1 for testing
	TCCR1B = 0b00000001;

	//initialize variables
	haltFlag = 0;
	jumpStartCount = 0;
	dDelay = 0;


	sei();
	initUART();
}