#include <librealsense2/rs.hpp>
#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include "pcl-converter.hpp"

struct ColoredPoints
{
    rs2::points points;
    rs2::video_frame color;

    ColoredPoints(rs2::points p, rs2::video_frame c) : points(p), color(c) {}
};

void configure_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill);
void apply_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::depth_frame &depth_frame);
ColoredPoints scan(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe);

int main()
try
{
    rs2::pipeline pipe;

    pipe.start();

    // define filters
    rs2::threshold_filter thr_filter; // Threshold  - removes values outside recommended range
    rs2::spatial_filter spat_filter;  // Spatial    - edge-preserving spatial smoothing
    // rs2::hole_filling_filter hole_filter;                    // Hole filling - fills small holes in the depth image
    const std::string disparity_filter_name = "Disparity"; // Declare disparity transform from depth to disparity and vice versa
    rs2::disparity_transform depth_to_disparity(true);
    rs2::disparity_transform disparity_to_depth(false);
    configure_filters(thr_filter, spat_filter, 0.0, 0.5, 5, 1, 1, 5);

    ColoredPoints points = scan(thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);

    PCLConverter::convert_and_save_ply("output.ply", points.points, points.color);

    return EXIT_SUCCESS;
}
catch (const rs2::error &e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception &e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

void configure_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill)
{
    thr_filter.set_option(RS2_OPTION_MIN_DISTANCE, min_dist);             // 0.0
    thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, max_dist);             // 0.5
    spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, magnitude);       // 5
    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, smooth_alpha); // 1
    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, smooth_delta); // 1
    spat_filter.set_option(RS2_OPTION_HOLES_FILL, holes_fill);            // 5
    // hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1); //1
}

/* Apply filters.
    The implemented flow of the filters pipeline is in the following order:
    1. apply decimation filter
    2. apply threshold filter
    3. transform the scene into disparity domain
    4. apply spatial filter
    5. apply temporal filter
    6. revert the results back (if step Disparity filter was applied
    to depth domain (each post processing block is optional and can be applied independantly).
    */
void apply_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::depth_frame &depth_frame)
{
    depth_frame = thr_filter.process(depth_frame);
    depth_frame = depth_to_disparity.process(depth_frame);
    depth_frame = spat_filter.process(depth_frame);
    depth_frame = disparity_to_depth.process(depth_frame);
    // filtered = hole_filter.process(depth_frame);
}

ColoredPoints scan(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe)
{
    rs2::pointcloud pointcloud;
    rs2::points points;

    // let the auto exposure settle
    for (auto i = 0; i < 30; ++i)
        pipe.wait_for_frames();

    rs2::frameset data = pipe.wait_for_frames();
    rs2::video_frame color = data.get_color_frame();
    if (!color)
        color = data.get_infrared_frame();
    rs2::depth_frame depth_frame = data.get_depth_frame();

    apply_filters(thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, depth_frame);

    pointcloud.map_to(color);
    points = pointcloud.calculate(depth_frame);

    return ColoredPoints(points, color);
}