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

#include  "Graph.h"
#include  "buffer.h"



using namespace jbxl;


#ifndef  DISABLE_KINECT_SDK


using namespace jbxwl;




CKinectWin::CKinectWin(void)
{
	device			= NULL;
	m_err_mesg		= _T("");

	m_scale			= 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_enable_motor	= TRUE;

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

	pViewData		= NULL;
	hasDepthData	= FALSE;

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

	clearAvatarDetected();
}




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

	DWORD mode = NUI_INITIALIZE_FLAG_USES_COLOR |
				 NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | 
				 NUI_INITIALIZE_FLAG_USES_SKELETON;

	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();
//	free_Buffer(&m_err_mesg);
	return;
}



void  CKinectWin::deleteDevice(void)
{
	if (device!=NULL) {
		delete(device);
		device = NULL;
	}
	return;
}




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




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

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

	return ret;
}




void  CKinectWin::freeRingBuffer(void)
{
	for (int j=0; j<KINECT_MAX_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_MAX_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_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  CKinectWin::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  CKinectWin::checkAvatarDetected(void)
{
	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_FOOT].c>m_confidence && crdVect[NI_JNT_L_FOOT].c>0.0) {
				m_ground_level = Min(m_ground_level, posVect[NI_JNT_L_ANKLE].z - startPos.z);
			}
		}
		else if (posVect[NI_JNT_L_FOOT].c>m_confidence && crdVect[NI_JNT_L_FOOT].c>0.0) {
			m_ground_level = posVect[NI_JNT_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;
}




void  CKinectWin::makeDisplayImage()
{
	int      index;
	int		 label = 0;
	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 lj = j*m_scale;
			int ls = lj*device->m_xsize;

			for (int i=0; i<pViewData->xsize; i++) {
				// Mirroring
				if (m_is_mirroring) li = i*m_scale;
				else                li = (pViewData->xsize-i-1)*m_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

				// 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) NiSetUserColor(label, ptr);
				}
			}
		}
	}

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

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

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

				// 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) NiSetUserColor(label, ptr, FALSE);
				}
			}
		}
	}
}




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

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

	BOOL ret = device->start_Detection();
	if (ret) setLEDColor(NI_LED_BLINK_ORANGE);
	else m_err_mesg = device->m_err_mesg;

	return ret;
}



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

	if (!ret) m_err_mesg = device->m_err_mesg;

	return ret;
}




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

	return vect;
}



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

	if (hasDepthData) {
		//		
		int tuser = device->tracking_user;
		//
		BOOL ret = device->wait_Skeleton();	
	
		if (tuser!=device->tracking_user) {
			m_is_detected = FALSE;
			if (device->tracking_user==0) {
				clearAvatarDetected();
				lostTrackingUser(tuser);					// virtual
			}
			else {
				clearJointsData();
				detectTrackingUser(device->tracking_user);	// virtual
			}
		}


		// Tracking
		if (ret && device->tracking_user>0 && device->tracking_user<=KINECT_MAX_USERS_NUM) {
			//
			getJointsPosData();
			//
			if (m_is_detected) {
				convertJointsData();
				//
				backup2RingBuffer();
				saveJointsData();
				logingJointsData();
				//
				drawSkeleton(NiGetSkeletonColor(device->tracking_user));
			}
			m_is_tracking = TRUE;
		}
	}
	else {
		m_is_detected = FALSE;
	}

	return m_is_tracking;
}



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

	//
	for (int j=0; j<KINECT_MAX_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);
	}
	posVect[NI_JNT_TORSO] = 2.0*posVect[NI_JNT_PELVIS] - (posVect[NI_JNT_R_HIP] + posVect[NI_JNT_L_HIP])/2.0;

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

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




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

	drawJointConnection(NI_JNT_NECK,	NI_JNT_L_SHLDR,	col);
	drawJointConnection(NI_JNT_L_SHLDR,	NI_JNT_L_ELBOW,	col);
	drawJointConnection(NI_JNT_L_ELBOW,	NI_JNT_L_WRIST,	col);
	drawJointConnection(NI_JNT_L_WRIST,	NI_JNT_L_HAND,	col);

	drawJointConnection(NI_JNT_NECK,	NI_JNT_R_SHLDR,	col);
	drawJointConnection(NI_JNT_R_SHLDR,	NI_JNT_R_ELBOW,	col);
	drawJointConnection(NI_JNT_R_ELBOW,	NI_JNT_R_WRIST,	col);
	drawJointConnection(NI_JNT_R_WRIST,	NI_JNT_R_HAND,	col);

	drawJointConnection(NI_JNT_PELVIS,	NI_JNT_L_HIP,	col);
	drawJointConnection(NI_JNT_L_HIP,	NI_JNT_L_KNEE,	col);
	drawJointConnection(NI_JNT_L_KNEE,	NI_JNT_L_ANKLE,	col);
	drawJointConnection(NI_JNT_L_ANKLE,	NI_JNT_L_FOOT,	col);

	drawJointConnection(NI_JNT_PELVIS,	NI_JNT_R_HIP,	col);
	drawJointConnection(NI_JNT_R_HIP,	NI_JNT_R_KNEE,	col);
	drawJointConnection(NI_JNT_R_KNEE,	NI_JNT_R_ANKLE,	col);
	drawJointConnection(NI_JNT_R_ANKLE,	NI_JNT_R_FOOT,	col);
}




void  CKinectWin::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  CKinectWin::set2DCoordinate(void)
{
	for (int j=0; j<KINECT_MAX_JOINT_NUM; j++) {
		crdVect[j].init(-1.0);
	}

	//
	if (device->depth!=NULL && pViewData!=NULL) {
		//
		for (int j=0; j<KINECT_MAX_JOINT_NUM; j++) {
			float x, y;
			NuiTransformSkeletonToDepthImage(device->jointPosData[j], &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_scale;
				if (ys>=0) ys = ys/m_scale;

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

	return;
}





#endif		// DISABLE_KINECT_SDK





