#include "class_model/mission.h"
#include "class_model/choose_leader.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();   

    ModeClass mode_object;
    ControlClass control_object;
    ReceiverClass receiver_object;
    MissionClass mission_object;
    CommandClass command_object;
    // SelectionClass selection_object;
    
    
    mode_object.set_Mode("GUIDED");
    control_object.arm();
    control_object.takeoff(4.5);
    sleep(5);

    // selection_object.choose_leader();
    // mission_object.start();   //default hover in goose formation

    while(ros::ok()){

    //json_object.get_command();       //for follower

    }

    ros::waitForShutdown();

    return 0;
}


/*
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(5,0);
    // }else if (type == "hex"){
    // select_formation.Hex_formation();
    // }
    select_formation.row_formation(-5,0);
    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");
    }
     }
*/