#pragma config(Sensor, S1,     YesBtn,         sensorTouch)
#pragma config(Sensor, S2,     NoBtn,          sensorTouch)
#pragma config(Sensor, S3,     ClrSns,         sensorCOLORFULL)
#pragma config(Sensor, S4,     SledPos,        sensorTouch)
#pragma config(Motor,  motorA,          SledMtr,       tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,          StprMtr,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          ScannerMtr,    tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define UNKNOWN 0
#define NO 1
#define YES 2

void MoveSled(byte Location);

byte LastLoc = 0;
byte SledLoc = 3;

task main()
{
   typedef struct
   {
      bool ColorDefined;
      byte WhereDoesColorBelong;
      byte DoesColorBelong[6];
   }BRICK_STRUCT;

   BRICK_STRUCT BrickInfo[6];

   bool EndProgram = false;

   for(int i = 0; i <= 5; i++)
   {
      BrickInfo[i].ColorDefined = false;
      BrickInfo[i].WhereDoesColorBelong = 0;

      for(int x = 0; x <= 5; x++)
      {
         BrickInfo[i].DoesColorBelong[x] = UNKNOWN;
      }
   }

   eraseDisplay();
   nxtDisplayRICFile(22, 4, "smile 01.ric");

   //while(true);

   //SledPosThrs = SensorValue[SledPos] - 20;

   motor[SledMtr] = -40;
   while(SensorValue[SledPos] == 0);
   motor[SledMtr] = 0;



   while(EndProgram == false)
   {
      SensorType[ClrSns] = sensorCOLORNONE;

      wait1Msec(500);

      motor[ScannerMtr] = 25;
      time1[T1] = 0;
      while(SensorValue[ClrSns] >= 20 && time1[T1] <= 2000);
      if(SensorValue[ClrSns] < 20)
      {
         wait1Msec(150);
         SensorType[ClrSns] = sensorCOLORFULL;
         motor[ScannerMtr] = 0;

         motor[StprMtr] = -50;
         wait1Msec(100);
         motor[StprMtr] = 0;

         wait1Msec(200);

         int BrickColor = SensorValue[ClrSns] - 1;

         //PlayImmediateTone(1000, 20);

         switch(SensorValue[ClrSns])
         {
         case BLACKCOLOR:
            PlaySoundFile("Black.rso");
            break;
         case WHITECOLOR:
            PlaySoundFile("White.rso");
            break;
         case YELLOWCOLOR:
            PlaySoundFile("Yellow.rso");
            break;
         case REDCOLOR:
            PlaySoundFile("Red.rso");
            break;
         case GREENCOLOR:
            PlaySoundFile("Green.rso");
            break;
         case BLUECOLOR:
            PlaySoundFile("Blue.rso");
            break;
         }

         while(bSoundActive);

         int RandomLoc = random[5];

         if(BrickInfo[BrickColor].ColorDefined == true)
         {
            eraseDisplay();
            nxtDisplayRICFile(22, 4, "smile 01.ric");

            MoveSled(BrickInfo[BrickColor].WhereDoesColorBelong);
         }
         else
         {
            eraseDisplay();
            nxtDisplayRICFile(22, 4, "smile 02.ric");

            bool FoundUnkownLoc = false;

            while(FoundUnkownLoc == false)
            {
               if(BrickInfo[BrickColor].DoesColorBelong[RandomLoc] == UNKNOWN)
               {
                  FoundUnkownLoc = true;
               }
               else
               {
                  RandomLoc = random[5];
               }
            }

            MoveSled(RandomLoc + 1);

            while(SensorValue[YesBtn] == 0 && SensorValue[NoBtn] == 0);

            if(SensorValue[YesBtn] == 1)
            {
               //PlayImmediateTone(1250, 20);

               eraseDisplay();
               nxtDisplayRICFile(22, 4, "smile 01.ric");

               while(SensorValue[YesBtn] == 1);

               BrickInfo[BrickColor].ColorDefined = true;
               BrickInfo[BrickColor].WhereDoesColorBelong = RandomLoc + 1;
               BrickInfo[BrickColor].DoesColorBelong[RandomLoc] = YES;

               for(int y = 0; y <= 5; y++)
               {
                  if(BrickInfo[y].DoesColorBelong[RandomLoc] == UNKNOWN)
                  {
                     BrickInfo[y].DoesColorBelong[RandomLoc] = NO;
                  }
               }

               switch(random[4])
               {
               case 0:
                  PlaySoundFile("Aha.rso");
                  break;
               case 1:
                  PlaySoundFile("Right.rso");
                  break;
               case 2:
                  PlaySoundFile("Champion.rso");
                  break;
               case 3:
                  PlaySoundFile("You're Good.rso");
                  break;
               }
               while(bSoundActive);
            }
            else if(SensorValue[NoBtn] == 1)
            {
               //PlayImmediateTone(800, 20);

               eraseDisplay();
               nxtDisplayRICFile(22, 4, "smile 03.ric");

               while(SensorValue[NoBtn] == 1);

               BrickInfo[BrickColor].DoesColorBelong[RandomLoc] = NO;

               switch(random[6])
               {
               case 0:
                  PlaySoundFile("Woops.rso");
                  break;
               case 1:
                  PlaySoundFile("Sorry.rso");
                  break;
               case 3:
                  PlaySoundFile("Crying 02.rso");
                  break;
               case 4:
                  PlaySoundFile("Ahnoo.rso");
                  break;
               case 5:
                  PlaySoundFile("Buuuhh.rso");
                  break;
               }
               while(bSoundActive);
            }
         }
      }
      else
      {
         motor[ScannerMtr] = 0;

         PlaySound(soundException);

         eraseDisplay();
         nxtDisplayCenteredTextLine(0, "Out of bricks!");
         nxtDisplayCenteredTextLine(2, "After reloading,");
         nxtDisplayCenteredTextLine(3, "press the green");
         nxtDisplayCenteredTextLine(4, "button.  If done");
         nxtDisplayCenteredTextLine(5, "using, press");
         nxtDisplayCenteredTextLine(6, "the red button.");

         while(SensorValue[YesBtn] == 0 && SensorValue[NoBtn] == 0);

         if(SensorValue[NoBtn] == 1)
         {
            EndProgram = true;

            PlaySoundFile("Goodbye.rso");
            while(bSoundActive);
            wait1Msec(500);
            PlaySoundFile("NiceDay.rso");
            while(bSoundActive);
         }
      }
   }
}

void MoveSled(byte Location)
{
   int LocVals[] = {0, 165, 165 * 2, 165 * 3, 165 * 4, 165 * 5};

   if(LastLoc == Location);
   else if(Location == 1)
   {
      motor[SledMtr] = -40;
      while(SensorValue[SledPos] == 0);
      motor[SledMtr] = 0;

      nMotorEncoder[SledMtr] = 0;
   }
   else if(nMotorEncoder[SledMtr] > LocVals[Location - 1])
   {
      motor[SledMtr] = -40;
      while(nMotorEncoder[SledMtr] > LocVals[Location - 1]);
      motor[SledMtr] = 0;
   }
   else if(nMotorEncoder[SledMtr] < LocVals[Location - 1])
   {
      motor[SledMtr] = 40;
      while(nMotorEncoder[SledMtr] < LocVals[Location - 1]);
      motor[SledMtr] = 0;
   }


   motor[ScannerMtr] = 25;
   wait1Msec(500);
   motor[ScannerMtr] = 0;

   motor[StprMtr] = 50;
   wait1Msec(90);
   motor[StprMtr] = 0;

   LastLoc = Location;
}