#include <iostream>
#include <fstream>
#include <XnOpenNI.h>
#include <XnCppWrapper.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// xmlでの設定と，ファイルパスは適宜変更してください
#define SAMPLE_XML_PATH "C:/Program Files (x86)/OpenNI/Data/SamplesConfig.xml"
#define MAX_DEPTH (8000.0)

std::string win_image = "image";
std::string win_depth = "depth";

int
SavePointCloud(std::string filename, cv::Mat mat_depth, XnUInt64 zpd, XnDouble zpps)
{
	std::ofstream ofs;
	ofs.open(filename.c_str()); // ファイルオープン
	if(!ofs.is_open()) {
		std::cout << "ofs open failed" << std::endl;
		return -1;
	}

	for(int i=0; i<mat_depth.rows; i++) {
		for(int j=0; j<mat_depth.cols; j++) {
			int x, y, z;
			z = mat_depth.at<unsigned short>(i, j);
			x = (j - mat_depth.cols/2) * z * zpps / zpd;
			y = (i - mat_depth.rows/2) * z * zpps / zpd;
			ofs << x << " " << y << " " << z << "\n";
		}
		ofs << "\n";
	}
	ofs.close(); // ファイルクローズ

	return 0;
}

int main()
{
	cv::Mat mat_image, mat_depth, mat_depthshow;
	xn::Context xn_context;
	xn::EnumerationErrors xn_error;
	XnStatus xn_status = XN_STATUS_OK;
	XnUInt64 xn_zpd; // 焦点距離 Zero Plane Distance
	XnDouble xn_zpps; // ピクセルサイズ Zero Plane Pixel Size
	int depth_max = 0, depth_min = 0;
	double min, max;
	cv::Point point_min, point_max;

	xn_status = xn_context.InitFromXmlFile(SAMPLE_XML_PATH, &xn_error);
	
	if (xn_status == XN_STATUS_NO_NODE_PRESENT) {		
		XnChar xn_error_str[1024];
		xn_error.ToString(xn_error_str, sizeof(xn_error_str));
		std::cout << xn_error_str << std::endl;
		return xn_status;
	}
	else if (xn_status != XN_STATUS_OK) {
		std::cout << "Open failed: " << xnGetStatusString(xn_status) << std::endl;
		return xn_status;
	}
	
	// image
	xn::ImageMetaData xn_imageMD;
	xn::ImageGenerator xn_image;
	xn_context.FindExistingNode(XN_NODE_TYPE_IMAGE, xn_image);  
	xn_image.GetMetaData(xn_imageMD);
	cv::namedWindow(win_image, CV_WINDOW_AUTOSIZE); // image用ウインドウ生成
	mat_image.create(xn_imageMD.YRes(), xn_imageMD.XRes(), CV_8UC3);
	
	// depth
	xn::DepthMetaData xn_depthMD;
	xn::DepthGenerator xn_depth;
	xn_context.FindExistingNode(XN_NODE_TYPE_DEPTH, xn_depth);
	xn_depth.GetMetaData(xn_depthMD);
	xn_depth.GetIntProperty("ZPD", xn_zpd); 
	xn_depth.GetRealProperty("ZPPS", xn_zpps);
	std::cout << xn_zpd << " " << xn_zpps << std::endl;

	cv::namedWindow(win_depth, CV_WINDOW_AUTOSIZE); // depth用ウインドウ生成
	cv::createTrackbar("depth_max", win_depth, &depth_max, 255);
	cv::setTrackbarPos("depth_max", win_depth, 0);
	cv::createTrackbar ("depth_min", win_depth, &depth_min, 255);
	cv::setTrackbarPos("depth_min", win_depth, 240);
	mat_depth.create(xn_depthMD.YRes(), xn_depthMD.XRes(), CV_16SC1);
	
	while (1) {
		xn_status = xn_context.WaitOneUpdateAll(xn_depth);
		if (xn_status != XN_STATUS_OK) continue;
		
		// depth
		xn_depth.GetMetaData(xn_depthMD);
		mat_depth.data = (uchar *)xn_depthMD.WritableData();		
		mat_depth.convertTo(mat_depthshow, CV_8U, -255/MAX_DEPTH, 255); // 表示用に変換
		cv::threshold(mat_depthshow, mat_depthshow, 
			depth_max, 255, cv::THRESH_TOZERO); // 背景をカット
		cv::threshold(mat_depthshow, mat_depthshow, 
			depth_min, 255, cv::THRESH_TOZERO_INV); // 前景をカット
		cv::imshow(win_depth, mat_depthshow);
		cv::minMaxLoc(mat_depth, &min, &max, &point_min, &point_max); // 最浅，最深位置を取得
		std::cout << "max, min = " << max << ", " << min << std::endl;
		
		// image
		xn_image.GetMetaData(xn_imageMD);
		mat_image.data = (uchar *)xn_imageMD.WritableData();
		cv::cvtColor(mat_image, mat_image, CV_RGB2BGR);				
		cv::circle(mat_image, point_max, 10, cv::Scalar(0,0,255), -1); // 最浅位置に赤丸
		cv::imshow(win_image, mat_image);
		
		int key = cv::waitKey(1);
		if(key=='q') break;
		else if(key=='g') SavePointCloud("data.txt", mat_depth, xn_zpd, xn_zpps);
	}  
	xn_context.Shutdown();

	return 0;
}