/*Collection of starting functions*/

/*global variables*/
/*temporarily set for debugging*/
int th4=91; /*threshold for IR --another idea-3 threshold values, one for each sensor*/
int th5=125;
int th6=119;

/*global variable that stores the robot's ID (1 or 0) for the match, gets set by get_bot_num(), doesn't change throughout match */
int botID;


/*keep track of IR sensor ports! 4-analog(16)-front, 5-back left analog(5), 6-back right analog(6) */ 

/*Orientation function*/

/*function to determine orientation based on IR sensors */
/*returns '0', '1', '2' or '3' */

int orient() {	
	int f4, b5, b6, trash;  /*black/white IR sensors, threshold value */
	trash=analog(16); /*first flush buffer*/
	trash=analog(5);
	trash=analog(6);

	f4= analog(16);
	b5= analog(5);
	b6= analog(6);
	
		/* case '0'   4-w 5-w 6-w or 4-b 5-b 6-w */
	if ( (f4<th4 && b5<th5 && b6<th6)  || (f4>th4 && b5>th5 && b6<th6)) {
		/*printf("0! (%d) (%d) (%d)\n", f4 , b5, b6);*/
		/*start_press();*/
		return 0;

	}
		/* case '1'   4-b 5-w 6-w or 4-b 5-b 6-b */
	else if ( (f4>th4 && b5<th5 && b6<th6)  || (f4>th4 && b5>th5 && b6>th6)) {
		/*printf("1! (%d) (%d) (%d)\n", f4 , b5, b6);*/
		/*start_press();*/
		return 1;
	}

		/* case '2'   4-w 5-b 6-w or 4-w 5-b 6-b */
	else if ( (f4<th4 && b5>th5 && b6<th6)  || (f4<th4 && b5>th5 && b6>th6)) {
		/*printf("2! (%d) (%d) (%d)\n", f4 , b5, b6);*/
		/*start_press();*/
		return 2;
	}
		/* case '3'   4-w 5-w 6-b or 4-b 5-w 6-b */ 
	else if ( (f4<th4 && b5<th5 && b6>th6)  || (f4>th4 && b5<th5 && b6>th6)) {
		/*printf("3! (%d) (%d) (%d)\n", f4 , b5, b6);*/
		/*start_press();*/
		return 3;
	}

	else {
		printf("Error\n");
		/*start_press();*/
	}

	
	/*if something else, wait, try again*/

	/*if timed out...??*/

}


/*return character 'g' or 'r'*/
char face_green_or_red(int ori) { 
	int f4, b5, b6, trash;  /*black/white IR sensors, threshold value */
	trash=analog(16); /*first flush buffer*/
	trash=analog(5);
	trash=analog(6);

	f4= analog(16);
	b5= analog(5);
	b6= analog(6);
	/* if orientation is 2, and 4-w 5-b 6-b return g */
	if (ori == '2' && f4<th4 && b5>th5 && b6>th6) {
		printf("face green\n");
		start_press();
		return 'g';
	}
	/* if orientation is 2, and 4-w 5-b 6-w return r*/
	else if (ori == '2' && f4<th4 && b5>th5 && b6<th6) {
		printf("face red\n");
		start_press();
		return 'r';
	}
	/* if orientation is 0, and 4-b 5-b 6-w return r*/
	else if (ori == '0' && f4>th4 && b5>th5 && b6<th6) {
		printf("face red\n");
		start_press();
		return 'r';
	}
	/* if orientation is 0, and 4-w 5-w 6-w return g*/
	else if (ori == '0' && f4<th4 && b5<th5 && b6<th6) {
		printf("face green\n");
		start_press();
		return 'g';
	}

	/* if something else, wait, try again*/ 
	else {
		printf("Error getting red/green orientation\n");
		start_press(); 
	}
	/* if timed out, return an arbitrary character */
}


/*calibrating the IR sensors */
void calibrate_IR(){
	int trash, f4w, b5w, b6w, f4b, b5b, b6b;
	printf("Place robot on WHITE surface, press start .\n");
	start_press();

	/*take IR readings, empty the buffer first */

	trash = analog(16);
	trash = analog (5);
	trash = analog (6);
	
	f4w = analog(16); /*front-left IR sensor*/
	b5w = analog(5);  /*back-left IR sensor*/
	b6w = analog(6); /*back-right IR sensor*/
	
	
	printf("White: %d %d %d\n", f4w, b5w, b6w);
	
	start_press();
	printf("Place robot on BLACK surface, press start .\n");
	start_press();

	/*take IR readings, empty the buffer first */

	trash = analog(16);
	trash = analog (5);
	trash = analog (6);
	
	f4b = analog(16); /*front-left IR sensor*/
	b5b = analog(5);  /*back-left IR sensor*/
	b6b = analog(6); /*back-right IR sensor*/
	
	th4 = (f4w + f4b) / 2;
	th5 = (b5w + b5b) / 2;
	th6 = (b6w + b6b) / 2;

	printf("Black: %d %d %d\n", f4b, b5b, b6b);
	start_press();
	printf("Thresh: %d %d %d\n", th4, th5, th6);
	start_press();
 }


/*returns 1-we're starting on white side, 0-we're starting on black side*/
int my_start_side() {
	int bcount, wcount, f4, b5, b6, trash;
	
	bcount=wcount=0;
	trash=analog(16);
	trash=analog(5);
	trash=analog(6); /*flush buffer*/
	f4 = analog(16); /*front-left IR sensor*/
	b5 = analog(5);  /*back-left IR sensor*/
	b6 = analog(6); /*back-right IR sensor*/

	/*check sensors, tally black and white readings*/
	if (f4>=th4) {
	 	bcount++;
      }
	else if (f4<th4) {
		wcount++;
      }
	if (b5>= th5) {
		bcount++;
      }
	else if (b5 < th5) {
		wcount++;
      }
	if (b6>=th6) {
		bcount++;
      }
	else if (b6< th6) { 
		wcount++;
	}
	/*determine my start side based on black and white readings*/
	if (bcount >=2) {
		return 0;  /*we're on black side*/
	}

	else if (wcount >=2) {
		
		return 1; /*we're on white side*/
	}
	
}	



void get_bot_num() {
	int bcount, wcount, f4, b5, b6, trash;
	
	bcount=wcount=0;
	trash=analog(16);
	trash=analog(5);
	trash=analog(6); /*flush buffer*/
	f4 = analog(16); /*front-left IR sensor*/
	b5 = analog(5);  /*back-left IR sensor*/
	b6 = analog(6); /*back-right IR sensor*/

	/*check sensors, tally black and white readings*/
	if (f4>=th4) 
	 	bcount++;
	else if (f4 <th4) 
		wcount++;

	if (b5>= th5)
		bcount++;
	else if (b5 < th5)
		wcount++;

	if (b6>=th6)
		bcount++;
	else if (b6< th6) 
		wcount++;
	
	/*determine bot number based on black and white readings*/
	if (bcount >=2 && wcount <=1) {
		if (rf_y0 < 0) {
			printf("Bot#: 1");
			botID=1;
		}
		else { 
			printf("Bot#: 0");
			botID=0;
		}
	}

	else if (wcount >=2 && bcount <=1) {
		if (rf_y0 <0 ) {
			printf("Bot#: 0");
			botID=0;

		}
		else {
			printf("Bot#: 1");
			botID=1;

		}
	}
	
	/*otherwise, we have skewed IR sensor readings, wait and try again */
	else {
		printf("Skewed readings, press start again");
		start_press();  
		get_bot_num();
	}
}


/*get's robot's x & y coordinates*/
int my_x(int bot_num) {
	int myX, i;
	if (bot_num ==0) {
		for(i=0; i<100000; i++) {
			myX=rf_x0;
		}
		return rf_x0;
	}
	else if (bot_num ==1) {
		for(i=0; i<100000; i++) {
			myX=rf_x1;
		}
		return rf_x1;
	}

}

int my_y(int bot_num) {
	int myY, i;
	if (bot_num ==0) {
		for(i=0; i<100000; i++) {
			myY=rf_y0;
		}
		return rf_y0;
	}
	else if (bot_num ==1) {
		for(i=0; i<100000; i++) {
			myY=rf_y1;
		}
		return rf_y1;
	}
}

int opp_x(int bot_num) {
	int oppX, i;
	if (bot_num ==0) {
		for(i=0; i<100000; i++) {
			oppX=rf_x1;
		}
		return rf_x1;
	}
	else if (bot_num ==1) {
		for(i=0; i<100000; i++) {
			oppX=rf_x0;
		}
		return rf_x0;
	}
}

int opp_y(int bot_num) {
	int oppY, i;
	if (bot_num ==0) {
		for(i=0; i<100000; i++) {
			oppY=rf_y1;
		}
		return rf_y1;
	}
	else if (bot_num ==1) {
		for(i=0; i<100000; i++) {
			oppY=rf_y0;
		}
		return rf_y0;
	}
}

