#include "StdAfx.h"


#ifndef DISABLE_OPENNI


#ifdef  WIN64
#pragma comment(lib, "XnVNITE64_1_5_2.lib")
#pragma comment(lib, "OpenNI64.lib")
#else
#pragma comment(lib, "XnVNITE_1_5_2.lib")
#pragma comment(lib, "OpenNI.lib")
#endif





#include  "ExOpenNiWin.h"
#include  "ExClass.h"






/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CExOpenNiWin クラス
//

CExOpenNiWin::CExOpenNiWin()
{
	dev_backup		= NULL;
	niJoints		= NULL;
	niNetwork		= NULL;
	sharedMem		= NULL;

	pLogFrame		= NULL;
	pSensorFrame	= NULL;
	pSkeletonFrame	= NULL;
	pLogDoc			= NULL;

	vect_up.set  (0.0, 0.0,  1.0, 1.0);
	vect_down.set(0.0, 0.0, -1.0, 1.0);

	appParam.init();
	saveTempFilePath = MakeWorkingFolderPath(OPENNI_RECORDE_TEMP_FILE, "\\NSL\\SLKinect\\");
}




CExOpenNiWin::~CExOpenNiWin()
{
	if (dev_backup!=NULL) delete(dev_backup);

	free_Buffer(&saveTempFilePath);
}




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

void  CExOpenNiWin::setLogFramePtr(CExFrame* pfrm)
{
	pLogFrame = pfrm;
	
	if (pfrm!=NULL) pLogDoc = (CLogWndDoc*)(((CLogWndFrame*)pLogFrame)->pDoc);
	else            pLogDoc = NULL;
}



void  CExOpenNiWin::setMotion(CParameterSet param) 
{ 
	m_profile		= param.detectParts;
	m_confidence	= param.confidence;
//	m_mvav_smooth	= param.smoothMVAV;
	m_nite_smooth	= param.smoothNITE;
	m_use_mvav_smth = param.useMvavSmooth;
	m_use_nite_smth = param.useNiteSmooth;
}





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

BOOL  CExOpenNiWin::makeFileDevice(char* fname)
{
	BOOL ret = FALSE;

	if (dev_backup!=NULL) {
		ret = remakeFileDevice(fname);
	}
	else {
		dev_backup = device;
		device = new COpenNiDevice();
		ret = device->init(fname, TRUE);
		if (!ret) restoreDevice();
	}

	return ret;
}



BOOL  CExOpenNiWin::remakeFileDevice(char* fname)
{
	device->delete_Depth();
	device->delete_Image();
	device->delete_Player();

	BOOL ret = device->init(fname, TRUE);
	return ret;
}



BOOL  CExOpenNiWin::restoreDevice(void)
{
	if (dev_backup!=NULL) {
		delete(device);
		device = dev_backup;
		dev_backup = NULL;
	}

	return TRUE;
}






///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Virtual Function
//

void  CExOpenNiWin::logingJointsData(void)
{
	if (!isNull(pLogDoc)) {
		pLogDoc->lock();
		for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
			XnVector3D  pos = jointPositionData(j);
			XnMatrix3X3 rot = jointRotationData(j);
			
			if (appParam.printPosMode || appParam.printRotMode || appParam.printQutMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: %s (%2d)\n", NiSDK2JointName(j, NiSDK_OpenNI).c_str(), j);
			}
			
			if (appParam.printPosMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: POS1: %f, %f, %f\n", pos.X, pos.Y, pos.Z);
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: POS2: %f, %f, %f\n", posVect[j].x, posVect[j].y, posVect[j].z);
			}
			if (appParam.printRotMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: ROT1: %f, %f, %f\n", rot.elements[0], rot.elements[1], rot.elements[2]);
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: ROT2: %f, %f, %f\n", rot.elements[3], rot.elements[4], rot.elements[5]);
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: ROT3: %f, %f, %f\n", rot.elements[6], rot.elements[7], rot.elements[8]);
			}
			if (appParam.printQutMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: QUAT: %f, %f, %f, %f\n", rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
			}
		}
		if (!isNull(pLogDoc)) pLogDoc->unlock();
	}
}




void  CExOpenNiWin::convertData(void)
{
	if (appParam.usePosData) convertPos2SLData();
	else                     convertRot2SLData();

	exportSLData();
}




void  CExOpenNiWin::convertPos2SLData(void)
{
	//
	for (int j=0; j<OPENNI_MAX_JOINT_NUM; j++) {
		if (posVect[j].c<m_confidence) {
			Vector<double>* ptr = (Vector<double>*)posRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) posVect[j] = *ptr;
			else posVect[j].init(-1.0);
		}
	}

	//
	if (appParam.useMvavSmooth) niJoints->PosMovingAverage(NiSDK_OpenNI);
	if (appParam.useJointConst) niJoints->PosVibNoiseCanceler();
	
	// NI_PELVIS
	if (posVect[NI_R_HIP].c>=m_confidence && posVect[NI_L_HIP].c>=m_confidence) { 
		Vector<double> vect = posVect[NI_R_HIP] - posVect[NI_L_HIP];
		double thz = atan2(-vect.x, vect.y);
		rotQuat[NI_PELVIS].setRotation(thz, 0.0, 0.0, 1.0);
		rotQuat[NI_PELVIS].c = vect.c;
	}
	else {
		rotQuat[NI_PELVIS].init();
	}


	//
	Vector<double> shldr_left  = posVect[NI_L_SHLDR] - posVect[NI_R_SHLDR];
	Vector<double> shldr_right = - shldr_left;

	// To Relative Coordinate
	rotQuat[NI_TORSO]   = VPPQuaternion(vect_up,     posVect[NI_TORSO],   posVect[NI_NECK]);
	rotQuat[NI_L_SHLDR] = VPPQuaternion(shldr_left,  posVect[NI_L_SHLDR], posVect[NI_L_ELBOW]);
	rotQuat[NI_R_SHLDR] = VPPQuaternion(shldr_right, posVect[NI_R_SHLDR], posVect[NI_R_ELBOW]);
	rotQuat[NI_L_HIP]	= VPPQuaternion(vect_down,	 posVect[NI_L_HIP],   posVect[NI_L_KNEE]);
	rotQuat[NI_R_HIP]	= VPPQuaternion(vect_down,   posVect[NI_R_HIP],   posVect[NI_R_KNEE]);

	rotQuat[NI_NECK]	= PPPQuaternion(posVect[NI_TORSO],	 posVect[NI_NECK],    posVect[NI_HEAD]);
	rotQuat[NI_L_ELBOW] = PPPQuaternion(posVect[NI_L_SHLDR], posVect[NI_L_ELBOW], posVect[NI_L_HAND]);
	rotQuat[NI_R_ELBOW] = PPPQuaternion(posVect[NI_R_SHLDR], posVect[NI_R_ELBOW], posVect[NI_R_HAND]);
	rotQuat[NI_L_KNEE]	= PPPQuaternion(posVect[NI_L_HIP],	 posVect[NI_L_KNEE],  posVect[NI_L_FOOT]);
	rotQuat[NI_R_KNEE]	= PPPQuaternion(posVect[NI_R_HIP],	 posVect[NI_R_KNEE],  posVect[NI_R_FOOT]);

	// To Avatar Coordinate
	rotQuat[NI_L_ELBOW] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_ELBOW]*rotQuat[NI_PELVIS];
	rotQuat[NI_R_ELBOW] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_ELBOW]*rotQuat[NI_PELVIS];
	rotQuat[NI_L_SHLDR] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_SHLDR]*rotQuat[NI_PELVIS];
	rotQuat[NI_R_SHLDR] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_SHLDR]*rotQuat[NI_PELVIS];
	rotQuat[NI_L_KNEE]	= ~rotQuat[NI_PELVIS]*rotQuat[NI_L_KNEE] *rotQuat[NI_PELVIS];
	rotQuat[NI_R_KNEE]	= ~rotQuat[NI_PELVIS]*rotQuat[NI_R_KNEE] *rotQuat[NI_PELVIS];
	rotQuat[NI_L_HIP]	= ~rotQuat[NI_PELVIS]*rotQuat[NI_L_HIP]	 *rotQuat[NI_PELVIS];
	rotQuat[NI_R_HIP]	= ~rotQuat[NI_PELVIS]*rotQuat[NI_R_HIP]	 *rotQuat[NI_PELVIS];
	rotQuat[NI_NECK]	= ~rotQuat[NI_PELVIS]*rotQuat[NI_NECK]	 *rotQuat[NI_PELVIS];
	rotQuat[NI_TORSO]   = ~rotQuat[NI_PELVIS]*rotQuat[NI_TORSO]	 *rotQuat[NI_PELVIS];

	
	//
	for (int j=0; j<OPENNI_MAX_JOINT_NUM; j++) {
		if (rotQuat[j].c<m_confidence) {
			Quaternion* ptr = (Quaternion*)rotRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) rotQuat[j] = *ptr;
			else rotQuat[j].init();
		}
	}

	//
	if (appParam.useJointConst) niJoints->CheckBoneRotation();
}




void  CExOpenNiWin::convertRot2SLData(void)
{
	// NI_PELVIS
	if (posVect[NI_R_HIP].c<m_confidence) {
		Vector<double>* ptr = (Vector<double>*)posRing[NI_R_HIP].get(-1);
		if (ptr!=NULL && ptr->c>0.0) posVect[NI_R_HIP] = *ptr;
		else posVect[NI_R_HIP].init(-1.0);
	}
	if (posVect[NI_L_HIP].c<m_confidence) {
		Vector<double>* ptr = (Vector<double>*)posRing[NI_L_HIP].get(-1);
		if (ptr!=NULL && ptr->c>0.0) posVect[NI_L_HIP] = *ptr;
		else posVect[NI_L_HIP].init(-1.0);
	}

	if (posVect[NI_R_HIP].c>=m_confidence && posVect[NI_L_HIP].c>=m_confidence) { 
		Vector<double> vect = posVect[NI_R_HIP] - posVect[NI_L_HIP];
		double thz = atan2(-vect.x, vect.y);
		rotQuat[NI_PELVIS].setRotation(thz, 0.0, 0.0, 1.0);
		rotQuat[NI_PELVIS].c = vect.c;
	}
	else {
		rotQuat[NI_PELVIS].init();
	}

	
	//
	rotQuat[NI_R_KNEE]  = ~rotQuat[NI_R_HIP]  *rotQuat[NI_R_KNEE];
	rotQuat[NI_L_KNEE]  = ~rotQuat[NI_L_HIP]  *rotQuat[NI_L_KNEE];
	rotQuat[NI_R_ELBOW] = ~rotQuat[NI_R_SHLDR]*rotQuat[NI_R_ELBOW];
	rotQuat[NI_L_ELBOW] = ~rotQuat[NI_L_SHLDR]*rotQuat[NI_L_ELBOW];

	rotQuat[NI_R_SHLDR] = ~rotQuat[NI_NECK]   *rotQuat[NI_R_SHLDR];
	rotQuat[NI_L_SHLDR] = ~rotQuat[NI_NECK]   *rotQuat[NI_L_SHLDR];
	rotQuat[NI_R_HIP]   = ~rotQuat[NI_PELVIS] *rotQuat[NI_R_HIP];
	rotQuat[NI_L_HIP]   = ~rotQuat[NI_PELVIS] *rotQuat[NI_L_HIP];
	rotQuat[NI_NECK]    = ~rotQuat[NI_TORSO]  *rotQuat[NI_NECK];
	rotQuat[NI_TORSO]   = ~rotQuat[NI_PELVIS] *rotQuat[NI_TORSO];


	// Confidence
	for (int j=0; j<OPENNI_MAX_JOINT_NUM; j++) {
		if (rotQuat[j].c<m_confidence) {
			Quaternion* ptr = (Quaternion*)rotRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) rotQuat[j] = *ptr;
			else rotQuat[j].init();
		}
	}

	//	
	if (appParam.useMvavSmooth) niJoints->RotMovingAverage(NiSDK_OpenNI);
	if (appParam.useJointConst) {
		niJoints->RotVibNoiseCanceler();
		niJoints->CheckBoneRotation();
	}
}





void  CExOpenNiWin::exportSLData(void)
{
	if (niNetwork->sendSocket>0) {
		if (niNetwork->appParam.netOutMode==NETandLOCAL) {
			sharedMem->updateLocalAnimationData(posVect, rotQuat, NiSDK_OpenNI, OPENNI_MAX_JOINT_NUM);
		}

		if (niNetwork->appParam.netFastMode) {
			sendAnimationData<float> (posVect, rotQuat, niNetwork, NiSDK_OpenNI, OPENNI_MAX_JOINT_NUM);
		}
		else {
			sendAnimationData<double>(posVect, rotQuat, niNetwork, NiSDK_OpenNI, OPENNI_MAX_JOINT_NUM);
		}
	}

	else {
		sharedMem->updateLocalAnimationData(posVect, rotQuat, NiSDK_OpenNI, OPENNI_MAX_JOINT_NUM);
	}
}





//////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// for TEST
//

bool  CExOpenNiWin::setStartBoneLength()
{
	bool ret = false;

	memset(stbnLen, 0, sizeof(double)*OPENNI_MAX_JOINT_NUM);

	if (stbnLen[ 1]==0.0 && posVect[ 1].n!=0.0 && posVect[ 2].n!=0.0) {
		stbnLen[ 1] = VectorDist(posVect[ 1], posVect[ 2]);
	}
	if (stbnLen[ 2]==0.0 && posVect[ 2].n!=0.0 && posVect[ 3].n!=0.0) {
		stbnLen[ 2] = VectorDist(posVect[ 2], posVect[ 3]);
	}
	if (stbnLen[ 3]==0.0 && posVect[ 3].n!=0.0 && posVect[ 0].n!=0.0) {
		stbnLen[ 3] = VectorDist(posVect[ 3], posVect[ 0]);
	}

	if (stbnLen[ 6]==0.0 && posVect[ 6].n!=0.0 && posVect[ 2].n!=0.0) {
		stbnLen[ 6] = VectorDist(posVect[ 6], posVect[ 2]);
	}
	if (stbnLen[ 7]==0.0 && posVect[ 7].n!=0.0 && posVect[ 6].n!=0.0) {
		stbnLen[ 7] = VectorDist(posVect[ 7], posVect[ 6]);
	}
	if (stbnLen[ 9]==0.0 && posVect[ 9].n!=0.0 && posVect[ 7].n!=0.0) {
		stbnLen[ 9] = VectorDist(posVect[ 9], posVect[ 7]);
	}

	if (stbnLen[12]==0.0 && posVect[12].n!=0.0 && posVect[ 2].n!=0.0) {
		stbnLen[12] = VectorDist(posVect[12], posVect[ 2]);
	}
	if (stbnLen[13]==0.0 && posVect[13].n!=0.0 && posVect[12].n!=0.0) {
		stbnLen[13] = VectorDist(posVect[13], posVect[12]);
	}
	if (stbnLen[15]==0.0 && posVect[15].n!=0.0 && posVect[13].n!=0.0) {
		stbnLen[15] = VectorDist(posVect[15], posVect[13]);
	}

	if (stbnLen[17]==0.0 && posVect[17].n!=0.0 && posVect[ 0].n!=0.0) {
		stbnLen[17] = VectorDist(posVect[17], posVect[ 0]);
	}
	if (stbnLen[18]==0.0 && posVect[18].n!=0.0 && posVect[17].n!=0.0) {
		stbnLen[18] = VectorDist(posVect[18], posVect[17]);
	}
	if (stbnLen[20]==0.0 && posVect[20].n!=0.0 && posVect[18].n!=0.0) {
		stbnLen[20] = VectorDist(posVect[20], posVect[18]);
	}

	if (stbnLen[21]==0.0 && posVect[21].n!=0.0 && posVect[ 0].n!=0.0) {
		stbnLen[21] = VectorDist(posVect[21], posVect[ 0]);
	}
	if (stbnLen[22]==0.0 && posVect[22].n!=0.0 && posVect[21].n!=0.0) {
		stbnLen[22] = VectorDist(posVect[22], posVect[21]);
	}
	if (stbnLen[24]==0.0 && posVect[24].n!=0.0 && posVect[22].n!=0.0) {
		stbnLen[24] = VectorDist(posVect[24], posVect[22]);
	}

	return ret;
}








////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// スレッド
//

UINT  openniEventLoop(LPVOID pParam)
{
	if (pParam==NULL) return 1;

	CExOpenNiWin* openni = (CExOpenNiWin*)pParam;
	if (openni->device->context==NULL) return 1; 
	if (openni->pSensorFrame==NULL)    return 1; 
	
	CExView* pview = openni->pSensorFrame->pView;
	openni->pViewData = &pview->viewData;
	
	//
	try {
		Loop {
			//
			if (openni->getDevState()==NI_STATE_DETECT_STOPPING || openni->getDevState()==NI_STATE_SAVE_WORKING) continue;
			//openni->context->WaitAndUpdateAll();		
			if (openni->device->image!=NULL && openni->device->imageMD!=NULL && openni->m_use_image) {
				openni->device->context->WaitOneUpdateAll(*(openni->device->image));
				openni->device->image->GetMetaData(*(openni->device->imageMD));
				//if (openni->imageData!=NULL) {
				//	memcpy(openni->imageData, openni->imageMD->RGB24Data(), openni->outputMode.nXRes*openni->outputMode.nYRes*3);
				//}
			}

			if (openni->getDevState()==NI_STATE_DETECT_STOPPING || openni->getDevState()==NI_STATE_SAVE_WORKING) continue;
			openni->device->context->WaitOneUpdateAll(*(openni->device->depth));
			openni->hasDepthData = FALSE;
			if (openni->getDevState()==NI_STATE_DETECT_EXEC && openni->device->user!=NULL) {
				openni->device->context->WaitOneUpdateAll(*(openni->device->user));
				openni->device->user->GetUserPixels(0, *openni->device->sceneMD);
				openni->hasDepthData = TRUE;
				//if (openni->device->depthData!=NULL) {
				//	memcpy(openni->depthData, openni->device->sceneMD->Data(), openni->device->outputMode.nXRes*openni->device->outputMode.nYRes*sizeof(XnLabel));
				//}
			}

			if (openni->getDevState()==NI_STATE_DETECT_STOPPING || openni->getDevState()==NI_STATE_SAVE_WORKING) continue;
			if (isNull(openni->pSensorFrame)) break;
			openni->makeDisplayImage();			// need Depth Data when detected users are painted

			if (openni->getDevState()==NI_STATE_DETECT_EXEC) {
				openni->trackingJoints(openni->appParam.useRotData);	// need Depth Data
			}

			//
			if (isNull(openni->pSensorFrame)) break;
			if (isNull(openni->pSensorFrame->pView)) break;
			if (!pview->SetNewSurface())    break;

			//
			if (isNull(openni->pSensorFrame)) break;
			pview->ExecRender();
		}
	}

	catch (std::exception& ex) {
		copy_s2Buffer("openniEventLoop() Exception: ", &openni->m_err_mesg);
		cat_s2Buffer (ex.what(), &openni->m_err_mesg);
		return 2;
	}

	return 0;
}


#endif