#include "pclBody.h"

//Kinect v2を使用する準備をする
pclBody::pclBody()
{
	hResult = GetDefaultKinectSensor(&kinect);
	if (FAILED(hResult)){
		std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
	}
	hResult = kinect->Open();
	if (FAILED(hResult)){
		std::cerr << "Error : IKinectSensor::Open()" << std::endl;
	}

	hResult = kinect->get_BodyFrameSource(&bodyFrameSource);
	if (FAILED(hResult)){
		std::cerr << "Error : IKinectSensor::get_BodyFrameSource()" << std::endl;
	}
	hResult = bodyFrameSource->OpenReader(&bodyFrameReader);
	if (FAILED(hResult)){
		std::cerr << "Error : IBodyFrameSource::OpenReader()" << std::endl;
	}

	for (auto& body : bodies){
		body = nullptr;
	}
}

//骨格検出の更新処理を行う
void pclBody::updateBodyFrame()
{
	//フレームを取得する
	IBodyFrame* bodyFrame = nullptr;
	hResult = bodyFrameReader->AcquireLatestFrame(&bodyFrame);
	if (FAILED(hResult)){
		std::cerr << "Error : IBodyFrameReader::AcquireLatestFrame()" << std::endl;
		return;
	}

	//前回のBodyを開放する
	for (auto& body : bodies){
		if (body != nullptr){
			body->Release();
			body = nullptr;
		}
	}

	//データを取得する
	hResult = bodyFrame->GetAndRefreshBodyData(BODY_COUNT, bodies);
	if (FAILED(hResult)){
		std::cerr << "Error : IBodyFrame::GetAndRefreashBodyData()" << std::endl;
	}

	if (bodyFrameReader != nullptr){
		bodyFrameReader->Release();
		bodyFrameReader = nullptr;
	}

	if (bodyFrame != nullptr){
		bodyFrame->Release();
		bodyFrame = nullptr;
	}
}

//Kinect v2の骨格の座標情報を3D点群情報に変換する。
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclBody::jointsToPointCloud()
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointXYZRGB point;

	BOOLEAN isTracked = false;

	for (auto body : bodies){
		if (body == nullptr){
			continue;
		}

		hResult = body->get_IsTracked(&isTracked);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::get_IsTracked()" << std::endl;
		}

		hResult = body->GetJoints(JointType::JointType_Count, joints);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::GetJoints()" << std::endl;
		}
		if (isTracked){
			for (auto joint : joints){
				point.b = 0;
				point.g = 255;
				point.r = 255;
				point.x = joint.Position.X;
				point.y = joint.Position.Y;
				point.z = joint.Position.Z;

				pointcloud->push_back(point);
			}
		}
	}
	return pointcloud;
}

//kinect v2の指先の座標情報を3D点群に変換する
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclBody::handsToPointCloud()
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointXYZRGB point;

	BOOLEAN isTracked = false;

	for (auto body : bodies){
		if (body == nullptr){
			continue;
		}

		hResult = body->get_IsTracked(&isTracked);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::get_IsTracked()" << std::endl;
		}

		hResult = body->GetJoints(JointType::JointType_Count, joints);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::GetJoints()" << std::endl;
		}
		if (isTracked){
			for (auto joint : joints){
				if (joint.JointType == 21 || joint.JointType == 23)
				{
					point.b = 0;
					point.g = 0;
					point.r = 255;
					point.x = joint.Position.X;
					point.y = joint.Position.Y;
					point.z = joint.Position.Z;
					pointcloud->push_back(point);
				}
			}
		}
	}
	return pointcloud;
}

//平面と指先の接触判定をする
BOOLEAN pclBody::touchPlane(double a, double b, double c, double d, int jointType)
{
	BOOLEAN isTracked = false;

	for (auto body : bodies){
		if (body == nullptr){
			continue;
		}

		hResult = body->get_IsTracked(&isTracked);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::get_IsTracked()" << std::endl;
		}

		hResult = body->GetJoints(JointType::JointType_Count, joints);
		if (FAILED(hResult)){
			std::cerr << "Error : IBody::GetJoints()" << std::endl;
		}
		if (isTracked){
			for (auto joint : joints){
				if (joint.JointType == jointType)
				{
					double perpendicular = (joint.Position.X*a + joint.Position.Y*b + joint.Position.Z*c + d) / sqrt(pow(a, 2) + pow(b, 2) + pow(c, 2));
					std::cerr << joint.JointType << std::endl;
					std::cerr << perpendicular << std::endl;
					if (perpendicular <= 0.01 && perpendicular >= -0.01)
						return true;
				}
			}
		}
	}
	return false;
}