#include "class_model/mission.h"
#include "class_model/choose_leader.h"
#include "class_model/PubInfo.h"
#include "yaml-cpp/yaml.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();
    // ros::Rate rate(5);   

    ModeClass mode_object;
    ControlClass control_object;
    ReceiverClass receiver_object;
    MissionClass mission_object;
    CommandClass command_object;
    SelectClass select_formation;
    PubInfoClass PubInfo_object;
    
    // SelectionClass selection_object;
    
    // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
    std::string type = "",pre_type ="x";
    int flag = 0;

    mode_object.set_Mode("GUIDED");
    while(flag!=1){
        type = receiver_object.check_command();
        if(type == "go"){
            mode_object.set_Mode("GUIDED");
            control_object.arm();
            control_object.takeoff(2);
            sleep(2);
            flag = 1;
            type ="";
        }   
    }


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

    // INFO msg;
    // msg.type = "V_formation";
    // msg.mission = "fly2target";

    

    while(ros::ok() && flag == 1){
        type = receiver_object.check_command();
        
    if(pre_type == type){
        type = "alreadly_set";
        // ROS_INFO("set");
    }
    
// TODO: switch case
    if(type == "init"){
        select_formation.goose_formation();
        ROS_INFO("init formation");
    }else if(type == "line"){
        select_formation.line_formation();
        pre_type ="line";
    }else if(type == "row"){
        select_formation.row_formation();
        pre_type ="row";
    }else if(type == "v1"){                         //PID error
        select_formation.goose_formation(0,5);
        pre_type ="v1";
    }else if(type == "v2"){
        select_formation.line_formation(-5,0);
        pre_type ="v2";
    }else if(type == "v3"){
        select_formation.goose_formation(0,-5);
        pre_type ="v3";
    }else if(type == "v4"){
        select_formation.row_formation(5,0);
        pre_type ="v4";
    }else if(type == "stop"){
        select_formation.stop();
    }else if(type == "land"){
        // ROS_INFO("Land");
        mode_object.set_Mode("LAND");
    }

    }

    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");
    }
     }
*/