#include "StdAfx.h"


#ifndef DISABLE_KINECT_SDK


#pragma comment(lib, "Kinect10.lib")






#include  "ExKinectWin.h"




using namespace jbxwl;





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

CExKinectWin::CExKinectWin()
{
	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();
	clearBoneData();
}




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

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



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




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

void  CExKinectWin::logingJointsData(void)
{
	if (!isNull(pLogDoc)) {
		pLogDoc->lock();
		for (int j=0; j<KINECT_MAX_JOINT_NUM; j++) {
			Vector4 pos = jointPositionData(j);

			if (appParam.printPosMode || appParam.printQutMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: %s (%2d)\n", NiSDK2JointName(j, NiSDK_Kinect).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.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  CExKinectWin::convertData(void)
{
	convertPos2SLData();
	exportSLData();
}




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

	//
	if (appParam.useMvavSmooth) niJoints->PosMovingAverage(NiSDK_Kinect);
	if (appParam.useJointConst) niJoints->PosVibNoiseCanceler();

	//
	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> vect_left  = posVect[NI_L_SHLDR] - posVect[NI_R_SHLDR];
	Vector<double> vect_right = - vect_left;

	rotQuat[NI_TORSO] = VPPQuaternion(vect_up, posVect[NI_TORSO], posVect[NI_NECK]);
	rotQuat[NI_NECK]  = PPPQuaternion(posVect[NI_TORSO],  posVect[NI_NECK],  posVect[NI_HEAD]);

	//
	if (m_is_mirroring) {
		rotQuat[NI_L_SHLDR] = VPPQuaternion(vect_left,     posVect[NI_L_SHLDR], posVect[NI_L_ELBOW]);
		rotQuat[NI_R_SHLDR] = VPPQuaternion(vect_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_L_ANKLE] = VPPQuaternion(vect_ft_left,  posVect[NI_L_ANKLE], posVect[NI_L_FOOT]);
//		rotQuat[NI_R_ANKLE] = VPPQuaternion(vect_ft_right, posVect[NI_R_ANKLE], posVect[NI_R_FOOT]);
		//
		rotQuat[NI_L_ELBOW] = PPPQuaternion(posVect[NI_L_SHLDR], posVect[NI_L_ELBOW], posVect[NI_L_WRIST]);
		rotQuat[NI_L_WRIST] = PPPQuaternion(posVect[NI_L_ELBOW], posVect[NI_L_WRIST], posVect[NI_L_HAND]);
		rotQuat[NI_R_ELBOW] = PPPQuaternion(posVect[NI_R_SHLDR], posVect[NI_R_ELBOW], posVect[NI_R_WRIST]);
		rotQuat[NI_R_WRIST] = PPPQuaternion(posVect[NI_R_ELBOW], posVect[NI_R_WRIST], posVect[NI_R_HAND]);
		//
		rotQuat[NI_L_KNEE]  = PPPQuaternion(posVect[NI_L_HIP],   posVect[NI_L_KNEE],  posVect[NI_L_ANKLE]);
		rotQuat[NI_R_KNEE]  = PPPQuaternion(posVect[NI_R_HIP],   posVect[NI_R_KNEE],  posVect[NI_R_ANKLE]);
	}
	else {
		rotQuat[NI_L_SHLDR] = VPPQuaternion(vect_right,	   posVect[NI_R_SHLDR], posVect[NI_R_ELBOW]);
		rotQuat[NI_R_SHLDR] = VPPQuaternion(vect_left,	   posVect[NI_L_SHLDR], posVect[NI_L_ELBOW]);
		rotQuat[NI_L_HIP]   = VPPQuaternion(vect_down,     posVect[NI_R_HIP],   posVect[NI_R_KNEE]);
		rotQuat[NI_R_HIP]   = VPPQuaternion(vect_down,     posVect[NI_L_HIP],   posVect[NI_L_KNEE]);
//		rotQuat[NI_L_ANKLE] = VPPQuaternion(vect_ft_right, posVect[NI_R_ANKLE], posVect[NI_R_FOOT]);
//		rotQuat[NI_R_ANKLE] = VPPQuaternion(vect_ft_left,  posVect[NI_L_ANKLE], posVect[NI_L_FOOT]);
		//
		rotQuat[NI_L_ELBOW] = PPPQuaternion(posVect[NI_R_SHLDR], posVect[NI_R_ELBOW], posVect[NI_R_WRIST]);
		rotQuat[NI_L_WRIST] = PPPQuaternion(posVect[NI_R_ELBOW], posVect[NI_R_WRIST], posVect[NI_R_HAND]);
		rotQuat[NI_R_ELBOW] = PPPQuaternion(posVect[NI_L_SHLDR], posVect[NI_L_ELBOW], posVect[NI_L_WRIST]);
		rotQuat[NI_R_WRIST] = PPPQuaternion(posVect[NI_L_ELBOW], posVect[NI_L_WRIST], posVect[NI_L_HAND]);
		//
		rotQuat[NI_L_KNEE]  = PPPQuaternion(posVect[NI_R_HIP],   posVect[NI_R_KNEE],  posVect[NI_R_ANKLE]);
		rotQuat[NI_R_KNEE]  = PPPQuaternion(posVect[NI_L_HIP],   posVect[NI_L_KNEE],  posVect[NI_L_ANKLE]);
	}


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

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




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

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

	else {
		sharedMem->updateLocalAnimationData(posVect, rotQuat, NiSDK_Kinect, KINECT_MAX_JOINT_NUM);
	}
}





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

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

	CExKinectWin* kinect = (CExKinectWin*)pParam;
	if (kinect->device->context==NULL) return 1; 
	if (kinect->pSensorFrame==NULL)    return 1; 
	
	CExView* pview = kinect->pSensorFrame->pView;
	kinect->pViewData = &pview->viewData;
	
	BOOL ret;
	//
	try {
		Loop {
			//if (kinect->getDevState()==NI_STATE_DETECT_STOPPING || kinect->getDevState()==NI_STATE_SAVE_WORKING) continue;
			if (kinect->getDevState()==NI_STATE_DETECT_STOPPING) continue;

			if (kinect->m_use_image) {
				ret = kinect->device->wait_Image();
			}

			if (kinect->getDevState()==NI_STATE_DETECT_STOPPING) continue;
			kinect->hasDepthData = FALSE;
			if (kinect->getDevState()==NI_STATE_DETECT_EXEC) {
				ret = kinect->device->wait_Depth();
				if (ret) kinect->hasDepthData = TRUE;
			}

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

			if (kinect->getDevState()==NI_STATE_DETECT_EXEC) {
				kinect->trackingJoints();
			}

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

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

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




#endif