// Cのmin,maxマクロを無効にする
#define NOMINMAX

// 安全でないメソッドの呼び出しで警告を無効にする
#define _SCL_SECURE_NO_WARNINGS

#include <iostream>

#include <Windows.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

#include "kinect2_grabber.h"
#include "pclBody.h"
#include "kinectAr.h"
#include "pclFilter.h"

// 点群の型を定義しておく
typedef pcl::PointXYZRGB PointType;

//プロトタイプ宣言
void voxelGridFilter(pcl::PointCloud<PointType>::Ptr cloud);
pcl::PointIndices::Ptr segmentation(pcl::PointCloud<PointType>::Ptr& cloud);
void extractIndicies(pcl::PointCloud<PointType>::Ptr cloud, pcl::PointIndices::Ptr inliers);

pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
int main()
{
	try{
		//ビューアー
		pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");

		//点群
				pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);

		//点群の排他処理
				boost::mutex mutex;

		//データの更新ごとに呼ばれる関数(オブジェクト)
				boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function = [&cloud, &mutex](const pcl::PointCloud<PointType>::ConstPtr &new_cloud) {
					boost::mutex::scoped_lock lock(mutex);
					pcl::copyPointCloud( *new_cloud, *cloud);
				};

		//kinect2Grabberを開始する
				pcl::Kinect2Grabber grabber;
				grabber.registerCallback(function);
				grabber.start();

		//ビューアーが終了されるまで動作する
		while (!viewer.wasStopped()){
			//表示を更新する
			viewer.spinOnce();

			//PCLのフィルターを扱う準備をする
			pclFilter pclFilter;

			//関節認識の準備をする
			pclBody pclBody;
			pclBody.updateBodyFrame();
			auto jointsCloud = pclBody.jointsToPointCloud();
			auto handsCloud = pclBody.handsToPointCloud();

			//点群がある場合
			boost::mutex::scoped_try_lock lock(mutex);
			if ((cloud->size() != 0) && lock.owns_lock()){
				//点群を間引く
				pclFilter.voxelGridFilter(cloud);

				//平面を検出
				pclFilter.segmentation(cloud, coefficients, inliers);

				if (inliers->indices.size() != 0)
				{
					if (pclBody.touchPlane(coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], 21))
						std::cerr << "Left touch:true" << std::endl;
					else std::cerr << "Left touch:false" << std::endl;
					if (pclBody.touchPlane(coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], 23))
						std::cerr << "Right touch:true" << std::endl;
					else std::cerr << "Right touch:false" << std::endl;
				}

				//平面を抽出
				pclFilter.extractIndicies(cloud, inliers);

				//点群を更新する
				auto ret = viewer.updatePointCloud(jointsCloud, "jointsCloud");
				if (!ret){
					//更新がエラーになった場合は未作成なので新しい点群として追加する
					viewer.addPointCloud(jointsCloud, "jointsCloud");
				}
				ret = viewer.updatePointCloud(handsCloud, "handsCloud");
				if (!ret){
					//更新がエラーになった場合は未作成なので新しい点群として追加する
					viewer.addPointCloud(handsCloud, "handsCloud");
				}
				ret = viewer.updatePointCloud(cloud, "cloud");
				if (!ret){
					//更新がエラーになった場合は未作成なので新しい点群として追加する
					viewer.addPointCloud(cloud, "cloud");
				}
			}
			
			//ESCが押されたら終了する
			if (GetKeyState(VK_ESCAPE) < 0){
				break;
			}
		}
		grabber.stop();
	}
	catch (std::exception& ex){
		std::cout << ex.what() << std::endl;
	}
}