/************************************************************
AI Robotics
Fall 2003

Line Follower
Karan M. Gupta (karanmg@gmail.com)
*************************************************************/


// PERFECT II

#define THRESHOLD_1 775
#define THRESHOLD_2 735
#define THRESHOLD_3 745
#define PWR_A 3
#define PWR_C 3
#define PWR_OUTER 3
#define PWR_INNER 0
#define WAIT_TIME 0

/*
#define THRESHOLD_1 775
#define THRESHOLD_2 735
#define THRESHOLD_3 745
#define PWR_A 7
#define PWR_C 7
#define PWR_OUTER 3
#define PWR_INNER 0
#define WAIT_TIME 0
*/
task main()
{
     setup();
     while(true)
     {
                // if sensor2 reads black, keep going...
                while(SENSOR_2 > THRESHOLD_2)
                {
                    Fwd(OUT_A);
                    Fwd(OUT_C);
                    SetPower(OUT_A, PWR_A);
                    SetPower(OUT_C, PWR_C);
                    On(OUT_A);
                    On(OUT_C);
                 }
                 // if sensor2 reads white
                 while(SENSOR_2 < THRESHOLD_2)
                 {
                                     Fwd(OUT_A);
                                     Fwd(OUT_C);
                                     SetPower(OUT_A, PWR_A);
                                     SetPower(OUT_C, PWR_C);
                                     On(OUT_C);
                                     On(OUT_A);
                        // if sensor1 reads black there is a left turn, turn
                        // until sensor1 reads white
                        if(SENSOR_1 > THRESHOLD_1)
                        {
                                    Fwd(OUT_C);
                                    Rev(OUT_A);
                                    SetPower(OUT_C, PWR_OUTER);
                                    SetPower(OUT_A, PWR_INNER);
                                    On(OUT_C);
                                    On(OUT_A);
                                    while(SENSOR_2 < THRESHOLD_2)
                                    {
                                         if (SENSOR_3 > THRESHOLD_3)
                                            break;
                                    }
                                    ResetBomb();
                         }
                         // if sensor3 reads black, there is a right turn, turn
                         // until sensor3 reads white
                         if(SENSOR_3 > THRESHOLD_3)
                         {
                                     Fwd(OUT_A);
                                     Rev(OUT_C);
                                     SetPower(OUT_A, PWR_OUTER);
                                     SetPower(OUT_C, PWR_INNER);
                                     On(OUT_A);
                                     On(OUT_C);
                                     while(SENSOR_2 < THRESHOLD_2)
                                     {
                                          if (SENSOR_1 > THRESHOLD_1)
                                            break;
                                     }
                                     ResetBomb();
                          }
                 }
     }
}

void ResetBomb()
{
     Fwd(OUT_A);
     Fwd(OUT_C);
     SetPower(OUT_A, PWR_A);
     SetPower(OUT_C, PWR_C);
     //Wait(WAIT_TIME);
}

void setup()
{
     SetSensor(SENSOR_1, SENSOR_LIGHT);
     SetSensor(SENSOR_3, SENSOR_LIGHT);
     SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);
     SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);
}