#include "class_model/Leader.h"
#include "class_model/convert_degree.h"

LeaderClass::LeaderClass() : node_handle_(""){
  	
}

LeaderClass::~LeaderClass() { ros::shutdown(); }

void LeaderClass::leader(global_location target){

    std::string command = "",pre_command = receiver_object.check_command();;
    std_msgs::String message;

    target_location.lon = target.lon*1e5;
    target_location.lat = target.lat*1e5;
    sleep(3);
    flag = 0;
    heading_status = 0;
    while(true){
        if(flag==0){
            go2target(target.lon,target.lat);
        }else{
            // next_command.publish(message);
            ROS_INFO("break");
            break;
        }

        command = receiver_object.check_command();
        while(command =="stop" || command == "land"){
            // command_object.set_velocity(0,0,0,0,1);
            // command = receiver_object.check_command();
          
            if(command == "land"){
                mode_object.set_Mode("LAND");
            }else if(command != "stop"){
                break;
            }
            
        }

        if(command != pre_command){
            // ROS_INFO("change formation");
            break;
        }
        pre_command = command;
    }
}

void LeaderClass::go2target(float x,float y){

    current_location=GPS_object.gps_position(); 

    float target_heading,error_heading;
    float heading = GPS_object.get_heading();
    target_heading = ConvertDeg(x,y);   

    // float error_yaw = 0.2;
    // error_heading =  target_heading - heading;
    // error_yaw = check_direction(error_heading)*error_yaw;              //check yaw direction

    while(heading_status != 1){
        face2target(target_heading);
    }

    drone_speed.x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid);   //leader_pid defined in header file
    drone_speed.y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid);

    // float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
    // float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );

    if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){                     //ignore small errors                  
        drone_speed.x = 0;
    }
    if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
        drone_speed.y = 0;
    }
    if (drone_speed.x < -1){                     //max velociy                 
        drone_speed.x = -1;
    }
    if (drone_speed.x > 1){                                      
        drone_speed.x = 1;
    }
    if (drone_speed.y < -1){
        drone_speed.y = -1;
    }
    if (drone_speed.y > 1){
        drone_speed.y = 1;
    }

    if (drone_speed.x == 0 && drone_speed.y == 0){
        flag = 1;
    }

    PubData_object.publishData(drone_speed);
    command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
    
    // ROS_INFO("slope:%f,%f,%f",slope,x,y);
    ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
    // ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
    // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat);
    // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
    ROS_INFO("************************************");
    


}

void LeaderClass::face2target(float target_heading){

    float error_heading;
    float heading = GPS_object.get_heading();
    float error_yaw = 0.2;
    error_heading =  target_heading - heading;
    error_yaw = check_direction(error_heading)*error_yaw; 

    if (error_heading < 5 && error_heading > -5 ){
        error_yaw = 0;
        heading_status = 1;
    }
    command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
    ROS_INFO("face2target...");
}