#include "StdAfx.h"

#include "ExKinectWnd.h"






/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CExKinctWnd ƒNƒ‰ƒX
//

CExKinectWnd::CExKinectWnd(void)
{
	clearBoneData();
	clearStartPosData();
//	prvPos = Vector<double>(0.0, 0.0, 0.0);

	isUseCamera		= TRUE;

	printPosMode	= TRUE;
	printRotMode	= FALSE;
	printQutMode	= TRUE;
	printNetMode    = FALSE;
	printChkMode	= FALSE;

	networkMode		= FALSE;
	networkOutMode	= NETonly;
	networkFastMode	= FALSE;

	kinectNet		= NULL;
	sharedMem		= NULL;
}




CExKinectWnd::~CExKinectWnd()
{
	DEBUG_ERR("CExKinectWnd DESTRUCTOR");
}





void  CExKinectWnd::clearBoneData(void)
{
	memset(posVect, 0, sizeof(Vector<double>)*OPENNI_MAX_JOINT_NUM);
	memset(prvVect, 0, sizeof(Vector<double>)*OPENNI_MAX_JOINT_NUM);

	memset(rotQuat, 0, sizeof(Quaternion)*OPENNI_MAX_JOINT_NUM);	
	memset(prvQuat, 0, sizeof(Quaternion)*OPENNI_MAX_JOINT_NUM);

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




void  CExKinectWnd::clearStartPosData(void)
{
	startPos = Vector<double>(0.0, 0.0, 0.0);
	isCalibrated = FALSE;
}




void  CExKinectWnd::setStartPosData(Vector<double> vect)
{
	startPos = vect;
	isCalibrated = TRUE;
}




void	CExKinectWnd::logingJointsData(int uid)
{
	if (pLogDoc!=NULL) {
		for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
			XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
			XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
			
			if (printPosMode || printRotMode || printQutMode) {
				pLogDoc->printFormat("LOCAL: %s (%2d)\n", getJointName((XnSkeletonJoint)j).c_str(), j);
			}
			
			if (printPosMode) {
				pLogDoc->printFormat("LOCAL: POS: %f, %f, %f\n", pos.X, pos.Y, pos.Z);
			}
			if (printRotMode) {
				pLogDoc->printFormat("LOCAL: ROT: %f, %f, %f\n", rot.elements[0], rot.elements[1], rot.elements[2]);
				pLogDoc->printFormat("LOCAL: ROT: %f, %f, %f\n", rot.elements[3], rot.elements[4], rot.elements[5]);
				pLogDoc->printFormat("LOCAL: ROT: %f, %f, %f\n", rot.elements[6], rot.elements[7], rot.elements[8]);
			}
			if (printQutMode) {
				pLogDoc->printFormat("LOCAL: QUT: %f, %f, %f, %f\n", rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
			}
		}
	}
}





void	CExKinectWnd::convertData(int uid)
{
	convertRot2SLData(uid);
	//convertPos2SLData(uid);
}




void	CExKinectWnd::convertRot2SLData(int uid)
{
	Vector<double> vect;

	//
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		if (OpenNI2SLJointNum(j)>=0) {
			if (jointPositionConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { 
				XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
				posVect[j].set(-pos.Z/1000., pos.X/1000., pos.Y/1000.);
				prvVect[j] = posVect[j];
			}
			else {
				posVect[j] = prvVect[j];
			}
		}
	}


	// mPelvis
	posVect[0] = (posVect[17] + posVect[21])*0.5;
	vect = posVect[21] - posVect[17];
	double thz = atan2(-vect.x, vect.y);
	rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0);
	
	if (!isCalibrated) setStartPosData(posVect[0]);
	posVect[0] = posVect[0] - startPos;


	//
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		//normalJoint[j] = true;

		if (OpenNI2SLJointNum(j)>=0) {	
			if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { 
				XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)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);
				prvQuat[j] = rotQuat[j];
			}
			else {
				rotQuat[j] = prvQuat[j];
			}
		}
	}


	// Right Leg
	rotQuat[22] = ~rotQuat[21]*rotQuat[22];
	rotQuat[18] = ~rotQuat[17]*rotQuat[18];

	// Right Hand
	rotQuat[13] = ~rotQuat[12]*rotQuat[13];
	rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];

	// Left Hand
	rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
	rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];

	rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
	rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];
	


	/*if (pLogDoc!=NULL) {
		Vector<double> v1, v2;
		v1 = Quaternion2EulerXYZ(rotQuat[18], &v2);
		pLogDoc->printFormat("L = (%f, %f, %f) (%f, %f, %f)\n", v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);
		v1 = Quaternion2EulerXYZ(rotQuat[22], &v2);
		pLogDoc->printFormat("R = (%f, %f, %f) (%f, %f, %f)\n", v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);
	}*/


	if (networkMode) {
		if (networkOutMode==NETandLOCAL) sharedMem->updateLocalAnimationData(posVect, rotQuat);
		if (kinectNet->sendSocket>0) {
			//if (pLogDoc!=NULL) pLogDoc->printFormat("NET: SEND: %s\n", (char*)(LPCSTR)sharedMem->animationUUID);
			if (networkFastMode) sendAnimationData<float> (posVect, rotQuat, kinectNet);
			else                 sendAnimationData<double>(posVect, rotQuat, kinectNet);
		}
	}
	else {
		sharedMem->updateLocalAnimationData(posVect, rotQuat);
	}
}























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



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

	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;
}




void	CExKinectWnd::convertPos2SLData(int uid)
{
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		int n = OpenNI2SLJointNum(j);
		if (n>=0) {
			XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
			XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)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);
			posVect[j].set(-pos.Z, pos.X, pos.Y);
		}
	}
	//
	if (!isCalibrated) setStartPosData(posVect[3]);

	//
	rotQuat[ 2] = V2VQuaternion(posVect[ 3], posVect[ 2], posVect[ 1]);

	rotQuat[ 6] = V2VQuaternion(posVect[ 2], posVect[ 6], posVect[ 7]);
	rotQuat[ 7] = V2VQuaternion(posVect[ 6], posVect[ 7], posVect[ 9]);

	rotQuat[12] = V2VQuaternion(posVect[ 2], posVect[12], posVect[13]);
	rotQuat[13] = V2VQuaternion(posVect[12], posVect[13], posVect[15]);

	rotQuat[18] = V2VQuaternion(posVect[17], posVect[18], posVect[20]);
	rotQuat[22] = V2VQuaternion(posVect[21], posVect[22], posVect[24]);

/*
	// Set to Shared Memory
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		int n = OpenNI2SLJointNum[j];
		if (n>=0) {
			double* shm = theApp.ptrMapData[n];
			if (shm!=NULL) {
				shm[4] = rotQuat[j].x;
				shm[5] = rotQuat[j].y;
				shm[6] = rotQuat[j].z;
				shm[7] = rotQuat[j].s;
			}
		}
	}

	// Position Data from XN_SKEL_TORSO(3) to mPelvis(0)
	double* shm = theApp.ptrMapData[0];
	if (shm!=NULL) {
		shm[1] = (posVect[3].x - startPos.x)/1000.;
		shm[2] = (posVect[3].y - startPos.y)/1000.;
		shm[3] = (posVect[3].z - startPos.z)/1000.;
	}
	*/
}