#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, "drone5_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,leaderIndex,flag;
    int routeNum,routeIndex,OnTarget;
    float errorX,errorY;
    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["route1"]["x"].as<float>()*1e7;
    target.lat = config["route1"]["y"].as<float>()*1e7;
    routeNum = config["routeNum"]["num"].as<int>();

    std::string route[4]={"route1","route2","route3","route4"};

    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(4);
    // 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();

            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;

            checkLeader = check_leader(*pos,index).is_leader;
            leaderID = check_leader(*pos,index).leader_ID;
            leaderIndex = check_leader(*pos,index).leader_index;
         
            std::cout << leaderID << std::endl;
            std::cout << target.lon << std::endl;
            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
            std::cout <<"x: "<< errorX << std::endl;
            std::cout <<"y: "<< errorY << std::endl;

        while(errorX > 100 || errorY > 100){

            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){
                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
                leader_object.leader(target);

                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
                // leader_object.leader(target);

                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
                // leader_object.leader(target);

                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500; 
                // 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
            }
            // OnTarget = leader_object.isOnTarget();
            // std::cout <<"tar: "<< OnTarget << std::endl;
            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
            std::cout <<"x: "<< errorX << std::endl;
            std::cout <<"y: "<< errorY << std::endl;
        }

        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
            routeIndex++;
            // OnTarget = 0;
        }else if(routeIndex >= routeNum){
            mode_object.set_Mode("LAND");
        }
        // if(type == "land"){
        //     mode_object.set_Mode("LAND");
        // }

        // mode_object.set_Mode("LAND");
    }
    ros::waitForShutdown();

    return 0;

}