#include "class_model/mission.h"

int main(int argc, char **argv) {

    // Init ROS node
    ros::init(argc, argv, "drone1_node");

    // reate Assync spiner
    ros::AsyncSpinner spinner(0);
	spinner.start();   

    ThreadGPSClass gps_object;
    ModeClass mode_object;
    ControlClass control_object;
    SelectClass select_formation;
    ReceiverClass receiver_object;
    MissionClass mission_object;
    CommandClass command_object;
    std::string type = "";

    mode_object.set_Mode("GUIDED");
    control_object.arm();
    control_object.takeoff(4.5);
    sleep(5);

    while(ros::ok()){

    type = receiver_object.check_command();  
    // // ROS_INFO("%s",type.c_str());
    
    if(type == "" || type == "init"){
        select_formation.goose_formation();
        ROS_INFO("init formation");
    }else if(type == "line"){
        select_formation.line_formation();
        // ROS_INFO("line foemation");
    }else if(type == "row"){
        select_formation.row_formation();
    }else if(type == "circle"){
        select_formation.circle_formation();
    }else if(type == "stop"){
        select_formation.stop();
    }else if(type == "land"){
        mode_object.set_Mode("LAND");
    }
    

    //mission_object.cruise(20,20);
    // select_formation.goose_formation();
    // }else if (type == "hex"){
    // select_formation.Hex_formation();
    // }
    // select_formation.Hex_formation();
    sleep(0.5);
    // command_object.set_velocity(0 ,0 ,0,0,1);

    if(type == "stop"){
        select_formation.stop();
    }else if(type == "land"){
        mode_object.set_Mode("LAND");
    }
     }

    
    // select_formation.square();

    // RequestClass test_object;

    ros::waitForShutdown();

    return 0;
}