#include <stdio.h>
#include <XnVNite.h>

#include "KinectTrack.h"

#define N_COLORS 6

XnVPointDrawer::XnVPointDrawer(XnUInt32 nHistory, xn::DepthGenerator depthGenerator) :
	XnVPointControl("XnVPointDrawer"),
	mNHistorySize(nHistory), mDepthGenerator(depthGenerator), mBDrawDM(false), mBFrameID(false) {
	mPFPositionBuffer = new XnFloat[nHistory*3];
}

XnVPointDrawer::~XnVPointDrawer() {
	std::map<XnUInt32, std::list<XnPoint3D> >::iterator iter;
	for (iter = mHistory.begin(); iter != mHistory.end(); ++iter)
	{
		iter->second.clear();
	}
	mHistory.clear();

	delete []mPFPositionBuffer;
}

void XnVPointDrawer::setDepthMap(XnBool bDrawDM) {
	mBDrawDM = bDrawDM;
}

void XnVPointDrawer::Update(XnVMessage* pMessage) {
	// PointControl's Update calls all callbacks for each hand
	XnVPointControl::Update(pMessage);

	if (mBDrawDM)
	{
		// Draw depth map
		xn::DepthMetaData depthMD;
		mDepthGenerator.GetMetaData(depthMD);
		//drawDepthMap(depthMD);
	}
	if (mBFrameID)
	{
		// Print out frame ID
		xn::DepthMetaData depthMD;
		mDepthGenerator.GetMetaData(depthMD);
		//DrawFrameID(depthMD.FrameID());
	}

	// Draw hands
	Draw();
}

int XnVPointDrawer::getPosition(float &x, float &y, float &z) const {
	std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator itr;
	itr = mHistory.begin();
	if (itr == mHistory.end()) return -1;
	std::list<XnPoint3D>::const_iterator itr2;
	itr2 = itr->second.begin();
	if (itr2 == itr->second.end()) return -1;
	XnPoint3D pt(*itr2);
	x = pt.X;
	y = pt.Y;
	z = pt.Z;
	return 1;
}
void XnVPointDrawer::Draw() const {
	std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator itr;
	itr = mHistory.begin();
	if (itr == mHistory.end()) return;
	std::list<XnPoint3D>::const_iterator itr2;
	itr2 = itr->second.begin();
	if (itr2 == itr->second.end()) return;
	XnPoint3D pt(*itr2);
//	printf("%f, %f, %f\n", pt.X, pt.Y, pt.Z);
	

	// std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator PointIterator;
	
	// // Go over each existing hand
	// for (PointIterator = mHistory.begin();
	// 	PointIterator != mHistory.end();
	// 	++PointIterator)
	// {
	// 	// Clear buffer
	// 	XnUInt32 nPoints = 0;
	// 	XnUInt32 i = 0;
	// 	XnUInt32 Id = PointIterator->first;

	// 	// Go over all previous positions of current hand
	// 	std::list<XnPoint3D>::const_iterator PositionIterator;
	// 	for (PositionIterator = PointIterator->second.begin();
	// 		PositionIterator != PointIterator->second.end();
	// 		++PositionIterator, ++i)
	// 	{
	// 		// Add position to buffer
	// 		XnPoint3D pt(*PositionIterator);
	// 		printf("%f, %f, %f\n", pt.X, pt.Y, pt.Z);
	// 		mPFPositionBuffer[3*i] = pt.X;
	// 		mPFPositionBuffer[3*i + 1] = pt.Y;
	// 		mPFPositionBuffer[3*i + 2] = 0;//pt.Z();
	// 		break;
	// 	}
		
	// 	// Set color
	// 	XnUInt32 nColor = Id % N_COLORS;
	// 	//XnUInt32 nSingle = GetPrimaryID();
	// 	if (Id == GetPrimaryID())
	// 		nColor = 6;
	// }
}

static XnBool bShouldPrint = false;
void XnVPointDrawer::OnPointCreate(const XnVHandPointContext* cxt)
{
	printf("** %d\n", cxt->nID);
	// Create entry for the hand
	mHistory[cxt->nID].clear();
	bShouldPrint = true;
	OnPointUpdate(cxt);
	bShouldPrint = true;
}
// Handle new position of an existing hand
void XnVPointDrawer::OnPointUpdate(const XnVHandPointContext* cxt)
{
	// positions are kept in projective coordinates, since they are only used for drawing
	XnPoint3D ptProjective(cxt->ptPosition);

	if (bShouldPrint)printf("Point (%f,%f,%f)", ptProjective.X, ptProjective.Y, ptProjective.Z);
	mDepthGenerator.ConvertRealWorldToProjective(1, &ptProjective, &ptProjective);
	if (bShouldPrint)printf(" -> (%f,%f,%f)\n", ptProjective.X, ptProjective.Y, ptProjective.Z);

	// Add new position to the history buffer
	mHistory[cxt->nID].push_front(ptProjective);
	// Keep size of history buffer
	if (mHistory[cxt->nID].size() > mNHistorySize)
		mHistory[cxt->nID].pop_back();
	bShouldPrint = false;
}

// Handle destruction of an existing hand
void XnVPointDrawer::OnPointDestroy(XnUInt32 nID)
{
	// No need for the history buffer
	mHistory.erase(nID);
}