#include "class_model/FindLeader.h"
#include "class_model/Leader.h"
#include "class_model/follower.h"
#include "yaml-cpp/yaml.h"

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

    // Init ROS node
    ros::init(argc, argv, "drone2_node");
 
    // reate Assync spiner
    ros::AsyncSpinner spinner(0);
	spinner.start();
    // ros::Rate rate(5);   

    ModeClass mode_object;
    ControlClass control_object;
    RequestClass request_object;
    FollowerClass follower_object;
    LeaderClass leader_object;
    ReceiverClass receiver_object;
    

    global_location target,self,M1,M2,M3,M4;
    int ref_ID,checkLeader,leaderID,flag;
    std::string type = "",pre_type ="x";
    
    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
    target.lon = config["routh1"]["x"].as<float>()*1e7;
    target.lat = config["routh1"]["y"].as<float>()*1e7;

    sleep(5);
    self = request_object.get_self_GPS();
    M1 = request_object.get_M1_GPS();
    M2 = request_object.get_M2_GPS();
    M3 = request_object.get_M3_GPS();
    M4 = request_object.get_M4_GPS();


    global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
    global_location* member[] = {&M1,&M2,&M3,&M4};
    size_t index = sizeof(pos)/sizeof(pos[0]);
    size_t m_index = sizeof(member)/sizeof(member[0]);

    mode_object.set_Mode("GUIDED");
    control_object.arm();
    control_object.takeoff(2);
    // 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 ="";
    //     }   
    // }

    checkLeader = check_leader(*pos,index).is_leader;
    leaderID = check_leader(*pos,index).leader_ID;

    if(checkLeader != 1){
        ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
    }

    while(ros::ok()){

        // type = receiver_object.check_command();
        self = request_object.get_self_GPS();
        M1 = request_object.get_M1_GPS();
        M2 = request_object.get_M2_GPS();
        M3 = request_object.get_M3_GPS();
        M4 = request_object.get_M4_GPS();

        if(checkLeader == 1){
            leader_object.leader(target);
        }else{
            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
        }

        // if(type == "land"){
        //     mode_object.set_Mode("LAND");
        // }

        
    }
    ros::waitForShutdown();

    return 0;

}