#define path_state 60       // Thresholds
#define white_state 20
#define speed_normal 85     // Speeds
#define plus_correction 95
#define minus_correction 65
#define pause 50            // Wait time after sensor change

task main()
{


    SetSensorLight(IN_1);
    SetSensorLight(IN_2);
    SetSensorLight(IN_3);

    bool check = false; // If true = Stop, if false = keep going

    while(true)
    {
        OnFwd(OUT_AB,speed_normal); //20

        while(true)
            {
                if(IN_2 >= path_state)
                {
                    if(IN_3 <= white_state && IN_1 <= white_state)
                        {}
                    else if(IN_1 >= white_state)
                        {
                        OnFwd(OUT_A, minus_correction);
                        OnFwd(OUT_B, plus_correction);
                        }
                    else if(IN_3 >= white_state)
                        {
                        OnFwd(OUT_A, plus_correction);
                        OnFwd(OUT_B, minus_correction);
                        }
                    else
                        {
                        ClearScreen();
                        NumOut(60, LCD_Line1, 1);
                        break;
                        }

                } //40

                if(IN_2 <= path_state)
                {
                    if(IN_3 <= white_state && IN_1 <= white_state)
                    {
                        while(true)
                        {
                            Wait(pause);
                            if(IN_2 >= path_state)
                                {
                                check = true;
                                Off(OUT_AB);
                                }
                            else
                                {
                                NumOut(60, LCD_Line2, 2);
                                break;
                                }
                        }


                        if(IN_1 >= white_state)
                            {
                            OnFwd(OUT_A, minus_correction);
                            OnFwd(OUT_B, plus_correction);  //60
                            }
                        else if(IN_3 >= white_state)
                            {
                            OnFwd(OUT_A, plus_correction);
                            OnFwd(OUT_B, minus_correction);
                            }
                        else
                            {
                            NumOut(60, LCD_Line3, 3);
                            break;
                            }
                    }


                while(check)
                    {
                        // Do fancy shit with Lego Gun here
                    }
            }
        }
    }
}