#include "Kinect.h"
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;

const int Kinect::Width = 640;
const int Kinect::Height = 480;

Kinect::Kinect():
	Image_Generator(0),
	Depth_Generator(0),
	ImageMD(0),
	Color_Image(0),
	DepthMD(0),
	Depth_Image(0),
	Kinect_ID(0),
	Intrinsic_Matrix(0){}
//アドレスに0を代入してアクセス違反を防ぐ
Kinect::~Kinect(){
	delete   Image_Generator;
	delete   Depth_Generator;
	delete     	     ImageMD;
	delete       Color_Image;
	delete  	     DepthMD;
	delete  	 Depth_Image;
	delete	Intrinsic_Matrix;
	Image_Generator = 0;
	Depth_Generator = 0;
	ImageMD = 0;
	Color_Image = 0;
	DepthMD = 0;
	Depth_Image = 0;
	Intrinsic_Matrix = 0;
}
void Kinect::SetImageGenerator(xn::ImageGenerator* image_generator){
	Image_Generator = image_generator;
}
void Kinect::SetDepthGenerator(xn::DepthGenerator* depth_generator){
	Depth_Generator = depth_generator;
}
void Kinect::SetKinectID(int kinect_id){
	Kinect_ID = kinect_id;
}
xn::DepthMetaData*  Kinect::GetDepthMD()const{
	return DepthMD;
}

cv::Mat_<cv::Vec3b>* Kinect::GetColorImage()const{
	return Color_Image;
}
cv::Mat_<cv::Vec3b>* Kinect::GetDepthImage()const{
	return Depth_Image;
}
cv::Mat_<double>* Kinect::GetIntrinsicMatrix()const{
	return Intrinsic_Matrix;
}
int Kinect::GetKinectID()const{
	return Kinect_ID;
}
void Kinect::ProjectToReal(XnPoint3D& proj, XnPoint3D& real){
	Depth_Generator->ConvertProjectiveToRealWorld(1, &proj, &real);
}
void Kinect::RealToProject(XnPoint3D& real, XnPoint3D& proj){
	Depth_Generator->ConvertRealWorldToProjective(1, &real, &proj);
}
void Kinect::InitAllData(){

	// デプスの座標をイメージに合わせる
	xn::AlternativeViewPointCapability viewPoint =
		Depth_Generator->GetAlternativeViewPointCap();
	//ビューポイント機能を取得する
	//ビューポイントを指定したノードに合わせる
	viewPoint.SetViewPoint(*Image_Generator);
	//depthMD作成
	DepthMD = new xn::DepthMetaData();
	//imageMD作成
	ImageMD = new xn::ImageMetaData();
	//RGB画像の作成
	XnMapOutputMode outputmode;
	Image_Generator->GetMapOutputMode(outputmode);
	//Color画像の作成
	Color_Image = new cv::Mat_<cv::Vec3b>(Kinect::Height, Kinect::Width);
	//Depth画像の作成
	Depth_Image = new cv::Mat_<cv::Vec3b>(Kinect::Height, Kinect::Width);
	//IntrinsicMatrix
	Intrinsic_Matrix = new cv::Mat_<double>(3, 3);
	XnDouble pixel_size;
	unsigned long long F;
	Depth_Generator->GetIntProperty("ZPD", F);
	Depth_Generator->GetRealProperty("ZPPS", pixel_size);
	*Intrinsic_Matrix = (cv::Mat_<double>(3, 3) << (double)F/(double)(2.0*pixel_size), 0.0, Kinect::Width/2.0,
													0.0, (double)F/(double)(2.0*pixel_size), Kinect::Height/2.0,
														0.0, 0.0, 1.0);
}

void Kinect::UpdateAllData(){
	//imageMetadata作成
	Image_Generator->GetMetaData(*ImageMD);
	//depthMetadata作成
	Depth_Generator->GetMetaData(*DepthMD);
	//color_image作成
	memcpy(Color_Image->data, ImageMD->Data(), Color_Image->step * Color_Image->rows); 
	cv::cvtColor(*Color_Image, *Color_Image, CV_RGB2BGR);
	//depthimage作成
	CreateDepthImage();
}

void Kinect::CreateDepthImage(){
	
	//デプスの傾向を計算する(アルゴリズムはNiSimpleViewer.cppを利用)
	const int MAX_DEPTH = Depth_Generator->GetDeviceMaxDepth();
	//デプスの最大値取得
	std::vector<float> depth_hist(MAX_DEPTH);

	unsigned int points = 0;
	//points デプスがとれた全点の数
	const XnDepthPixel* pDepth = DepthMD->Data();
	for (XnUInt y = 0; y < DepthMD->YRes(); ++y) {
		for (XnUInt x = 0; x < DepthMD->XRes(); ++x, ++pDepth) {
			if (*pDepth != 0) {
				//depth_hist[*pDepth]++;
				//std::cout << depth_data[y*Image_Size->width+x]<< std::endl;
				depth_hist[*pDepth]++;
				points++;
			}
			//もしデプスがとれたらpDepthの位置のデプスの値番目のヒストグラムをカウント
			//デプスがとれた点の数もカウント
		}
	}

	for (int i = 1; i < MAX_DEPTH; ++i) {
		depth_hist[i] += depth_hist[i-1];
		//全部のデプス値のヒストグラムの値についてそれより値が小さいものの値を加算
		//これによりデプスの大きな値のヒストグラムは大きな値になる
	}

	if ( points != 0) {
		for (int i = 1; i < MAX_DEPTH; ++i) {
			depth_hist[i] =
				(unsigned int)(256 * (1.0f - (depth_hist[i] / points)));
			//デプスが大きいものほど小さいヒストグラムを割り当てる
			//std::cout << depth_hist[i] << std::endl;
		}
	}

	for(int y=0; y < DepthMD->YRes(); y++){
		for(int x=0; x < DepthMD->XRes(); x++){
			Depth_Image->at<cv::Vec3b>(y, x) = cv::Vec3b(0, depth_hist[(*DepthMD)(x, y)], depth_hist[(*DepthMD)(x, y)]); 
		}
	}
}

void Kinect::VisualizationPcdFile(string pathTofile)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_input (new pcl::visualization::PCLVisualizer ("Input Viewer"));
	viewer_input->initCameraParameters ();

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr input( new pcl::PointCloud<pcl::PointXYZRGB>);


	pcl::io::loadPCDFile<pcl::PointXYZRGB>(pathTofile, *input);
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_input(input);
	viewer_input->addPointCloud<pcl::PointXYZRGB> (input, rgb_input, "input1");
	viewer_input->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.5, "input1");


	bool ShouldRun = true;
	while (ShouldRun) {  
		viewer_input->spinOnce (100);

		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
	}

	return;

}

void Kinect::SavePointcloud(double range_near, double range_far){

	//カラー画像と距離画像から3次元点群を作成し保存
	//その際，range_nearとrange_farの間にある点群のみを保存


	//各点の情報（1点の(x, y, z)と(R, G, B)の情報を持つ）
	pcl::PointXYZRGB points;

	//取得した点群（pcl::PointXYZRGBの集合）
	//イメージとしてはpointsをどんどんcloudに追加していく感じ
	pcl::PointCloud<pcl::PointXYZRGB> cloud;
	

	//変換用の3次元座標を用意
	XnPoint3D proj, real;


	//①距離画像のy座標をたどる
	///////////////////↓に書き込む//////////////////////////////
	for(int y = 0; y < ; y++){
	///////////////////↑に書き込む//////////////////////////////
	

		//②距離画像のx座標をたどる
		///////////////////↓に書き込む//////////////////////////////
		for(int x = 0; x < ; x++){
		///////////////////↑に書き込む//////////////////////////////


			//③画像座標(x, y)での距離の値を取得
			///////////////////↓に書き込む//////////////////////////////
			double depth_tmp = ;
			///////////////////↑に書き込む//////////////////////////////


			//④距離の値がrange_nearとrange_farの間にあったら
			//ただし，距離の値は[mm]でrange_nearとrange_farは[m]
			///////////////////↓に書き込む//////////////////////////////
			if( ){ 
			///////////////////↑に書き込む//////////////////////////////


				//⑤変換準備．画像座標と距離の値をprojに登録
				///////////////////↓に書き込む//////////////////////////////
				proj.X = ;
				proj.Y = ;
				proj.Z = ;
				///////////////////↑に書き込む//////////////////////////////


				//⑥世界座標へ変換
				///////////////////↓に書き込む//////////////////////////////
				
				///////////////////↑に書き込む//////////////////////////////
				

				//⑦3次元点に(x, y, z)と(R, G, B)の情報を登録
				//ただし，(x, y, z)は[m]でdouble型，(R, G, B)はunsigned char型
				///////////////////↓に書き込む//////////////////////////////
				points.x = ;
				points.y = ;
				points.z = ;
				points.b = ;
				points.g = ;
				points.r = ;
				///////////////////↑に書き込む//////////////////////////////


				//pointcloudに格納
				cloud.push_back(points);
			}
		}
	}

	//cloudをpcdファイルとして保存
	pcl::io::savePCDFile("input_points.pcd", cloud);
}

SingleKinect::SingleKinect(){
	//ContextとGeneratorの初期化
	Registration();
	//memberの初期化
	InitAllData();
}

SingleKinect::~SingleKinect(){
	delete Kinect_Context;
	Kinect_Context = 0;
}
void SingleKinect::UpdateContextAndData(){
	Kinect_Context->WaitAndUpdateAll();
	UpdateAllData();
}
void SingleKinect::ShowImage(){


	//基礎輪講2週目用　課題
	//この関数を完成させてください．


	bool ShouldRun = true;
	char key;
	while( ShouldRun ){
		//データ更新を待つ
		Kinect_Context->WaitAndUpdateAll();

		//①whileループが回るたびにKinectからデータをもらって更新する．
		//具体的には，
		//xn::ImageGenerator（このクラスではImage_Generator）を使ってxn::ImageMetaData（このクラスではImageMD）にカラーのデータを登録
		//xn::DepthGenerator（このクラスではDepth_Generator）を使ってxn::DepthMetaData（このクラスではDepthMD）に距離のデータを登録
		//ImageMDからMat型のカラー画像（このクラスではColor_Image）を作成
		//DepthMDからMat型の距離画像（このクラスではDepth_Image）を作成
		//をする．
		//（上記をやってくれてるメンバ関数があるので探してください！）
		///////////////////↓に書き込む//////////////////////////////
		
		///////////////////↑に書き込む//////////////////////////////



		//②カラー画像表示
		///////////////////↓に書き込む//////////////////////////////
		
		///////////////////↑に書き込む//////////////////////////////
		

		//キー入力を待つ
		key = cv::waitKey(1);


		//③距離画像表示
		///////////////////↓に書き込む//////////////////////////////
		
		///////////////////↑に書き込む//////////////////////////////
		

		//キー入力を待つ
		key = cv::waitKey(1); 


		//④"s"が押されたらカラー画像と3次元点群を保存

		//もし入力されたキーが"s"だったら
		///////////////////↓に書き込む//////////////////////////////
		if( ){ 
		///////////////////↑に書き込む//////////////////////////////


			//"color_image.jpg"という名前でカラー画像を保存し
			//"save color_image"と標準出力に表示
			///////////////////↓に書き込む//////////////////////////////
			

			///////////////////↑に書き込む//////////////////////////////



			//0mから10mの範囲の3次元点群（point cloud）を保存し
			//（つまりSavePointcloud関数を引数を指定して実行）
			//"save pointcloud"と標準出力に表示
			///////////////////↓に書き込む//////////////////////////////
			

			///////////////////↑に書き込む//////////////////////////////


			ShouldRun = false;
		}
	}
}

void SingleKinect::Registration(){
	// コンテキストの初期化
	Kinect_Context = new xn::Context();
	XnStatus rc = Kinect_Context->InitFromXmlFile("SamplesConfig.xml");
	if (rc != XN_STATUS_OK) {
		throw std::runtime_error(xnGetStatusString(rc));
	}

	// イメージジェネレータの作成
	Image_Generator = new xn::ImageGenerator();
	//指定したノードのインスタンスを作成する
	rc = Kinect_Context->FindExistingNode(XN_NODE_TYPE_IMAGE, *Image_Generator);
	if (rc != XN_STATUS_OK) {
		throw std::runtime_error(xnGetStatusString(rc));
	}

	// デプスジェネレータの作成
	Depth_Generator = new xn::DepthGenerator();
	//指定したノードのインスタンスを作成する
	rc = Kinect_Context->FindExistingNode(XN_NODE_TYPE_DEPTH, *Depth_Generator);
	if (rc != XN_STATUS_OK) {
		throw std::runtime_error(xnGetStatusString(rc));
	}

}
