/*A Program For
Robot Two*/

//set integers and variables
int black1=41 + 3;  // black reading for sensor 1
int white1=57 - 3;  // white reading for sensor 1

int black3=41 + 3;  // black reading for sensor 3
int white3=57 - 3;  // white reading for sensor 3

int foil1=61;  // foil reading for sensor 1
int foil3=61;  // foil reading for sensor 3

int offboard1=35 + 3;  // reading off the board
int offboard3=36 + 3;  // reading off the board

int bottle_distance;           // distance of bottle from robot
int ave_distance, v1, v2, v3;  // moving average of distance sensor
int scan_times=12;             //number of times to scan back and forth
                                      
int value;            // reading of GP2D12 sensor
int sensorstate = 0;  // sensor is in charging state (motors on)

// dual light sensor line-follower subroutine
sub follow_line_to_foil ()
{
          // back up and return to line if falling off board
          if((SENSOR_1 < offboard1 && SENSOR_3 < offboard3))
          {
             SetPower(OUT_A + OUT_C, 7);
             OnRev(OUT_A + OUT_C);
             until(SENSOR_1 > white1);
             until(SENSOR_1 < black1);
             OnFwd(OUT_A + OUT_C);
             until(SENSOR_1 > white1);
             Wait(15);
             OnRev(OUT_A);
             until(SENSOR_1 < black1);
          }
          if(SENSOR_1 < black1)
          {
             SetPower(OUT_A + OUT_C, 5);
             OnFwd (OUT_C);  // spin left
             OnRev (OUT_A);
          }
          else if(SENSOR_3 < black3)
          {
             SetPower(OUT_A + OUT_C, 5);
             OnFwd (OUT_A);  // spin right
             OnRev (OUT_C);
          }
           else
          {
             SetPower(OUT_A + OUT_C, 5);
             OnFwd (OUT_A + OUT_C);  // go forward
          }
}

    
task main()
{
    int dropped_bottles=1; // dropped bottle count
    //set sensors                                                                // Exit Base and Turn to Dashed Line
    SetSensor (SENSOR_1, SENSOR_LIGHT);
    SetSensor (SENSOR_3, SENSOR_LIGHT);
    
    // manual start if top touch sensor is pressed
    if (SENSOR_3 > 90)
    {
      Wait(25);
      goto begin_program;
    }

    //charge and begin monitoring GP2D12 IR sensor
    SetUserDisplay (value,0); // Display "value" on LCD
    SetSensor(SENSOR_2,SENSOR_LIGHT); // Active mode: starts charging sensor
    Wait(100); // Wait for full charge on startup
    start monitor_GP2D12;
    
    //wait for second robot to leave base
    Wait(100);
    until (value < 330);
    Wait(500);

    //wait for second robot to return to base
    until (value > 450);
    Wait (600);
    stop monitor_GP2D12;
    Wait (25);

    begin_program:

    // forward into intersection
    OnFwd(OUT_A + OUT_C);
    until(SENSOR_1 < black1);
    until(SENSOR_1 > white1);
    Wait(75);

    // spin towards crossroad
    OnRev(OUT_C);
    Wait(75);
    Off(OUT_A + OUT_C);

    // follow dashed line to foil                                                // Follow Dashed Line to Foil
    until((SENSOR_1 < black1 && SENSOR_3 < black3))
    {
          if((SENSOR_1 > foil1 || SENSOR_3 > foil3))
          {
             goto end_of_road;
          }
          if(SENSOR_1 < black1)
          {
             OnFwd (OUT_C);  // turn left
             Off (OUT_A);
          }
          else if(SENSOR_3 < black3)
          {
             OnFwd (OUT_A);  // turn right
             Off (OUT_C);
          }
          else
          {
             OnFwd (OUT_A + OUT_C);  // go forward
          }
    }

    end_of_road:
    // forward past foil and spin to line
    OnFwd(OUT_A + OUT_C);
    Wait(20);
    OnRev(OUT_C);
    until(SENSOR_1 < black1);
    OnFwd (OUT_C);
    OnRev (OUT_A);
    until(SENSOR_1 > white1);
    Off (OUT_A+OUT_C);
    
    // follow line around back to foil                                           // Follow Loop Back to Foil to
    while((SENSOR_1 < foil1 && SENSOR_3 < foil3))                                // Knock Off First Two Bottles
    {
       follow_line_to_foil ();
    }
    SetPower(OUT_A + OUT_C, 7);

    // position for scan
    OnFwd (OUT_A + OUT_C);
    until(SENSOR_1 < foil1);
    Wait(15);
    Off(OUT_A + OUT_C);
    OnFwd(OUT_C);
    OnRev(OUT_A);
    until(SENSOR_1 > white1);
    until(SENSOR_3 < black3);
    Off(OUT_A + OUT_C);
    
    //start charging GP2D12 IR sensor
    SetUserDisplay (value,0); // Display "value" on LCD
    SetSensor(SENSOR_2,SENSOR_LIGHT); // Active mode: starts charging sensor
    Wait(100); // Wait for full charge on startup

    //start monitoring GP2D12 IR sensor
    start monitor_GP2D12;
    Wait(15);
    
    //check to see if a bottle is directly in front
    if ((value < 250 || value > 600))
    {
      dropped_bottles -= 1;
    }
                                                                                 // Scan for and Push Off Two Remaining
    // clear two remaining bottles                                               // Bottles, One At a Time,
    until (dropped_bottles == 3)                                                 // And Return to Foil
    {
    scan_again:
    //spin off line
    OnFwd(OUT_C);
    OnRev(OUT_A);
    until(SENSOR_1 > white1);
    Off(OUT_A + OUT_C);

    start machine_gun;
    //start scan spin
    SetPower(OUT_A + OUT_C, 7);
    OnFwd(OUT_C);
    OnRev(OUT_A);

    // find bottles
    while (true)
    {
       //scan for bottle
       while (SENSOR_1 > black1)
       {
         if (value < 380)
         {
             if(sensorstate == 1)
             {
                SetPower(OUT_A + OUT_C, 3);
                OnFwd(OUT_C);
                OnRev(OUT_A);
             }
             else
             {
               Off(OUT_A+OUT_C);
             }
         }
         else
         {
            goto found_bottle;
         }
       }

       //scan other way for bottle
       while (SENSOR_3 > black3)
       {
         if (value < 380)
         {
             if(sensorstate == 1)
             {
                SetPower(OUT_A + OUT_C, 3);
                OnFwd(OUT_A);
                OnRev(OUT_C);
             }
             else
             {
               Off(OUT_A+OUT_C);
             }
         }
         else
         {
           goto found_bottle;
         }
       }
    }
    found_bottle:
    Off(OUT_A + OUT_C);
    stop machine_gun;

    bottle_distance = value;
    v2 = value;
    v3 = value;

    // forward to bottle
    SetPower(OUT_A + OUT_C, OUT_FULL);
    OnFwd(OUT_A + OUT_C);
    
    // check to see if the robot is nearing the bottle
    while (true)
    {
      if (SENSOR_1 < black1)
      {
        // spin to line
        SetPower(OUT_A + OUT_C, 7);
        OnFwd(OUT_A + OUT_C);
        Wait(20);
        OnRev(OUT_A);
        until(SENSOR_1 < black1);
        until(SENSOR_1 > white1);
        goto scan_again;
      }

      if (value < 250)
      {
        goto hit_bottle;
      }

      v1 = v2;
      v2 = v3;
      v3 = value;
      ave_distance = (v1+v2+v3) / 3;
      SetUserDisplay(ave_distance, 0);

      //stop and re-find bottle if robot is not aimed at it
      if (ave_distance < bottle_distance)
      {
        Off(OUT_A + OUT_C);
        //scan back and forth for bottle

        scan_times=12;
        while(true)
        {
          repeat (scan_times)
          {
            if (SENSOR_1 < black1)
            {
              goto scan_again;
            }
            
            if (value < 380)
            {
                if(sensorstate == 1)
                {
                   SetPower(OUT_A + OUT_C, 3);
                   OnFwd(OUT_A);
                   OnRev(OUT_C);
                }
                else
                {
                  Off(OUT_A+OUT_C);
                  until(sensorstate == 1);
                }
            }
            else
            {
              goto found_bottle;
            }
          }
          scan_times += 12;
        
          repeat (scan_times)
          {
            if (SENSOR_1 < black1)
            {
              goto scan_again;
            }
            
            if (value < 380)
            {
                if(sensorstate == 1)
                {
                   SetPower(OUT_A + OUT_C, 3);
                   OnFwd(OUT_C);
                   OnRev(OUT_A);
                }
                else
                {
                  Off(OUT_A+OUT_C);
                  until(sensorstate == 1);
                }
            }
            else
            {
              goto found_bottle;
            }
          }
          scan_times += 12;
          
        }  //"while true" end
        
        
      }   //"if" end
    } //"while true" end

    hit_bottle:
    until(SENSOR_1 < black1);
    until(SENSOR_1 > white1);

    // count bottles that are dropped off
    if ((value < 250 || value > 600))
     {
       dropped_bottles += 1;
     }

    // creep forward until bottle drops off
    off_board:
    SetPower(OUT_A + OUT_C, 2);
    OnFwd(OUT_A + OUT_C);
    until(value > 250);
    Off(OUT_A + OUT_C);
    Wait(70);

    //check to see if bottle is really pushed off board
    if (value > 600)
    {
       OnFwd(OUT_A + OUT_C);
       until(value < 250);
       Wait(1);
       goto off_board;
    }
    Off(OUT_A + OUT_C);
    PlaySound (SOUND_UP);

    // back up and spin to line
    SetPower(OUT_A + OUT_C, 7);
    OnRev(OUT_A + OUT_C);
    until(SENSOR_1 > white1);
    until(SENSOR_1 < black1);
    OnFwd(OUT_A + OUT_C);
    Wait(20);
    OnRev(OUT_A);
    until(SENSOR_1 < black1);
    until(SENSOR_1 > white1);
    }                                                                            // Repeat End
    
    // follow line back to foil and stop
    while((SENSOR_1 < foil1 && SENSOR_3 < foil3))
    {
       // back up and reposition on line if robot hits gate
       if (value < 200)
       {
         SetPower(OUT_A + OUT_C, 7);
         OnRev(OUT_A + OUT_C);
         until(SENSOR_1 > white1);
         until(SENSOR_1 < black1);
         OnFwd(OUT_A + OUT_C);
         until(SENSOR_1 > white1);
         Wait(15);
         OnRev(OUT_A);
         until(SENSOR_1 < black1);
       }
       follow_line_to_foil ();
    }
    Off(OUT_A + OUT_C);

    //turn off distance sensor
    stop monitor_GP2D12;
    
    // forward past foil
    OnFwd(OUT_A + OUT_C);
    Wait(30);

    // spin towards center of board
    OnRev(OUT_C);
    until(SENSOR_3 > white3);
    until(SENSOR_3 < black3);
    until(SENSOR_1 > white1);
    until(SENSOR_1 < black1);
    OnFwd(OUT_C);
    OnRev(OUT_A);
    Wait(15);
    
    // follow line home                                                // Follow Dashed Line to Foil
    until((SENSOR_1 > 70 || SENSOR_3 > 70))
    {
          if(SENSOR_1 < black1)
          {
             OnFwd (OUT_C);  // turn left
             Off (OUT_A);
          }
          else if(SENSOR_3 < black3)
          {
             OnFwd (OUT_A);  // turn right
             Off (OUT_C);
          }
          else
          {
             OnFwd (OUT_A + OUT_C);  // go forward
          }
    }
    OnRev (OUT_A + OUT_C);
    Wait (10);
    Off (OUT_A + OUT_C);
}                                                                                // Task Main End

task monitor_GP2D12 ()                                                           // Read Distance Sensor and
 {                                                                               // Retrieve Reading to Variable
      while (true)
      {
            SetSensor(SENSOR_2,SENSOR_TOUCH); // Passive mode: starts measure
            sensorstate = 1;
            Wait(5); // Wait for GP2D12 conversion time (50ms)
            SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
            value=SENSOR_2; // reads and display value
            SetSensor(SENSOR_2,SENSOR_LIGHT); // Recharge sensor
            sensorstate = 0;
            Wait(25); // for 250 ms
      }
 }
 
task machine_gun ()
 {
      while(true)
      {
            PlayTone (150,3);
            Wait(8);
      }
 }