#define S1_ON_TRACK SENSOR_1 > black - treshold       //Egyes szemzor a pálya felett van
#define S3_ON_TRACK SENSOR_3 > black - treshold       //Hármas szemzor a pálya felett van
#define ON_THE_TRACK S3_ON_TRACK && !(S1_ON_TRACK)    //A robot a pályán van
#define TURN_90_TIME 75                               //90°-os fordulathoz szükséges idő
#define COLL_FWD_T 80                                 //Két fal melletti "tapogatózás" közti idő

int black, white, treshold;        //Kezdeti kalibráláshoz szükséges változók
int searching;

sub calibrate()                          //Színek bekalibrálása
{
    black = SENSOR_3;                    //A  3-as szenzor alatti a fekete
    white = SENSOR_1;                    //Az 1-es szenzor alatti a fehér
    treshold = abs(black - white) / 2;   //A kettő közti részt megfelezzük
                                         //efelett pályának tekintjük, alatta nem
}

void turn_right()     //Jobbra fordulás
{
    OnFwd(OUT_A);
    OnRev(OUT_C);
}

void turn_left()      //Balra fordulás
{
    OnFwd(OUT_C);
    OnRev(OUT_A);
}

void forward()        //Egyenesen előre
{
    OnFwd(OUT_A+OUT_C);
}

void reverse()        //Hátra
{
    OnRev(OUT_A+OUT_C);
}

void halt()           //Megállás
{
    Off(OUT_A+OUT_C);
}

void turn_90_left()      //90°-os fordulat balra
{
    turn_left();         //Balra fordulunk
    Wait(TURN_90_TIME);  //a megfelelő ideig
    halt();
}

void turn_90_right()     //90°-os fordulat balra
{
    turn_right();        //Jobbra fordulunk
    Wait(TURN_90_TIME);  //a megfelelő ideig
    halt();
}

task follow_track()      //Pálya követése
{
    start collision;     //Figyeljük az ütközést

    while(true)
    {
        forward();                 //Előre
        until(!(ON_THE_TRACK));    //amíg a pályán vagyunk

        if(!(S1_ON_TRACK))         //Ha balra tértünk le
        {
            turn_right();          //jobbra fordulunk
        }
        else                       //Különben (ha jobbra) pedig
        {
            turn_left();           //balra
        }
        until(ON_THE_TRACK);       //míg újra nem leszünk rajt a pályán
    }
}

task collision()        //Ütközés detektálása és akadály megkerülése
{
    searching = 0;      //Még nem keressük a pályát
    while(true)
    {
        until(SENSOR_2 == 1);   //Ha ütközünk
        stop follow_track;      //abbahagyjuk a pálya követését
        start blinking;         //és villogással jelezzük, hogy kerülünk

        do
        {
            reverse();          //Visszatolatunk
            Wait(10);           //egy kicsit
            halt();
            turn_90_right();    //Jobbra 90°
            forward();          //Előre
            Wait(COLL_FWD_T);   //"tapogatózási-időnyit"
            if(searching == 0)       //Ha még nem keressük a pályát
            {
                searching = 1;       //akkor jelezzük, hogy mostantól keressük
                start search_track;  //és el is indítjuk
            }
            turn_90_left();
            forward();
            ClearTimer(0);
            until(SENSOR_2 == 1 || Timer(0)>=5);  //Ha ütközünk vagy
                                                  //½ másodpercig tudunk egyenesen haladni
            halt();                               //akkor megállunk
        } while(SENSOR_2 == 1);                   //Amíg ütközés miatt állunk meg
                                                  //Ha nem, akkor túljutottunk az első sarkon

        turn_90_left();
        forward();                   //Előre,

        while(true)
        {
            until(SENSOR_2 == 1);    //amíg nem ütközünk
            halt();
            reverse();               //Tolassunk
            Wait(10);                //egy kicsit
            halt();
            turn_90_right();         //Jobbra 90°
            forward();               //Előre
            Wait(COLL_FWD_T);        //egy tapogatózásnyit
            turn_90_left();
            forward();
        }
    }
}

task search_track()      //Pálya keresése
{
    until(!(S1_ON_TRACK) && !(S3_ON_TRACK));  //Amíg mindkét szenzor nincs a pálya fölött
    stop collision;     //Akadálykikerülés leállítása
    stop blinking;      //Villogás leállítása
    Off(OUT_B);         //A lámpa biztosan legyen kikapcsolva
    halt();
    start follow_track; //Kövessük a pályát (mivel mindkét érzékelő a pálya fölött van, jobbra fogunk fordulni)
}

task blinking()   //Villogás
{
    while(true)
    {
        On(OUT_B);  //Lámpa be
        Wait(25);   //¼ másodpercig marad így
        Off(OUT_B); //aztán kikapcs
        Wait(25);   //szintén ¼ másodpercig
    }
}

task main()
{
    SetSensor(SENSOR_1, SENSOR_LIGHT);   //Kezdeti
    SetSensor(SENSOR_2, SENSOR_TOUCH);   //szenzor
    SetSensor(SENSOR_3, SENSOR_LIGHT);   //beállítások
    
    ClearSensor(SENSOR_1);   //Biztos ami biztos
    ClearSensor(SENSOR_2);   //a szenzorok
    ClearSensor(SENSOR_3);   //nullázása
    
    calibrate();             //Kezdeti kalibrálás
    
    start follow_track;      //Kezjünk el menni a pályán
}