/*****************************************************************************
*                                                                            *
*  OpenNI 1.0 Alpha                                                          *
*  Copyright (C) 2010 PrimeSense Ltd.                                        *
*                                                                            *
*  This file is part of OpenNI.                                              *
*                                                                            *
*  OpenNI is free software: you can redistribute it and/or modify            *
*  it under the terms of the GNU Lesser General Public License as published  *
*  by the Free Software Foundation, either version 3 of the License, or      *
*  (at your option) any later version.                                       *
*                                                                            *
*  OpenNI is distributed in the hope that it will be useful,                 *
*  but WITHOUT ANY WARRANTY; without even the implied warranty of            *
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the              *
*  GNU Lesser General Public License for more details.                       *
*                                                                            *
*  You should have received a copy of the GNU Lesser General Public License  *
*  along with OpenNI. If not, see <http://www.gnu.org/licenses/>.            *
*                                                                            *
*****************************************************************************/




//---------------------------------------------------------------------------
// Includes
//---------------------------------------------------------------------------
#include "SceneDrawer.h"
#include <Windows.h>
#include <cmath>
bool KeyIsPressed ( unsigned char k ) {
	USHORT status = GetAsyncKeyState ( k );
	return (( ( status & 0x8000 ) >> 15 ) == 1) || (( status & 1 ) == 1);
}

#include "GL/glut.h"
#include <time.h>





clock_t time_endhandcalibrate;
bool finishedhandcali = false;
bool startedhandcali = false;

clock_t time_endforwardcalibrate;
bool startedforwardcali = false;
bool finishedforwardcali = false;

clock_t time_endbackcalibrate;
bool startedbackcali = false;
bool finishedbackcali = false;

clock_t time_endstandcalibrate;
bool startedstandcali = false;
bool finishedstandcali = false;

clock_t time_endleftcalibrate;
bool startedleftcali = false;
bool finishedleftcali = false;

clock_t time_endrightcalibrate;
bool startedrightcali = false;
bool finishedrightcali = false;

clock_t time_endinventorycalibrate;
bool startedinventorycali = false;
bool finishedinventorycali = false;

XnPoint3D lefthand_inventorypos;
XnPoint3D righthand_inventorypos;

XnFloat punchdist_lefthand;
XnFloat punchdist_righthand;

XnFloat leanbackdist;
XnFloat leanforwarddist;

XnFloat leanrightdist;
XnFloat leanleftdist;

XnFloat avgX = 0;
XnFloat avgY = 0;
XnFloat avgZ = 0;
XnFloat headYstart = 0;

XnFloat rightXstart = 0;

XnFloat leftXstart = 0;

XnFloat rightYstart = 0;

bool lastleftmb = false, lastrightmb = false;

bool lastinventorypress = false;
bool lastscrollpress = false;

bool lastjumppress = false;

int currentscrollposition = 1;

bool lastforward = false, lastback = false;

bool lastleft = false, lastright = false;

extern void glOrthoBegin();

extern void glOrthoEnd();

extern xn::UserGenerator g_UserGenerator;
extern xn::DepthGenerator g_DepthGenerator;

extern XnBool g_bDrawBackground;
extern XnBool g_bDrawPixels;
extern XnBool g_bDrawSkeleton;
extern XnBool g_bPrintID;
extern XnBool g_bPrintState;
extern bool g_spacekeydown;

#define MAX_DEPTH 10000
float g_pDepthHist[MAX_DEPTH];
unsigned int getClosestPowerOfTwo(unsigned int n)
{
	unsigned int m = 2;
	while(m < n) m<<=1;

	return m;
}
GLuint initTexture(void** buf, int& width, int& height)
{
	GLuint texID = 0;
	glGenTextures(1,&texID);

	width = getClosestPowerOfTwo(width);
	height = getClosestPowerOfTwo(height); 
	*buf = new unsigned char[width*height*4];
	glBindTexture(GL_TEXTURE_2D,texID);

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

	return texID;
}

GLfloat texcoords[8];
void DrawRectangle(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	GLfloat verts[8] = {	topLeftX, topLeftY,
		topLeftX, bottomRightY,
		bottomRightX, bottomRightY,
		bottomRightX, topLeftY
	};
	glVertexPointer(2, GL_FLOAT, 0, verts);
	glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

	//TODO: Maybe glFinish needed here instead - if there's some bad graphics crap
	glFlush();
}
void DrawTexture(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	glTexCoordPointer(2, GL_FLOAT, 0, texcoords);

	DrawRectangle(topLeftX, topLeftY, bottomRightX, bottomRightY);

	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}

XnFloat Colors[][3] =
{
	{0,1,1},
	{0,0,1},
	{0,1,0},
	{1,1,0},
	{1,0,0},
	{1,.5,0},
	{.5,1,0},
	{0,.5,1},
	{.5,0,1},
	{1,1,.5},
	{1,1,1}
};
XnUInt32 nColors = 10;

void glPrintString(void *font, char *str)
{
	int i,l = strlen(str);

	for(i=0; i<l; i++)
	{
		glutBitmapCharacter(font,*str++);
	}
}

void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2, bool ortho = false)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;
	pt[1] = joint2.position;

	if (!ortho)
		{
		//g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
		glVertex3i(pt[0].X/32, pt[0].Y/32, (pt[0].Z-avgZ)/32);
		glVertex3i(pt[1].X/32, pt[1].Y/32, (pt[1].Z-avgZ)/32);
		}
	else
		{
		g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
		glVertex3i(640-pt[0].X, pt[0].Y+32, 0);
		glVertex3i(640-pt[1].X, pt[1].Y+32, 0);
		}
}

XnFloat getJointPointX(XnUserID player, XnSkeletonJoint eJoint)
	{
	XnSkeletonJointPosition jnt;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, jnt);
	if (jnt.fConfidence > 0.5)
		return jnt.position.X;
	return 123456;
	}

XnFloat getJointPointY(XnUserID player, XnSkeletonJoint eJoint)
	{
	XnSkeletonJointPosition jnt;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, jnt);
	if (jnt.fConfidence > 0.5)
		return jnt.position.Y;
	return 123456;
	}

XnFloat getJointPointZ(XnUserID player, XnSkeletonJoint eJoint)
	{
	XnSkeletonJointPosition jnt;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, jnt);
	if (jnt.fConfidence > 0.5)
		return jnt.position.Z;
	return 123456;
	}

XnFloat getJointDist(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
	{
	XnSkeletonJointPosition jnt1;
	XnSkeletonJointPosition jnt2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, jnt1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, jnt2);
	if (jnt1.fConfidence < 0.5 || jnt1.fConfidence < 0.5)
		return 123456;
	XnFloat xx = jnt1.position.X-jnt2.position.X;
	XnFloat yy = jnt1.position.Y-jnt2.position.Y;
	XnFloat zz = jnt1.position.Z-jnt2.position.Z;
	return sqrt(xx*xx+yy*yy+zz*zz);
	}

XnFloat getJointDistToPoint(XnUserID player, XnSkeletonJoint eJoint1, XnPoint3D pnt)
	{
	XnSkeletonJointPosition jnt1;
	XnSkeletonJointPosition jnt2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, jnt1);
	if (jnt1.fConfidence < 0.5)
		return 123456;
	XnFloat xx = jnt1.position.X-pnt.X;
	XnFloat yy = jnt1.position.Y-pnt.Y;
	XnFloat zz = jnt1.position.Z-pnt.Z;
	return sqrt(xx*xx+yy*yy+zz*zz);
	}

void PressKey(unsigned char key)
	{
	INPUT Input={0};
	Input.type = INPUT_KEYBOARD;
	Input.ki.wVk = key;
	SendInput( 1, &Input, sizeof(INPUT) );	
	}
void ReleaseKey(unsigned char key)
	{
	INPUT Input={0};
	Input.type = INPUT_KEYBOARD;
	Input.ki.wVk = key;
	Input.ki.dwFlags = KEYEVENTF_KEYUP;
	SendInput( 1, &Input, sizeof(INPUT) );	
	}

void MoveMouse(int dx, int dy)
	{
	INPUT Input={0};
	Input.type = INPUT_MOUSE;
	Input.mi.dx = dx;
	Input.mi.dy = dy;
	Input.mi.dwFlags = MOUSEEVENTF_MOVE;
	SendInput( 1, &Input, sizeof(INPUT) );	
	}

void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, const xn::ImageMetaData& imd)
{
	glOrthoBegin();
	static bool bInitialized = false;	
	static GLuint depthTexID;
	static GLuint imageTexID;
	static unsigned char* pDepthTexBuf;
	static unsigned char* pImageTexBuf;
	static int texWidth, texHeight;
	static int imgtexWidth, imgtexHeight;
	float topLeftX;
	float topLeftY;
	float bottomRightY;
	float bottomRightX;
	float texXpos;
	float texYpos;

	if (!bInitialized)
		{
		texWidth =  getClosestPowerOfTwo(dmd.XRes());
		texHeight = getClosestPowerOfTwo(dmd.YRes());
		imgtexWidth =  getClosestPowerOfTwo(imd.XRes());
		imgtexHeight = getClosestPowerOfTwo(imd.YRes());
		depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight);
		imageTexID = initTexture((void**)&pImageTexBuf,imgtexWidth,imgtexHeight);
		//memset(texcoords, 0, 8*sizeof(float));
		//texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
		bInitialized = true;
		}
	unsigned int nValue = 0;
	unsigned int nHistValue = 0;
	unsigned int nIndex = 0;
	unsigned int nX = 0;
	unsigned int nY = 0;
	unsigned int nNumberOfPoints = 0;
	XnUInt16 g_nXRes = dmd.XRes();
	XnUInt16 g_nYRes = dmd.YRes();
	XnUInt16 imgXRes = imd.XRes();
	XnUInt16 imgYRes = imd.YRes();

	unsigned char* pDestImage = pDepthTexBuf;
	const XnDepthPixel* pDepth = dmd.Data();
	const XnLabel* pLabels = smd.Data();
	const XnUInt8* pImage = imd.Data();

	// Calculate the accumulative histogram
	memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
	for (nY=0; nY<g_nYRes; nY++)
	{
		for (nX=0; nX<g_nXRes; nX++)
		{
			nValue = *pDepth;

			if (nValue != 0)
			{
				g_pDepthHist[nValue]++;
				nNumberOfPoints++;
			}

			pDepth++;
		}
	}

	for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}
	if (nNumberOfPoints)
	{
		for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
		}
	}

	pDepth = dmd.Data();
	if (g_bDrawPixels)
	{
		XnUInt32 nIndex = 0;
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++, nIndex++)
			{

				pDestImage[0] = 0;
				pDestImage[1] = 0;
				pDestImage[2] = 0;
				if (g_bDrawBackground || *pLabels != 0)
				{
					nValue = *pDepth;
					XnLabel label = *pLabels;
					XnUInt32 nColorID = label % nColors;
					if (label == 0)
					{
						nColorID = nColors;
					}

					if (nValue != 0)
					{
						nHistValue = g_pDepthHist[nValue];

						pDestImage[0] = nHistValue * Colors[nColorID][0]; 
						pDestImage[1] = nHistValue * Colors[nColorID][1];
						pDestImage[2] = nHistValue * Colors[nColorID][2];
					}
				}

				pDepth++;
				pLabels++;
				pDestImage+=3;
			}

			pDestImage += (texWidth - g_nXRes) *3;
		}
	}
	else
	{
		xnOSMemSet(pImageTexBuf, 0, imgXRes*imgYRes*4*3);
		xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
	}
	glColor4f(1,1,1,1);
	glEnable(GL_TEXTURE_2D);

	glBindTexture(GL_TEXTURE_2D, imageTexID);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImage);
	glBegin(GL_QUADS);
	glTexCoord2f(0,0);
	glVertex2f(640,0);
	glTexCoord2f(1,0);
	glVertex2f(0,0);
	glTexCoord2f(1,1);
	glVertex2f(0,480);
	glTexCoord2f(0,1);
	glVertex2f(640,480);
	glEnd();

	glBindTexture(GL_TEXTURE_2D, depthTexID);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
	glBegin(GL_QUADS);
	glTexCoord2f(0,0);
	glVertex2f(640,360);
	glTexCoord2f(640.0f/1024.0f,0);
	glVertex2f(480,360);
	glTexCoord2f(640.0f/1024.0f,480.0f/512.0f);
	glVertex2f(480,480);
	glTexCoord2f(0,480.0f/512.0f);
	glVertex2f(640,480);
	glEnd();

	glDisable(GL_TEXTURE_2D);

	char strLabel[128] = "";
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		if (g_bPrintID)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(aUsers[i], com);
			g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

			xnOSMemSet(strLabel, 0, sizeof(strLabel));
			if (!g_bPrintState)
			{
				// Tracking
				sprintf(strLabel, "%d", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
			{
			// Standing Calibration
			//if (!finishedstandcali)
			//	{
				if (!startedstandcali)
					{
					time_endstandcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedstandcali = true;
					}
				else if (!finishedstandcali)
					{
					if (clock() >= time_endstandcalibrate)
						{
						avgX = (getJointPointX(aUsers[i],XN_SKEL_LEFT_SHOULDER)+getJointPointX(aUsers[i],XN_SKEL_RIGHT_HIP))/2;
						avgY = getJointPointY(aUsers[i],XN_SKEL_HEAD);
						avgZ = (getJointPointZ(aUsers[i],XN_SKEL_LEFT_SHOULDER)+getJointPointZ(aUsers[i],XN_SKEL_RIGHT_HIP))/2;
						finishedstandcali = true;
						}
					}
			//	}
			// Hand/Punching Calibration
			if (finishedstandcali)
				{
				if (!startedhandcali)
					{
					time_endhandcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedhandcali = true;
					}
				else if (!finishedhandcali)
					{
					if (clock() >= time_endhandcalibrate)
						{
						XnFloat lefthandz = getJointPointZ(aUsers[i],XN_SKEL_LEFT_HAND);
						XnFloat righthandz = getJointPointZ(aUsers[i],XN_SKEL_RIGHT_HAND);
						punchdist_lefthand = (avgZ-lefthandz);
						punchdist_righthand = (avgZ-righthandz);
						leftXstart = getJointPointX(aUsers[i],XN_SKEL_LEFT_HAND);
						rightXstart = getJointPointX(aUsers[i],XN_SKEL_RIGHT_HAND);
						rightYstart = getJointPointY(aUsers[i],XN_SKEL_RIGHT_HAND);
						finishedhandcali = true;
						}
					}
				}
			// Leaning Forward Calibration
			if (finishedhandcali)
				{
				if (!startedforwardcali)
					{
					time_endforwardcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedforwardcali = true;
					}
				else if (!finishedforwardcali)
					{
					if (clock() >= time_endforwardcalibrate)
						{
						XnFloat headz = getJointPointZ(aUsers[i],XN_SKEL_HEAD);
						leanforwarddist = avgZ-headz;
						finishedforwardcali = true;
						}
					}
				}
			// Leaning Back Calibration
			if (finishedforwardcali)
				{
				if (!startedbackcali)
					{
					time_endbackcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedbackcali = true;
					}
				else if (!finishedbackcali)
					{
					if (clock() >= time_endbackcalibrate)
						{
						XnFloat headz = getJointPointZ(aUsers[i],XN_SKEL_HEAD);
						leanbackdist = headz-avgZ;
						finishedbackcali = true;
						}
					}
				}
			// Leaning Left Calibration
			if (finishedbackcali)
				{
				if (!startedleftcali)
					{
					time_endleftcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedleftcali = true;
					}
				else if (!finishedleftcali)
					{
					if (clock() >= time_endleftcalibrate)
						{
						XnFloat headx = getJointPointX(aUsers[i],XN_SKEL_HEAD);
						leanleftdist = headx-avgX;
						finishedleftcali = true;
						}
					}
				}
			// Leaning Right Calibration
			if (finishedleftcali)
				{
				if (!startedrightcali)
					{
					time_endrightcalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedrightcali = true;
					}
				else if (!finishedrightcali)
					{
					if (clock() >= time_endrightcalibrate)
						{
						XnFloat headx = getJointPointX(aUsers[i],XN_SKEL_HEAD);
						leanrightdist = headx-avgX;
						finishedrightcali = true;
						}
					}
				}
			// Inventory Cali
			if (finishedrightcali)
				{
				if (!startedinventorycali)
					{
					time_endinventorycalibrate = clock() + 5*CLOCKS_PER_SEC;
					startedinventorycali = true;
					}
				else if (!finishedinventorycali)
					{
					if (clock() >= time_endinventorycalibrate)
						{
						XnSkeletonJointPosition jnt1;
						g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, jnt1);
						lefthand_inventorypos = jnt1.position;
						g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, jnt1);
						righthand_inventorypos = jnt1.position;
						finishedinventorycali = true;
						}
					}
				}
			if (finishedinventorycali)
				{
				//bool lefthand_abovehead = (getJointPointY(aUsers[i],XN_SKEL_LEFT_HAND) > getJointPointY(aUsers[i],XN_SKEL_HEAD));
				bool leftp = (getJointPointX(aUsers[i],XN_SKEL_LEFT_HAND) < leftXstart-20 && getJointPointZ(aUsers[i],XN_SKEL_LEFT_HAND) < avgZ-punchdist_lefthand*.5 && getJointDist(aUsers[i],XN_SKEL_LEFT_HAND,XN_SKEL_LEFT_SHOULDER) > punchdist_lefthand*.8);
				bool rightp = (getJointPointX(aUsers[i],XN_SKEL_LEFT_HAND) > leftXstart+20 && getJointPointZ(aUsers[i],XN_SKEL_LEFT_HAND) < avgZ-punchdist_lefthand*.5 && getJointDist(aUsers[i],XN_SKEL_LEFT_HAND,XN_SKEL_LEFT_SHOULDER) > punchdist_lefthand*.8);
				bool moveright = (getJointPointX(aUsers[i],XN_SKEL_HEAD) > avgX+leanleftdist*.65);
				bool moveleft = (getJointPointX(aUsers[i],XN_SKEL_HEAD) < avgX+leanrightdist*.65);
				bool moveforward = (getJointPointZ(aUsers[i],XN_SKEL_HEAD) < avgZ-leanforwarddist*.65);
				bool moveback = (getJointPointZ(aUsers[i],XN_SKEL_HEAD) > avgZ+leanbackdist*.65);
				bool scrollpress = (getJointDistToPoint(aUsers[i],XN_SKEL_LEFT_HAND,lefthand_inventorypos) < 340 && getJointDistToPoint(aUsers[i],XN_SKEL_RIGHT_HAND,righthand_inventorypos) < 340);
				bool jumppress = (getJointPointY(aUsers[i],XN_SKEL_HEAD) < avgY-60);
				XnFloat heady = getJointPointY(aUsers[i],XN_SKEL_HEAD);
				bool inventorypress = (getJointDist(aUsers[i],XN_SKEL_LEFT_HAND,XN_SKEL_RIGHT_HAND) < 140);
				if (leftp && rightp)
					sprintf(strLabel,"Both hand punch!");
				else if (leftp)
					sprintf(strLabel,"\nLeft hand punch!\n");
				else if (rightp)
					sprintf(strLabel,"\nRight hand punch!\n");
				HWND target = GetForegroundWindow();
				POINT pt;
				RECT wrect;
				GetCursorPos ( &pt );
				GetClientRect ( target, &wrect );
				if (!scrollpress)
					{
					if (leftp && !lastleftmb)
						{
						//PostMessage ( target, WM_LBUTTONDOWN, 0, ( pt.x - wrect.left ) & ( ( pt.y - wrect.top ) << 16 ) );
						INPUT Input={0};
						Input.type = INPUT_MOUSE;
						Input.mi.dwFlags = MOUSEEVENTF_LEFTDOWN;
						SendInput( 1, &Input, sizeof(INPUT) );	
						}
					if (!leftp && lastleftmb)
						{
						//PostMessage ( target, WM_LBUTTONUP, 0, ( pt.x - wrect.left ) & ( ( pt.y - wrect.top ) << 16 ) );
						INPUT Input={0};
						Input.type = INPUT_MOUSE;
						Input.mi.dwFlags = MOUSEEVENTF_LEFTUP;
						SendInput( 1, &Input, sizeof(INPUT) );	
						}				
					if (rightp && !lastrightmb)
						{
						INPUT Input={0};
						Input.type = INPUT_MOUSE;
						Input.mi.dwFlags = MOUSEEVENTF_RIGHTDOWN;
						SendInput( 1, &Input, sizeof(INPUT) );
						}
					if (!rightp && lastrightmb)
						{
						INPUT Input={0};
						Input.type = INPUT_MOUSE;
						Input.mi.dwFlags = MOUSEEVENTF_RIGHTUP;
						SendInput( 1, &Input, sizeof(INPUT) );	
						}
					}
				const unsigned char VK_W = 0x57, VK_A = 0x41, VK_S = 0x53, VK_D = 0x44, VK_I = 0x49;
				/*if (lefthand_abovehead)
					{
					ReleaseKey(VK_A);
					ReleaseKey(VK_W);
					ReleaseKey(VK_D);
					ReleaseKey(VK_S);*/
				if (!scrollpress)
					{
					if (getJointDist(aUsers[i],XN_SKEL_RIGHT_HAND,XN_SKEL_RIGHT_SHOULDER) > punchdist_righthand*.8)
						{
						XnFloat x1 = getJointPointX(aUsers[i],XN_SKEL_RIGHT_HAND);
						XnFloat y1 = getJointPointY(aUsers[i],XN_SKEL_RIGHT_HAND);
						XnFloat dx = (rightXstart-x1)/25;
						XnFloat dy = (rightYstart-y1)/25;
						MoveMouse(-dx,0);
						MoveMouse(0,dy);
						}
					}
				//else
				//	{
					if (moveleft && !lastleft)
						PressKey(VK_A);
					if (moveforward && !lastforward)
						PressKey(VK_W);
					if (moveright && !lastright)
						PressKey(VK_D);
					if (moveback && !lastback)
						PressKey(VK_S);
					if (inventorypress && !lastinventorypress)
						PressKey(VK_I);
					if (scrollpress && !lastscrollpress)
						{
						currentscrollposition += 1;
						if (currentscrollposition > 9)
							currentscrollposition = 1;
						unsigned char key = 0x30 + currentscrollposition;
						PressKey(key);
						}
					if (jumppress && !lastjumppress)
						PressKey(VK_SPACE);
	
					if (!moveleft && lastleft)
						ReleaseKey(VK_A);
					if (!moveforward && lastforward)
						ReleaseKey(VK_W);
					if (!moveright && lastright)
						ReleaseKey(VK_D);
					if (!moveback && lastback)
						ReleaseKey(VK_S);
					if (!inventorypress && lastinventorypress)
						ReleaseKey(VK_I);
					if (!scrollpress && lastscrollpress)
						{
						unsigned char key = 0x30 + currentscrollposition;
						ReleaseKey(key);
						}
					if (!jumppress && lastjumppress)
						ReleaseKey(VK_SPACE);
				//	}

				lastleftmb = leftp;
				lastrightmb = rightp;
				lastleft = moveleft;
				lastright = moveright;
				lastforward = moveforward;
				lastback = moveback;
				lastinventorypress = inventorypress;
				lastscrollpress = scrollpress;
				lastjumppress = jumppress;
				if (moveforward)
					{
					sprintf(strLabel,"MOVE FORWARD");
					}
				if (moveback)
					{
					sprintf(strLabel,"MOVE BACKWARDS");
					}
				if (moveright)
					{
					sprintf(strLabel,"TURN RIGHT");
					}
				if (moveleft)
					{
					sprintf(strLabel,"TURN LEFT");
					}
				if (moveleft && moveforward)
					{
					sprintf(strLabel,"TURN LEFT MOVE FORWARD");
					}
				if (moveleft && moveback)
					{
					sprintf(strLabel,"TURN LEFT MOVE BACK");
					}
				if (moveright && moveforward)
					{
					sprintf(strLabel,"TURN RIGHT MOVE FORWARD");
					}
				if (moveright && moveback)
					{
					sprintf(strLabel,"TURN RIGHT MOVE BACK");
					}
				if (moveright && moveleft)
					{
					sprintf(strLabel,"AAAAAAAAAAAAAA");
					}
				}
			if (startedstandcali && !finishedstandcali)
				{
				sprintf(strLabel,"Stand still. Do nothing. Time remaining: %ld", (time_endstandcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedhandcali && !finishedhandcali)
				{
				sprintf(strLabel,"Hold your arms out straight.\nCalibrating punch distance. Time remaining: %ld", (time_endhandcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedforwardcali && !finishedforwardcali)
				{
				sprintf(strLabel,"Lean FORWARD. Calibrating Lean. Time remaining: %ld", (time_endforwardcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedbackcali && !finishedbackcali)
				{
				sprintf(strLabel,"Lean BACK. Calibrating Lean. Time remaining: %ld", (time_endbackcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedleftcali && !finishedleftcali)
				{
				sprintf(strLabel,"Lean RIGHT. Calibrating Lean. Time remaining: %ld", (time_endleftcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedrightcali && !finishedrightcali)
				{
				sprintf(strLabel,"Lean LEFT. Calibrating Lean. Time remaining: %ld", (time_endrightcalibrate-clock())/CLOCKS_PER_SEC);
				}
			if (startedinventorycali && !finishedinventorycali)
				{
				sprintf(strLabel,"Stick your arms out horizontally. Time remaining: %ld", (time_endinventorycalibrate-clock())/CLOCKS_PER_SEC);
				}
			}
			else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
			{
				// Calibrating
				sprintf(strLabel, "%d - Calibrating...", aUsers[i]);
			}
			else
			{
				// Nothing
				sprintf(strLabel, "%d - Looking for pose", aUsers[i]);
			}


			glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
			//glOrthoBegin();
			glRasterPos2i(32,32);
			glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
			//glOrthoEnd();
		}
	if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
			glBegin(GL_LINES);
			glColor4f(1,1,1,1);
			//glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
			DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK,true);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER,true);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW,true);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND,true);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER,true);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW,true);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND,true);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO,true);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO,true);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP,true);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE,true);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT,true);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP,true);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE,true);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT,true);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP,true);
			glColor4f(1.0f,0.0f,0.0f,1.0f); // red should be lefter
			glVertex3i(avgX+leanleftdist*.75,0,0);
			glVertex3i(avgX+leanleftdist*.75,480,0);
			glColor4f(0.0f,1.0f,0.0f,1.0f); // green should be righter on the screen
			glVertex3i(avgX-leanrightdist*.75,0,0);
			glVertex3i(avgX-leanrightdist*.75,480,0);
			glEnd();
		}


	// Display the OpenGL texture map
	/*glColor4f(0.75,0.75,0.75,1);

	glEnable(GL_TEXTURE_2D);
	DrawTexture(dmd.XRes(),dmd.YRes(),320,240);	
	glDisable(GL_TEXTURE_2D);*/
	//delete pImageTexBuf;
	//delete pDepthTexBuf;
	//glDeleteTextures(1,&depthTexID);
	//glDeleteTextures(1,&imageTexID);
		glOrthoEnd();
/*
		glPushMatrix();
		glTranslatef(45,45,32);
		glRotatef(90,1,0,0);
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
			glBegin(GL_LINES);
			glColor4f(1,1,1,1);
			//glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
			DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);

			DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);

			DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);

			glEnd();
		}*/
	}
		glPopMatrix();

}
