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

global_location leader_position;
std::string command; 

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

  mqtt_data = node_handle_.subscribe("/uav_message", 10,
                                   &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) {

	std::string data = sensor->data;
  jsonToInt(data);

}

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

	std::string data = message->data;
  

}

global_location RequestClass::get_leader_GPS(){

  return leader_position; 
}

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

std::string RequestClass::get_formation_message(){

  return command;
}

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

  std::string lat,lon,degree;
  std::string list[5]={"","","","",""};
  int j = 0;

  // lat.append(data,9,10);
  // lon.append(data,29,10);
  // degree.append(data,53,6);

  // heading = std::stoi(degree);
  // leader_position.lat=std::stoi(lat);   
	// leader_position.lon=std::stoi(lon);

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

  leader_position.lat=std::stoi(list[0]);   
	leader_position.lon=std::stoi(list[1]);
  leader_position.alt=std::stoi(list[2]);
  heading = std::stoi(list[3]);



  // std::cout<<sizeof(data)<<std::endl;
  //std::cout<<data<<std::endl;
  // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
  // ROS_INFO("leader_heading: %f",heading);
}

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 = list[1];


  // std::cout<<sizeof(data)<<std::endl;
  //std::cout<<data<<std::endl;
  // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
  // ROS_INFO("leader_heading: %f",heading);
}