#define path_state 60       // Thresholds
#define white_state 20
#define speed_normal 90     // Speeds
#define plus 95
#define minus 87
#define minusM 60
#define plusP 100
#define pause 50            // Wait time after sensor change

task main()
{


    SetSensorLight(IN_1);   // Edge
    SetSensorLight(IN_2);   // Pfad


    bool check = false; // If true = Stop, if false = keep going
    int angle = 1;      // To determine weather it's left or right


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

        while(true)
            {
                if(IN_2 >= path_state)
                {
                    if(IN_1 >= path_state)
                        {

                        OnFwd(OUT_B,plus);
                        }
                    else if(IN_1 <= path_state)
                        {
                        OnFwd(OUT_A,plus);
                        }
                }
                else
                    break;
            }

        if(IN_2 <= path_state)
            {
            while(true)
                        {
                            Wait(pause);
                            if(IN_2 >= path_state)
                                {
                                check = true;
                                Off(OUT_AB);
                                goto gunnery;
                                }
                            else
                                {
                                Off(OUT_AB);
                                OnFwd(OUT_A,plusP);
                                OnRev(OUT_B,minusM);
                                Wait(5000);
                                break;
                                }
                        }
            }

                    while(check)
                        {
                        gunnery:
                            if(angle = 1 || angle = 3)
                                {
                                OnFwd(OUT_C,100);
                                Wait(1000);
                                check = false;
                                angle++;
                                break;
                                }
                            else if(angle = 2)
                                {
                                OnRev(OUT_C, 100);
                                Wait(1000);
                                check = false;
                                angle++;
                                break;
                                }
                            else
                                {
                                angle = 1;
                                check = false;
                                break;
                                }
                        }
            }
        }
    }