#pragma comment(lib, "k4a.lib")
#include <k4a/k4a.h>

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  

using namespace cv;

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <iostream>
using namespace std;
//**************Symbolic Constant Macros (defines)  *************
#define STREAM_RUN_TIME_SEC 4
#define ERROR_START_STREAM_TIME 10000
#define K4A_DEPTH_MODE_NFOV_2X2BINNED_EXPECTED_SIZE 184320 // 368640
#define K4A_DEPTH_MODE_NFOV_UNBINNED_EXPECTED_SIZE 737280  // 1474560
#define K4A_DEPTH_MODE_WFOV_2X2BINNED_EXPECTED_SIZE 524288 // 1048576
#define K4A_DEPTH_MODE_WFOV_UNBINNED_EXPECTED_SIZE 2097152 // 4194304
#define K4A_DEPTH_MODE_PASSIVE_IR_EXPECTED_SIZE 2097152    // 4194304
#define DEPTH_MODE_EXPECTED_FPS_30 30
#define DEPTH_MODE_EXPECTED_FPS_15 15
#define DEPTH_MODE_EXPECTED_FPS_5 5
#define MAX_BUFFER_SIZE 256
#define SERIAL_NUMBER_SIZE 12

static void clean_up(k4a_device_t device)
{
    if (device != NULL)
    {
        k4a_device_close(device);
    }
}
/*********  获取 2.5D 深度渲染图 ********/ 
static bool get_depth_image(cv::Mat depth,cv::Mat depth_show, k4a_calibration_t calibration)
{
    float fx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fx;
    float fy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fy;
    float cx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cx;
    float cy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cy;
    for (int row = 0; row < depth.rows; ++row)
    {
        for (int col = 0; col < depth.cols; ++col)
        {

            CvPoint3D32f v;
            float z = depth.at<unsigned short>(row, col);
            if (z > 0)
            {
                v.z = (float)(z) / 1000;
                v.x = (col - cx_d) * v.z / fx_d;
                v.y = (row - cy_d) * v.z / fy_d;
                //v.w() = 1;
                //printf("%f,%f,%f", v.x,v.y, v.z);
                float d_c = 0.75;
                if (v.z <= d_c)
                {
                    depth_show.at<unsigned char>(row, col) = (unsigned char)(v.z * 256 * 1. / d_c);
                }
                else
                {
                    depth_show.at<unsigned char>(row, col) = (unsigned char)(0);
                }
            }
            else {
                depth_show.at<unsigned char>(row, col) = (unsigned char)(0);
            }
        }
    }
    return true;
}

/*********  获取 ir 深度渲染图 ********/
static bool get_ir_image(cv::Mat ir_show, cv::Mat ir_show_8u, float dd_thr)
{
    for (int row = 0; row < ir_show.rows; ++row)
    {
        for (int col = 0; col < ir_show.cols; ++col)
        {
            
            if (ir_show.at<unsigned short>(row, col) > dd_thr)
            {
                ir_show_8u.at<unsigned char>(row, col) = 255;
            }
            else
            {
                ir_show_8u.at<unsigned char>(row, col) = (unsigned char)(ir_show.at<unsigned short>(row, col) / dd_thr * 255);
            }
        }
    }
    return true;
}
static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handle,
    const k4a_image_t depth_image,
    const k4a_image_t color_image,
    cv::Mat depth,
    cv::Mat color_show,
    cv::Mat ir_show,
    k4a_calibration_t calibration,
    int ir_thr,
    int id
    )
{
    int depth_image_width_pixels = k4a_image_get_width_pixels(depth_image);
    int depth_image_height_pixels = k4a_image_get_height_pixels(depth_image);
    k4a_image_t transformed_color_image = NULL;
    if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
        depth_image_width_pixels,
        depth_image_height_pixels,
        depth_image_width_pixels * 4 * (int)sizeof(uint8_t),
        &transformed_color_image))
    {
        printf("Failed to create transformed color image\n");
        return false;
    }
    else {
        printf("create transformed color image \n");
    }

    k4a_image_t point_cloud_image = NULL;
    if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
        depth_image_width_pixels,
        depth_image_height_pixels,
        depth_image_width_pixels * 3 * (int)sizeof(int16_t),
        &point_cloud_image))
    {
        printf("Failed to create point cloud image\n");
        return false;
    }
    else {
        printf("create point cloud image \n");
    }

    if (K4A_RESULT_SUCCEEDED != k4a_transformation_color_image_to_depth_camera(transformation_handle,
        depth_image,
        color_image,
        transformed_color_image))
    {
        printf("Failed to compute transformed color image\n");
        return false;
    }
    else {
        printf("compute transformed color image \n");
    }

    if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_point_cloud(transformation_handle,
        depth_image,
        K4A_CALIBRATION_TYPE_DEPTH,
        point_cloud_image))
    {
        printf("Failed to compute point cloud\n");
        return false;
    }
    else {
        printf("compute point cloud \n");
    }

    //-------------------------
    uint8_t* Ptr_c = (uint8_t*)k4a_image_get_buffer(transformed_color_image);
    int _resolution_y = k4a_image_get_height_pixels(depth_image);
    int _resolution_x = k4a_image_get_width_pixels(depth_image);
    cv::Mat c_show(_resolution_y, _resolution_x, CV_8UC4);
    memcpy(c_show.data, Ptr_c, sizeof(uint8_t) * _resolution_x * _resolution_y*4);
    namedWindow("transformed_color_image", 0);
    imshow("transformed_color_image", c_show);
    //-----------------------
    float fx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fx;
    float fy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fy;
    float cx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cx;
    float cy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cy;
    float k1 = calibration.depth_camera_calibration.intrinsics.parameters.param.k1;
    float k2 = calibration.depth_camera_calibration.intrinsics.parameters.param.k2;
    float k3 = calibration.depth_camera_calibration.intrinsics.parameters.param.k3;
    float p1 = calibration.depth_camera_calibration.intrinsics.parameters.param.p1;
    float p2 = calibration.depth_camera_calibration.intrinsics.parameters.param.p2;
    //
    float fx_c = calibration.color_camera_calibration.intrinsics.parameters.param.fx;
    float fy_c = calibration.color_camera_calibration.intrinsics.parameters.param.fy;
    float cx_c = calibration.color_camera_calibration.intrinsics.parameters.param.cx;
    float cy_c = calibration.color_camera_calibration.intrinsics.parameters.param.cy;
    float k1c = calibration.color_camera_calibration.intrinsics.parameters.param.k1;
    float k2c = calibration.color_camera_calibration.intrinsics.parameters.param.k2;
    float k3c = calibration.color_camera_calibration.intrinsics.parameters.param.k3;
    float p1c = calibration.color_camera_calibration.intrinsics.parameters.param.p1;
    float p2c = calibration.color_camera_calibration.intrinsics.parameters.param.p2;
    //
    
    cv::Mat depth_color(_resolution_y, _resolution_x, CV_8UC3);
    vector<CvPoint3D32f>pts_3d;
    pts_3d.clear();
    for (int row = 0; row < depth.rows; ++row)
    {
        for (int col = 0; col < depth.cols; ++col)
        {
            CvPoint3D32f v;
            float z = depth.at<unsigned short>(row, col);
            if (z > 0)
            {
                
                v.z = (float)(z) / 1000; // 转换为单位米
                v.x = (col - cx_d) * v.z / fx_d;
                v.y = (row - cy_d) * v.z / fy_d;
                //v.w() = 1;
                //printf("%f,%f,%f", v.x,v.y, v.z);
                float d_c = 0.6;
                if ((v.z <= 0.6))
                {
                    pts_3d.push_back(v);
                    if (ir_show.at<unsigned char>(row, col) > ir_thr)
                    {
                        depth_color.at<cv::Vec3b>(row, col)[0] = c_show.at<cv::Vec4b>(row, col)[0];
                        depth_color.at<cv::Vec3b>(row, col)[1] = c_show.at<cv::Vec4b>(row, col)[1];
                        depth_color.at<cv::Vec3b>(row, col)[2] = c_show.at<cv::Vec4b>(row, col)[2];
                    }
                    else
                    {
                        depth_color.at<cv::Vec3b>(row, col)[0] = 0;
                        depth_color.at<cv::Vec3b>(row, col)[1] = 0;
                        depth_color.at<cv::Vec3b>(row, col)[2] = 0;
                    }
                }
                else
                {
                    depth_color.at<cv::Vec3b>(row, col)[0] = 0;
                    depth_color.at<cv::Vec3b>(row, col)[1] = 0;
                    depth_color.at<cv::Vec3b>(row, col)[2] = 0;
                }
            }
        }
    }

    namedWindow("depth_color", 0);
    imshow("depth_color", depth_color);
    char buf[256];
    printf("img_id : %06d", id);
    sprintf(buf, "E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/datasets/%6d.jpg", id);
    imwrite(buf, depth_color);

    //使用迭代器访问元素
    printf("pts 3D size : %d\n", pts_3d.size());
    cv::Mat color_distortion(depth_color.rows, depth_color.cols, CV_8UC3);
    resize(color_show, color_distortion, color_distortion.size(), 0, 0, INTER_LINEAR);
    vector<CvPoint3D32f>::iterator it;
    //
    cv::Mat depth2color(3, 4, CV_32FC1);
    depth2color.at<float>(0, 0) = calibration.color_camera_calibration.extrinsics.rotation[0];
    depth2color.at<float>(0, 1) = calibration.color_camera_calibration.extrinsics.rotation[1];
    depth2color.at<float>(0, 2) = calibration.color_camera_calibration.extrinsics.rotation[2];
    depth2color.at<float>(0, 3) = calibration.color_camera_calibration.extrinsics.translation[0] / 1000;
    //depth2color(0, 3) = calib.color_camera_calibration.extrinsics.translation[0];
    depth2color.at<float>(1, 0) = calibration.color_camera_calibration.extrinsics.rotation[3];
    depth2color.at<float>(1, 1) = calibration.color_camera_calibration.extrinsics.rotation[4];
    depth2color.at<float>(1, 2) = calibration.color_camera_calibration.extrinsics.rotation[5];
    depth2color.at<float>(1, 3) = calibration.color_camera_calibration.extrinsics.translation[1] / 1000;
    //depth2color(1, 3) = calib.color_camera_calibration.extrinsics.translation[1];
    depth2color.at<float>(2, 0) = calibration.color_camera_calibration.extrinsics.rotation[6];
    depth2color.at<float>(2, 1) = calibration.color_camera_calibration.extrinsics.rotation[7];
    depth2color.at<float>(2, 2) = calibration.color_camera_calibration.extrinsics.rotation[8];
    depth2color.at<float>(2, 3) = calibration.color_camera_calibration.extrinsics.translation[2] / 1000;
    //
    for (it = pts_3d.begin(); it != pts_3d.end(); it++)
    {
        CvPoint3D32f v = *it;
        float Xd = v.x;
        float Yd = v.y;
        float Zd = v.z;

        float x_p = Xd / Zd;
        float y_p = Yd / Zd;
        // 点云转为深度二维图
        float x_depth = fx_d * x_p + cx_d;
        float y_depth = fy_d * y_p + cy_d;
        circle(depth_color, Point(x_depth, y_depth), 0, Scalar(88,255,88), -1);
        //----------------------------------- 

        cv::Mat pt_3D(4, 1, CV_32FC1);
        pt_3D.at<float>(0, 0) = Xd;
        pt_3D.at<float>(1, 0) = Yd;
        pt_3D.at<float>(2, 0) = Zd;
        pt_3D.at<float>(3, 0) = 1.;

        cv::Mat pt3D_2_Color = depth2color * pt_3D; // Depth Camera 坐标系转为 RGB 坐标系
        // 三维点云做 RGB 相机投影
        float Xdc = pt3D_2_Color.at<float>(0, 0);
        float Ydc = pt3D_2_Color.at<float>(1, 0);
        float Zdc = pt3D_2_Color.at<float>(2, 0);
       // printf("%f, %f , %f", pt3D_2_Color.at<float>(0, 0), pt3D_2_Color.at<float>(1, 0), pt3D_2_Color.at<float>(2, 0));

        float x_pc = Xdc / Zdc;
        float y_pc = Ydc / Zdc;
        
        //printf("(x,y,z) = (%.3f ,%.3f ,%.3f)\n", v.x, v.y, v.z);
        float r2 = x_pc * x_pc + y_pc * y_pc;
        float x_2pc = x_pc*(1. + k1c * r2 + k2c * r2 * r2 + k3c * r2 * r2 * r2) + 2. * p1c * x_pc * y_pc + p2c * (r2 + 2. * x_pc * x_pc);
        float y_2pc = y_pc * (1. + k1c * r2 + k2c * r2 * r2 + k3c * r2 * r2 * r2) + 2. * p2c * x_pc * y_pc + p1c * (r2 + 2. * y_pc * y_pc);
       // 
        float x_color = fx_c * x_2pc + cx_c;
        float y_color = fy_c * y_2pc + cy_c;
        circle(color_show, Point(x_color, y_color), 0, Scalar(255, 88, 88), -1);
    }
    namedWindow("pt3d_2_depth_color", 0);
    imshow("pt3d_2_depth_color", depth_color);
    namedWindow("color_distortion", 0);
    imshow("color_distortion", color_show);
    
    //-------------------------------------------------

    k4a_image_release(transformed_color_image);
    k4a_image_release(point_cloud_image);

    return true;
}
/************* main 函数 ***********/
int main()
{
    int img_id = 0;// 图片帧 ID

    printf("Hello World!\n");

    /***************** opencv 测试 ****************/
    // 读入一张图片  
    Mat img = imread("../sample/image.jpg");
    // 创建一个名为 "图片"窗口    
    namedWindow("image",0);
    // 在窗口中显示图片   
    imshow("image", img);  
    waitKey(1);

    //vector<k4a_float3_t> points_3d = { { { 0.f, 0.f, 1000.f } },          // color camera center
    //                                   { { -1000.f, -1000.f, 1000.f } },  // color camera top left
    //                                   { { 1000.f, -1000.f, 1000.f } },   // color camera top right
    //                                   { { 1000.f, 1000.f, 1000.f } },    // color camera bottom right
    //                                   { { -1000.f, 1000.f, 1000.f } } }; // color camera bottom left
    int timeout_ms;
    
    uint32_t count = k4a_device_get_installed_count();
    if (count == 0)
    {
        printf("No k4a devices attached!\n");
        return 1;
    }

    // Open the first plugged in Kinect device
    k4a_device_t device = NULL;
    if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device)))
    {
        printf("Failed to open k4a device!\n");
        return 1;
    }
    
    // Get the size of the serial number
    size_t serial_size = 0;
    k4a_device_get_serialnum(device, NULL, &serial_size);

    // Allocate memory for the serial, then acquire it
    char* serial = (char*)(malloc(serial_size));
    k4a_device_get_serialnum(device, serial, &serial_size);
    printf("Opened device: %s\n", serial);
    free(serial);

    // Configure a stream of 4096x3072 BRGA color data at 15 frames per second
    k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
    config.camera_fps = K4A_FRAMES_PER_SECOND_15; // 帧率 
    config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
    config.color_resolution = K4A_COLOR_RESOLUTION_720P;// 分辨率
    config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
    
    
    //config.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED;
    config.synchronized_images_only = true;
    //
    k4a_transformation_t transformation = NULL;
    k4a_calibration_t calibration;
    if (K4A_RESULT_SUCCEEDED !=
        k4a_device_get_calibration(device, config.depth_mode, config.color_resolution, &calibration))
    {
        printf("Failed to get calibration\n");
        return 1;
    }
    transformation = k4a_transformation_create(&calibration);
    // Start the camera with the given configuration
    if (K4A_FAILED(k4a_device_start_cameras(device, &config)))
    {
        printf("Failed to start cameras!\n");
        k4a_device_close(device);
        return 1;
    }

    // 相机内参获取
    float fx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fx;
    float fy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fy;
    float cx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cx;
    float cy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cy;
   

    // Camera capture and application specific code would go here
    while (true) {
        k4a_capture_t capture;

    
        timeout_ms = ERROR_START_STREAM_TIME;
        // Access the depth16 image 
        while (K4A_WAIT_RESULT_SUCCEEDED != k4a_device_get_capture(device, &capture, timeout_ms)) {
            printf("get %d\n", k4a_device_get_capture(device, &capture, timeout_ms));
        }

        k4a_image_t depth_image = NULL;
        k4a_image_t color_image = NULL;
        k4a_image_t ir_image = NULL;

        depth_image = k4a_capture_get_depth_image(capture);// 深度图
        color_image = k4a_capture_get_color_image(capture);// RGB彩色图
        ir_image = k4a_capture_get_ir_image(capture);// IR 图
        if (depth_image != NULL) {
            printf(" | Depth16 res:%4dx%4d stride:%5d\n",
                k4a_image_get_height_pixels(depth_image),
                k4a_image_get_width_pixels(depth_image),
                k4a_image_get_stride_bytes(depth_image)); // Release the image 
        }

        // k4a project function
       /* vector<k4a_float2_t> k4a_points_2d(points_3d.size());
        for (size_t i = 0; i < points_3d.size(); i++)
        {
            int valid = 0;
            k4a_calibration_3d_to_2d(&calibration,
                &points_3d[i],
                K4A_CALIBRATION_TYPE_COLOR,
                K4A_CALIBRATION_TYPE_DEPTH,
                &k4a_points_2d[i],
                &valid);
        }*/

        if (color_image != NULL) {
            printf(" | color res:%4dx%4d stride:%5d\n",
                k4a_image_get_height_pixels(color_image),
                k4a_image_get_width_pixels(color_image),
                k4a_image_get_stride_bytes(color_image)); // Release the image 
        }

        if (ir_image != NULL) {
            printf(" | IR res:%4dx%4d stride:%5d\n",
                k4a_image_get_height_pixels(ir_image),
                k4a_image_get_width_pixels(ir_image),
                k4a_image_get_stride_bytes(ir_image)); // Release the image 
        }
        //---------------------------------------------------------------
        uint8_t* colorPtr = k4a_image_get_buffer(color_image);
        uint16_t* depthPtr = (uint16_t*)k4a_image_get_buffer(depth_image);
        uint16_t* irPtr = (uint16_t*)k4a_image_get_buffer(ir_image);
        int _resolution_y = k4a_image_get_height_pixels(depth_image);
        int _resolution_x = k4a_image_get_width_pixels(depth_image);
        cv::Mat depth(_resolution_y, _resolution_x, CV_16UC1);
        memcpy(depth.data, depthPtr, sizeof(uint16_t) * _resolution_x * _resolution_y);

        cv::Mat depth_show(_resolution_y, _resolution_x, CV_8UC1);
        get_depth_image(depth, depth_show,calibration); // 获取 2.5D 深度渲染图
        
        namedWindow("depth_render", 0);//  绘制 2.5D 深度渲染图
        imshow("depth_render", depth_show);

        char buf[256];
        //printf("img_id : %06d", img_id);
        sprintf(buf, "E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/datasets/%6d-depth.jpg", img_id);
        imwrite(buf, depth_show);


        _resolution_y = k4a_image_get_height_pixels(color_image);
        _resolution_x = k4a_image_get_width_pixels(color_image);
        cv::Mat color_show(_resolution_y, _resolution_x, CV_8UC4);
        memcpy(color_show.data, colorPtr, sizeof(uint8_t) * _resolution_x * _resolution_y * 4);
        namedWindow("color_show", 0);
        imshow("color_show", color_show);

        _resolution_y = k4a_image_get_height_pixels(ir_image);
        _resolution_x = k4a_image_get_width_pixels(ir_image);
        cv::Mat ir_show(_resolution_y, _resolution_x, CV_16UC1);
        cv::Mat ir_show_8u(_resolution_y, _resolution_x, CV_8UC1);
        memcpy(ir_show.data, irPtr, sizeof(uint16_t) * _resolution_x * _resolution_y * 1);
        //normalize(ir_show, ir_show_8u, 0, 256 * 256, NORM_MINMAX, CV_16UC1);
       
        get_ir_image(ir_show, ir_show_8u, 9000);
        namedWindow("ir_show", 0);
        imshow("ir_show", ir_show_8u);

        int ir_thr = 25; // 用作三维点云分割辅助使用 ir 阈值

        if (waitKey(10) == 27){
            break;
        }
        //-------------------------------------------
        // 彩色图映射到深度图
        point_cloud_color_to_depth(transformation, depth_image, color_image, depth, color_show,ir_show_8u,calibration, ir_thr,img_id);
        img_id++;
        // releae image
        k4a_image_release(depth_image);
        k4a_image_release(color_image);
        k4a_image_release(ir_image);
        depth.release();
        depth_show.release();
        color_show.release();
        ir_show.release();
        ir_show_8u.release();
    
        // Release the capture 
        k4a_capture_release(capture);
    }

    // Shut down the camera when finished with application logic
    k4a_device_stop_cameras(device);
    k4a_device_close(device);

    return 0;
}