#pragma once
#include <librealsense2/rs.hpp>
#include <tuple>
#include <filesystem>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/crop_box.h>

typedef pcl::PointXYZRGB PointRGB;
typedef pcl::PointCloud<PointRGB> PointCloudRGB;
typedef PointCloudRGB::Ptr PtrCloud;

class PCLHelper
{
public:
    using PointRGB = pcl::PointXYZRGB;
    using PointCloudRGB = pcl::PointCloud<PointRGB>;
    using PtrCloud = PointCloudRGB::Ptr;
    static PtrCloud points_to_pcl(const rs2::points &points, const rs2::video_frame &color);
    static void convert_and_save_ply(const std::string &fileName, const rs2::points &points, const rs2::video_frame &color);
    static void save_to_ply(const std::string folderPath, const std::string &fileName, const PtrCloud &cloud);
    static int icp(PtrCloud &cloud_target, PtrCloud &cloud_source, int iterations = 1);
    static PtrCloud crop_cloud(PtrCloud &cloud, float min_x, float min_y, float min_z, float max_x, float max_y, float max_z);

private:
    static std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords);
};