/**
@brief    OpenNI用 ツール
@file     OpenNiTool.cpp
@author   Fumi.Iseki (C)
*/


#include  "OpenNiTool.h"
#include  "xtools++.h"



using namespace jbxl;



#ifndef  DISABLE_OPENNI




COpenNiTool::COpenNiTool(void)
{
	device			= NULL;
	m_state			= NI_STATE_DETECT_STOPPED;
	m_err_mesg		= make_Buffer(LMESG);

	tracking_user	= (XnUserID)0;
	tracking_deny	= (XnUserID)0;


	recorder		= NULL;
	skeleton		= NULL;
	pose			= NULL;
	poseName		= NULL;

	userCallbacks	= NULL;
	poseCallbacks	= NULL;
	calibCallbacks	= NULL;

	usb_dev			= NULL;
	devPath			= NULL;

	memset(dUsers, 0, sizeof(XnUserID)*OPENNI_MAX_USERS_NUM);

	clear_JointsData();
}




BOOL  COpenNiTool::init(BOOL use_image)
{
	device = new COpenNiDevice();
	BOOL ret = device->init(use_image);
	if (ret) {
		// default is mirror mode
		if (device->context!=NULL) device->context->SetGlobalMirror(TRUE);
	}
	else {
		copy_s2Buffer(&device->m_err_mesg, &m_err_mesg);
	}

	//
	m_state	= NI_STATE_DETECT_STOPPED;

	return ret;
}



void  COpenNiTool::free(void)
{
	clear_Tracking();
	clear_CallBacks();
	clear_Skeleton();

	delete_Device();

	free_Buffer(&m_err_mesg);
}




void  COpenNiTool::delete_Device(void)
{
	if (device!=NULL) {
		delete(device);
		device = NULL;
	}
	return;
}





void  COpenNiTool::clear_JointsData(void)
{
	memset(jointPosData, 0, sizeof(XnVector3D) *OPENNI_MAX_JOINT_NUM);
	memset(jointRotData, 0, sizeof(XnMatrix3X3)*OPENNI_MAX_JOINT_NUM);

	memset(jointPosConfidence, 0, sizeof(XnConfidence)*OPENNI_MAX_JOINT_NUM);
	memset(jointRotConfidence, 0, sizeof(XnConfidence)*OPENNI_MAX_JOINT_NUM);
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Create Capabilities


BOOL  COpenNiTool::make_Skeleton(BOOL force_pose, int profile, float smooth)
{
	if (device->user==NULL) {
		copy_s2Buffer("COpenNiTool::make_Skeleton ERROR: user is NULL", &m_err_mesg);
		return FALSE;
	}
	if (!device->user->IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
		copy_s2Buffer("COpenNiTool::make_Skeleton ERROR: not support skeleton", &m_err_mesg);
		return FALSE;
	}

	if (skeleton==NULL) {
		skeleton = new xn::SkeletonCapability(*device->user);
		if (skeleton==NULL) {
			copy_s2Buffer("COpenNiTool::make_Skeleton ERROR: fail to make skeleton capability", &m_err_mesg);
			return FALSE;
		}
	}

	// キャリブレーションにはポーズが必要．
	if (skeleton->NeedPoseForCalibration() || force_pose) {	
		if (!device->user->IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
			copy_s2Buffer("COpenNiTool::make_Skeleton ERROR: not support pose detection", &m_err_mesg);
			clear_Skeleton();
			return FALSE;
		}

		pose = new xn::PoseDetectionCapability(*device->user);
		if (pose==NULL) {
			copy_s2Buffer("COpenNiTool::make_Skeleton ERROR: fail to make pose capability", &m_err_mesg);
			clear_Skeleton();
			return FALSE;
		}

		// キャリブレーションポーズ名の取得．
		poseName = (XnChar*)malloc(OPENNI_LEN_POSE_NAME);
		skeleton->GetCalibrationPose(poseName);
	}

	skeleton->SetSkeletonProfile((XnSkeletonProfile)profile);
	skeleton->SetSmoothing((XnFloat)smooth);
	return TRUE;
}




////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Clear Capabilities

//
void  COpenNiTool::clear_Skeleton(void)
{
	if (skeleton!=NULL) delete(skeleton);
	if (pose!=NULL)		delete(pose);
	if (poseName!=NULL) ::free(poseName);
	
	skeleton = NULL;
	pose     = NULL;
	poseName = NULL;
}



void  COpenNiTool::clear_Tracking(void)
{
	if (device!=NULL && device->user!=NULL) {
		nUsers = OPENNI_MAX_USERS_NUM;
		device->user->GetUsers(dUsers, nUsers);
		for (int i=0; i<nUsers; i++) {
			if (skeleton!=NULL && skeleton->IsTracking(dUsers[i])) {
				skeleton->StopTracking(dUsers[i]);
			}
			if (poseName!=NULL) device->user->GetPoseDetectionCap().StopPoseDetection(dUsers[i]);
		}
	}
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Start and Stop Detection (Reset)

BOOL  COpenNiTool::start_Detection(BOOL force_pose, int profile, float smooth)
{
	if (m_state==NI_STATE_DETECT_EXEC) {
		copy_s2Buffer("COpenNiTool::start_Detection WARNING: detection is already executed", &m_err_mesg);
		return FALSE;
	}

	m_state = NI_STATE_DETECT_STARTING;
	clear_JointsData();
	tracking_user = (XnUserID)0;
	tracking_deny = (XnUserID)0;

	BOOL     ret = device->create_User();
	if (ret) ret = make_Skeleton(force_pose, profile, smooth);
	if (ret) ret = setup_CallBacks();

	if (!ret) {
		m_state = NI_STATE_DETECT_STOPPING;
		clear_Skeleton();
		device->delete_User();
		device->delete_Depth();
		m_state = NI_STATE_DETECT_STOPPED;
		copy_s2Buffer("COpenNiTool::start_Detection ERROR: Myabe NITE is not installed!!", &m_err_mesg);
		return FALSE;
	}

	m_state = NI_STATE_DETECT_EXEC;

	return TRUE;
}



BOOL  COpenNiTool::stop_Detection(void)
{
	if (m_state==NI_STATE_DETECT_STOPPED) {
		copy_s2Buffer("COpenNiTool::stop_Detection WARNING: detection is already stopped", &m_err_mesg);
		return FALSE;
	}

	m_state = NI_STATE_DETECT_STOPPING;
	Sleep(NI_STOP_WAIT_TIME);

	clear_Tracking();
	clear_CallBacks();
	clear_Skeleton();
	device->delete_User();

	tracking_user = (XnUserID)0;
	tracking_deny = (XnUserID)0;

	m_state = NI_STATE_DETECT_STOPPED;
	
	return TRUE;
}



XnUserID  COpenNiTool::get_TrackingUser(void)
{
	XnUserID id = (XnUserID)0;

	if (device!=NULL && device->user!=NULL && skeleton!=NULL) {
		nUsers = OPENNI_MAX_USERS_NUM;
		device->user->GetUsers(dUsers, nUsers);

		int start = 0;
		for (int i=0; i<nUsers; i++) {
			if (tracking_deny==dUsers[i]) {
				start = i + 1;
				break;
			}
		}

		//
		for (int i=0; i<nUsers; i++) {
			int idx = (start + i) % nUsers;
			if (skeleton->IsTracking(dUsers[idx])) {
				id = dUsers[idx];
				break;
			}
		}
	}

	return id;
}




void  COpenNiTool::set_DenyTrackingSearch(XnUserID user)
{
	if ((int)user>0) tracking_deny = user;
	else             tracking_deny = (XnUserID)0;

	return;
}






/////////////////////////////////////////////////////////////////////////////////////////////////////
// Joint Data

void  COpenNiTool::get_JointsPositionData(XnUserID nId)
{
	XnSkeletonJointPosition  joint;
	//memset(jointPosData, 0, sizeof(XnVector3D)*OPENNI_MAX_JOINT_NUM);

	if (skeleton!=NULL) {
		for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
			skeleton->GetSkeletonJointPosition(nId, (XnSkeletonJoint)j, joint);
			jointPosData[j] = joint.position;
			jointPosConfidence[j] = joint.fConfidence;
		}
	}
}



void  COpenNiTool::get_JointsRotationData(XnUserID nId)
{
	XnSkeletonJointOrientation  joint;
	//memset(jointRotData, 0, sizeof(XnMatrix3X3)*OPENNI_MAX_JOINT_NUM);

	if (skeleton!=NULL) {
		for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
			skeleton->GetSkeletonJointOrientation(nId, (XnSkeletonJoint)j, joint);
			jointRotData[j] = joint.orientation;
			jointRotConfidence[j] = joint.fConfidence;
		}
	}
}




XnVector3D	 COpenNiTool::jointPositionData(int j)
{ 
	XnVector3D vect;

	if (j>=0 && j<OPENNI_MAX_JOINT_NUM) {
		vect = jointPosData[j];
	}
	else {
		memset(&vect, 0, sizeof(XnVector3D));
	}

	return vect;
}



XnMatrix3X3	 COpenNiTool::jointRotationData(int j)
{
	XnMatrix3X3 mtrx;

	if (j>=0 && j<OPENNI_MAX_JOINT_NUM) {
		mtrx = jointRotData[j];
	}
	else {
		memset(&mtrx, 0, sizeof(XnMatrix3X3));
	}

	return mtrx;
}



float   COpenNiTool::jointPositionConfidence(int j)
{ 
	float cnfd = 0.0;

	if (j>=0 && j<OPENNI_MAX_JOINT_NUM) {
		cnfd = (float)jointPosConfidence[j];
	}

	return cnfd;
}



float   COpenNiTool::jointRotationConfidence(int j)
{ 
	float cnfd = 0.0;

	if (j>=0 && j<OPENNI_MAX_JOINT_NUM) {
		cnfd = (float)jointRotConfidence[j];
	}

	return cnfd;
}






////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// コールバック

/// コールバック関数の登録
BOOL  COpenNiTool::setup_CallBacks()
{
	BOOL     ret = set_UserCallbacks (&userDetect, &userLost, this);
	if (ret) ret = set_PoseCallbacks (&poseDetect, &poseLost, this);
	if (ret) ret = set_CalibCallbacks(&calibStart, &calibEnd, this);
	if (!ret) return FALSE;

	return TRUE;
}



void  COpenNiTool::clear_CallBacks()
{
	unset_UserCallbacks();
	unset_PoseCallbacks();
	unset_CalibCallbacks();
}



BOOL  COpenNiTool::set_UserCallbacks(xn::UserGenerator::UserHandler newUser, xn::UserGenerator::UserHandler lostUser, void* cookie)
{
	if (device->user==NULL) {
		copy_s2Buffer("COpenNiTool::set_UserCallbacks ERROR: user is NULL", &m_err_mesg);
		return FALSE;
	}

	rc = device->user->RegisterUserCallbacks(newUser, lostUser, cookie, userCallbacks);
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiTool::set_UserCallbacks ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		unset_UserCallbacks();
		return FALSE;
	}
	return TRUE;
}



BOOL  COpenNiTool::set_PoseCallbacks(xn::PoseDetectionCapability::PoseDetection detectPose, xn::PoseDetectionCapability::PoseDetection lostPose, void* cookie)
{
	if (pose==NULL) return TRUE;	// ポーズ不要

	rc = pose->RegisterToPoseCallbacks(detectPose, lostPose, cookie, poseCallbacks);
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiTool::set_PoseCallbacks ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		unset_PoseCallbacks();
		return FALSE;
	}
	return TRUE;
}



BOOL  COpenNiTool::set_CalibCallbacks(xn::SkeletonCapability::CalibrationStart calibStart, xn::SkeletonCapability::CalibrationEnd calibEnd, void* cookie)
{
	if (skeleton==NULL) {
		copy_s2Buffer("COpenNiTool::set_CalibCallbacks ERROR: skeleton is NULL", &m_err_mesg);
		return FALSE;
	}

	rc = skeleton->RegisterCalibrationCallbacks(calibStart, calibEnd, cookie, calibCallbacks);
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiTool::set_CalibCallbacks ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		unset_CalibCallbacks();
		return FALSE;
	}
	return TRUE;
}



void  COpenNiTool::unset_UserCallbacks()
{
	if (device!=NULL && device->user!=NULL && userCallbacks!=NULL) {
		device->user->UnregisterUserCallbacks(userCallbacks);
		userCallbacks = NULL;
	}
	return;
}



void  COpenNiTool::unset_PoseCallbacks()
{
	if (pose!=NULL && poseCallbacks!=NULL) {
		pose->UnregisterFromPoseCallbacks(poseCallbacks);
		poseCallbacks = NULL;
	}
	return;
}



void  COpenNiTool::unset_CalibCallbacks()
{
	if (skeleton!=NULL && calibCallbacks!=NULL) {
		skeleton->UnregisterCalibrationCallbacks(calibCallbacks);
		calibCallbacks = NULL;
	}
	return;
}





/////////////////////////////////////////////////////////////////////////////////////////////////////
// File

BOOL  COpenNiTool::create_Recorder(char* file_name, BOOL use_image)
{
	if (device->context==NULL) {
		copy_s2Buffer("COpenNiTool::create_Recorder ERROR: context is NULL", &m_err_mesg);
		return FALSE;
	}	
	if (device->depth==NULL) {
		copy_s2Buffer("COpenNiTool::create_Recorder ERROR: depth is NULL", &m_err_mesg);
		return FALSE;
	}

	recorder = new xn::Recorder();
	rc = recorder->Create(*device->context);
	if (rc==XN_STATUS_OK) rc = recorder->SetDestination(XN_RECORD_MEDIUM_FILE, file_name);
	if (rc==XN_STATUS_OK && use_image && device->image!=NULL) rc = recorder->AddNodeToRecording(*device->image, XN_CODEC_JPEG);
	if (rc==XN_STATUS_OK) rc = recorder->AddNodeToRecording(*device->depth, XN_CODEC_16Z);	
	
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiTool::create_Recorder ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		return FALSE;
	}

	return TRUE;
}



void	COpenNiTool::delete_Recorder(void)
{
	if (recorder!=NULL) {
		int prev_state = m_state;
		m_state = NI_STATE_SAVE_WORKING;
		{
			Sleep(NI_WORKING_WAIT_TIME);
			if (device->depth!=NULL) recorder->RemoveNodeFromRecording(*device->depth);
			if (device->image!=NULL) recorder->RemoveNodeFromRecording(*device->image);
			//if (use_camera && image!=NULL) recorder->RemoveNodeFromRecording(*image);
			recorder->Release();
			delete(recorder);
		}
		m_state = prev_state;
	}	
	recorder = NULL;
}



BOOL  COpenNiTool::start_Recorde(char* file_name, BOOL use_image)
{
	BOOL ret = FALSE;
	unlink(file_name);

	int prev_state = m_state;
	m_state = NI_STATE_SAVE_WORKING;
	{
		Sleep(NI_WORKING_WAIT_TIME);
		ret = create_Recorder(file_name, use_image);
		if (ret) {
			rc = recorder->Record();
			if (rc!=XN_STATUS_OK) {
				copy_s2Buffer("COpenNiTool::start_Recorde ERROR: ", &m_err_mesg);
				cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
				ret = FALSE;
			}
		}
	}
	m_state = prev_state;

	if (!ret) {
		delete_Recorder();
		unlink(file_name);
	}
	return ret;
}



void  COpenNiTool::stop_Recorde(void)
{
	delete_Recorder();
}









/////////////////////////////////////////////////////////////////////////////////////////////////////
// USB

BOOL  COpenNiTool::open_USB_Device(void)
{
	rc = ::xnUSBInit();
	if (rc!=XN_STATUS_OK && rc!=XN_STATUS_USB_ALREADY_INIT) return FALSE;
	
#ifdef WIN32

	XnUInt32 count;
	rc = ::xnUSBEnumerateDevices(NI_VID_MICROSOFT, NI_PID_NUI_MOTOR, (const XnUSBConnectionString**)&devPath, &count);
	if (rc!=XN_STATUS_OK) {
		::xnUSBShutdown();
		return FALSE;
	}

	rc = ::xnUSBOpenDeviceByPath(*devPath, &usb_dev);
	if (rc!=XN_STATUS_OK) {
		::xnUSBFreeDevicesList(devPath);
		return FALSE;
	}

#else

	rc = ::xnUSBOpenDevice(OPENNI_VID_MICROSOFT, OPENNI_PID_NUI_MOTOR, NULL, NULL, &usb_dev);
	if (rc!=XN_STATUS_OK) {
		::xnUSBShutdown();
		return FALSE;
	}

#endif

	return TRUE;
}




void  COpenNiTool::close_USB_Device(void)
{
	if (usb_dev!=NULL) {
		::xnUSBCloseDevice(usb_dev);
		usb_dev = NULL;
	}

#ifdef WIN32

	if (devPath!=NULL) {
		::xnUSBFreeDevicesList(devPath);
		devPath = NULL;
	}

#endif

	::xnUSBShutdown();
}



void  COpenNiTool::set_LED_Color(int col)
{
	if (usb_dev==NULL) return;

	::xnUSBSendControl(usb_dev, XN_USB_CONTROL_TYPE_VENDOR, 0x06, (XnUInt16)col, 0, NULL, 0, 0);

	return;
}



void  COpenNiTool::set_Tilt_Motor(int ang)
{
	if (usb_dev==NULL) return;
	//if (ang>30 || ang<-30) return;

	::xnUSBSendControl(usb_dev, XN_USB_CONTROL_TYPE_VENDOR, 0x31, (XnUInt16)(ang*2), 0, NULL, 0, 0);

	
	Sleep(1000);
	return;
}






/////////////////////////////////////////////////////////////////////////////////////////////////////
// External Callback Functions

void  XN_CALLBACK_TYPE jbxl::userDetect(xn::UserGenerator& user, XnUserID nId, void* cookie)
{
	DEBUG_ERR("User is detected. (%d)", nId);
	if (cookie==NULL) return;
	
	XnChar* poseName = ((COpenNiTool*)cookie)->poseName;
	if (poseName!=NULL) {
		user.GetPoseDetectionCap().StartPoseDetection(poseName, nId);
	}
	else {
		user.GetSkeletonCap().RequestCalibration(nId, TRUE);
	}
	
	DEBUG_ERR("Start Pose Detection. (%d)", nId);
}



void  XN_CALLBACK_TYPE jbxl::userLost(xn::UserGenerator& user, XnUserID nId, void* cookie)
{
	DEBUG_ERR("Lost user. (%d)", nId);

	if (user.GetSkeletonCap().IsValid()) {
		if (user.GetSkeletonCap().IsTracking(nId)) {
			user.GetSkeletonCap().StopTracking(nId);
			DEBUG_ERR("Stop Tracking. (%d)", nId);
		}
	}
}




// Pose Detection

void  XN_CALLBACK_TYPE jbxl::poseDetect(xn::PoseDetectionCapability& pose, const XnChar* poseName, XnUserID nId, void* cookie)
{
	DEBUG_ERR("Pose is detected. (%d)", nId);
	if (cookie==NULL) return;

	xn::UserGenerator* user = ((COpenNiTool*)cookie)->device->user;
	if (user!=NULL) {
		if (!user->GetSkeletonCap().IsCalibrated(nId) && !user->GetSkeletonCap().IsCalibrating(nId)) {
			user->GetPoseDetectionCap().StopPoseDetection(nId);
			user->GetSkeletonCap().RequestCalibration(nId, TRUE);
			DEBUG_ERR("Request Caribration. (%d)", nId);
		}
	}
}



void  XN_CALLBACK_TYPE jbxl::poseLost(xn::PoseDetectionCapability& pose, const XnChar* poseName, XnUserID nId, void* cookie)
{
	DEBUG_ERR("Lost pose. (%d)", poseName, nId);

	if (pose.IsValid()) {
		if (poseName!=NULL) {
			pose.StartPoseDetection(poseName, nId);
			DEBUG_ERR("Restart Pose Detection. (%d)", poseName, nId);
		}
	}
}




// Calibration

void  XN_CALLBACK_TYPE jbxl::calibStart(xn::SkeletonCapability& skeleton, XnUserID nId, void* cookie)
{
	DEBUG_ERR("Start Calibration. (%d)", nId);
}



void  XN_CALLBACK_TYPE jbxl::calibEnd(xn::SkeletonCapability& skeleton, XnUserID nId, XnBool success, void* cookie)
{
	DEBUG_ERR("End Calibration. (%d)", nId);
	if (cookie==NULL) return;

	//
	xn::UserGenerator* user = ((COpenNiTool*)cookie)->device->user;
	if (user!=NULL) {
		if (success) {
			if (user!=NULL) {
				user->GetSkeletonCap().StartTracking(nId);
				DEBUG_ERR("Start Tracking. (%d)", nId);
			}
		}
		else {
			DEBUG_ERR("Faile Calibration. (%d)", nId);
			XnChar* poseName = ((COpenNiTool*)cookie)->poseName;
			if (poseName!=NULL) {
				user->GetPoseDetectionCap().StartPoseDetection(poseName, nId);
			}
			DEBUG_ERR("Restart Pose Detection. (%d)", nId);
		}
	}
}






#endif		// ifndef DISABLE_OPENNI