void setup_ramp ()
{
	ramp_up = 0;
	white_balls = 0;
	black_balls = 0;
	white_accounted = 0;
	black_accounted = 0;
	GRABBING_BALL = 0;
	servo(SORTER_SERVO, SORTER_CENTER);
}

void sort_ball ()
{
	int val;

	val = get_analog(BALL_LIGHT, 15);
/*
	printf("%d (%d)\n", val, THRESHOLD_BALL);
*/

	if (val < THRESHOLD_BALL) {
		white_balls++;
		white_accounted++;
		if (white_balls != 3) {
			servo(SORTER_SERVO, SORTER_LEFT);
			sleep(0.5);
			servo(SORTER_SERVO, SORTER_CENTER);
		}
	} else {
		black_balls++;
		black_accounted++;
		if (black_balls != 3) {
			servo(SORTER_SERVO, SORTER_RIGHT);
			sleep(0.5);
			servo(SORTER_SERVO, SORTER_CENTER);
		}
	}
}

void ramp_watcher ()
{
	while (1) {
		if (get_digital(RAMP_BUTTON)) {
			GRABBING_BALL = 1;
			do_ramp();
			GRABBING_BALL = 0;
		}
		defer();
	}
}

void do_ramp ()
{
	raise_ramp();

	sleep(0.25);

	sort_ball();

	if (white_balls == 3 || black_balls == 3) {
		/* do nothing - keep the ramp up! */
	} else {
		sleep(0.25);
		lower_ramp();
	}
}

void raise_ramp ()
{
	fd(RAMP_MOTOR);
	while (get_digital(RAMP_TOP_SENSOR)) {
		sleep(0.01);
	}
	off(RAMP_MOTOR);
	ramp_up = 1;
}

void lower_ramp ()
{
	bk(RAMP_MOTOR);
	sleep(0.18);
	off(RAMP_MOTOR);
	ramp_up = 0;
}

void open_gate_after (int gate, float time)
{
	sleep(time);
	if (gate == WHITE) {
		open_white_gate();
	} else {
		open_black_gate();
	}
}

void open_white_gate ()
{
	fd(GATE_MOTOR);
	sleep(0.2);
	off(GATE_MOTOR);

	servo(SORTER_SERVO, SORTER_LEFT);

	white_balls = 0;
}

void close_white_gate ()
{
	bk(GATE_MOTOR);
	sleep(0.2);
	off(GATE_MOTOR);

	servo(SORTER_SERVO, SORTER_CENTER);

	fd(THUMPER_MOTOR);
}

void open_black_gate ()
{
	bk(GATE_MOTOR);
	sleep(0.2);
	off(GATE_MOTOR);

	servo(SORTER_SERVO, SORTER_RIGHT);

	black_balls = 0;
}

void close_black_gate ()
{
	fd(GATE_MOTOR);
	sleep(0.2);
	off(GATE_MOTOR);

	servo(SORTER_SERVO, SORTER_CENTER);

	fd(THUMPER_MOTOR);
}