#pragma config(Sensor, S2,     colour,         sensorEV3_Color)
#pragma config(Motor,  motorB,          leftwheel,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightwheel,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorD,          arm,              tmotorEV3_Large, PIDControl, encoder)


bool mone = true;
bool colorBlacks = true;

void unpackege()
{
   if(mone)
      {
         mone = false;
       motor[leftwheel] = 0;
       motor[rightwheel] = 0;
         setMotorReversed(leftwheel, true);
         setMotorReversed(rightwheel, true);
      sleep(500);
      setMotorTarget(leftwheel, 2500, 20);
          setMotorTarget(rightwheel, 2500, 20);
      sleep(2500);
      if (colorBlacks)
         {
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, true);
            setMotorReversed(rightwheel, false);
            sleep(1500);
            setMotorTarget(leftwheel, 150, 10);
            setMotorTarget(rightwheel, 150, 10);
            sleep(750);

            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, false);
               setMotorReversed(rightwheel, false);
            sleep(500);
            setMotorTarget(leftwheel, 200, 25);
                setMotorTarget(rightwheel, 200, 25);
                sleep(500);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);

            setMotorReversed(arm, false);
            sleep(500);
            setMotorTarget(arm, 0, 50);
            sleep(1000);

            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, true);
               setMotorReversed(rightwheel, true);
            sleep(500);
            setMotorTarget(leftwheel, 200, 25);
                setMotorTarget(rightwheel, 200, 25);
                sleep(500);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);

            setMotorReversed(leftwheel, false);
            setMotorReversed(rightwheel, true);
            sleep(250);
            setMotorTarget(leftwheel, 150, 10);
            setMotorTarget(rightwheel, 150, 10);
            sleep(750);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            }



            if (!colorBlacks)
            {
               motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, false);
            setMotorReversed(rightwheel, true);
            sleep(500);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorTarget(leftwheel, 150, 10);
            setMotorTarget(rightwheel, 150, 10);
            sleep(750);

            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, false);
               setMotorReversed(rightwheel, false);
            sleep(500);
            setMotorTarget(leftwheel, 200, 25);
                setMotorTarget(rightwheel, 200, 25);
                sleep(500);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);

            setMotorReversed(arm, false);
            sleep(500);
            setMotorTarget(arm, 0, 50);
            sleep(1000);

            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, true);
               setMotorReversed(rightwheel, true);
            sleep(500);
            setMotorTarget(leftwheel, 200, 25);
                setMotorTarget(rightwheel, 200, 25);
                sleep(500);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorReversed(leftwheel, true);
            setMotorReversed(rightwheel, false);
            sleep(250);
            motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
            setMotorTarget(leftwheel, 150, 10);
            setMotorTarget(rightwheel, 150, 10);
            sleep(750);
           motor[leftwheel] = 0;
            motor[rightwheel] = 0;
            sleep(250);
         }
    }
}
void checkBoxes()
{
   int colorsencore = 0;
   int lowest = 50;
   int highest = 0;


   if (SensorValue[colour] > highest)
    {
      highest = SensorValue[colour];
    }
  colorsencore = highest;

   if(colorsencore > 22)
    {
       motor[leftwheel] = 0;
         motor[rightwheel] = 0;
         sleep(750);
         highest = SensorValue[colour];
      colorsencore = highest;
      sleep(750);
    if(colorsencore > 40)
    {
       colorBlacks = false;
       displayCenteredBigTextLine(4, "white");
    }
    if(colorsencore < 40)
    {
       colorBlacks = true;
       displayCenteredBigTextLine(4, "purple");
    }

         sleep(1000);

         if(mone)
         {
            setMotorReversed(arm, true);
            sleep(500);
            setMotorTarget(arm, 800, 100);
            sleep(1000);
            unpackege();
            sleep(1000);
            setMotorReversed(arm, false);
            sleep(500);
            setMotorTarget(arm, 0, 50);
            sleep(1000);
            }
               colorsencore =0;
      }
}

void StartRobot()
{
   if (!mone)
      {
         mone = true;
         motor[leftwheel] = 0;
         motor[rightwheel] = 0;
         setMotorReversed(leftwheel, false);
          setMotorReversed(rightwheel, false);
      }
   motor[leftwheel] = 25;
  motor[rightwheel] = 25;
   checkBoxes();
}

task main()
{
       //motor[arm] = 0;
    //motor[arm] = -10;
     //setMotorReversed(arm, true);
      //setMotorTarget(arm, 800, 100);
     //setMotorReversed(arm, false);
     //setMotorTarget(arm, 800, 100);

   displayCenteredBigTextLine(4, "none");
  checkBoxes();
  while (true)
  {
     StartRobot();
  }
}