#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif


#include  "..\stdafx.h"

#include  "OpenNiWin.h"
#include  "NiJointsTool.h"

#include  "Graph.h"
#include  "WinTools.h"



using namespace jbxl;


#ifndef  DISABLE_OPENNI


using namespace jbxwl;




COpenNiWin::COpenNiWin(void)
{
	m_err_mesg		= _T("");

	m_scale			= 2;
	m_is_tracking	= FALSE;
	m_is_detected	= FALSE;
	m_is_mirroring	= TRUE;
	m_use_image		= TRUE;
	m_use_led		= FALSE;
	m_use_motor		= FALSE;
	m_enable_motor	= TRUE;

	m_use_nite_smth = TRUE;
	m_nite_smooth	= 0.0;

	m_confidence	= 0.75;
	m_profile		= XN_SKEL_PROFILE_ALL;
	m_ground_level	= NI_DEFAULT_GROUND_LEVEL;

	pViewData		= NULL;
	hasDepthData	= FALSE;

	clearAvatarDetected();
}



BOOL  COpenNiWin::init(void)
{
	BOOL ret = COpenNiTool::init(m_use_image);

	return ret;
}




void  COpenNiWin::free(void)
{
	freeRingBuffer();
	COpenNiTool::free();

	return;
}




CString	 COpenNiWin::get_err_message(void)
{
	m_err_mesg = mbs2ts((char*)COpenNiTool::m_err_mesg.buf);

	return m_err_mesg;
}




void  COpenNiWin::clearJointsData(void)
{
	for (int i=0; i<OPENNI_MAX_JOINT_NUM; i++) {
		posVect[i].init(-1.0);
		rotQuat[i].init(-1.0);
	}

}




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

	for (int j=0; j<OPENNI_MAX_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("COpenNiWin::initRingBuffer WARNING: Out of Memory.");
			ret = FALSE;
			break;
		}
	}

	return ret;
}




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




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




void  COpenNiWin::backup2RingBuffer(void)
{
	for (int j=0; j<OPENNI_MAX_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  COpenNiWin::clearAvatarDetected(void)
{
	clearJointsData();
	clearRingBuffer();

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

	return;
}


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

			// m_ground_level is not used now
			if (posVect[NI_JNT_R_ANKLE].c>=m_confidence && crdVect[NI_JNT_R_ANKLE].c>0.0) {
				m_ground_level = posVect[NI_JNT_R_ANKLE].z - startPos.z;
				if (posVect[NI_JNT_L_ANKLE].c>=m_confidence && crdVect[NI_JNT_L_ANKLE].c>0.0) {
					m_ground_level = Min(m_ground_level, posVect[NI_JNT_L_ANKLE].z - startPos.z);
				}
			}
			else if (posVect[NI_JNT_L_ANKLE].c>=m_confidence && crdVect[NI_JNT_L_ANKLE].c>0.0) {
				m_ground_level = posVect[NI_JNT_L_ANKLE].z - startPos.z;
			}
		}
	}
	//
	else if (m_profile==XN_SKEL_PROFILE_UPPER) {
		double confidence = Min(posVect[NI_JNT_R_SHLDR].c, posVect[NI_JNT_L_SHLDR].c);
		if (confidence>=m_confidence) m_is_detected = TRUE;
	}

	return m_is_detected;
}



void  COpenNiWin::setMirroring(BOOL mirror)
{
	if (m_is_mirroring!=mirror) {
		if (device!=NULL && device->context!=NULL) {
			device->context->SetGlobalMirror(mirror);
		}
	}
	m_is_mirroring = mirror;
}



//
//
//
void  COpenNiWin::setTiltMotor(int ang) 
{ 
	if (m_use_motor && m_enable_motor) {
		m_enable_motor = FALSE;
		set_Tilt_Motor(ang);	// Sleep(1000)
		m_enable_motor = TRUE;
	}
}	



//
//
//
BOOL  COpenNiWin::startDetection(BOOL force_pose)
{
	clearJointsData();

	float smooth = 0.0;
	if (m_use_nite_smth) smooth = m_nite_smooth;

	BOOL ret = start_Detection(force_pose, m_profile, smooth);
	if (ret) setLEDColor(NI_LED_BLINK_ORANGE);
	return ret;
}



BOOL  COpenNiWin::stopDetection(void)
{
	BOOL ret = stop_Detection();
	setLEDColor(NI_LED_GREEN);
	return ret;
}




//
//
//
void  COpenNiWin::makeDisplayImage()
{
	int      index;
	XnLabel	 label = 0;
	XnLabel* lbl = NULL;
	uByte*	 src = NULL;
	uByte*   ptr;

	if (pViewData==NULL) return;

	if (device->imageMD!=NULL && m_use_image) {
		//if (imageData!=NULL) src = imageData;
		//else                 src = (uByte*)imageMD->RGB24Data();
		src = (uByte*)device->imageMD->RGB24Data();
	}
	if (getDevState()==NI_STATE_DETECT_EXEC && hasDepthData) {
		//if (depthData!=NULL) lbl = depthData;
		//else                 lbl = (XnLabel*)sceneMD->Data();
		lbl = (XnLabel*)device->sceneMD->Data();
	}

	if (src!=NULL) {
		for(int j=0; j<pViewData->ysize; j++) {
			int lj = j*m_scale;
			int ls = lj*device->outputMode.nXRes;

			for(int i=0; i<pViewData->xsize; i++) {
				int li = i*m_scale;
				ptr    = &(pViewData->point(i, j));
				index  = (ls + li)*3;
				ptr[0] = src[index+2];		// B
				ptr[1] = src[index+1];		// G
				ptr[2] = src[index];		// R
				ptr[3] = 0;					// A

				if (lbl!=NULL) {
					label = lbl[ls+li];
					if (label>0) NiSetUserColor(label, ptr);
				}	
			}
		}
	}

	// No Camera Image
	else {
		for(int j=0; j<pViewData->ysize; j++) {
			int lj = j*m_scale;
			int ls = lj*device->outputMode.nXRes;

			for(int i=0; i<pViewData->xsize; i++) {
				// Mirror
				int li = i*m_scale;
				ptr = &(pViewData->point(i, j));
				ptr[0] = ptr[1] = ptr[2] = 224;		// Gray Background
				ptr[3] = 0;

				if (lbl!=NULL) {
					label = lbl[ls + li];
					if (label>0) NiSetUserColor(label, ptr, FALSE);
				}
			}
		}
	}
}



//
// Tracking Main
//
BOOL  COpenNiWin::trackingJoints(BOOL use_rot_data)
{
	BOOL ret = FALSE;
	
	//if (device->user!=NULL && skeleton!=NULL && hasDepthData) {
	if (hasDepthData) {
		// Tracking User
		if (tracking_user>0) {
			if (!skeleton->IsTracking(dUsers[tracking_user-1])) {
				clearAvatarDetected();
				lostTrackingUser(tracking_user);	// virtual
				tracking_user = 0;
			}
		}
		if (tracking_user==0) {
			m_is_detected = FALSE;
			tracking_user = getTrackingUser();
			if (tracking_user>0 && tracking_user<=OPENNI_MAX_USERS_NUM) {
				clearJointsData();
				detectTrackingUser(tracking_user);	// virtual
			}
		}

		// Tracking
		if (tracking_user>0 && tracking_user<=OPENNI_MAX_USERS_NUM) {
			//
			getJointsPosData(tracking_user);
			if (use_rot_data) getJointsRotData(tracking_user);
			//
			if (m_is_detected) {
				convertJointsData();
				//
				backup2RingBuffer();
				saveJointsData();
				logingJointsData();
				//
				drawSkeleton(NiGetSkeletonColor(tracking_user));
			}
			ret = TRUE;
		}
	}
	else {
		m_is_detected = FALSE;
	}

	if (ret!=m_is_tracking) {
		m_is_tracking = ret;
		if (ret) setLEDColor(NI_LED_RED);
		else     setLEDColor(NI_LED_BLINK_ORANGE);
	}

	return ret;
}



//
// Calculate posVect, crdVect
//
void  COpenNiWin::getJointsPosData(int uid)
{
	XnUserID nId = dUsers[tracking_user-1];
	get_JointsPositionData(nId);

	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		XnVector3D pos = jointPosData[j];
		posVect[j].set(-pos.Z/1000., pos.X/1000., pos.Y/1000., 0.0, jointPosConfidence[j]);
	}

	if (m_profile==XN_SKEL_PROFILE_ALL) {
		posVect[NI_JNT_PELVIS] = (posVect[NI_JNT_R_HIP] + posVect[NI_JNT_L_HIP])*0.5;
		posVect[NI_JNT_PELVIS].c = Min(jointPosConfidence[NI_JNT_R_HIP], jointPosConfidence[NI_JNT_L_HIP]);
	}
	else if (m_profile==XN_SKEL_PROFILE_UPPER) {
		// Not Yet
	}


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

	//
	for (int j=0; j<OPENNI_MAX_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();
}



//
// Calculate rotQuat
//
void  COpenNiWin::getJointsRotData(int uid)
{
	XnUserID nId = dUsers[tracking_user-1];
	get_JointsRotationData(nId);

	//
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		XnMatrix3X3 rot = jointRotData[j];

		double m11 = rot.elements[0];
		double m12 = rot.elements[1];
		double m13 = rot.elements[2];
		double m21 = rot.elements[3];
		double m31 = rot.elements[6];
		double m32 = rot.elements[7];
		double m33 = rot.elements[8];

		Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
		Vector<double> vct(-eul.x, -eul.y, eul.z);	// Mirror: vct(-eul.x, eul.y, -eul.z);
		rotQuat[j].setEulerYZX(vct);
		rotQuat[j].c = jointRotConfidence[j];

		if (rotQuat[j].c<m_confidence || crdVect[j].c<0.0) rotQuat[j].c = -1.0;
	}
}




void  COpenNiWin::drawSkeleton(int col)
{
	drawJointConnection(NI_JNT_HEAD,	NI_JNT_NECK,	col);
	drawJointConnection(NI_JNT_NECK,	NI_JNT_TORSO,	col);
	drawJointConnection(NI_JNT_TORSO,	NI_JNT_PELVIS,  col);

	drawJointConnection(NI_JNT_NECK,	NI_JNT_L_SHLDR,	col);
	drawJointConnection(NI_JNT_NECK,	NI_JNT_R_SHLDR,	col);

	drawJointConnection(NI_JNT_L_SHLDR,	NI_JNT_L_ELBOW,	col);
	drawJointConnection(NI_JNT_R_SHLDR,	NI_JNT_R_ELBOW,	col);
	drawJointConnection(NI_JNT_L_ELBOW,	NI_JNT_L_HAND,	col);
	drawJointConnection(NI_JNT_R_ELBOW,	NI_JNT_R_HAND,	col);

	drawJointConnection(NI_JNT_PELVIS,	NI_JNT_L_HIP,	col);
	drawJointConnection(NI_JNT_PELVIS,	NI_JNT_R_HIP,	col);
	drawJointConnection(NI_JNT_L_HIP,	NI_JNT_L_KNEE,	col);
	drawJointConnection(NI_JNT_R_HIP,	NI_JNT_R_KNEE,	col);
	drawJointConnection(NI_JNT_L_KNEE,	NI_JNT_L_FOOT,	col);
	drawJointConnection(NI_JNT_R_KNEE,	NI_JNT_R_FOOT,	col);
}




void  COpenNiWin::drawJointConnection(int j1, int j2, int col)
{
	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);
	}
}



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

	//
	if (device->depth!=NULL && pViewData!=NULL) {
		//
		for (int j=0; j<OPENNI_MAX_JOINT_NUM; j++) {
			float cnfd;
			if (j==0) {
				cnfd = (float)Min(jointPosConfidence[NI_JNT_R_HIP], jointPosConfidence[NI_JNT_L_HIP]);
			}
			else {
				cnfd = (float)jointPosConfidence[j];
			}

			if (cnfd>=m_confidence) {
				XnVector3D pos;
				if (j==0) {
					pos.X = (XnFloat)((jointPosData[NI_JNT_R_HIP].X + jointPosData[NI_JNT_L_HIP].X)*0.5);
					pos.Y = (XnFloat)((jointPosData[NI_JNT_R_HIP].Y + jointPosData[NI_JNT_L_HIP].Y)*0.5);
					pos.Z = (XnFloat)((jointPosData[NI_JNT_R_HIP].Z + jointPosData[NI_JNT_L_HIP].Z)*0.5);
				}
				else {
					pos = jointPosData[j];
				}

				XnPoint3D pt[1] = {pos};
				device->depth->ConvertRealWorldToProjective(1, pt, pt);
				int xs = (int)(pt[0].X/m_scale);
				int ys = (int)(pt[0].Y/m_scale);

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

	return;
}





#endif		// ifndef DISABLE_OPENNI