/* Initialization Routines */

void init_servo(){
	enable_servos();
    servo(servoport,servoTURN);
	return;
}



void calibrate_IR () {
	
	int frontright,backleft,backright;
	int dummy = 0;
	int port = 0;

	/* test each IR sensor's output */
	while (!press_stop()) {
		port = frontrightIRport;
		printf("\nfrontright: %d   >>%d<<",analog(port),frontrightIRthresh);
		msleep(100L);
	}
	while (!press_stop()) {
		port = backleftIRport;
		printf("\nbackleft: %d   >>%d<<",analog(port),backleftIRthresh);
		msleep(100L);
	}
	while (!press_stop()) {
		port = backrightIRport;
		printf("\nbackright: %d   >>%d<<",analog(port),backrightIRthresh);
		msleep(100L);
	}

	printf("\nStop to go, start to edit");
	while (1) {
		if (press_start()) {
			dummy = 1;
			break;
		}
		if (press_stop()) {
			break;
		}
	}

	if (dummy) {
		/* change threshold if necessary */
		while (!press_stop()) {
			port = frontrightIRport;
			printf("\nnew FR thresh:  %d",knob());
			msleep(100L);
		}
		frontrightIRthresh = knob();

		while (!press_stop()) {
			port = backleftIRport;
			printf("\nnew BL thresh:  %d",knob());
			msleep(100L);
		}
		backleftIRthresh = knob();

		while (!press_stop()) {
			port = backrightIRport;
			printf("\nnew BR thresh:  %d",knob());
			msleep(100L);
		}
		backrightIRthresh = knob();
	}
}

void find_team_and_orientation () {
	/* set globals teamisblack, initial_orient, rf_team */
	int frontright, backleft, backright;
	long starttime,elapsed;

	elapsed = 0L;
	starttime = mseconds();
	while (elapsed<50L) {
			frontright=analog(frontrightIRport);
			elapsed = mseconds()-starttime;
	}
	elapsed = 0L;
	starttime = mseconds();
	while (elapsed<50L) {
			backleft=analog(backleftIRport);
			elapsed = mseconds()-starttime;
	}
	elapsed = 0L;
	starttime = mseconds();
	while (elapsed<50L) {
			backright=analog(backrightIRport);
			elapsed = mseconds()-starttime;
	}

    
    /* 1 stands for blk, 0 for white*/
    if (frontright <= frontrightIRthresh)
		frontright = 1;
    else
		frontright = 0;
    
    if (backleft <= backleftIRthresh)
		backleft = 1;
    else
		backleft = 0;

    if (backright <= backrightIRthresh)
		backright = 1;
    else
		backright = 0;

    if (!frontright && !backleft && !backright) {
		teamisblack = true;
		initial_orient = 3;
    }
    if (!frontright && !backleft && backright) {
		teamisblack = true;
		initial_orient = 2;
    }
    if (!frontright && backleft && !backright) {
		teamisblack = true;
		initial_orient = 0;
    }
    if (frontright && !backleft && !backright) {
		teamisblack = true;
		initial_orient = 1;
    }
    if (!frontright && backleft && backright) {
		teamisblack = false;
		initial_orient = 3;
    }
    if (frontright && backleft && !backright) {
		teamisblack = false;
		initial_orient = 1;
    }
    if (frontright && backleft && backright) {
		teamisblack = false;
		initial_orient = 0;
    }
    if (frontright && !backleft && backright) {
		teamisblack = false;
		initial_orient = 2;
    }


	if (!teamisblack) {
			coord_factor = -1;
			heading_const = 180.0;
	}

	/* now, just need to set rf_our_team, sleep for safety */
	
	threadsafe_sleep(0.3);
	if (distance(rf_x0,rf_y0,0,(int)(15.0*unitsperinch))<distance(rf_x1,rf_y1,0,(int)(15.0*unitsperinch))) {
		/*rf_x0,rf_y0 correspond to black team*/	
		if(teamisblack) {
			rf_our_team = 0;
		}
		else {
			rf_our_team = 1;
		}
	}
	else {
		/*rf_x1,rf_y1 correspond to black team*/
		if(teamisblack) {
			rf_our_team = 1;
		}
		else {
			rf_our_team = 0;
		}
	}
	/*printf("\nx0,y0:%d,%d  ",rf_x0,rf_y0);
	printf("x1,y1:%d,%d",rf_x1,rf_y1);
	wait_start();
	*/

    
    return;
}