void setup_sensors ()
{
	enable_encoder(LEFT_MOTOR_ENCODER);
	enable_encoder(RIGHT_MOTOR_ENCODER);
}

int get_analog (int p, int n)
{
	int i;
	int sum;

	analog(p);
	sum = 0;
	for (i = 0; i < n; i++) {
		sum += analog(p);
	}
	return (sum / n);
}

int get_digital (int p)
{
	int sum = 0;

	digital(p);

	sum += digital(p) + digital(p) + digital(p);
	if (sum >= 2) {
		return 1;
	} else {
		return 0;
	}
}

void calibrate_light_sensors ()
{
	int min1, min2, min3, min4;
	int max1, max2, max3, max4;
	int val1, val2, val3, val4;
	
	min1 = min2 = min3 = min4 = -1;
	max1 = max2 = max3 = max4 = -1;
	while (!stop_button() && !start_button()) {
		val1 = get_analog(LEFT_FRONT_LIGHT,3);
		val2 = get_analog(RIGHT_FRONT_LIGHT,3);
		val3 = get_analog(LEFT_REAR_LIGHT,3);
		val4 = get_analog(RIGHT_REAR_LIGHT,3);
		
		if (val1 < min1 || min1 == -1) {
			min1 = val1;
		}
		if (val2 < min2 || min2 == -1) {
			min2 = val2;
		}
		if (val3 < min3 || min3 == -1) {
			min3 = val3;
		}
		if (val4 < min4 || min4 == -1) {
			min4 = val4;
		}

		if (val1 > max1 || max1 == -1) {
			max1 = val1;
		}
		if (val2 > max2 || max2 == -1) {
			max2 = val2;
		}
		if (val3 > max3 || max3 == -1) {
			max3 = val3;
		}
		if (val4 > max4 || max4 == -1) {
			max4 = val4;
		}
		printf("%d %d %d %d %d  %d  %d  %d\n", max1, max2, max3, max4, min1, min2, min3, min4);
		sleep(0.1);
	}
	
	while (start_button() || stop_button()) {
	}
	
	THRESHOLD_1 = (min1 + max1) / 2;
	THRESHOLD_2 = (min2 + max2) / 2;
	THRESHOLD_3 = (min3 + max3) / 2;
	THRESHOLD_4 = (min4 + max4) / 2;
}

void calibrate_ball_sensor ()
{
	int orange[3];
	int white[3];
	int val;
	int i;

	for (i = 0; i < 3; i++) {
		printf("Place orange    ball\n");

		fd(THUMPER_MOTOR);
		while (!get_digital(RAMP_BUTTON)) {
			sleep(0.01);
		}
		raise_ramp();

		while (!start_button()) {
			val = get_analog(BALL_LIGHT, 3);
			printf("Orange val: %d\n", val);
			sleep(0.1);
		}
		orange[i] = val;
		while (start_button()) {
		}

		bk(THUMPER_MOTOR);
		lower_ramp();
		sleep(0.5);
		off(THUMPER_MOTOR);
	}

	for (i = 0; i < 3; i++) {
		printf("Place white     ball\n");

		fd(THUMPER_MOTOR);
		while (!get_digital(RAMP_BUTTON)) {
			sleep(0.01);
		}
		raise_ramp();

		while (!start_button()) {
			val = get_analog(BALL_LIGHT, 3);
			printf("White val: %d\n", val);
			sleep(0.1);
		}
		white[i] = val;
		while (start_button()) {
		}

		bk(THUMPER_MOTOR);
		lower_ramp();
		sleep(0.5);
		off(THUMPER_MOTOR);
	}

	THRESHOLD_BALL = (orange[0] + orange[1] + orange[2] + white[0] + white[1] + white[2]) / 6;
}	

void sense_block ()
{
	int first, second;

	while (read_encoder(LEFT_MOTOR_ENCODER) < 120) {
	}

	start_process(open_white_gate());
	if (team == WHITE) {
		first = get_analog(LEFT_DISTANCE, 3);
	} else {
		first = get_analog(RIGHT_DISTANCE, 3);
	}

	while (read_encoder(RIGHT_MOTOR_ENCODER) < 370) {
	}

	start_process(close_white_gate());
	if (team == WHITE) {
		second = get_analog(LEFT_DISTANCE, 3);
	} else {
		second = get_analog(RIGHT_DISTANCE, 3);
	}

	if (first > 65) {
		block_spot = SOUTH_EAST;
	} else if (second > 65) {
		block_spot = SOUTH_WEST;
	}

	printf("a: %d   b: %d\n", first, second);
}