#define WHITE 45
#define BLACK 30
#define SILVER 62

#define LEFT OUT_A
#define RIGHT OUT_C

#define EYE SENSOR_2
#define BUMPER SENSOR_1
#define GLASS SENSOR_1 // SENSOR_3
#define TABLES 3
#define WAIT_FOR_GLASS 5

#define TURNTIME 300
#define DRIVELEN 67

int left_threshold, right_threshold;
int go_juice,step,t;

task main()
{
setup();go_juice=0;

while(true)
{
while (go_juice==0) test_glass();
turn_around();
get_juice();
}
}

void setup()
{
SetSensor(EYE, SENSOR_LIGHT);
SetSensor(BUMPER, SENSOR_TOUCH);
SetSensor(GLASS, SENSOR_TOUCH);

SelectDisplay(DISPLAY_SENSOR_2);

int white = WHITE; /*EYE*/

int black = BLACK; /*EYE*/
PlaySound(SOUND_CLICK);

int margin = (white - black) / 3;

left_threshold = black + margin;
right_threshold = white - margin;

// until(BUMPER == 1);
}

void test_glass()
{
go_juice=0;
if (GLASS== 1)
{
PlaySound(SOUND_CLICK);
Wait(50);
if (GLASS== 1) {go_juice=1; PlaySound(SOUND_DOWN); }
}
Wait(50);
PlaySound(SOUND_CLICK);

}

void turn_around()
{
OnRev(RIGHT);
OnRev(LEFT);
Wait(400);
OnFwd(LEFT);
Wait(TURNTIME);
OnFwd(LEFT+RIGHT);
Off(LEFT+RIGHT);
}

void drive_next()
{
for (step=0;step<DRIVELEN;step++)
{
do_drive();
}

}

void do_drive()
{

if (EYE <= left_threshold)
{
Off(LEFT);
On(RIGHT);

}
else if (EYE >= right_threshold)
{
Off(RIGHT);
On(LEFT);
}
else
{
On(LEFT+RIGHT);
}
}

void get_juice()
{
PlaySound(SOUND_DOUBLE_BEEP);
while (BUMPER==0) do_drive();
if (GLASS==1)
{
On(LEFT+RIGHT); Wait(100);
Off(LEFT+RIGHT);

PlaySound(SOUND_UP);
take_juice();
}
turn_around();
for (t=0;t<DRIVELEN;t++) drive_next();
Off(LEFT+RIGHT);
PlaySound(SOUND_UP);
until(GLASS==0);
PlaySound(SOUND_FAST_UP);
Wait (200);go_juice=0;
}

void take_juice()
{
Off(LEFT+RIGHT);
Wait(400);
}