#include "DEFINES.H"

#define PRIMESEND_FOCALE 1.3


#ifdef PRIMESENS_SUPPORT

#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnCore.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnIO.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnUSB.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVBackgroundRemover.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVDynamicUserExtractor.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnCore.lib")
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVision.lib")

#include <XnVision.h>
#include <XnVDynamicUserExtractor.h>
#include <XnVBackgroundRemover.h>

#include "TRACK.h"
#include "MEDIA.h" 

//#pragma optimize("",off)
#define DepthMAX (4000)
u32 ColorMap[DepthMAX];
u32 Stats[DepthMAX];
static u32 NBV = 0;

static XnVDevice * pDevice = NULL;
static XnVUserExtractor * pUserExtractor = NULL;
static XnVDynamicUserExtractor * pDynamicUserExtractor = NULL;
static XnVLabelMatrix * pLabels = NULL;

static unsigned char * pAlphaMap = NULL;
static XnVDepthMap * pOrigDm = NULL;
static XnVBackgroundRemover * pBackgroundRemover = NULL;

float fPRIMESENS_Equalize = 0.0f;
u32 PrimeSendResetBackGround = 0;
XnVPlane GroundPlane = {0,0,0,0};
Vector UserCoodinate = Vector (0,0,0);
static 	s32 XMin = 10000000,YMin = 10000000;
static 	s32 XMax= -10000000,YMax= -10000000;


u16 CurrentDepth[1024*1024];
u16 CurrentDUser[1024*1024];
s32 CurrentColor[1024*1024];


u32 PrimeSensAlphaSilhouette = 0;
u32 PrimeUserExtraction = 0;
MAP LastMapUsed;
char ERRORTEXT[2048];	
void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D);
void PRIMESENS_3D_to_2D_Coordinate(MAP *ZMap, Vector *p3D , Vector *p2D);

float TRK3DW_XMin = 0.5 - 0.15;
float TRK3DW_YMin = 0.5 - 0.15;
float TRK3DW_XMax = 0.5 + 0.15;
float TRK3DW_YMax = 0.5 + 0.35;
float TRK3DW_ZMin = 0.5 + 0.15;
float TRK3DW_ZMax = 0.5 + 0.35;
u32 TRK3DW_Flags = 0;

void *pZBUFFERMEDIA = 0;
class PRIMESENS_Grabber:public INPUT_IO
{
	
public:
	u32 DepthMode;
	MAP MaxDepth;
	WORLD *pSourceScene;
	int		INIT	(class WORLD*);
	int		FRAME	(void);
	int		END		(void);
	IOTYPE	GetType() {return IOtype_PRIMESENS;};
	virtual void *	GetData(int dataIdx) { return pZBUFFERMEDIA;};
};

s16 LocalMapBlur[1024*1024];

//#pragma optimize("",off)

u32 PRIMESENS_GetFloor(MATRIX *pDst)
{

	extern Vector	PRIMESENS_GroundZ;
	extern float	PRIMESENS_GroundDistance;
	pDst->Identity();

	if (PRIMESENS_GroundDistance)
	{
		pDst->T = Vector(0,0,0);
		pDst->I = Vector(1,0,0);
		pDst->J = Vector(0,0,1);
		pDst->K = Vector(0,-1,0);

		Vector Norm = Vector (0,0,1);
		Vector vQ = PRIMESENS_GroundZ + Norm;
		vQ.z = -vQ.z;
		vQ .Normalize();
		vQ = vQ^Norm  ;
		vQ = vQ * -1.0f;
		Quat Q;
		//vQ.z = 0.0f;
		*(Vector *)&Q = vQ;
		Q.NormalizeW();
		MATRIX M;
		M.Identity();
		Q.TransforToMatrix(&M);
		*pDst *= M;
		pDst->RotateAround_I(-90.0f * 3.1415927f/ 180.0f)		;
		pDst->T.x = pDst->T.y = 0.0f;
		pDst->T.z = PRIMESENS_GroundDistance;
		pDst->I *= -1;
		pDst->J *= -1;
		pDst->K *= -1;

		float HAngle;
		HAngle = asinf(pDst->K.x / sqrtf(pDst->K.y*pDst->K.y+pDst->K.z*pDst->K.z));
		pDst->RotateAround_Z(-HAngle);

		return 1;
	}
	return 0;
}

u32 PRIMESENS_GetUserMatrix(MATRIX *pDst)
{

	return 0;
}
u32 PRIMESENS_GetUser2DCoordinate(Vector *pDst,u32 Mode)
{

	switch(Mode)

	{
	case 0: // GroundGravity 
		{
			extern Vector	PRIMESENS_GroundZ;
			extern float	PRIMESENS_GroundDistance;
			Vector Coord = UserCoodinate;
			MATRIX Dst;
			PRIMESENS_GetFloor(&Dst);
			PRIMESENS_GroundZ.z = -PRIMESENS_GroundZ.z;
			PRIMESENS_GroundZ.x = -PRIMESENS_GroundZ.x;
			Coord  -= PRIMESENS_GroundZ * ((PRIMESENS_GroundZ * Coord) - PRIMESENS_GroundDistance);
			PRIMESENS_GroundZ.z = -PRIMESENS_GroundZ.z;
			PRIMESENS_GroundZ.x = -PRIMESENS_GroundZ.x;
			PRIMESENS_3D_to_2D_Coordinate(&LastMapUsed, &Coord , pDst);
		}
		return 1;
		break;
	case 1: // UserBVMin
		pDst->z = 0.0f;
		pDst->x = XMin;
		pDst->y = YMin;
		return 1;
		break;
	case 2: // UserBVMax 
		pDst->z = 0.0f;
		pDst->x = XMax;
		pDst->y = YMax;
		return 1;
		break;
	}
	return 0;
}

int		PRIMESENS_Grabber::FRAME	(void)
{
	ERRORTEXT[0] = '\0';

	MediaFocale = PRIMESEND_FOCALE;

	Vector MinV = Vector(TRK3DW_XMin * Image.SX,TRK3DW_YMin * Image.SY,TRK3DW_ZMin);
	Vector MaxV = Vector(TRK3DW_XMax * Image.SX,TRK3DW_YMax * Image.SY,TRK3DW_ZMax);

	LastMapUsed = Image;
	// read a new frame
	if (DepthMode == 3) // Normale Map
	{
		void MAP_PPRIMESENS_ComputeNormales(u16 *ZOoMap , MAP *pColorMap );
		void MAP_PPRIMESENS_ComputeMatting(MAP *pColorMap );
		s32 Counter = Image.SX * Image.SY * 3;
		s32 *pBase;

		XN_DEPTH_TYPE* pDepthUE = (XN_DEPTH_TYPE*)CurrentDUser;
		XN_DEPTH_TYPE* pDepthRAW = (XN_DEPTH_TYPE*)CurrentDepth;

		XN_IMAGE_TYPE* pImage = (XN_IMAGE_TYPE* )CurrentColor;
		u8 *pIM = (u8*)pImage;
		pBase = Image.BASE;

		pDepthUE += Image.SX * Image.SY;
		pDepthRAW += Image.SX * Image.SY;

		while (Counter)
		{
			Counter-=3;
			pDepthUE--;
			pDepthRAW--;
			*(pBase) = ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
			if ((*pDepthUE))
				*(pBase) |= 1<<24;
			else
			{
				if (*pDepthRAW)
					*(pBase) |= 2<<24;
			}
			pBase++;

		}

		MAP_PPRIMESENS_ComputeMatting( &Image );
	}
	else
	if (DepthMode == 2) // ShadowMap
	{
		XN_DEPTH_TYPE* pDepth = (XN_DEPTH_TYPE*)CurrentDUser;
extern double MEGADX;
extern double MEGADY;
extern Vector MEGADZ;
void MAP_PPRIMESENS_ReLightWithZ(u16 *ZOoMap , MAP *pColorMap , MAP *pShadowMAP , Vector *GCenter , float DX, float DY);
		//MAP_PPRIMESENS_ReLightWithZ(pDepth, &Image, NULL, &MEGADZ , MEGADX, MEGADY);
		s32 *pBase;
		pDepth += Image.SX * Image.SY;
		pBase = Image.BASE;
		memset(pBase,0,Image.SX * Image.SY * 4);
		for (s32 Y = 0;Y<Image.SY ; Y++)
			for (s32 X = 0 ;X<Image.SX; X++)
		{
extern 				double MEGADX ;
extern 				double MEGADY ;
	extern 			Vector MEGADZ ;
	extern Vector	PRIMESENS_GroundZ;
	extern float	PRIMESENS_GroundDistance;

			float VirtuaklZ;
			VirtuaklZ = (X - MEGADZ.x) * MEGADX + (Y - MEGADZ.y) * MEGADY + MEGADZ.z;
			pDepth--;
			if (*pDepth)
			{
						Vector p2D,p3D;
						p2D = Vector(X,Y,*pDepth);
						PRIMESENS_2D_to_3D_Coordinate(&Image, &p2D , &p3D);
			//PRIMESENS_GroundZ.y = -PRIMESENS_GroundZ.y;

						float Z = (PRIMESENS_GroundZ * p3D) + PRIMESENS_GroundDistance;
						p3D -= PRIMESENS_GroundZ * Z;

						if (Z < 500)
						{
							PRIMESENS_3D_to_2D_Coordinate(&Image, &p3D , &p2D);
							if ((p2D.x > 0) && (p2D.y > 0) && ((p2D.x < Image.SX-1)) && (p2D.y < Image.SY - 15))
							{
								s32 *pBaseZShad = Image.BASE + (s32)p2D.x + ((s32)p2D.y ) * Image.PITCH;
								float Z2;
								Z2 = Z;
								if (Z2 < 0) Z2 = 0;
								Z2 = 255.0f * (500.0f - Z2) / 500.0f;
								//if (*pBaseZShad == 0xffffffff)	
								{
									u32 Counter,Color;
									s32 Remont = (255 - Z2) / 20;
									Counter = 3 + Remont;
									if (Counter > 15) Counter = 15;
									Remont/=2;
									if (p2D.y < Remont) Remont = p2D.y;
									pBaseZShad -= Remont * Image.PITCH;
									Color = Z2;
									Color -= Color>>3;
									while (Counter--)
									{
										if (Color > *pBaseZShad)
										*pBaseZShad = Color;
										pBaseZShad += Image.PITCH;
									}
								}
							}
						}

			//PRIMESENS_GroundZ.y = -PRIMESENS_GroundZ.y;

/*				if ((s32)Z & 512)
				*(pBase) = 0xffffff00;
				else
					*(pBase) = 0xffff00ff;//*/
			}
			else
			{

/*				if (VirtuaklZ < 0.0f)
				*(pBase) = 0;*/
			}
			pBase++;
		}

			pBase = Image.BASE;
		for (s32 Y = 0;Y<Image.SY ; Y++)
			for (s32 X = 0 ;X<Image.SX; X++)
		{

			if (*pBase > 255) *pBase = 255;
			*pBase |= *pBase<<8;
			*pBase |= *pBase<<8;
			*pBase |= *pBase<<8;
			*pBase ^= 0xffffff;
			pBase++;
		}

	} else
	if (DepthMode == 1)
	{

		XnVStatus rc = pDevice->Read();
		if (XNV_STATUS_OK != rc)
		{
			sprintf(ERRORTEXT, "Read failed: %s\n", XnGetStatusString(pDevice->GetLastStatus()));
			OutputDebugString(ERRORTEXT);
			return 0;
		}
		XN_DEPTH_TYPE* pDepth = pDevice->GetDepthMap()->Data();
		XN_IMAGE_TYPE* pImage = pDevice->GetImageMap()->Data();


		// extract the user
		pDevice->GetDepthMap()->Clone(*pOrigDm);
		if (PrimeUserExtraction)
		{
//			pDynamicUserExtractor->Init();
			try { pDynamicUserExtractor->Run(pDevice->GetDepthMap(), pLabels); } 
			catch(...) {};
			
		}
 		//pDepth = pOrigDm->Data();//*/

		memcpy((s32*)CurrentDUser,pDepth,Image.SX * Image.SY * 2);
		memcpy((s32*)CurrentDepth,pOrigDm->Data(),Image.SX * Image.SY * 2);
		memcpy((s32*)CurrentColor,pImage,Image.SX * Image.SY * 3);


		char debugstr[1024];

	/*	if (!pDynamicUserExtractor->GetFloorPlane(&GroundPlane))
		{
			OutputDebugString("Failed to get floor plane\n");
		}
		else
		{
			sprintf(debugstr, "Floor height: %f\n", GroundPlane.fHeight);
			OutputDebugString(debugstr);
		}*/

		


		//GroundPlane.fHeight = 0.0f;
		/*
		if (!(pDynamicUserExtractor && pDynamicUserExtractor->GetFloorPlane(&GroundPlane)))
		{
			memset(&GroundPlane,0,sizeof(GroundPlane));
		} 
			*/


		// fix the silhouette
		pAlphaMap = NULL;
		if (PrimeSensAlphaSilhouette)
		{
			pBackgroundRemover->FixEdgesAndLearnBackRGB(pImage, pDepth, pOrigDm->Data(), pAlphaMap, false, true, false);
		}
		if (PrimeSendResetBackGround)
		{
			pBackgroundRemover->ResetBackRGB();
			pDynamicUserExtractor->Reset();
		}
		PrimeSendResetBackGround = 0;

		
		UserCoodinate = Vector(0,0,0);
		void MAP_ComputePlanes(MAP *ZMap,u16 *PrimsensZMap);
		MAP_ComputePlanes(&Image,pOrigDm->Data());
		u32 Counter;
		s32 *pBase;
		XMin = 10000000;YMin = 10000000;
		XMax= -10000000;YMax= -10000000;
		float Summ = 0.0f,SummSqr = 0.0f;
		u32 ulNumber = 0;
		u8 *pu8AlphaPtr;
		pBase = Image.BASE + Image.SX * Image.SY;
		pu8AlphaPtr = pAlphaMap;
		if (pu8AlphaPtr)
		{
			for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
				for (s32 X = Image.SX-1 ;X>=0 ; X--)
				{
					pBase--;
					//if ((*pDepth != 0) /*&& (*pDepth < 1500)*/)
					//if (*pu8AlphaPtr)//(*pDepth != 0) /*&& (*pDepth < 1500)*/)
					{
						if (X < XMin) XMin = X;
						if (X > XMax) XMax = X;
						if (Y < YMin) YMin = Y; 
						if (Y > YMax) YMax = Y;
						((u8*)pBase)[0] = (u8)pImage[2];
						((u8*)pBase)[1] = (u8)pImage[1];
						((u8*)pBase)[2] = (u8)pImage[0];
						((u8*)pBase)[3] = *pu8AlphaPtr;
						float brightness = (float)(((u32)pImage[2] + (u32)pImage[1] + (u32)pImage[1] + (u32)pImage[0])) * 0.25f;
						Summ += brightness;
						SummSqr += brightness * brightness;
						ulNumber ++;
					}
					
					pImage+=3;
					pDepth++;
					if (pu8AlphaPtr)
						pu8AlphaPtr++;
			}

		} else
		{
		for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
			for (s32 X = Image.SX-1 ;X>=0 ; X--)
			{
				pBase--;
				*(float *)pBase = (float)*pDepth;
				if (*pDepth && (*pDepth < TRK3DW_ZMin)) *(u32 *)pBase = (TRK3DW_Flags & 2)>>1;
				if (*pDepth > TRK3DW_ZMax)*(u32 *)pBase = (TRK3DW_Flags & 2)>>1;

/*				float DepthColor = *pDepth;
				if (DepthColor == 0) DepthColor = 10000000.0f;
				DepthColor = 1000.0f / DepthColor;
				DepthColor *= 256.0f ;
				if (DepthColor < 0) DepthColor = 0;
				u32 K = DepthColor;
				if ((K & 15) != 0) DepthColor = 0;
				if (DepthColor > 255) DepthColor = 255;*/
#if 0
				if ((*pDepth != 0) /*&& (*pDepth < 1500)*/)
				//if (*pu8AlphaPtr)//(*pDepth != 0) /*&& (*pDepth < 1500)*/)
				{
					if (X < XMin)
						XMin = X;
					if (X > XMax) XMax = X;
					if (Y < YMin) YMin = Y; 
					if (Y > YMax) YMax = Y;
					((u8*)pBase)[0] = (u8)pImage[2];
					((u8*)pBase)[1] = (u8)pImage[1];
					((u8*)pBase)[2] = (u8)pImage[0];
					((u8*)pBase)[2] = ((u8*)pBase)[1] = ((u8*)pBase)[0] = (u8)((float)256 * (float)*pDepth / (float)4000 );
					((u8*)pBase)[3] = 0xff;//*pu8AlphaPtr;
					Vector p2D,p3D;
					p2D = Vector(X,Y,*pDepth);
					PRIMESENS_2D_to_3D_Coordinate(&Image, &p2D , &p3D);
					UserCoodinate += p3D;

					float brightness = (float)(((u32)pImage[2] + (u32)pImage[1] + (u32)pImage[1] + (u32)pImage[0])) * 0.25f;
					Summ += brightness;
					SummSqr += brightness * brightness;
					ulNumber ++;
				}
				else
				{
					/*((u8*)pBase)[0] = (u8)pImage[2];
					((u8*)pBase)[1] = (u8)pImage[1];
					((u8*)pBase)[2] = (u8)pImage[0];
					((u8*)pBase)[3] = 0x00;*/
					((u8*)pBase)[0] = (u8)pImage[2];
					((u8*)pBase)[1] = (u8)pImage[1];
					((u8*)pBase)[2] = (u8)pImage[0];
					((u8*)pBase)[3] = 0;
				}
					/*((u8*)pBase)[0] = (u8)DepthColor;
					((u8*)pBase)[1] = (u8)DepthColor;
					((u8*)pBase)[2] = (u8)DepthColor;
					(					(u8*)pBase)[3] = 0xff;//*pu8AlphaPtr;*/
#endif
				
				pImage+=3;
				pDepth++;
				if (pu8AlphaPtr)
					pu8AlphaPtr++;
			}

			/* First edge corection */
			if (0)
			{
				MAP16 LocalMap;
				LocalMap.BASE = (s16*)LocalMapBlur;
				LocalMap.PITCH = LocalMap.SX = Image.SX;
				LocalMap.SY = Image.SY;
				u32 Countor = LocalMap.SX * LocalMap.SY;
				while (Countor--)
					LocalMapBlur[Countor] = ((u32)Image.BASE[Countor] )>>24;
				//MAP_2_MAP16(&Image,&LocalMap,24);
				MAP16_BLUR_Inplace(&LocalMap,2,1);
				Countor = LocalMap.SX * LocalMap.SY;
				while (Countor--)
				{
					if (LocalMapBlur[Countor] < 128) 
						LocalMapBlur[Countor] = 0;
					else
						LocalMapBlur[Countor] = 255;
				}
				Countor = LocalMap.SX * LocalMap.SY - 4;
				while (Countor--)
				{
					LocalMapBlur[Countor+1] &= LocalMapBlur[Countor];
				}

				MAP16_2_MAP(&LocalMap,&Image,24);
			}
		}

		if (ulNumber)
		UserCoodinate *= 1.0f/(ulNumber);
		
		pBase = Image.BASE + Image.SX * Image.SY;
		pDepth = (XN_DEPTH_TYPE*)CurrentDUser;
		pImage = (XN_IMAGE_TYPE* )CurrentColor;

		for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
			for (s32 X = Image.SX-1 ;X>=0 ; X--)
			{
				pBase--;
/*				if (!((u8*)pBase)[3])
				{
					((u8*)pBase)[0] = (u8)pImage[2];
					((u8*)pBase)[1] = (u8)pImage[1];
					((u8*)pBase)[2] = (u8)pImage[0];
					((u8*)pBase)[3] = 0xff;
				}*/
				pImage+=3;
				pDepth++;
				pu8AlphaPtr++;
			}//*/

		// EQUALIZE treatment
		if (ulNumber && fPRIMESENS_Equalize)
		{
			u8 ConvertTable[256];
			s32 *pBase,*pBaseLast;
			Summ /= (float)ulNumber;
			SummSqr /= (float)ulNumber;
			SummSqr -= Summ * Summ;
			if (SummSqr) // Then Normalize image 
			{
				float NewAverage,Factor;
				NewAverage = (128.0f + Summ) / 2.0f;
				Factor = (1.0f + (100.0f / sqrt(SummSqr))) / 2.0f;
				for (u32 i = 0 ; i < 256 ; i++)
				{
					float Res = ((float)i - Summ) * Factor + NewAverage;
					if (Res > 255.0f) Res = 255.0f;
					if (Res < 0.0f) Res = 0.0f;
					ConvertTable[i] =  (u8)Res;
				}

				pBase = Image.BASE;
				pBaseLast = Image.BASE + Image.SX * Image.SY;
				while (pBase < pBaseLast)
				{
					((u8*)pBase)[0] = ConvertTable[((u8*)pBase)[0]];
					((u8*)pBase)[1] = ConvertTable[((u8*)pBase)[1]];
					((u8*)pBase)[2] = ConvertTable[((u8*)pBase)[2]];
					pBase++;
				}
			}
			
		}
		//DRawTEXT_Soft_B((u8*)ERRORTEXT,(u32*)Image.BASE,100,100,Image.PITCH, Image.SY , 0xffff0000);
		//DrawQuad_R(&Image,XMin,YMin,XMax,YMax,0xffffffff,1,0.0f);
#define ZENV 2
		u32 Counttttt = DepthMAX;
		NBV = 0;
		while (Counttttt--)
		{
			if (Stats[Counttttt])
			 NBV++;
		}

		DrawQuadFull(&Image,0,0,Image.SX,MinV.y,0);
		DrawQuadFull(&Image,0,MinV.y,MinV.x,MaxV.y,0);
		DrawQuadFull(&Image,MaxV.x,MinV.y,Image.SX,MaxV.y,0);
		DrawQuadFull(&Image,0,MaxV.y,Image.SX,Image.SY,0);

	} else
		// ColorMap
	{
		s32 Counter = Image.SX * Image.SY * 3;
		s32 *pBase;
		XN_IMAGE_TYPE* pImage = (XN_IMAGE_TYPE* )CurrentColor;
		u8 *pIM = (u8*)pImage;
		pBase = Image.BASE;
		XN_DEPTH_TYPE* pDepth = pDevice->GetDepthMap()->Data();

if (TRK3DW_Flags & 1)
		{
			while (Counter)
			{
				Counter-=3;
				float ZZ = pDepth[Counter/3];

				*(pBase) = 0xff000000 | ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
				if (ZZ == 0)*pBase = 0;
				if (ZZ < TRK3DW_ZMin) *pBase = 0;
				if (ZZ > TRK3DW_ZMax) *pBase = 0;

				pBase++;
			}
		} else
			while (Counter)
			{
				Counter-=3;
				*(pBase++) = 0xff000000 | ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
			}//*/


/*
	if (pSourceScene->WSPC)
	{
		pSourceScene->WSPC->LastMin = Vector((pSourceScene->WSPC->DRAWMAP->SX>>1)-48,(pSourceScene->WSPC->DRAWMAP->SY>>1)-32,0);
		pSourceScene->WSPC->LastMax = Vector((pSourceScene->WSPC->DRAWMAP->SX>>1)+48,(pSourceScene->WSPC->DRAWMAP->SY>>1)+64,0);
		pSourceScene->WSPC->LastMin -= Vector(16,16,0);
		pSourceScene->WSPC->LastMax += Vector(16,16,0);
	}
*/
		DrawQuad_R(&Image,MinV.x,MinV.y,MaxV.x,MaxV.y,0xff00,2,0);
	}

	return 1;
}


// Local function variables
int PRIMESENS_Grabber::INIT(WORLD*psCene)
{
	char ErrorName[2048];
	DWORD Len = 2048;

	pSourceScene = psCene;
	
	
	GetCurrentDirectory(Len,ErrorName);
	memset(Stats,  0 , sizeof(Stats));

	if (NULL == pDevice)
	{
		XnStatus rcll;
		XnVStatus rc = XnVDevice::IOInit(".\\Xiron.ini", &rcll);

		// init XnVision
		if (rc != XNV_STATUS_OK)
		{
			OutputDebugString(ErrorName);
			sprintf(ErrorName,"Error initing XnVision: %s\n", XnGetStatusString(rcll));
			OutputDebugString(ErrorName);
			return 0;
		}
		
		// Create and open the device
		pDevice = new XnVDevice();
		rc = pDevice->OpenFromINI(".\\Xiron.ini", "DeviceInput");
		if (rc != XNV_STATUS_OK)
		{
			sprintf(ErrorName, "Error opening device from %s: %s\n", ".\\Xiron.ini", XnGetStatusString(pDevice->GetLastStatus()));
			OutputDebugString(ErrorName);
			delete pDevice;
			pDevice = NULL;
			return 0;
		}

		// user extraction stuff
		pUserExtractor = new XnVUserExtractor();
		pDynamicUserExtractor = new XnVDynamicUserExtractor("", pDevice);
		pLabels = new XnVLabelMatrix(pDevice->GetDepthMap()->m_nXRes, pDevice->GetDepthMap()->m_nYRes);
		//pDynamicUserExtractor->Init();
		pDynamicUserExtractor->SetFrontCutoff(1000);

		// silhouette fixing stuff
		pAlphaMap = (unsigned char*)_aligned_malloc (sizeof(unsigned char) * pDevice->GetImageMap()->m_nXRes * pDevice->GetImageMap()->m_nYRes, 16);
		pOrigDm = new XnVDepthMap();
		pOrigDm->Initialize(*pDevice->GetDepthMap(), pDevice->GetDepthMap()->m_nXRes, pDevice->GetDepthMap()->m_nYRes);
		pBackgroundRemover = new XnVBackgroundRemover(pDevice->GetStreamDetails()->m_StreamProperties);

		
	}

	if (DepthMode == 1)
	{
		pDevice->SetIOParam(XN_IO_PARAM_RGB_REGISTRAR_STATE, XN_IO_REGMODE_DEPTH_TO_IMAGE);
		Image.PITCH = MaxDepth.PITCH = MaxDepth.SX = Image.SX = pDevice->GetDepthMap()->m_nXRes;
		MaxDepth.SY = Image.SY = pDevice->GetDepthMap()->m_nYRes;
		Image.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
		memset(Image.BASE , 0 , Image.SX * Image.SY * 4);
		MaxDepth.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
		memset(MaxDepth.BASE , 0 , MaxDepth.SX * MaxDepth.SY * 4);

		pZBUFFERMEDIA = (void *)this;


		/* Compute colorMAP */
		for (int i = 0 ;  i < DepthMAX ; i++)
		{
			ColorMap[i] = i & 0xff;
			if (i & 0x100)
			{
				ColorMap[i] = 0xff - ColorMap[i];
			}
			ColorMap[i] |= (i & 0xffF0)<<4;
/*			ColorMap[i] = i>>3;
			ColorMap[i] |= ColorMap[i]<<8;
			ColorMap[i] |= ColorMap[i]<<8;
			if (i) ColorMap[i] ^= 0xffffff;
			if (i & 0xf800) ColorMap[i] = 0;//*/
			ColorMap[i] = 0;
			if (i)
			{
				u32 OOD;
				OOD = (u32)(((float)DepthMAX * 4.0f/ (float)256.0f) * (float)DepthMAX / (float)i);
				//OOD /= DepthMAX;
				ColorMap[i] = (OOD & 0x20)<<2;
				ColorMap[i] = OOD ;
				ColorMap[i] |= ColorMap[i]<<8;
				ColorMap[i] |= ColorMap[i]<<8;
			}
		}
	} else
	{
		Image.PITCH = Image.SX = pDevice->GetImageMap()->m_nXRes;
		Image.SY = pDevice->GetImageMap()->m_nYRes;
		Image.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
		memset(Image.BASE , 0 , Image.SX * Image.SY * 4);
	}



	return 1;
}

int		PRIMESENS_Grabber::END		(void)
{
	CC_free(MaxDepth.BASE);
	return 0;
}

INPUT_IO *PRIMESENS_CREATE(WORLD *pScene,char *Filename)
{
	INPUT_IO *Ret;
	Ret = (INPUT_IO *)new(PRIMESENS_Grabber);
	strcpy((char*)Ret->MediaName,Filename);
	Ret->dTotalTime = 0;
	Ret->dAvgFramePerSecond = 30.0;
	if (Upper(Filename[0]) == 'N')
		((PRIMESENS_Grabber*)Ret)->DepthMode = 3;
	else
	if (Upper(Filename[0]) == 'S')
		((PRIMESENS_Grabber*)Ret)->DepthMode = 2;
	else
	if (Upper(Filename[0]) == 'D')
		((PRIMESENS_Grabber*)Ret)->DepthMode = 1;
	else
		((PRIMESENS_Grabber*)Ret)->DepthMode = 0;
	if (!Ret->INIT(pScene))
	{
		delete(Ret);
		return NULL;
	}


	return (INPUT_IO *)Ret;
};

void PRIMESENS_SetTrackingWindow(Vector *Min, Vector *Max,u32 Flag)
{
	TRK3DW_XMin = Min->x;
	TRK3DW_YMin = Min->y;
	TRK3DW_ZMin = Min->z;
	TRK3DW_XMax = Max->x;
	TRK3DW_YMax = Max->y;
	TRK3DW_ZMax = Max->z;
	TRK3DW_Flags = Flag;
}

#endif