//Team Mr. Roboto (Matthew Gyurgyik, Ryan Odon, Sean Cahill, Tim Ziemba, Red Chopps, Dan Virost)
//Code V2.0.1
//Copyrighted 2007
//For use in the Feb. 20, 2007 Case Westren Robotics Competion

#define MOTORL OUT_A
#define MOTORR OUT_C
#define SENSEL SENSOR_3
#define SENSER SENSOR_1

#define SCORE_VALUE 340
int value;
int CurrValue;
int PrevValue;

task main() {
     SetSensor(SENSOR_1, SENSOR_LIGHT);                       //Define Type of
     SetSensor(SENSOR_3, SENSOR_LIGHT);                       //Sensors for Left and Right

     SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);                //Set Sensorsto read
     SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);                //to read RAW VALUES

     ClearMessage();                                          //This is going to clear the Message (so we can recieve 102)
     while(Message()!=102);                                 //goes into an empty loop until message 102 get sent

     SetPower(MOTORL + MOTORR, 7);
     OnRev(MOTORL + MOTORR);
     Wait(200);
     start lightSense;         //Senses for the light
     ClearTimer(3);
     start bumperSense;        //Checks for bumper hits
}


//THE VALUE DIMMEST STORES THE BRIGHTEST LIGHT VALUE BY THE END OF THIS TASK
task lightSense() {
     int dimmest = 1023;     //The lowest the light should ever go
     start onmot;  //Well, does the obvious
     ClearTimer(0);
     while(Timer(0) <= 10) {                  //this will spin the robot around for 1 second
                    if(SENSER < dimmest){     //If the sensor value is lower than what is set in dimmest
                              dimmest = SENSER;    //then dimmest will get the new brightest value
                    }     //basically the bot turns around for a total of 1 sec (about 2-3 spins) storing the brigtest value it finds in dimmest
     }
     PlaySound(SOUND_DOUBLE_BEEP);     //this lets us know when the bot has completed the fist loop

     SetPower(MOTORL+MOTORR, 6);      //Spin the bot
     OnFwd(MOTORL);
     OnRev(MOTORR);

     ClearTimer(2);
     while(SENSER > dimmest + 50) {    //While the Sensor reading is greater than the value for the brightest light + 50 to the brightest light restart
                  if(Timer(2) > 15) {
                              start restart;
                  }
     }
     Toggle(MOTORL + MOTORR); //reverse the motors
     SetPower(MOTORL + MOTORR, 7);  //change speed of the motor to 5
     Wait(18);       //wait .18 seconds
     Off(MOTORL + MOTORR); //turn off the motors
     value = dimmest;      //set dimmest to value
     start track;          //lets start tracking the light
}

task onmot() {
     SetPower(MOTORL + MOTORR, 7); //set the power of the motors
     Fwd(MOTORL);                 //set Motor Left to go FWD
     Rev(MOTORR);                 //set Motor Right to go in Reverse
     OnFor(MOTORL+MOTORR, 100);   //have the motors go for 1 second
}

task track() {
while(true){
        if ((abs(SENSEL - SENSER) < 10)  ) {
                SetPower(MOTORL, 7);
                SetPower(MOTORR, 7);
                OnFwd(MOTORL+MOTORR);
                //start findGreatLight;
                while(true) {
                        if (SENSER > SENSEL) {

                                SetPower(MOTORL, 6);
                                SetPower(MOTORR, 1);
                                OnFwd(MOTORL+MOTORR);
                                Wait(50);
                                Off(MOTORL+MOTORR);
                                     continue; }
                           else if(SENSEL > SENSER) {
                                     SetPower(MOTORR, 1);
                                     SetPower(MOTORL, 6);
                                     OnFwd(MOTORL+MOTORR);
                                     Wait(50);
                                     Off(MOTORR + MOTORL);
                                     continue; }
                           else {
                                SetPower(MOTORR + MOTORL, 7);
                                Wait(100);
                                Off(MOTORL+MOTORR);
                                Wait(100);
                                continue;
                           }
                        }
                       /* if ( (SENSEL < 540) || (SENSEL > 610) ) //if the bot is really close to the light then
                        {
                                Off(MOTORL+MOTORR); //turn bot off
                                PlaySound(SOUND_UP); //play a nice sound
                                Wait(300); //wait for the score
                                PlaySound(SOUND_UP); //play a nice sound
                                SetPower(MOTORL + MOTORR, 7);
                                OnRev(MOTORL + MOTORR);
                                Wait(100);
                                start restart;
                        } */
                }
        }
        PlaySound(SOUND_UP);
        Wait(300);
        PlaySound(SOUND_DOWN);
        Wait(300);
}


task findGreatLight() {
CurrValue = SENSER;
 PrevValue = CurrValue;
     while(true) {
     PlayTone(700, 50);
                   CurrValue = SENSER;
                // if (CurrValue > PrevValue) {
                if (PrevValue > CurrValue) {
                    stop track;
                    SetPower(MOTORL+MOTORR, 7);
                    OnFwd(MOTORL+MOTORR);
                    Wait(100);
                    Off(MOTORL+MOTORR);
                    Off(MOTORL+MOTORR); //turn bot off
                    PlaySound(SOUND_UP); //play a nice sound
                    Wait(300); //wait for the score
                    PlaySound(SOUND_UP); //play a nice sound
                    SetPower(MOTORL + MOTORR, 7);
                    OnRev(MOTORL + MOTORR);
                    Wait(150);
                    start restart;
                    }
                    PrevValue = CurrValue;
                 }
}

task bumperSense() {
     while(true) {
                 if(SENSEL < 200 ||  SENSER < 200 || Timer(3) > 300) {   //If theres a bumper hit, or the timer runs out
                           ClearTimer(3);
                           stop track;
                           Off(MOTORL + MOTORR);
                           OnRev(MOTORL + MOTORR);
                           SetPower(MOTORL, 7);
                           SetPower(MOTORR, 7);
                           Wait(100);
                           Off(MOTORL + MOTORR);
                           start restart;
                 }
     }
}

task restart() {
     stop findGreatLight;
     stop lightSense;
     start lightSense;
}