#include <librealsense2/rs.hpp>
#include <iostream>

void update_data_and_save(rs2::frame data_frame, rs2::points &points, rs2::pointcloud &pc, rs2::frame &color, const std::string &fileName);

int main()
try
{

    rs2::pointcloud original_pc;
    rs2::pointcloud filtered_pc;
    rs2::points original_points;
    rs2::points filtered_points;

    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.set_option(RS2_OPTION_MIN_DISTANCE, 0.0);
    thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, 0.5);
    spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 5);
    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 1);
    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 1);
    spat_filter.set_option(RS2_OPTION_HOLES_FILL, 5);
    // hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1);

    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();

    rs2::depth_frame filtered = depth_frame;

    /* 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).
    */

    filtered = thr_filter.process(filtered);
    filtered = depth_to_disparity.process(filtered);
    filtered = spat_filter.process(filtered);
    filtered = disparity_to_depth.process(filtered);
    // filtered = hole_filter.process(filtered);

    update_data_and_save(depth_frame, original_points, original_pc, color, "original");
    update_data_and_save(filtered, filtered_points, filtered_pc, color, "filtered");

    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 update_data_and_save(rs2::frame data_frame, rs2::points &points, rs2::pointcloud &pc, rs2::frame &color, const std::string &fileName)
{
    pc.map_to(color);                               // Map the colored image to the point cloud
    points = pc.calculate(data_frame);              // Generate pointcloud from the depth data
    points.export_to_ply(fileName + ".ply", color); // export pointcloud to .ply file
}