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

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

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

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

}

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

void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
  StringToJson(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){

  std::string list[1]={""};
  int j = 0;

  for(int i=0;i<data.length();i++){
    if(data[i] == ':'){
      i+=3;
      while(data[i] != ','){
        
        if(data[i] == ',' || data[i] == '}'){
            j++;
            break;
        }

        list[j]=list[j]+data[i];
        i++; 
        
        if(j>sizeof(list)){
            break;
        }     
      }
    }
  }

  command = std::stoi(list[1]);

  // ROS_INFO("command: %s",command);
}

void RequestClass::StringToJson(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=j_data["lat"];   
	leader_position.lon=j_data["lon"];
  leader_position.alt=j_data["alt"];
  heading = j_data["heading"];
  
  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;
}