int Colour[5];
int ColourCnt;

task main()
{
 SetSensor (SENSOR_1, SENSOR_ROTATION);
 SetSensor (SENSOR_2, SENSOR_LIGHT);
 SetSensor (SENSOR_3, SENSOR_TOUCH);
 int Bucket;

 Colour[1] = 0;
 Colour[2] = 0;
 Colour[3] = 0;
 ColourCnt = 0;
 Bucket = 0;
 Eject ();

repeat (8)   //repeats 8 times till chute is empty
   {
   ReturnSorter ();
   Inject();
   Wait (50);
   Calibrate (Bucket);
   PlaySound (SOUND_DOUBLE_BEEP);
   Rotate(Bucket);
   PlaySound (SOUND_FAST_UP);
   Eject ();
   PlaySound (SOUND_CLICK);
   ReturnSorter ();
   }
                SleepNow ();  // turns RCX off at end of program
}

sub Inject ()   //injects the bricks into the sorter
 {
 if (SENSOR_3 == 1)
   {
   OnRev (OUT_C);
   OnFor (OUT_C, 120);
   OnFwd (OUT_C);
   OnFor (OUT_C, 100);
   }
 Off (OUT_C);
 }

void Calibrate (int &Bucket)   /* calibrates the first 3 bricks and uses their
 colour to determine if it is a knowen or unknowen block colour */
{
int x;
int BlkClr;
int Blim;
int Tlim;


  BlkClr = SENSOR_2; //Read colour of block
  Bucket = 4;

  x=0;
  for (x=1; x<ColourCnt; x=x+1)
   {
   Blim = Colour[x] - 1;
   Tlim = Colour[x] + 1;
   if (Blim <= BlkClr && BlkClr <= Tlim) {Bucket = x; break;}
   }
  if (Bucket == 4 && ColourCnt < 4)
     {
     ColourCnt = ColourCnt + 1;
     Colour[ColourCnt] = BlkClr;
     Bucket = ColourCnt;
     }
}

sub Eject ()   // ejects blocks into bucket/glass
{
  SetDirection(OUT_B, OUT_FWD);
  OnFor (OUT_B, 80);
  SetDirection(OUT_B, OUT_REV);
  OnFor(OUT_B, 80);
  Off(OUT_B);
}

void Rotate (int Bucket)    //rotates to the correct bucket/glass
{
int  RotateTo;
 SetSensor (SENSOR_1, SENSOR_ROTATION);

switch (Bucket)
  {
  case 1 :
       RotateTo = 40;
       break;
  case 2 :
       RotateTo = 80;
       break;
  case 3 :
       RotateTo = 120;
       break;
  default :
       RotateTo = 160;
       break;
  }
do
{
  SetPower (OUT_A, 3);
  SetDirection(OUT_A, OUT_REV);
  OnFor(OUT_A, 2);
}
while(SENSOR_1 <  RotateTo);

}




sub ReturnSorter ()    //returns sorter to natural position
{
  do
{
  SetPower (OUT_A, 3);
  SetDirection(OUT_A, OUT_FWD);
  OnFor(OUT_A, 3);

}
 while(SENSOR_3 == 0);
}