#include"class_model/requestData.h"
#include <cstdlib>


int command;
// json j_data;   //Don't set json as global variable

RequestClass::RequestClass() : node_handle_("~"){

  // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
  //                                  &RequestClass::Data_callback, this);
  formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                   &RequestClass::Message_callback, this);
  // mqtt_data = node_handle_.subscribe("/uav_message", 10,
  //                                  &RequestClass::Data_callback, this);

  drone1_data = node_handle_.subscribe("/Dron650_Flight_Information_reciver", 10,
                                   &RequestClass::drone1_callback, this);       //test bionic
  drone2_data = node_handle_.subscribe("/Dron550_Flight_Information_reciver", 10,
                                   &RequestClass::drone2_callback, this);
  drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10,
                                   &RequestClass::drone3_callback, this);
}

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

void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
  L_INFO(sensor->data);
}

void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {

	std::string data = message->data;
  // jsonToString(data);
  

}

global_location RequestClass::get_leader_GPS(){

  return leader_position; 
}

float RequestClass::get_leader_heading(){
  
  return heading;
}

int RequestClass::get_formation_message(){

  return command;
}

void RequestClass::jsonToString(std::string data){

}

void RequestClass::L_INFO(std::string data){
	
  // std::cout <<"string: "<< data << std::endl;

  // document.Parse(data.c_str());                  //china's library
  // leader_position.lat=document["lat"].GetInt();   
	// leader_position.lon=document["lon"].GetInt();
  // leader_position.alt=document["alt"].GetInt();
  // heading = document["heading"].GetInt();

  // std::cout << document["lat"].GetInt() << std::endl;
  // std::cout << document["lon"].GetInt() << std::endl;
  // std::cout << document["alt"].GetInt() << std::endl;
  // std::cout << document["heading"].GetInt() << std::endl;

  //********************************************//
  j_data = json::parse(data);        //open source

  // std::string lat = "",lon = "",alt = "",degree = "";
  // lat = j_data["lat"];
  // lon = j_data["lon"];
  // alt = j_data["alt"];
  // degree = j_data["heading"];
  
  leader_position.lat = std::stoi(j_data["lat"].get<std::string>());      
	leader_position.lon = std::stoi(j_data["lon"].get<std::string>());
  // leader_position.alt = std::stoi(j_data["alt"].get<std::string>()); 
  heading = std::stoi(j_data["heading"].get<std::string>()); 
  
  // std::cout <<"Json: " <<j_data << std::endl;
  // std::cout << leader_position.lat << std::endl;
  // std::cout << leader_position.lon << std::endl;
  // std::cout << leader_position.alt << std::endl;
  // std::cout << heading << std::endl;
}

/*test biomic*/
void RequestClass::drone1_callback(const std_msgs::String::ConstPtr &sensor) {
  drone_ID = 1;
  j_data = json::parse(sensor->data);        //open source
  // std::cout <<"Json: " <<j_data["lat"] << std::endl;
  Bio_INFO(j_data,drone_ID);
}
void RequestClass::drone2_callback(const std_msgs::String::ConstPtr &sensor) {
  drone_ID = 2;
  j_data2 = json::parse(sensor->data);        //open source
  // std::cout <<"Json: " <<j_data2["lat"] << std::endl;
  Bio_INFO(j_data2,drone_ID);
}
void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) {
  drone_ID = 3;
  j_data3 = json::parse(sensor->data); 
  Bio_INFO(j_data3,drone_ID);
}

void RequestClass::Bio_INFO(json json_data,int drone_ID){

  std::remove(ID_array.begin(),ID_array.end(),self_ID);

  if (drone_ID == self_ID){
    self_position.lat = json_data["lat"]; 
	  self_position.lon = json_data["lon"]; 
    // self_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
    self_position.heading = json_data["heading"]; 
  }else if (drone_ID == ID_array[0]){
    M1_position.lat = json_data["lat"];   
	  M1_position.lon = json_data["lon"]; 
    // M1_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
    M1_position.heading = json_data["heading"];
  }else if (drone_ID == ID_array[1]){
    M2_position.lat = json_data["lat"]; 
	  M2_position.lon = json_data["lon"]; 
    // M2_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
    M2_position.heading = json_data["heading"];
  }

}
global_location RequestClass::get_self_GPS(){

  return self_position; 
}
global_location RequestClass::get_M1_GPS(){

  return M1_position; 
}
global_location RequestClass::get_M2_GPS(){

  return M2_position; 
}