/**



*/


#include  "OpenNiDevice.h"
#include  "xtools++.h"


using namespace jbxl;



#ifdef  ENABLE_OPENNI




COpenNiDevice::COpenNiDevice(void)
{
	context		 = NULL;
	image		 = NULL;
	user		 = NULL;
	depth		 = NULL;
	player		 = NULL;

	imageMD		 = NULL;
	depthMD		 = NULL;
	sceneMD		 = NULL;

	m_enu_dev	 = EnuDev_Unknown;
	m_has_camera = TRUE;
	m_err_mesg	 = make_Buffer(LMESG);

	m_state		 = NI_STATE_DETECT_STOPPED;
	m_xsize		 = OPENNI_DEPTH_XSIZE;
	m_ysize		 = OPENNI_DEPTH_YSIZE;
	m_nfps		 = OPENNI_DEPTH_FPS;

	//
	recorder	 = NULL;
	skeleton	 = NULL;
//	pose		 = NULL;

	usb_dev		 = NULL;
	devPath		 = NULL;

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

	memset(dUsers, 0, sizeof(XnUserID)*OPENNI_USERS_NUM);
}




BOOL  COpenNiDevice::init(BOOL use_image)
{
    outputMode.nXRes = OPENNI_DEPTH_XSIZE; 
    outputMode.nYRes = OPENNI_DEPTH_YSIZE; 
    outputMode.nFPS	 = OPENNI_DEPTH_FPS; 

	m_enu_dev = EnuDev_None;

	BOOL ret = create_Context();
	if (ret && use_image) create_Image();
	if (ret) ret = create_Depth();
	if (ret) m_enu_dev = EnuDev_PS1080;

	return ret;
}




BOOL  COpenNiDevice::init(char* fname, BOOL use_image)
{
	m_enu_dev = EnuDev_None;

	if (fname==NULL) {
		copy_s2Buffer("COpenNiDevice::init ERROR: File name is NULL", &m_err_mesg);
		return FALSE;
	}

	//
	BOOL ret = create_Context(fname);
	if (ret) ret = create_Player(TRUE);
	if (!ret) {
		copy_s2Buffer("COpenNiDevice::init ERROR: create error!!", &m_err_mesg);
		return FALSE;
	}
	
	//
	xn::NodeInfoList nodeList;
	player->EnumerateNodes(nodeList);

	for (xn::NodeInfoList::Iterator node=nodeList.Begin(); node!=nodeList.End(); node++) {
		if ((*node).GetDescription().Type==XN_NODE_TYPE_IMAGE) {
			if (use_image) {
				ret = create_Image(TRUE);
				if (!ret) break;
			}
		}
		if ((*node).GetDescription().Type==XN_NODE_TYPE_DEPTH) {
			ret = create_Depth(TRUE);
			if (!ret) break;
		}
	}

	//
	if (image!=NULL && use_image) {
		image->GetMapOutputMode(outputMode);
	}
	else if (depth!=NULL) {
		depth->GetMapOutputMode(outputMode);
	}
	else {
		copy_s2Buffer("COpenNiDevice::init ERROR: No Generators", &m_err_mesg);
		return FALSE;
	}

	m_enu_dev = EnuDev_OniFile;
	return TRUE;
}




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

	delete_User();
	delete_Depth();
	delete_Image();
	delete_Player();
	delete_Context();

	free_Buffer(&m_err_mesg);
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Create/Delete Generators

BOOL  COpenNiDevice::create_Context(char* fname)
{
	if (context==NULL) {
		context = new xn::Context();
		if (context==NULL) {
			copy_s2Buffer("COpenNiDevice::create_Context ERROR: context is NULL", &m_err_mesg);
			return FALSE;
		}
	}

	XnStatus rc = context->Init();
	if (rc==XN_STATUS_OK && fname!=NULL) rc = context->OpenFileRecording(fname);

	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiDevice::create_Context ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		return FALSE;
	}

	return TRUE;
}



BOOL  COpenNiDevice::create_Image(BOOL easy)
{
	if (!m_has_camera) return FALSE;

	if (context==NULL) {
		copy_s2Buffer("COpenNiDevice::create_Image ERROR: context is NULL", &m_err_mesg);
		return FALSE;
	}

	if (image==NULL) {
		image = new xn::ImageGenerator();
		if (image==NULL) {
			copy_s2Buffer("COpenNiDevice::create_Image ERROR: fail to create image generator", &m_err_mesg);
			m_has_camera = FALSE;
			return FALSE;
		}
	}


	XnStatus rc = XN_STATUS_ERROR;
	if (easy) {
		rc = context->FindExistingNode(XN_NODE_TYPE_IMAGE, *image);
	}
	else {
		rc = image->Create(*context);
		if (rc==XN_STATUS_OK) rc = image->StartGenerating();
		if (rc==XN_STATUS_OK) rc = image->SetMapOutputMode(outputMode); 
	}

	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiDevice::create_Image ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		delete_Image();
		return FALSE;
	}

	imageMD = new xn::ImageMetaData();
	//imageData = (uByte*)malloc(outputMode.nXRes*outputMode.nYRes*3);

	return TRUE;
}




BOOL  COpenNiDevice::create_Depth(BOOL easy)
{
	if (context==NULL) {
		copy_s2Buffer("COpenNiDevice::create_Depth ERROR: context is NULL", &m_err_mesg);
		return FALSE;
	}

	if (depth==NULL) {
		depth = new xn::DepthGenerator();
		if (depth==NULL) {
			copy_s2Buffer("COpenNiDevice::create_Depth ERROR: fail to create depth generator", &m_err_mesg);
			return FALSE;
		}
	}

	XnStatus rc = XN_STATUS_ERROR;
	if (easy) {
		rc = context->FindExistingNode(XN_NODE_TYPE_DEPTH, *depth);
	}
	else {
		rc = depth->Create(*context);
		if (rc==XN_STATUS_OK) rc = depth->StartGenerating();
		if (rc==XN_STATUS_OK) rc = depth->SetMapOutputMode(outputMode);
		if (rc==XN_STATUS_OK && image!=NULL) {
			rc = depth->GetAlternativeViewPointCap().SetViewPoint(*image);
			/*if (rc==XN_STATUS_OK && depth->IsCapabilitySupported(XN_CAPABILITY_FRAME_SYNC)) {
				if (depth->GetFrameSyncCap().CanFrameSyncWith(*image)) {
					rc = depth->GetFrameSyncCap().FrameSyncWith(*image);
				}
			}*/
		}
	}
	
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiDevice::create_Depth ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		delete_Depth();
		return FALSE;
	}

	depthMD = new xn::DepthMetaData();
	//depthData = (XnLabel*)malloc(outputMode.nXRes*outputMode.nYRes*sizeof(XnLabel));

	return TRUE;
}



BOOL  COpenNiDevice::create_User(BOOL easy)
{
	if (context==NULL) {
		copy_s2Buffer("COpenNiDevice::create_User ERROR: context is NULL", &m_err_mesg);
		return FALSE;
	}

	if (user==NULL) {
		user = new xn::UserGenerator();
		if (user==NULL) {
			copy_s2Buffer("COpenNiDevice::create_User ERROR: fail to create user generator", &m_err_mesg);
			return FALSE;
		}
	}

	XnStatus rc = XN_STATUS_ERROR;
	if (easy) {
		rc = context->FindExistingNode(XN_NODE_TYPE_USER, *user);
	}
	else {
		rc = user->Create(*context);
		if (rc==XN_STATUS_OK) rc = user->StartGenerating();
	}

	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiDevice::create_User ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		delete_User();
		return FALSE;
	}

	sceneMD = new xn::SceneMetaData();
	return TRUE;
}



BOOL  COpenNiDevice::create_Player(BOOL easy)
{
	if (context==NULL) {
		copy_s2Buffer("COpenNiDevice::create_Player ERROR: context is NULL", &m_err_mesg);
		return FALSE;
	}	

	if (player==NULL) {
		player = new xn::Player();
		if (player==NULL) {
			copy_s2Buffer("COpenNiDevice::create_Player ERROR: fail to create player generator", &m_err_mesg);
			return FALSE;
		}
	}

	XnStatus rc = XN_STATUS_ERROR;
	if (easy) {
		rc = context->FindExistingNode(XN_NODE_TYPE_PLAYER, *player);
	}
	else {
		rc = player->Create(*context, "oni");
	}
	
	if (rc!=XN_STATUS_OK) {
		copy_s2Buffer("COpenNiDevice::create_Player EROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		delete_Player();
		return FALSE;
	}

	return TRUE;
}




////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Delete Generators

void  COpenNiDevice::delete_Context(void)
{
	if (context!=NULL) {
		context->Release();
		delete(context);
	}
	context = NULL;
}



void  COpenNiDevice::delete_Image(void)
{
	if (image!=NULL) {
		if (image->IsValid()) {
			if (image->IsGenerating()) image->StopGenerating();
		}
		image->Release();
		delete(image);
	}
	if (imageMD!=NULL) delete(imageMD);

	image   = NULL;
	imageMD = NULL;
}



void  COpenNiDevice::delete_Depth(void)
{
	if (depth!=NULL) {
		if (depth->IsValid()) {
			if (depth->IsGenerating()) depth->StopGenerating();
		}
		depth->Release();
		delete(depth);
	}
	if (depthMD!=NULL) delete(depthMD);

	depth   = NULL;
	depthMD = NULL;
}



void  COpenNiDevice::delete_User(void)
{
	if (user!=NULL) {
		if (user->IsValid()) {
			if (user->IsGenerating()) user->StopGenerating();
		}
		user->Release();
		delete(user);
	}
	if (sceneMD!=NULL) delete(sceneMD);

	user    = NULL;
	sceneMD = NULL;
}



void	COpenNiDevice::delete_Player(void)
{
	if (player!=NULL) {
		player->Release();
		delete(player);
		player = NULL;	
	}	
}




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

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

	if (skeleton==NULL) {
		skeleton = new xn::SkeletonCapability(*user);
		if (skeleton==NULL) {
			copy_s2Buffer("COpenNiDevice::setup_Tracking ERROR: fail to make skeleton capability", &m_err_mesg);
			return FALSE;
		}
	}

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




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

//
void  COpenNiDevice::clear_Skeleton(void)
{
	if (skeleton!=NULL) delete(skeleton);
	skeleton = NULL;
}



void  COpenNiDevice::clear_Tracking(void)
{
	if (user!=NULL) {
		nUsers = OPENNI_USERS_NUM;
		user->GetUsers(dUsers, nUsers);
		for (int i=0; i<nUsers; i++) {
			if (skeleton!=NULL && skeleton->IsTracking(dUsers[i])) {
				skeleton->StopTracking(dUsers[i]);
			}
		}
	}
}






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

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

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

	return TRUE;
}



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



BOOL  COpenNiDevice::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("COpenNiDevice::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  COpenNiDevice::stop_Recorde(void)
{
	delete_Recorder();
}




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

BOOL  COpenNiDevice::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  COpenNiDevice::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  COpenNiDevice::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  COpenNiDevice::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;
}




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

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

	return TRUE;
}



void  COpenNiDevice::clear_CallBacks()
{
	unset_UserCallbacks();
	unset_CalibCallbacks();
}



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

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




BOOL  COpenNiDevice::set_CalibCallbacks(xn::SkeletonCapability::CalibrationStart calibStart, xn::SkeletonCapability::CalibrationEnd calibEnd, void* cookie)
{
	if (skeleton==NULL) {
		copy_s2Buffer("COpenNiDevice::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("COpenNiDevice::set_CalibCallbacks ERROR: ", &m_err_mesg);
		cat_s2Buffer (::xnGetStatusString(rc), &m_err_mesg);
		unset_CalibCallbacks();
		return FALSE;
	}
	return TRUE;
}



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



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










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

void  XN_CALLBACK_TYPE jbxl::userDetect(xn::UserGenerator& user, unsigned int nId, void* cookie)
{
	DEBUG_INFO("jbxl::userDetect(): User is detected. (%d)", nId);
	if (cookie==NULL) return;
	
	user.GetSkeletonCap().RequestCalibration(nId, TRUE);
	
	DEBUG_INFO("jbxl::userDetect(): Start Pose Detection. (%d)", nId);
}



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

	if (user.GetSkeletonCap().IsValid()) {
		if (user.GetSkeletonCap().IsTracking(nId)) {
			user.GetSkeletonCap().StopTracking(nId);
			DEBUG_INFO("jbxl::userLost(): Stop Tracking. (%d)", nId);
		}
	}
}



// Calibration

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



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

	//
	xn::UserGenerator* user = ((COpenNiDevice*)cookie)->user;
	if (user!=NULL) {
		if (success) {
			if (user!=NULL) {
				user->GetSkeletonCap().StartTracking(nId);
				DEBUG_INFO("jbxl::calibEnd(): Start Tracking. (%d)", nId);
			}
		}
	}
}


#endif		// ENABLE_OPENNI
