/*Follow the leader in a fixed formation */
#ifndef FOLLOWER_H
#define FOLLOWER_H

#include <ros/ros.h>
#include "class_model/sensor.h"
#include "class_model/mode.h"
#include "class_model/control.h"
#include "class_model/command.h"
#include "class_model/receiver.h"
#include "class_model/requestData.h"
#include "class_model/DroneStatus.h"
#include "class_model/PID.h"
#include "class_model/convert_degree.h"
#include "yaml-cpp/yaml.h"


class FollowerClass {
public:
    FollowerClass();
    virtual ~FollowerClass();
        //ClassObject
    ThreadGPSClass GPS_object;
    ModeClass mode_object;
    ControlClass control_object;
    ReceiverClass receiver_object;
    CommandClass command_object;
    RequestClass request_object;
    PIDClass PID_x;
    PIDClass PID_y;
    DroneStatus drone1;
    DroneStatus drone2;    
    DroneStatus drone3;
    DroneStatus drone4;
    DroneStatus drone5;
    DroneStatus self;
    DroneStatus samePlane[5];
    
    void follower(global_location** member_data, size_t index, int refID);
    int find_ref_drone(global_location** data, size_t index, int leaderID);

    float lon[3],lat[3];
    global_location vector_LT,vector_SM;
    

private:
    // ROS NodeHandle
    ros::NodeHandle node_handle_;

    global_location target_location;
    global_location follower_location;
    global_location leader_location;
    global_location leader_drone;
    global_location refpos;
    DroneStatus memberDrone[5];
    global_location (RequestClass::*ref)();

    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
    float distance = config["formation"]["distance"].as<float>();
    float angle = config["formation"]["angle"].as<float>(); 
    float error_lon,error_lat;
    float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
    float ignore_small = 0.50;
    int flag = 0;

    void calculate_position(float k,float theta,int plane);
    void plane(global_location* target, global_location* leader, global_location* follower);
    int direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member);
    void choose_leader();


};

#endif // FOLLOWER_H