#include <cpp_api/navigation_bridge.h>
#include <iostream>

int valeur;

int main(int argc, char *argv[])
{
    Navigation nav;

    std::cout<<"Menu déplacement du drone:"<<std::endl;
    std::cout<<"1- ENSISA Lumière"<<std::endl;
    std::cout<<"2- CNRS"<<std::endl;
    std::cout<<"3- ENSISA Werner"<<std::endl;
    std::cout<<"4- FLSH"<<std::endl;
    std::cout<<"5- ENSCMU"<<std::endl;
    std::cout<<"6- Maison de l'étudiant"<<std::endl;

    std::cin>>valeur;

    nav.waypoint_clear();

    switch(valeur){
        case 1:

            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers ENSISA Lumière"<<std::endl;
            nav.position_set_global(47.72941672911752, 7.310123478487526, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }

        break;
        case 2:
            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers CNRS"<<std::endl;
            nav.position_set_global(47.7296187925679, 7.309835141018425, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }

        break;
        case 3:
            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers ENSISA Werner"<<std::endl;
            nav.position_set_global(47.73037869748831, 7.310995746290018, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }
        break;
        case 4:
            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers FLSH"<<std::endl;
            nav.position_set_global(47.730203698706084, 7.311611313258936, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }
        break;
        case 5:
            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers ENSCMU"<<std::endl;
            nav.position_set_global(47.73238368951076, 7.3133312582133385, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }
        break;
        case 6:
            if(nav.take_off(5.0)){
                std::cout<<"\nTakeOff Failed.. exiting\n";
                exit(0);
            }

            std::cout<<"Navigation vers Maison de l'Etudiant"<<std::endl;
            nav.position_set_global(47.73241630853428, 7.314371577421184, 7.0, 0.12, 2.0, false, true);

            if(nav.land(false)){
                std::cout<<"\nLand Failed.. exiting\n";
                exit(0);
            }
        break;

    }
}