
#include  "WinBaseLib.h"
#include  "KinectWin.h"
#include  "NiJointsTool.h"

#include  "Graph.h"




using namespace jbxl;


#ifdef  ENABLE_KINECT_SDK


using namespace jbxwl;




CKinectWin::CKinectWin(void)
{
	device			= NULL;
	audio			= NULL;
	speech			= NULL;

	m_err_mesg		= _T("");

	m_image_scale	= 2;
	m_depth_scale	= 2;
	m_skeleton_line = 2;

	m_is_detected   = FALSE;
	m_is_tracking	= FALSE;
	m_is_mirroring	= TRUE;
	//
	m_use_image		= TRUE;
	m_use_led		= FALSE;
	m_use_motor		= FALSE;
	m_use_face		= FALSE;
	m_use_speech	= TRUE;

	//
	m_enable_face	= FALSE;
	m_enable_speech = FALSE;
	m_enable_motor	= TRUE;
	
	m_use_knct_smth = TRUE;
	m_confidence	= 0.5;			// 現時点では 0.0より大きければ何でも良い．
	m_profile		= KINECT_SKEL_PROFILE_ALL;
	m_ground_level	= NI_DEFAULT_GROUND_LEVEL;

	pViewData		= NULL;
	pDepthData		= NULL;

	hasDepthData	= FALSE;
	isDetectShadow	= FALSE;
	isDetectFace	= FALSE;

	tracking_user	= 0;
	tracking_deny	= 0;

	//
	smoothParam.fSmoothing  = 0.5f;
	smoothParam.fCorrection = 0.5f;
	smoothParam.fPrediction = 0.5f;
	smoothParam.fJitterRadius = 0.05f;
	smoothParam.fMaxDeviationRadius = 0.04f;

	clearAvatarDetected();
}



CKinectWin::~CKinectWin(void)
{
	DEBUG_INFO("DESTRUCTOR: CKinectWin: START");
	free();
	DEBUG_INFO("DESTRUCTOR: CKinectWin: END");
}




BOOL  CKinectWin::init(void)
{
	device = new CKinectDevice();

	// for re-Initialize
	::NuiSetDeviceStatusCallback((NuiStatusProc)&KinectStatusProc, this);

	//
	DWORD mode = NUI_INITIALIZE_FLAG_USES_COLOR |
				 NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | 
				 NUI_INITIALIZE_FLAG_USES_SKELETON |
				 NUI_INITIALIZE_FLAG_USES_AUDIO;

	BOOL ret = device->init(mode, m_use_image);	
	if (!ret) {
		m_err_mesg = device->m_err_mesg;
	}

	//
	setDevState(NI_STATE_DETECT_STOPPED);

	return ret;
}



void  CKinectWin::free(void)
{
	freeRingBuffer();

	deleteDevice();
	return;
}



void  CKinectWin::deleteDevice(void)
{
	DEBUG_INFO("CKinectWin::deleteDevice(): START");

	deleteNull(device);

	DEBUG_INFO("CKinectWin::deleteDevice(): END");

	return;
}




CString	CKinectWin::get_err_message(void) 
{
	DEBUG_INFO("CKinectWin::get_err_message(): => %s", ::ts2mbs(m_err_mesg));
	CString mesg = m_err_mesg;
	m_err_mesg = _T("");

	return mesg;
}




void  CKinectWin::clearJointsData(void)
{
	for (int i=0; i<KINECT_JOINT_NUM; i++) {
		rotQuat[i].init(-1.0);
		posVect[i].init(-1.0);
		jntAngl[i] = 0.0;
	}
}




BOOL  CKinectWin::initRingBuffer(void)
{
	BOOL ret = TRUE;

	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		posRing[j].init(NI_RING_BUFFER_SIZE, sizeof(Vector<double>));
		rotRing[j].init(NI_RING_BUFFER_SIZE, sizeof(Quaternion));
		if (posRing[j].state<0 || rotRing[j].state<0) {
			freeRingBuffer();
			m_err_mesg = _T("CKinectWin::initRingBuffer(): WARNING: Out of Memory.");
			ret = FALSE;
			break;
		}
	}

	return ret;
}




void  CKinectWin::freeRingBuffer(void)
{
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (posRing[j].enable) posRing[j].free();
		if (rotRing[j].enable) rotRing[j].free();
	}
}




void  CKinectWin::clearRingBuffer(void)
{
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (posRing[j].enable) posRing[j].clear();
		if (rotRing[j].enable) rotRing[j].clear();
	}
}




void  CKinectWin::backup2RingBuffer(void)
{
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (posVect[j].c>=m_confidence) posRing[j].put(&posVect[j]);
		if (rotQuat[j].c>=m_confidence) rotRing[j].put(&rotQuat[j]);
	}
}




/////////////////////////////////////////////////////////////////////////////////////////////////
//
//
//

void  CKinectWin::clearAvatarDetected(void)
{
	clearJointsData();
	clearRingBuffer();

	startPos   = Vector<double>(0.0, 0.0, 0.0);
	currentPos = Vector<double>(0.0, 0.0, 0.0);
	m_is_detected = FALSE;
	m_ground_level= NI_DEFAULT_GROUND_LEVEL;

	return;
}



//
//
//
BOOL  CKinectWin::checkAvatarDetected(void)
{
	if (posVect[NI_SDK_PELVIS].c>m_confidence && crdVect[NI_SDK_PELVIS].c>0.0) {
		startPos = posVect[NI_SDK_PELVIS];
		m_is_detected = TRUE;

		// m_ground_level is not used now
		if (posVect[NI_SDK_R_ANKLE].c>m_confidence && crdVect[NI_SDK_R_ANKLE].c>0.0) {
			m_ground_level = posVect[NI_SDK_R_ANKLE].z - startPos.z;
			if (posVect[NI_SDK_L_FOOT].c>m_confidence && crdVect[NI_SDK_L_FOOT].c>0.0) {
				m_ground_level = Min(m_ground_level, posVect[NI_SDK_L_ANKLE].z - startPos.z);
			}
		}
		else if (posVect[NI_SDK_L_FOOT].c>m_confidence && crdVect[NI_SDK_L_FOOT].c>0.0) {
			m_ground_level = posVect[NI_SDK_L_ANKLE].z - startPos.z;
		}
	}

	return m_is_detected;
}




void  CKinectWin::setTiltMotor(int ang) 
{ 
	if (m_use_motor && m_enable_motor) { 
		m_enable_motor = FALSE;
		NuiCameraElevationSetAngle(ang); 
		Sleep(1000);
		m_enable_motor = TRUE;
	}
}




void  CKinectWin::setMirroring(BOOL mirror)
{ 
	m_is_mirroring = mirror;
}





BOOL  CKinectWin::startDetection(void)
{
	clearJointsData();

	device->smoothParam = NULL;
	if (m_use_knct_smth) device->smoothParam = &smoothParam;

	tracking_user = 0;
	tracking_deny = 0;
	BOOL ret = device->start_Detection(m_profile);
	if (ret) setLEDColor(NI_LED_BLINK_ORANGE);
	else m_err_mesg = device->m_err_mesg;

	// Start Face Tracking
	if (ret && m_use_face) {
		ret = device->create_Face();
		if (!ret) m_err_mesg = device->m_err_mesg;
	}

	return ret;
}




BOOL  CKinectWin::stopDetection(void)
{
	BOOL ret = device->stop_Detection();
	if (!ret) m_err_mesg = device->m_err_mesg;
	setLEDColor(NI_LED_GREEN);	

	tracking_user = 0;
	tracking_deny = 0;

	// Stop Face Tracking
	isDetectFace = FALSE;
	device->delete_Face();

	return ret;
}




BOOL  CKinectWin::restartDetection(void)
{
	BOOL ret = stopDetection();
	if (ret) ret = startDetection();

	return ret;
}




Vector4  CKinectWin::jointPositionData(int j)
{ 
	Vector4 vect;
	
	if (j>=0 && j<KINECT_JOINT_NUM) {
		vect = device->jointPosData[j];
	}
	else {
		memset(&vect, 0, sizeof(Vector4));
	}

	return vect;
}




//
// color is GRAPH_COLOR_BGRA
//
void  CKinectWin::makeDisplayImage()
{
	int    index;
	uByte* src = NULL;
	uByte* ptr;

	if (pViewData==NULL) return;

	if (device->image!=NULL && m_use_image) {
		src = (uByte*)device->image->m_data;
	}

	//
	if (src!=NULL) {
		for (int j=0; j<pViewData->ysize; j++) {
			int li;
			int ls = j*m_image_scale*device->m_xsize;

			for (int i=0; i<pViewData->xsize; i++) {
				// Mirroring
				if (m_is_mirroring) li = i*m_image_scale;
				else                li = (pViewData->xsize-i-1)*m_image_scale; 

				ptr = &(pViewData->point(i, j));
				index  = (ls + li)*4;
				ptr[0] = src[index];		// B
				ptr[1] = src[index+1];		// G
				ptr[2] = src[index+2];		// R
				ptr[3] = src[index+3];		// X
			}
		}
	}

	// No Camera Image
	else {
		for (int j=0; j<pViewData->ysize; j++) {
			int li;
			int ls = j*m_image_scale*device->m_xsize;

			for (int i=0; i<pViewData->xsize; i++) {
				// Mirror
				if (m_is_mirroring) li = i*m_image_scale;
				else                li = (pViewData->xsize-i-1)*m_image_scale; 

				ptr = &(pViewData->point(i, j));
				ptr[0] = ptr[1] = ptr[2] = 224;		// Gray Background
				ptr[3] = 0;
			}
		}
	}
}




//
// color is GRAPH_COLOR_MONO16
// 表示データは 8bit
//
void  CKinectWin::makeDisplayDepth(CExView* pview)
{
	if (pview==NULL) return;
	pDepthData = &(pview->viewData);

	uWord* src = NULL;
	if (device->depth!=NULL && hasDepthData) {
		src = (uWord*)device->depth->m_data;
	}
	if (src==NULL) return;

	//
	uWord  max = pview->cMax;
	uWord  min = pview->cMin;
	//
	uWord* wrk = (uWord*)malloc(sizeof(uWord)*pDepthData->ysize*pDepthData->xsize);

	//
	for (int j=0; j<pDepthData->ysize; j++) {
		int jj = j*pDepthData->xsize;
		int ls = j*m_depth_scale*device->m_xsize;

		for (int i=0; i<pDepthData->xsize; i++) {
			// Mirroring
			int li = i*m_depth_scale;
			if (!m_is_mirroring) li = device->m_xsize - li - 1; 
			//
			int k = jj + i;
			wrk[k] = ::NuiDepthPixelToDepth(src[ls+li]);
			if      (wrk[k]>max) wrk[k] = max;
			else if (wrk[k]<min) wrk[k] = min;
		}
	}

	//
	float  dif = (float)(max - min);
	uByte* ptr = (uByte*)pDepthData->grptr;

	if (dif!=0.0) {
		for (int i=0; i<pDepthData->ysize*pDepthData->xsize; i++) {
			ptr[i] = (uByte)((max - wrk[i])/dif*255);
		}
	}
	else {
		for (int i=0; i<pDepthData->ysize*pDepthData->xsize; i++) {			
			ptr[i] = (uByte)(255 - wrk[i]/256);
		}
	}
	::free(wrk);

	return;
}






//////////////////////////////////////////////////////////////////////////////////
//
// Tracking Main
//
BOOL  CKinectWin::trackingJoints(void)
{
	m_is_tracking = FALSE;

	if (hasDepthData) {
		//
		//isDetectShadow = detectShadow();	// not use
		int tuser = tracking_user;
		tracking_user = device->wait_Skeleton(tracking_user, tracking_deny);	
	
		if (tuser!=tracking_user) {
			m_is_detected = FALSE;
			if (tracking_user==0) {
				clearAvatarDetected();
				lostTrackingUser(tuser);					// virtual
			}
			else {
				clearJointsData();
				detectTrackingUser(tracking_user);			// virtual
			}
		}

		// Tracking
		if (tracking_user>0 && tracking_user<=KINECT_USERS_NUM) {
			//
			getJointsPosData();
			//
			if (m_is_detected) {
				convertJointsData();	// virtual
				//
				backup2RingBuffer();
				saveJointsData();		// virtual
				loggingJointsData();	// virtual
			}
				//
			paintShadow();
			if (m_is_detected) {
				drawSkeleton(NiGetSkeletonColor(tracking_user), m_skeleton_line);
				drawAddition(NiGetSkeletonColor(tracking_user), m_skeleton_line);	// virtual
			}
			//
			m_is_tracking = TRUE;
		}
	}
	else {
		m_is_detected = FALSE;
	}

	return m_is_tracking;
}




Vector4	 CKinectWin::joint_PositionData(int j)
{ 
	Vector4 vect;

	if (j>=0 && j<KINECT_JOINT_NUM) {
		vect = device->jointPosData[j];
	}
	else {
		memset(&vect, 0, sizeof(Vector4));
	}

	return vect;
}



//
// Calculate posVect, crdVect
//
void  CKinectWin::getJointsPosData(void)
{
	if (!m_is_mirroring) {
		for (int j=0; j<KINECT_JOINT_NUM; j++) {
			device->jointPosData[j].x = - device->jointPosData[j].x;
		}
	}

	//
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		Vector4 pos = device->jointPosData[j];
		int n = j;
		if (m_is_mirroring) n = NiSDKMirrorJointNum(j, NiSDK_Kinect);
		posVect[n].set(-pos.z, pos.x, pos.y);
	}

	// Tracking Mode
	if (m_profile==KINECT_SKEL_PROFILE_ALL) {
		posVect[NI_SDK_TORSO] = 2.0*posVect[NI_SDK_PELVIS] - (posVect[NI_SDK_R_HIP] + posVect[NI_SDK_L_HIP])/2.0;
		posVect[NI_SDK_TORSO].c = Min(posVect[NI_SDK_R_HIP].c, posVect[NI_SDK_L_HIP].c);
		posVect[NI_SDK_TORSO].c = Min(posVect[NI_SDK_TORSO].c, posVect[NI_SDK_PELVIS].c);
	}
	else if (m_profile==KINECT_SKEL_PROFILE_UPPER) {
		/* Neck の座標は体軸上にない
		// NECK + (CSHLDR-NECK)*4
		Vector<double> torso_down = (posVect[NI_SDK_R_SHLDR] + posVect[NI_SDK_L_SHLDR])/2.0 - posVect[NI_SDK_NECK];
		if (torso_down.z>0.0) torso_down = - torso_down;
		Vector<double> vect = posVect[NI_SDK_NECK] + torso_down*4.0;

		posVect[NI_SDK_TORSO] = vect;
		posVect[NI_SDK_TORSO].c = Min(posVect[NI_SDK_R_SHLDR].c, posVect[NI_SDK_L_SHLDR].c);
		posVect[NI_SDK_TORSO].c = Min(posVect[NI_SDK_TORSO].c,   posVect[NI_SDK_NECK].c);
		*/

		Vector<double> torso_down(0.0, 0.0, -0.2);

		posVect[NI_SDK_TORSO] = (posVect[NI_SDK_R_SHLDR] + posVect[NI_SDK_L_SHLDR])/2.0 + torso_down;
		posVect[NI_SDK_TORSO].c = Min(posVect[NI_SDK_R_SHLDR].c, posVect[NI_SDK_L_SHLDR].c);
		posVect[NI_SDK_PELVIS] = posVect[NI_SDK_TORSO] + torso_down;
	}

	//
	set2DCoordinate();
	checkBoneLength();
	if (!m_is_detected) checkAvatarDetected();

	//
	currentPos = posVect[NI_SDK_PELVIS];
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		posVect[j] = posVect[j] - startPos;
		if (posVect[j].c<m_confidence || crdVect[j].c<0.0) posVect[j].c = -1.0;
	}
	checkGroundLevel();
}







/////////////////////////////////////////////////////////////////////////////////////
//
//
//

BOOL  CKinectWin::detectShadow(void)
{
	int label = 0;

	if (pViewData==NULL) return FALSE;

	//
	for (int j=0; j<pViewData->ysize; j++) {
		int li;
		int lj = j*m_image_scale;

		for (int i=0; i<pViewData->xsize; i++) {
			// Mirroring
			if (m_is_mirroring) li = i*m_image_scale;
			else                li = (pViewData->xsize-i-1)*m_image_scale; 

			// User Label
			if (getDevState()==NI_STATE_DETECT_EXEC && hasDepthData) {
				//label = device->depth->get_user(li/2, lj/2);	// 320x240
				label = device->depth->get_user_index(li, lj);	// 640x480
				if (label>0) {
					if (tracking_user==label) return TRUE;
				}
			}
		}
	}

	return FALSE;
}




void  CKinectWin::paintShadow(void)
{
	int	   label = 0;
	uByte* ptr;

	if (pViewData==NULL) return;

	if (getDevState()==NI_STATE_DETECT_EXEC && hasDepthData) {
		//
		for (int j=0; j<pViewData->ysize; j++) {
			int li;
			int lj = j*m_image_scale;

			for (int i=0; i<pViewData->xsize; i++) {
				// Mirroring
				if (m_is_mirroring) li = i*m_image_scale;
				else                li = (pViewData->xsize-i-1)*m_image_scale; 

				// User Label
				label = device->depth->get_user_index(li, lj);	// 640x480
				if (label>0) {
					ptr = &(pViewData->point(i, j));
					NiSetUserColor(label, ptr, m_use_image);
				}
			}
		}
	}
}




void  CKinectWin::drawSkeleton(int col, int line)
{
	drawJointConnection(NI_SDK_PELVIS,	NI_SDK_TORSO,	col, line);
	drawJointConnection(NI_SDK_TORSO,	NI_SDK_NECK,	col, line);
	drawJointConnection(NI_SDK_NECK,	NI_SDK_HEAD,	col, line);

	drawJointConnection(NI_SDK_NECK,	NI_SDK_L_SHLDR,	col, line);
	drawJointConnection(NI_SDK_L_SHLDR,	NI_SDK_L_ELBOW,	col, line);
	drawJointConnection(NI_SDK_L_ELBOW,	NI_SDK_L_WRIST,	col, line);
	drawJointConnection(NI_SDK_L_WRIST,	NI_SDK_L_HAND,	col, line);

	drawJointConnection(NI_SDK_NECK,	NI_SDK_R_SHLDR,	col, line);
	drawJointConnection(NI_SDK_R_SHLDR,	NI_SDK_R_ELBOW,	col, line);
	drawJointConnection(NI_SDK_R_ELBOW,	NI_SDK_R_WRIST,	col, line);
	drawJointConnection(NI_SDK_R_WRIST,	NI_SDK_R_HAND,	col, line);

	drawJointConnection(NI_SDK_PELVIS,	NI_SDK_L_HIP,	col, line);
	drawJointConnection(NI_SDK_L_HIP,	NI_SDK_L_KNEE,	col, line);
	drawJointConnection(NI_SDK_L_KNEE,	NI_SDK_L_ANKLE,	col, line);
	drawJointConnection(NI_SDK_L_ANKLE,	NI_SDK_L_FOOT,	col, line);

	drawJointConnection(NI_SDK_PELVIS,	NI_SDK_R_HIP,	col, line);
	drawJointConnection(NI_SDK_R_HIP,	NI_SDK_R_KNEE,	col, line);
	drawJointConnection(NI_SDK_R_KNEE,	NI_SDK_R_ANKLE,	col, line);
	drawJointConnection(NI_SDK_R_ANKLE,	NI_SDK_R_FOOT,	col, line);
}




void  CKinectWin::drawJointConnection(int j1, int j2, int col, int line)
{
	if (pViewData==NULL) return;

	MSGraph<unsigned int> vp;
	vp.xs = pViewData->xsize;
	vp.ys = pViewData->ysize;
	vp.zs = 1;
	vp.gp = (unsigned int*)pViewData->grptr;

	if (crdVect[j1].c>0.0 && crdVect[j2].c>0.0) {
		MSGraph_Line(vp, crdVect[j1].x, crdVect[j1].y, crdVect[j2].x, crdVect[j2].y, col);
		for (int i=1; i<line; i++) {
			MSGraph_Line(vp, crdVect[j1].x+i, crdVect[j1].y, crdVect[j2].x+i, crdVect[j2].y, col);
			MSGraph_Line(vp, crdVect[j1].x-i, crdVect[j1].y, crdVect[j2].x-i, crdVect[j2].y, col);
			MSGraph_Line(vp, crdVect[j1].x, crdVect[j1].y+i, crdVect[j2].x, crdVect[j2].y+i, col);
			MSGraph_Line(vp, crdVect[j1].x, crdVect[j1].y-i, crdVect[j2].x, crdVect[j2].y-i, col);
		}
	}
}





void  CKinectWin::set2DCoordinate(void)
{
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		crdVect[j].init(-1.0);
	}

	//
	if (device->depth!=NULL && pViewData!=NULL) {
		//
		for (int j=0; j<KINECT_JOINT_NUM; j++) {
			Vector4 pos;

			if (j==NI_SDK_PELVIS || j==NI_SDK_TORSO) {
				if (m_profile==KINECT_SKEL_PROFILE_UPPER) {
					Vector4 pos_neck = device->jointPosData[NI_SDK_NECK];
					Vector4 pos_rshd = device->jointPosData[NI_SDK_R_SHLDR];
					Vector4 pos_lshd = device->jointPosData[NI_SDK_L_SHLDR];

					pos.x = (FLOAT)((pos_rshd.x + pos_lshd.x)*1.5 - pos_neck.x*2.0);	// NECK + (CSHLDR-NECK)*3	
					pos.y = (FLOAT)((pos_rshd.y + pos_lshd.y)*1.5 - pos_neck.y*2.0);	
					pos.z = (FLOAT)((pos_rshd.z + pos_lshd.z)*1.5 - pos_neck.z*2.0);	
					//pos.w = (FLOAT)((pos_rshd.w + pos_lshd.w)*1.5 - pos_neck.w*2.0);
				}
				else if (j==NI_SDK_TORSO) {
					Vector4 pos_pelv = device->jointPosData[NI_SDK_PELVIS];
					Vector4 pos_rhip = device->jointPosData[NI_SDK_R_HIP];
					Vector4 pos_lhip = device->jointPosData[NI_SDK_L_HIP];
					
					pos.x = (FLOAT)(pos_pelv.x*2.0 -(pos_rhip.x+pos_lhip.x)*0.5);
					pos.y = (FLOAT)(pos_pelv.y*2.0 -(pos_rhip.y+pos_lhip.y)*0.5);
					pos.z = (FLOAT)(pos_pelv.z*2.0 -(pos_rhip.z+pos_lhip.z)*0.5);
					//pos.w = (FLOAT)(pos_pelv.w*2.0 -(pos_rhip.w+pos_lhip.w)*0.5);
				}
				else {
					pos = device->jointPosData[j];	// j==NI_SDK_PELVIS
				}
			}
			else {
				pos = device->jointPosData[j];
			}

			float x, y;
			NuiTransformSkeletonToDepthImage(pos, &x, &y, NUI_IMAGE_RESOLUTION_640x480);

			int xs = (int)x;
			int ys = (int)y;
			if (xs>0 && ys>0) {	// 縁(0)は除外
				device->depth->get_image_coord(&xs, &ys);
				if (xs>=0) xs = xs/m_image_scale;
				if (ys>=0) ys = ys/m_image_scale;

				crdVect[j].x = xs;
				crdVect[j].y = ys;
				if (xs>0 && xs<pViewData->xsize && ys>0 && ys<pViewData->ysize) {
					crdVect[j].c = 1.0;
				}
			}
		}
	}

	return;
}




/////////////////////////////////////////////////////////////////////////////////////
//
// Speech Platform
//

BOOL  CKinectWin::initSpeech(void)
{
	m_err_mesg = _T("");

	if (audio==NULL) audio = new CKinectAudio();

	BOOL ret = audio->init(device->context, KINECT_AUDIO_SINGLE_AEC);
	if (!ret) m_err_mesg = _T("CKinectWin::initSpeech(): ERROR: Audio Initialization Error!!");
	
	//
	if (ret) {
		if (speech==NULL) speech = makeSpeech();
		WAVEFORMATEX format = audio->getAudioFormat();
		IStream*     stream = audio->getIStream();
		ret = speech->init(stream, &format);
		if (!ret) m_err_mesg = _T("CKinectWin::initSpeech(): ERROR: Speech Initialization Error!!");
	}

	//
	if (!ret) {
		if (speech!=NULL) {
			speech->free();
			deleteNull(speech);
		}
		deleteNull(audio);
		return FALSE;
	}
	
	return TRUE;
}




BOOL  CKinectWin::createSpeech(LPCTSTR lang, LPCTSTR grfile)
{
	m_err_mesg = _T("");

	BOOL ret = TRUE;
	if (speech==NULL) ret = initSpeech(); 

	if (ret) {
		ret = speech->create(lang);
		if (!ret) m_err_mesg = _T("CKinectWin::createSpeech(): ERROR: Speech Context Creation Error!!\nPerhaps, Language Pack is not installed.");
	}
	//
	if (ret) {
		ret = speech->load(grfile);
		if (!ret) {
			m_err_mesg  = _T("CKinectWin::createSpeech(): ERROR: Grammar File Loading Error!!\nFile = ");
			m_err_mesg += grfile;
		}
	}
	
	//
	if (!ret) return FALSE;
	return TRUE;
}




BOOL  CKinectWin::startSpeech(float confidence)
{
	m_err_mesg = _T("");

	if (audio==NULL || speech==NULL) {
		m_err_mesg = _T("CKinectWin::startSpeech(): ERROR: Audio or Speech Instance is/are NULL!!");
		return FALSE;
	}

	BOOL ret = audio->startCapture();
	if (!ret) m_err_mesg = _T("CKinectWin::startSpeech(): ERROR: Audio Capture Starting Error!!");
	//
	if (ret) {
		ret = speech->start(confidence);
		if (!ret) {
			audio->stopCapture();
			m_err_mesg = _T("CKinectWin::startSpeech(): ERROR: Speech Platform Starting Error!!");
		}
	}
	//
	if (!ret) {
		if (m_err_mesg==_T("")) m_err_mesg = _T("CKinectWin::startSpeech(): ERROR: Unknown Error!!");
		return FALSE;
	}
	
	return TRUE;
}




void  CKinectWin::stopSpeech(void)
{
	if (speech!=NULL) {
		speech->stop();
	}
	//
	if (audio!=NULL) {
		audio->stopCapture();
	}

	return;
}




void  CKinectWin::deleteSpeech(BOOL rls)
{
	DEBUG_INFO("CKinectWin::deleteSpeech(): START");

	if (speech!=NULL) {
		stopSpeech();
		//
		////////////////////////////////////////////////////////////////
		if (rls) speech->free();
		delete speech;
		speech = NULL;
	}

	//
	deleteNull(audio);

	DEBUG_INFO("CKinectWin::deleteSpeech(): END");
	return;
}






/////////////////////////////////////////////////////////////////////////////////////
//
// Face
//

Quaternion   CKinectWin::getFaceRotation(void)
{
	Quaternion quat(1.0, 0.0, 0.0, 0.0, 1.0);
	Vector<double> vect;
	Vector<double> eulr = device->face->getFaceEulerXYZ();

	if (m_is_mirroring) {
		vect.set(-eulr.x, -eulr.y, eulr.z);
	}
	else {
		vect.set(-eulr.x, eulr.y, -eulr.z);
	}

	quat.setEulerYZX(vect);

	return quat;
}







/////////////////////////////////////////////////////////////////////////////////////
//
// CallBack 
//

void  CKinectWin::callback_status_changed(BOOL success, const OLECHAR* instanceName, const OLECHAR* deviceName)
{
	if (success) {
		DEBUG_INFO("CKinectWin:::called_change_status(): SUCCEEDED: %s %s", (char*)instanceName, (char*)deviceName);
		// Initialize the Kinect sensor identified by the instanceName parameter.
	}      
	else {
		DEBUG_INFO("CKinectWin::called_change_status(): ERROR: %s %s", (char*)instanceName, (char*)deviceName);
		// Uninitialize the Kinect sensor identified by the instanceName parameter.
    }
}





//////////////////////////////////////////////////////////////////////////////////////////
//
// CallBack 
//

void  CALLBACK  jbxwl::KinectStatusProc(HRESULT hr, const OLECHAR* instanceName, const OLECHAR* deviceName, void* pUserData)
{
	CKinectWin* kinect = (CKinectWin*)pUserData;

	if (SUCCEEDED(hr)) {
		if (kinect!=NULL) kinect->callback_status_changed(TRUE, instanceName, deviceName);
	}      
	else {
		if (kinect!=NULL) kinect->callback_status_changed(FALSE, instanceName, deviceName);
    }
}
 




#endif		// ENABLE_KINECT_SDK





