#include "pcl-helper.hpp"

// Get RGB values based on normals - texcoords, normals value [u v]
std::tuple<uint8_t, uint8_t, uint8_t> PCLHelper::get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
{
    const int w = texture.get_width(), h = texture.get_height();

    // convert normals [u v] to basic coords [x y]
    int x = std::min(std::max(int(texcoords.u * w + .5f), 0), w - 1);
    int y = std::min(std::max(int(texcoords.v * h + .5f), 0), h - 1);

    int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
    const auto texture_data = reinterpret_cast<const uint8_t *>(texture.get_data());
    return std::tuple<uint8_t, uint8_t, uint8_t>(texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);
}

PtrCloud PCLHelper::points_to_pcl(const rs2::points &points, const rs2::video_frame &color)
{

    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    PtrCloud cloud(new PointCloudRGB);

    // Config of PCL Cloud object
    cloud->width = static_cast<uint32_t>(sp.width());
    cloud->height = static_cast<uint32_t>(sp.height());
    cloud->is_dense = false;
    cloud->points.resize(points.size());

    auto tex_coords = points.get_texture_coordinates();
    auto vertices = points.get_vertices();

    // Iterating through all points and setting XYZ coordinates and RGB values
    for (int i = 0; i < points.size(); ++i)
    {
        cloud->points[i].x = vertices[i].x;
        cloud->points[i].y = vertices[i].y;
        cloud->points[i].z = vertices[i].z;

        std::tuple<uint8_t, uint8_t, uint8_t> current_color;
        current_color = get_texcolor(color, tex_coords[i]);

        // Reversed order- 2-1-0 because of BGR (Blue, Green, Red) model used in camera
        cloud->points[i].r = std::get<2>(current_color);
        cloud->points[i].g = std::get<1>(current_color);
        cloud->points[i].b = std::get<0>(current_color);
    }

    return cloud;
}

void PCLHelper::convert_and_save_ply(const std::string &fileName, const rs2::points &points, const rs2::video_frame &color)
{
    PtrCloud cloud = points_to_pcl(points, color);
    pcl::io::savePLYFile(fileName, *cloud);
}

void PCLHelper::save_to_ply(const std::string folderPath, const std::string &fileName, const PtrCloud &cloud)
{
    std::filesystem::create_directories(folderPath);
    pcl::io::savePLYFile(folderPath + fileName, *cloud);
}

int PCLHelper::icp(PtrCloud &cloud_target, PtrCloud &cloud_source, int iterations)
{
    std::cout << "\ntarget cloud size is: " << cloud_target->size() << std::endl;
    std::cout << "\nsource cloud size is: " << cloud_source->size() << std::endl;
    // TODO: make this faster
    // icp algo will align source pointcloud to fit target pointcloud
    pcl::IterativeClosestPoint<PointRGB, PointRGB> icp;
    icp.setMaximumIterations(iterations);
    icp.setInputSource(cloud_source); // will be transformed to fit target
    icp.setInputTarget(cloud_target); // will be the base cloud
    icp.align(*cloud_source);

    if (icp.hasConverged())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
        return 0;
    }
    else
    {
        PCL_ERROR("\nICP has not converged.\n");
        return (-1);
    }
}

PtrCloud PCLHelper::crop_cloud(PtrCloud &cloud, float min_x, float min_y, float min_z, float max_x, float max_y, float max_z)
{
    Eigen::Vector4f minPoint(min_x, min_y, min_z, 1);
    Eigen::Vector4f maxPoint(max_x, max_y, max_z, 1);

    pcl::CropBox<PointRGB> cropFilter;
    cropFilter.setInputCloud(cloud);
    cropFilter.setMin(minPoint);
    cropFilter.setMax(maxPoint);
    // cropFilter.setKeepOrganized(true);

    PtrCloud cropped_cloud(new PointCloudRGB);
    cropFilter.filter(*cropped_cloud);

    return cropped_cloud;
}