#if 1
//AUTHOR:       Metrobotit
//DESCRIPTION:  Zumo yrittää pysyä ringin (sumo radan) sisällä.
//              Ohjelma koostuu kahdesta loopista: alkuloopista jolla viedään zumo viivalle odottamaan IR-signaalia
//              ja päälooppi johon on ohjelmoitu painin liittyvä logiikka.
//              Strategia: Zumo yrittää välttää fyysisiä yhteenottoja.
//              Zumo väistää ultra sensoreilla edestä, tekee väistöliikkeitä
//              ja tulostaa ne koneelle kun saa osumia (neljästä eri suunnasta).
//              Zumo ajaa oletuksena radan poikki ja peruutta sit kääntyy kun saavuttaa viivan.
//NOTES:        Accelometer: default x 0, default y 0, default z 16000, pienikin kallistus vaikuttaa arvoihin +-500.
//              Oletusarvoinen nopeus: 150, seuraavassa versiossa nopeutta voisi lisätä.
void zmain(void)
{
    //Kutsutaan & alustetaan sensorit
    IR_Start();
    IR_flush();
    Ultra_Start();
    motor_start();
    motor_forward(0,0);
    reflectance_start();
    reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000);
    LSM303D_Start();
    send_mqtt("Zumo038/Debug", "Boot");

    //Luodaan & alustetaan muuttujat
    struct accData_ data;
    struct sensors_ dig;

    int summaX = 0;
    int summaY = 0;
    int summaSensori = 0;

    //Looppi ajaa zumon ekalle viivalle käynnistäessä
    for(;;){
        if(SW1_Read() == 0){
            vTaskDelay(500); //Wait 500 ms after launch

            while(true){
                reflectance_digital(&dig); //Tallennetaan sensorin arvot taulukkoon
                summaSensori = dig.l1 + dig.l2 + dig.l3 + dig.r1 + dig.r2 + dig.r3; //Sensoreiden arvot yht.

                motor_forward(100,0);//Liikkuu eteenpäin kunnes näkee viivan, sit odottaa IR-signaalia

                if(summaSensori > 2){
                    motor_forward(0,0);
                    IR_wait();
                    break;//breakkaa while loopista kun saa IR-signaalin
                }
            }
            break;//breakkaa for loopista kun saa IR-signaalin
        }
    }

    //Siirtyy radan keskelle ja aloittaa main lööpsin
    motor_forward(255, 666);


    for(;;)
    {
        motor_forward(150,0);//Default speed

        //Laskee 5 peräkkäisen accelometerin x- ja y-arvojen summan
        for(int i = 0; i < 5; i++){
            LSM303D_Read_Acc(&data);
            summaX += data.accX;
            summaY += data.accY;
        }

        //Laskee keskiarvon
        summaX /= 5; //Levossa: ka = 0
        summaY /= 5; // Levossa: ka = 0

        //Väistö liikkeet kun robotti törmää (toteuttaa aina yhden neljästä)
        if(abs(summaX) > abs(summaY)) { //Väistöliikkeet x-akselin suhteen
            if(summaX>12000){
                print_mqtt("Zumo038/hit","osuma edesta");
            }
            if(summaX<-12000){
                print_mqtt("Zumo038/hit","osuma takaa");
                motor_turn(200,5,500);
                motor_forward(0,0);
            }
        }

        if(abs(summaY) > abs(summaX)) {//Väistöliikkeet y-akselin suhteen
            if(summaY>12000){
                print_mqtt("Zumo038/hit","osuma vasemmalta");
                motor_tankturn_right(200,750);
                motor_forward(0,0);
            }
            if(summaY<-12000){
                print_mqtt("Zumo038/hit","osuma oikealta");
                motor_tankturn_left(200,750);
                motor_forward(0,0);
            }
        }

        reflectance_digital(&dig); //Tallennetaan sensorin arvot taulukkoon
        summaSensori = dig.l1 + dig.l2 + dig.l3 + dig.r1 + dig.r2 + dig.r3; //Sensoreiden arvot yht.


        if(summaSensori >=2) {//Peruuttaa ja kääntyy kun ultra sensorit havaitsevat esteen
            motor_forward(0,0);
            motor_backward(200,500);
            motor_tankturn_left(200,700);
            motor_forward(0,0);
        }

        //Välttää osuman edestä ultraäänisensorilla
        if(Ultra_GetDistance() < 15){
            reverse_turn(0,255,350);
            motor_forward(0,0);
        }
    }
 }

#endif