// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "example.hpp"          // Include short list of convenience functions for rendering

#include <algorithm>            // std::min, std::max
#define _USE_MATH_DEFINES
#include <math.h>
#include <queue>
#include <unordered_set>
#include <map>
#include <thread>
#include <atomic>
#include <mutex>

#include <iostream>
#include <iomanip>
#include <map>
#include <set>
#include <cstring>
#include <cmath>
#include <limits>
#include <thread>
using namespace std;
using namespace rs2;


std::vector<std::string> tokenize_floats(string input, char separator) {
	std::vector<std::string> tokens;
	stringstream ss(input);
	string token;

	while (std::getline(ss, token, separator)) {
		tokens.push_back(token);
	}

	return tokens;
}

void print_intrinsics(const rs2_intrinsics& intrinsics)
{
	stringstream ss;
	ss << left << setw(14) << "  Width: " << "\t" << intrinsics.width << endl <<
		left << setw(14) << "  Height: " << "\t" << intrinsics.height << endl <<
		left << setw(14) << "  PPX: " << "\t" << setprecision(15) << intrinsics.ppx << endl <<
		left << setw(14) << "  PPY: " << "\t" << setprecision(15) << intrinsics.ppy << endl <<
		left << setw(14) << "  Fx: " << "\t" << setprecision(15) << intrinsics.fx << endl <<
		left << setw(14) << "  Fy: " << "\t" << setprecision(15) << intrinsics.fy << endl <<
		left << setw(14) << "  Distortion: " << "\t" << rs2_distortion_to_string(intrinsics.model) << endl <<
		left << setw(14) << "  Coeffs: ";

	for (auto i = 0u; i < sizeof(intrinsics.coeffs) / sizeof(intrinsics.coeffs[0]); ++i)
		ss << "\t" << setprecision(15) << intrinsics.coeffs[i] << "  ";

	float fov[2];
	rs2_fov(&intrinsics, fov);
	ss << endl << left << setw(14) << "  FOV (deg): " << "\t" << setprecision(4) << fov[0] << " x " << fov[1];

	cout << ss.str() << endl << endl;
}

void print_extrinsics(const rs2_extrinsics& extrinsics)
{
	stringstream ss;
	ss << " Rotation Matrix:\n";

	// Align displayed data along decimal point
	for (auto i = 0; i < 3; ++i)
	{
		for (auto j = 0; j < 3; ++j)
		{
			std::ostringstream oss;
			oss << extrinsics.rotation[j * 3 + i];
			auto tokens = tokenize_floats(oss.str().c_str(), '.');
			ss << right << setw(4) << tokens[0];
			if (tokens.size() > 1)
				ss << "." << left << setw(12) << tokens[1];
		}
		ss << endl;
	}

	ss << "\n Translation Vector: ";
	for (auto i = 0u; i < sizeof(extrinsics.translation) / sizeof(extrinsics.translation[0]); ++i)
		ss << setprecision(15) << extrinsics.translation[i] << "  ";

	cout << ss.str() << endl << endl;
}

void getRealSense3DPoint(float result3DPoint[3], int c, int r, const rs2::depth_frame& depth, rs2_intrinsics* depth_intrin, rs2_intrinsics* color_intrin, rs2_extrinsics* color_extrin_to_depth, rs2_extrinsics* depth_extrin_to_color)
{

	float rgb_pixel[2], depth_pixel[2];
	int int_depth_pixel[2];
	rgb_pixel[0] = float(c); // col in RGB frame
	rgb_pixel[1] = float(r); // row  in RGB frame

							 //auto depth_sensor = profile.get_device().first<rs2::depth_sensor>();
	float depth_scale = depth.get_units(); //depth_sensor.get_depth_scale();

	rs2_project_color_pixel_to_depth_pixel(depth_pixel,
		reinterpret_cast<const uint16_t*>(depth.get_data()),
		depth_scale,
		0.20, 2.0,  // From 0 to 2 meters Min to Max depth
		depth_intrin,
		color_intrin,
		color_extrin_to_depth,
		depth_extrin_to_color, rgb_pixel);

	int_depth_pixel[0] = (int)round(depth_pixel[0]);
	int_depth_pixel[1] = (int)round(depth_pixel[1]);
	depth_pixel[0] = int_depth_pixel[0];
	depth_pixel[1] = int_depth_pixel[1];

	auto dist = depth.get_distance(static_cast<int>(depth_pixel[0]), static_cast<int>(depth_pixel[1]));

	rs2_deproject_pixel_to_point(result3DPoint, depth_intrin, depth_pixel, dist);

}

// Helper functions
void register_glfw_callbacks(window& app, glfw_state& app_state);

int main(int argc, char * argv[]) try
{
    // Create a simple OpenGL window for rendering:
    window app(1280, 720, "RealSense Pointcloud Example");
    // Construct an object to manage view state
    glfw_state app_state;
    // register callbacks to allow manipulation of the pointcloud
    register_glfw_callbacks(app, app_state);

    // Declare pointcloud object, for calculating pointclouds and texture mappings
    rs2::pointcloud pc;
    // We want the points object to be persistent so we can display the last cloud when a frame drops
    rs2::points points;
	rs2::align align_to(RS2_STREAM_COLOR);

    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Start streaming with default recommended configuration
    //pipe.start();
	rs2::config cfg;

	//cfg.enable_stream(RS2_STREAM_DEPTH); // Enable default depth
	// For the color stream, set format to RGBA
	// To allow blending of the color frame on top of the depth frame
	//cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_RGBA8);
	cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 15);
	cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGBA8, 15);

	auto profile = pipe.start(cfg);

	while (app) // Application still alive?
	{
		// Wait for the next set of frames from the camera
		auto frames = pipe.wait_for_frames();

		auto depth = frames.get_depth_frame();
		auto color = frames.get_color_frame();

		rs2_intrinsics intr_depth = depth.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
		rs2_intrinsics intr_color = color.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
		rs2_extrinsics color_extrin_to_depth = color.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(depth.get_profile().as<rs2::video_stream_profile>());
		rs2_extrinsics depth_extrin_to_color = depth.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(color.get_profile().as<rs2::video_stream_profile>());


		// pixel in color image
		float u = 0.609375; // 780
		float v = 0.488889; // 352

		int w = color.get_width();
		int h = color.get_height();

		int c = w * u;
		int r = h * v;

		float pixel[2];
		pixel[0] = c;
		pixel[1] = r;

		float point_in_color_coordinates[3]; // 3D point in color sensor coordinates
		float point_in_depth_coordinates[3]; // 3D point in depth sensor coordinates

		// method#2
		getRealSense3DPoint(point_in_depth_coordinates, pixel[0], pixel[1], depth, &intr_depth, &intr_color, &color_extrin_to_depth, &depth_extrin_to_color);
		std::cout << "method#2 wxh=" << w << "," << h << ", uv(" << c << "," << r << ") ----> " << " [" << point_in_color_coordinates[0] << "," << point_in_color_coordinates[1] << "," << point_in_color_coordinates[2] << "] --> 3d point in depth sensor coordinates dp[" << point_in_depth_coordinates[0] << "," << point_in_depth_coordinates[1] << "," << point_in_depth_coordinates[2] << "]" << std::endl;

		// method#1
		frames = frames.apply_filter(align_to);
		auto color_aligned = frames.get_color_frame();

		// For cameras that don't have RGB sensor, we'll map the pointcloud to infrared instead of color
		if (!color_aligned)
			color_aligned = frames.get_infrared_frame();

		// Tell pointcloud object to map to this color frame
		pc.map_to(color_aligned);

		auto depth_aligned = frames.get_depth_frame();

		// Generate the pointcloud and texture mappings
		points = pc.calculate(depth_aligned);

		rs2_intrinsics intr_depth_aligned = depth_aligned.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
		rs2_intrinsics intr_color_aligned = color_aligned.get_profile().as<rs2::video_stream_profile>().get_intrinsics();

		// pixel in color image
		w = color_aligned.get_width();
		h = color_aligned.get_height();

		c = w * u;
		r = h * v;

		int index = c + w * r;

		pixel[0] = c;
		pixel[1] = r;

		// method# 1
		auto dist = depth.get_distance(static_cast<int>(pixel[0]), static_cast<int>(pixel[1]));
		rs2_deproject_pixel_to_point(point_in_color_coordinates, &intr_depth_aligned, pixel, dist);

		rs2_transform_point_to_point(point_in_depth_coordinates, &color_extrin_to_depth, point_in_color_coordinates);
//		print_extrinsics(color_extrin_to_depth);

		std::cout << "method#1 wxh=" << w << "," << h << ", uv(" << c << "," << r << ") ----> " << " [" << point_in_color_coordinates[0] << "," << point_in_color_coordinates[1] << "," << point_in_color_coordinates[2] << "] --> 3d point in depth sensor coordinates dp[" << point_in_depth_coordinates[0] << "," << point_in_depth_coordinates[1] << "," << point_in_depth_coordinates[2] << "]" << std::endl;

		// method# 3
		// find coresponding depth point from vertics
		const rs2::vertex* tv = points.get_vertices();
		float x = tv[index].x;
		float y = tv[index].y;
		float z = tv[index].z;

		point_in_color_coordinates[0] = x;
		point_in_color_coordinates[1] = y;
		point_in_color_coordinates[2] = z;

		// transform 3d point from color coordinates into depth coordinates
		rs2_transform_point_to_point(point_in_depth_coordinates, &color_extrin_to_depth, point_in_color_coordinates);
//		print_extrinsics(color_extrin_to_depth);

		std::cout << "method#3 wxh=" << w <<"," << h << ", uv(" << c << "," << r << ") ----> " << " [" << point_in_color_coordinates[0] << "," << point_in_color_coordinates[1] << "," << point_in_color_coordinates[2] << "] --> 3d point in depth sensor coordinates dp[" << point_in_depth_coordinates[0] << "," << point_in_depth_coordinates[1] << "," << point_in_depth_coordinates[2] << "]" << std::endl;


        // Upload the color frame to OpenGL
        app_state.tex.upload(color);

        // Draw the pointcloud
        draw_pointcloud(app.width(), app.height(), app_state, points);
    }

    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;
}