#include "MyKinect.h"

const float MyKinect::MAX_DEPTH = 4096.0;


MyKinect::MyKinect() {
	::NuiSetDeviceStatusCallback(StatusChanged, this);

	// 接続されているKinectの数を取得
	int count = 0;
	ERROR_CHECK(::NuiGetSensorCount(&count));

	if (count != 0) {
		// 最初のKinectのインスタンスを生成
		ERROR_CHECK(::NuiCreateSensorByIndex(0, &kinect));

		// Kinectの状態を取得
		HRESULT status = kinect->NuiStatus();
		if (status != S_OK) {
			throw std::runtime_error("Kinectが利用可能ではありません");
		}
	}

	if (kinect != 0) {
		initInstance();
	}
}


MyKinect::~MyKinect()
{
	// 終了処理
	if (kinect != 0) {
		kinect->NuiShutdown();
		kinect->Release();
	}
}



void MyKinect::showColorAndDepthImg()
{
	// メインループ
	while (1) {
		// Kinectが未接続，未初期化の場合は，少しスリープして処理しない
		if ((kinect == 0) || !isInitialized) {
			::Sleep(1000);
			continue;
		}

		try {
			// データの更新を待つ
			DWORD ret = ::WaitForSingleObject(streamEvent, INFINITE);
			if ((ret == WAIT_FAILED) || (streamEvent == 0)) {
				continue;
			}
			::ResetEvent(streamEvent);

			// カラー画像，距離画像の取得
			setRgbImage();
			setDepthImage();

			// 画像を表示
			cv::imshow("ColorImage", rgbImage);
			cv::imshow("rawDepthImage", rawDepthImage);
			cv::imshow("DepthImage", depthImage);

			// 終了のためのキー入力チェック兼表示のためのウェイト
			int key = cv::waitKey(10);
			if (key == 'q') {
				break;
			}
		}
		catch (std::exception &ex) {
			std::cout << ex.what() << "\n";
		}
	}

	return;
}


void MyKinect::showPointCloud()
{

	// メインループ
	while (1) {
		// Kinectが未接続，未初期化の場合は，少しスリープして処理しない
		if ((kinect == 0) || !isInitialized) {
			::Sleep(1000);
			continue;
		}

		try {
			// データの更新を待つ
			DWORD ret = ::WaitForSingleObject(streamEvent, INFINITE);
			if ((ret == WAIT_FAILED) || (streamEvent == 0)) {
				continue;
			}
			::ResetEvent(streamEvent);

			// カラー画像・距離画像・PointCloudのセット
			setRgbImage();
			setDepthImage();
			//setPointCloud();

			// 画像の左右反転
			flipImgs();

			// カラー画像・距離画像を表示
			cv::imshow("ColorImage", rgbImage);
			cv::imshow("RawDepthImage", rawDepthImage);
			cv::imshow("DepthImage", depthImage);

			// PointCloudの表示
			viewer->showCloud(cloud);


			int key = cv::waitKey(10);
			// 終了のためのキー入力チェック兼表示のためのウェイト
			if (key == 'q')
				break;
			else if(key == 's') {	//各データを保存
				saveData();
			}

		}
		catch (std::exception &ex) {
			std::cout << ex.what() << "\n";
		}
	}

	cout << "frm: " << frm << endl;

	return;
}



cv::Mat MyKinect::getColorImage()
{


	return rgbImage;
}


cv::Mat MyKinect::getDepthImage()
{
	return depthImage;
}


cv::Mat MyKinect::getDepthImageShowed()
{
	return depthImage;
}

void MyKinect::saveData()
{
	//カラー画像・距離画像などを出力
	//Matをxml形式で出力
	//PointCloudを出力
	//できればファイル名固定は避ける(上書き防止のため)
	//保存の度にファイル名をコマンドプロンプトで入力する，フレーム数を引数としてファイル名に組み込む，etc...
	//
	//
	//
	//

	return;
}


void MyKinect::initInstance()
{
	// Kinectの設定を初期化
	ERROR_CHECK(kinect->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR |
		NUI_INITIALIZE_FLAG_USES_DEPTH));

	// RGBカメラを初期化
	ERROR_CHECK(kinect->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, COLOR_RESOLUTION, 0, 2, 0, &imageStreamHandle));

	// Dephtカメラを初期化
	ERROR_CHECK(kinect->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH, DEPTH_RESOLUTION, 0, 2, 0, &depthStreamHandle));

	// フレーム更新イベントのハンドルを作成
	streamEvent = ::CreateEvent(0, TRUE, FALSE, 0);
	ERROR_CHECK(kinect->NuiSetFrameEndEvent(streamEvent, 0));

	//PointCloudビューワの初期化
	viewer = new pcl::visualization::CloudViewer("Kinect Point Cloud");

	// 指定した解像度の画面サイズを取得
	::NuiImageResolutionToSize(COLOR_RESOLUTION, width, height);

	isInitialized = true;

	return;
}

// Kincetの状態が変わったときに呼ばれるコールバック関数(クラス関数)
void CALLBACK MyKinect::StatusChanged(HRESULT hrStatus,
									  const OLECHAR* instanceName,
									  const OLECHAR* uniqueDeviceName,
									  void* pUserData)
{
	((MyKinect *)pUserData)->StatusChanged(hrStatus, instanceName, uniqueDeviceName);
}


// Kincetの状態が変わったときに呼ばれるコールバック関数(メンバ関数)
void CALLBACK MyKinect::StatusChanged(HRESULT hrStatus,
									  const OLECHAR* instanceName,
									  const OLECHAR* uniqueDeviceName)
{
	// Kinectが接続された
	if (hrStatus == S_OK) {
		// 接続されたKinectのインスタンスを生成
		ERROR_CHECK(::NuiCreateSensorById(instanceName, &kinect));

		initInstance();
	}
	// Kinectが抜かれた
	else if (hrStatus == E_NUI_NOTCONNECTED) {
		close();
	}
}


void MyKinect::close()
{
	// 終了処理
	if (kinect != 0) {
		kinect->NuiShutdown();
		kinect->Release();
		kinect = 0;
		isInitialized = false;

		// イベントハンドルを閉じる
		if (streamEvent != 0) {
			::SetEvent(streamEvent);
			::CloseHandle(streamEvent);
			streamEvent = 0;
		}
	}

	return;
}


void MyKinect::setRgbImage()
{
	try {
		// RGBカメラのフレームデータを取得
		NUI_IMAGE_FRAME imageFrame = {0};
		ERROR_CHECK(kinect->NuiImageStreamGetNextFrame(imageStreamHandle, 0, &imageFrame));

		// 画像データを取得
		NUI_LOCKED_RECT colorData;
		imageFrame.pFrameTexture->LockRect(0, &colorData, 0, 0);

		// 画像データをコピー
		rgbImage = cv::Mat(height, width, CV_8UC4, colorData.pBits);
		// CV_8UC3に変える
		cv::cvtColor(rgbImage, rgbImage, CV_RGBA2RGB);
		// 表示画像を反転
		//cv::flip(rgbImage, rgbImage, 1);

		// フレームデータを解放
		ERROR_CHECK(kinect->NuiImageStreamReleaseFrame(imageStreamHandle, &imageFrame));

	}
	catch (std::exception &ex) {
		std::cout << ex.what() << std::endl;
	}

	return;
}


void MyKinect::setDepthImage()
{
	try {

		// Depthカメラのフレームデータを取得
		NUI_IMAGE_FRAME depthFrame = {0};
		ERROR_CHECK(kinect->NuiImageStreamGetNextFrame(depthStreamHandle, 0, &depthFrame));

		// 距離データを取得
		NUI_LOCKED_RECT depthData = {0};
		depthFrame.pFrameTexture->LockRect(0, &depthData, 0, 0);

		USHORT* depth = (USHORT*)depthData.pBits;

		// 距離画像準備(距離画像座標系とカラー画像座標系それぞれ)
		rawDepthData = cv::Mat(height, width, CV_16UC1, depthData.pBits);
		rawDepthImage = cv::Mat(height, width, CV_8U, cv::Scalar(0));

		depthData_Mat = cv::Mat(height, width, CV_16UC1, cv::Scalar(0));
		depthImage = cv::Mat(height, width, CV_8U, cv::Scalar(0));

		// PointCloud準備
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr points(new pcl::PointCloud<pcl::PointXYZRGB>);
		// PointCloudのサイズ指定
		points->width = width;
		points->height = height;

		for(int row = 0; row < rawDepthData.rows; row++) {
			for(int col = 0; col < rawDepthData.cols; col++) {
				LONG colorX;																	//対応するカラー画像のx座標
				LONG colorY;																	//対応するカラー画像のy座標
				USHORT distance = ::NuiDepthPixelToDepth(rawDepthData.at<USHORT>(row, col));	//距離(単位mm)


				// 距離カメラ座標系の距離画像を保存--------------------------------------------------------
				// 距離に応じて明度値を設定(近いほど明るいor暗い)
				//
				//
				//
				//

				// 距離取得範囲外を黒にする	ここを狭めれば任意の範囲の距離画像になる
				if(((float)distance < 500.0) || (MAX_DEPTH < (float)distance))
					rawDepthImage.at<UCHAR>(row, col) = 0;


				// カラーカメラ座標系の距離画像を保存------------------------------------------------------
				// Depthカメラの座標をRGBカメラの座標に変換
				kinect->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(COLOR_RESOLUTION,
					DEPTH_RESOLUTION, 0, col, row, rawDepthData.at<USHORT>(row, col), &colorX, &colorY);

				// 変換後の座標がカラー画像座標(640x480)中に含まれているかチェック
				//
				//
				//
				//

				
				// 距離取得範囲内に色づけ
				//
				//
				//
				//


				// PointCloudに保存------------------------------------------------------------------------
				// 3次元座標系に変換(単位m)
				//
				//
				//
				//

				//この条件を狭めれば任意の範囲の距離での点群になる
				if(500.0 < (float)distance && (float)distance < MAX_DEPTH) {
					pcl::PointXYZRGB point;
					// この3次元点の位置と色を書き込む
					//
					//
					//
					//

					// pointsに格納
					points->push_back(point);
				}

			}
		}

		cloud = points;

		// フレームデータを解放
		ERROR_CHECK(kinect->NuiImageStreamReleaseFrame(depthStreamHandle, &depthFrame));
	}
	catch (std::exception &ex) {
		std::cout << ex.what() << std::endl;
	}

	return;
}


void MyKinect::setPointCloud()
{
	try {

		// PointCloud準備
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr points(new pcl::PointCloud<pcl::PointXYZRGB>);
		// PointCloudのサイズ指定
		points->width = width;
		points->height = height;

		for(int row = 0; row < rawDepthData.rows; row++) {
			for(int col = 0; col < rawDepthData.cols; col++) {
				LONG colorX;																	//対応するカラー画像のx座標
				LONG colorY;																	//対応するカラー画像のy座標
				USHORT distance = ::NuiDepthPixelToDepth(rawDepthData.at<USHORT>(row, col));	//距離(単位mm)

				// Depthカメラの座標をRGBカメラの座標に変換
				kinect->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(COLOR_RESOLUTION,
					DEPTH_RESOLUTION, 0, col, row, rawDepthData.at<USHORT>(row, col), &colorX, &colorY);

				// PointCloudに保存------------------------------------------------------------------------
				//　3次元座標系に変換(単位m)
				//
				//
				//
				//

				//この条件を狭めれば任意の範囲の距離での点群になる
				if(500.0 < (float)distance && (float)distance < MAX_DEPTH) {
					pcl::PointXYZRGB point;
					// この3次元点の位置と色を書き込む
					//
					//
					//
					//

					//pointsに格納
					points->push_back(point);
				}

			}
		}

		cloud = points;

	}
	catch (std::exception &ex) {
		std::cout << ex.what() << std::endl;
	}

	return;
}

void MyKinect::flipImgs()
{
	cv::flip(rgbImage, rgbImage, 1);

	cv::flip(rawDepthData, rawDepthData, 1);
	cv::flip(rawDepthImage, rawDepthImage, 1);

	cv::flip(depthData_Mat, depthData_Mat, 1);
	cv::flip(depthImage, depthImage, 1);

	return;
}