#include "StdAfx.h"

#include "KinectSMemory.h"
#include "Rotation.h"

#include "anm_data.h"




CKinectSMemory::CKinectSMemory(void)
{
	animationUUID = "";

	indexHandle  = NULL;
	ptrAnimIndex = NULL;
		
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		mapHandle[i]  = NULL;
		ptrMapData[i] = NULL;
	}

	isCreated = createSharedMemory();
}





CKinectSMemory::~CKinectSMemory(void)
{
	DEBUG_ERR("CKinectSMemory DESTRUCTOR");

	closeSharedMemory();
}






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


BOOL  CKinectSMemory::createSharedMemory(void)
{
	BOOL ret = TRUE;

	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		mapHandle[i] = CreateFileMapping((HANDLE)0xffffffff, NULL, PAGE_READWRITE, 0, ANM_COM_LEN_DATA*SLKSMEM_CHANNEL_NUM, SLJointName(i).c_str()); 
		if (mapHandle[i]==NULL) {
			ret = FALSE;
			break;
		}
		ptrMapData[i] = (double*)MapViewOfFile(mapHandle[i], FILE_MAP_ALL_ACCESS, 0, 0, 0);
		if (ptrMapData[i]==NULL) {
			ret = FALSE;
			break;
		}
		memset((char*)ptrMapData[i], 0, ANM_COM_LEN_DATA*SLKSMEM_CHANNEL_NUM);
	}

	// Index
	if (ret) {
		indexHandle = CreateFileMapping((HANDLE)0xffffffff, NULL, PAGE_READWRITE, 0, SLKSMEM_INDEX_LEN*SLKSMEM_CHANNEL_NUM, SLKSMEM_INDEX_NAME); 
		if (indexHandle!=NULL) ptrAnimIndex = (char*)MapViewOfFile(indexHandle, FILE_MAP_ALL_ACCESS, 0, 0, 0);
		if (indexHandle==NULL || ptrAnimIndex==NULL) ret = FALSE;
	}

	if (!ret) closeSharedMemory();

	clearSharedMemory();
	return ret;
}





void  CKinectSMemory::closeSharedMemory(void)
{
	clearSharedMemory();

	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrMapData[i]!=NULL) {
			memset((char*)ptrMapData[i], 0, ANM_COM_LEN_DATA*SLKSMEM_CHANNEL_NUM);
			UnmapViewOfFile(ptrMapData[i]);
			ptrMapData[i] = NULL;
		}
		if (mapHandle[i]!=NULL) {
			CloseHandle(mapHandle[i]);
			mapHandle[i] = NULL;
		}
	}


	if (ptrAnimIndex!=NULL) {
		memset(ptrAnimIndex, 0, SLKSMEM_INDEX_LEN*SLKSMEM_CHANNEL_NUM);
		UnmapViewOfFile(ptrAnimIndex);
		ptrAnimIndex = NULL;
	}
	
	if (indexHandle!=NULL) {
		CloseHandle(indexHandle);
		indexHandle = NULL;
	}

	isCreated = FALSE;
}




void  CKinectSMemory::clearSharedMemory(void)
{
	clearLocalAnimationIndex();
	clearNetworkAnimationIndex();

	clearAnimationData();
}




///////////////////////////////////////////////////////////////////////////
// Index


void  CKinectSMemory::setAnimationIndex(char* uuid, int n)
{
	char* ptr = ptrAnimIndex + n*SLKSMEM_INDEX_LEN;
	memcpy(ptr, uuid, ANM_COM_LEN_UUID);
}




void  CKinectSMemory::setLocalAnimationIndex(void)
{
	if (animationUUID!="") {
		setAnimationIndex((char*)(LPCSTR)animationUUID, 0);
	}
}




void  CKinectSMemory::clearLocalAnimationIndex(void)
{
	memset(ptrAnimIndex, 0, SLKSMEM_INDEX_LEN);
}




int	 CKinectSMemory::getNetworkAnimationIndexNum(void)
{
	int n = 0;
	char* ptr = ptrAnimIndex + SLKSMEM_INDEX_LEN;

	for (int i=1; i<SLKSMEM_CHANNEL_NUM; i++) {
		if (*ptr!=0x00) n++;
		ptr += SLKSMEM_INDEX_LEN;
	}

	return n;
}




int	 CKinectSMemory::checkNetworkAnimationIndex(char* uuid)
{
	int   n, spidx = -1;
	char* ptr = ptrAnimIndex + SLKSMEM_INDEX_LEN;
	if (ptr==NULL) return -1;

	for (n=1; n<SLKSMEM_CHANNEL_NUM; n++) {
		if (!strncasecmp(ptr, uuid, ANM_COM_LEN_UUID)) break;
		else if (spidx==-1 && *ptr==0x00) spidx = n;
		ptr += SLKSMEM_INDEX_LEN;
	}

	if (n==SLKSMEM_CHANNEL_NUM) {
		if (spidx!=-1) {
			ptr = ptrAnimIndex + spidx*SLKSMEM_INDEX_LEN;
			memcpy(ptr, uuid, ANM_COM_LEN_UUID);
		}
		n = spidx;
	}

	return n;
}




int  CKinectSMemory::clearNetworkAnimationIndex(char* uuid)
{
	char* ptr = ptrAnimIndex;
	if (ptr==NULL) return -1;

	int n;
	for (n=1; n<SLKSMEM_CHANNEL_NUM; n++) {
		if (!strncasecmp(ptr, uuid, ANM_COM_LEN_UUID)) {
			memset(ptr, 0, SLKSMEM_INDEX_LEN);
			break;
		}
		ptr += SLKSMEM_INDEX_LEN;
	}

	if (n<SLKSMEM_CHANNEL_NUM) return n;
	return -1;
}




void  CKinectSMemory::clearNetworkAnimationIndex()
{
	if (ptrAnimIndex!=NULL) {
		memset(ptrAnimIndex+SLKSMEM_INDEX_LEN, 0, SLKSMEM_INDEX_LEN*(SLKSMEM_CHANNEL_NUM-1));
	}
}





//////////////////////////////////////////////////////////////////////////
// Data

void  CKinectSMemory::updateLocalAnimationData(Vector<double>* posVect, Quaternion* rotQuat)
{
	// mPelvis(0)
	double* shm = ptrMapData[0];
	if (shm!=NULL) {
		shm[1] = posVect[0].x;
		shm[2] = posVect[0].y;
		shm[3] = posVect[0].z;
		shm[4] = rotQuat[0].x;
		shm[5] = rotQuat[0].y;
		shm[6] = rotQuat[0].z;
		shm[7] = rotQuat[0].s;
	}

	for (int i=1; i<OPENNI_MAX_JOINT_NUM; i++) {
		int n = OpenNI2SLJointNum(i);
		if (n>=0) {
			shm = ptrMapData[n];
			if (shm!=NULL) {
				shm[4] = rotQuat[i].x;
				shm[5] = rotQuat[i].y;
				shm[6] = rotQuat[i].z;
				shm[7] = rotQuat[i].s;
			}
		}
	}
}




void  CKinectSMemory::clearLocalAnimationData(void)
{
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrMapData[i]!=NULL) {
			memset((char*)ptrMapData[i], 0, ANM_COM_LEN_DATA);
		}
	}
}




void  CKinectSMemory::clearNetworkAnimationData(void)
{
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrMapData[i]!=NULL) {
			memset((char*)ptrMapData[i]+ANM_COM_LEN_DATA, 0, ANM_COM_LEN_DATA*(SLKSMEM_CHANNEL_NUM-1));
		}
	}
}




void  CKinectSMemory::clearAnimationData(void)
{
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrMapData[i]!=NULL) {
			memset((char*)ptrMapData[i], 0, ANM_COM_LEN_DATA*SLKSMEM_CHANNEL_NUM);
		}
	}
}





//////////////////////////////////////////////////////////////////////////
// Index + Data

void  CKinectSMemory::updateNetworkAnimation(char* data, char* uuid, int joints, int size)
{
	if (data==NULL) {
		clearNetworkAnimation(uuid);
		return;
	}

	int channel = checkNetworkAnimationIndex(uuid);
	if (channel<0) return;
	int chnlpos = channel*ANM_COM_NUM_DATA;

	if (size==4) {
		float* net = (float*)data;
		for (int i=0; i<joints; i++) {
			int n = *(int*)net;
			if (n>=0 && n<SL_MAX_JOINT_NUM) {
				if (ptrMapData[n]!=NULL) {
					double* shm = ptrMapData[n] + chnlpos;
					*(int*)shm = n; 
					shm[1] = (double)net[1];
					shm[2] = (double)net[2];
					shm[3] = (double)net[3];
					shm[4] = (double)net[4];
					shm[5] = (double)net[5];
					shm[6] = (double)net[6];
					shm[7] = (double)net[7];
				}
			}
			net += ANM_COM_NUM_DATA;
		}
	}

	else {
		double* net = (double*)data;
		for (int i=0; i<joints; i++) {
			int n = *(int*)net;
			if (n>=0 && n<SL_MAX_JOINT_NUM) {
				if (ptrMapData[n]!=NULL) {
					memcpy((char*)(ptrMapData[n]+chnlpos), (char*)net, ANM_COM_LEN_DATA);
				}
			}
			net += ANM_COM_NUM_DATA;
		}
	}
}





void  CKinectSMemory::clearNetworkAnimation(char* uuid)
{
	char* ptr = ptrAnimIndex;
	if (ptr==NULL) return;

	int n;
	for (n=1; n<SLKSMEM_CHANNEL_NUM; n++) {
		if (!strncasecmp(ptr, uuid, ANM_COM_LEN_UUID)) {
			memset(ptr, 0, SLKSMEM_INDEX_LEN);
			break;
		}
		ptr += SLKSMEM_INDEX_LEN;
	}

	if (n<SLKSMEM_CHANNEL_NUM) {
		for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
			if (ptrMapData[i]!=NULL) {
				memset((char*)ptrMapData[i]+ANM_COM_LEN_DATA*n, 0, ANM_COM_LEN_DATA);
			}
		}
	}

	return;
}