#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;
    
    // SelectionClass selection_object;

    global_location target,self,M1,M2;
    int ref_ID,checkLeader;
    
    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.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();

    std::cout <<M1.lat<<M1.ID<<std::endl;

    global_location pos[] = {target,self,M1,M2};
    global_location member[] = {M1,M2};
    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);

    checkLeader = check_leader(pos,index).is_leader;
    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(true){
        
        if(checkLeader == 1){
            // leader_object.leader(target);
        }else{
            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
        }
    }
    ros::waitForShutdown();

    return 0;

}