#ifndef _MAP_LOADER_H_
#define _MAP_LOADER_H_

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <vector>
#include <pcl_ros/transforms.h>
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/reader.h"
#include "rapidjson/ostreamwrapper.h"
class MapLoader{
public:



    MapLoader(ros::NodeHandle &iNh, ros::NodeHandle &iNhPrivate);

private:
    ros::Publisher m_iPubCloudMap_;
    ros::Publisher m_iPubCloudTrajectory_;

    std::vector<std::string> m_vMapFileName_;
    std::string pcd_path_trajectory_;
    std::string m_cSaveTransformPointCloudPath_;
    float tf_x_{}, tf_y_{}, tf_z_{}, tf_roll_{}, tf_pitch_{}, tf_yaw_{};
    sensor_msgs::PointCloud2 CreatePcd();
    pcl::PointCloud<pcl::PointXYZI>::Ptr  __LoadPointCloudFromPcd();
    pcl::PointCloud<pcl::PointXYZI>::Ptr  __LoadPointCloudFromJson();
    void  __DeletPoint(pcl::PointCloud<pcl::PointXYZI>::Ptr pointCloud);
    sensor_msgs::PointCloud2 LoadPcd(std::string pcdPath);
    sensor_msgs::PointCloud2 __TransformMap(sensor_msgs::PointCloud2 & iMsgIn) const;
    sensor_msgs::PointCloud2 __TransformMap(pcl::PointCloud<pcl::PointXYZI>::Ptr in) const;
    static int functionSave(std::string cFileName, const pcl::PointCloud<pcl::PointXYZI>::Ptr& pCloudMap);
    template<typename PointT>
    inline int loadJson(const std::string &file_name, pcl::PointCloud<PointT> &cloud);
    double __Distance(pcl::PointXYZI p1, pcl::PointXYZI p2){
        std::sqrt(std::pow(p1.x - p2.x,2) + std::pow(p1.y - p2.y,2) + std::pow(p1.z - p2.z,2));
    }
    std::string __ReadFile(std::string cFileName);
}; //MapLoader

#endif