#include "ofxHandGenerator.h"
#include "ofxOpenNIMacros.h"
#include "ofxTrackedHand.h"

// ctor
//--------------------------------------------------------------
ofxHandGenerator::ofxHandGenerator(){
	// set defaults
	setMinDistBetweenHands(100);
	setMaxNumHands(2);
	setSmoothing(1);
	setMinTimeBetweenHands(1000);
}

// dtor
//--------------------------------------------------------------
ofxHandGenerator::~ofxHandGenerator()
{
	// TODO: Unregister callbacks on shutdown
	tracked_hands.clear();
	//removeGestures();
	hands_generator.Unref();
}

//--------------------------------------------------------------
// GESTURE EVENT LISTENER CALLBACK
//--------------------------------------------------------------
void ofxHandGenerator::gestureRecognized(gesture & last_gesture) {
	
	XnPoint3D handPos;
	bool startTracking = false;
	
	if (found_hands == 0) {		// if we don't have any hands lets try to track
		
		startTracking = true;
		
	} else {
		
		startTracking = true; // do negative test by looking for the distance between each existing hand
		
		for (int i = 0; i < max_hands; i++) {
			
			// get raw position of this hand
			handPos = tracked_hands[i]->rawPos;
			
			// calculate distance between End ID of gesture and this hand
			float distanceToHand = sqrt( pow( last_gesture.gesture_position.x-handPos.X, 2 ) +
										 pow( last_gesture.gesture_position.y-handPos.Y, 2 ) +
										 pow( last_gesture.gesture_position.z-handPos.Z, 2 ) );
			
			printf("Hand dist: %f pos: [%d, %d, %d]\n", distanceToHand, (int)handPos.X, (int)handPos.Y, (int)handPos.Z);
			
			// if the hand is within the min distance then don't track it
			if (distanceToHand < min_distance) {	
				startTracking = false;
				continue;
			}
		}
	}
	
	if (startTracking) {
		handPos.X = last_gesture.gesture_position.x;
		handPos.Y = last_gesture.gesture_position.y;
		handPos.Z = last_gesture.gesture_position.z;
		
		startHandTracking((const XnPoint3D*)&handPos);
	}
}

// HANDS CALLBACKS
// =============================================================================
// Callback: New Hand was detected
void XN_CALLBACK_TYPE HandCreate(
	xn::HandsGenerator& generator, 
	XnUserID nID, 
	const XnPoint3D* pPosition, 
	XnFloat fTime, 
	void* pCookie
) 
{
	printf("New Hand: %d\n", nID);
	ofxHandGenerator* gest = static_cast<ofxHandGenerator*>(pCookie);
	gest->newHand(nID, pPosition);
}

// Callback: A Hand has moved
void XN_CALLBACK_TYPE HandUpdate(
	 xn::HandsGenerator& generator, 
	 XnUserID nID, 
	 const XnPoint3D* pPosition, 
	 XnFloat fTime, void* 
	 pCookie
)
{
	//printf("Hand update %d\n", user);
	ofxHandGenerator* gest = static_cast<ofxHandGenerator*>(pCookie);
	gest->updateHand(nID, pPosition);
}

// Callback: A Hand was lost :(
void XN_CALLBACK_TYPE HandDestroy(
	xn::HandsGenerator& generator, 
	XnUserID nID, 
	XnFloat fTime, 
	void* pCookie
)
{
	printf("Hand Lost: %d\n", nID);
	ofxHandGenerator* gest = static_cast<ofxHandGenerator*>(pCookie);
	gest->destroyHand(nID);
}

//--------------------------------------------------------------
bool ofxHandGenerator::setup(ofxOpenNIContext* pContext) {
	
	context = pContext;
	context->getDepthGenerator(&depth_generator);
	
	XnStatus result = XN_STATUS_OK;	
	
	// setup a gesture generator using our ofxGestureGenerator wrapper
	gesture_generator.setup(context);
	ofAddListener(gesture_generator.gestureRecognized, this, &ofxHandGenerator::gestureRecognized); 
	
	// Try to fetch hands generator before creating one
	if(pContext->getHandsGenerator(&hands_generator)) {
		// found the hands generator - for do nothing		
	} else {
		result = hands_generator.Create(context->getXnContext());
		CHECK_RC(result, "Creating hand generator");
		hands_generator.StartGenerating();
	}
	
	XnCallbackHandle hand_cb_handle;
	hands_generator.RegisterHandCallbacks(HandCreate, HandUpdate, HandDestroy, this, hand_cb_handle);
	
	printf("Hands generator inited\n");
	
	// pre-generate the tracked users.
	tracked_hands.reserve(max_hands);
	for(int i = 0; i < max_hands; ++i) {
		printf("Creating hand: %d", i);
		ofxTrackedHand* hand = new ofxTrackedHand(context);
		tracked_hands.push_back(hand);
	}
	
	found_hands = 0;
	
	isFiltering = false;

	// Start looking for gestures
	this->addGestures();

	return true;
}

//--------------------------------------------------------------
void ofxHandGenerator::addGestures() {
	gesture_generator.addGesture("Click");
	gesture_generator.addGesture("Wave");
	gesture_generator.addGesture("RaiseHand");
}

//--------------------------------------------------------------
void ofxHandGenerator::removeGestures() {
	gesture_generator.removeGesture("Wave");
	gesture_generator.removeGesture("Click");
	gesture_generator.removeGesture("RaiseHand");

}

//--------------------------------------------------------------
int ofxHandGenerator::getNumTrackedHands() {
	return found_hands;
}

//--------------------------------------------------------------
void ofxHandGenerator::setSmoothing(float smooth) {
	if (smooth > 0.0f && smooth <= 1.0f) {
		smoothing_factor = smooth;
		if (hands_generator.IsValid()) {
			hands_generator.SetSmoothing(smooth);
		}
	}
}

//--------------------------------------------------------------
float ofxHandGenerator::getSmoothing() {
	return smoothing_factor;
}

//--------------------------------------------------------------
void ofxHandGenerator::setMaxNumHands(int number) {
	if (number > 0) max_hands = number; // do we want a maximum?
}

//--------------------------------------------------------------
int ofxHandGenerator::getMaxNumHands() {
	return max_hands;
}

//--------------------------------------------------------------
void ofxHandGenerator::setMinDistBetweenHands(int distance) {
	if (distance > 0) min_distance = distance; // do we want a minimum?
}

//--------------------------------------------------------------
int ofxHandGenerator::getMinDistBetweenHands() {
	return min_distance;
}

//--------------------------------------------------------------
void ofxHandGenerator::setMinTimeBetweenHands(int time) {
	if (time > 0) {
		min_time = time;
		gesture_generator.setMinTimeBetweenGestures(min_time);
	}
}

//--------------------------------------------------------------
int ofxHandGenerator::getMinTimeBetweenHands() {
	return min_time;
}

//--------------------------------------------------------------
void ofxHandGenerator::setFilterFactors(float filter) {
	// set all filter factors to the same number
	for(int i = 0;  i < max_hands; ++i) 
		setFilterFactor(filter, i);
}

//--------------------------------------------------------------
void ofxHandGenerator::setFilterFactor(float filter, int thIndex) {
	// set one filter factor to a number
	ofxTrackedHand *hand = tracked_hands[thIndex];
	hand->setFilterFactor(filter);
}

// Get hand at nID TODO: switch from vector to array & make the look up faster (ie., direct to ID rather than cycling through IDs)
//--------------------------------------------------------------
ofxTrackedHand* ofxHandGenerator::getHand(int thIndex)
{
	// Return thIndex tracked hand
	ofxTrackedHand *hand = tracked_hands[thIndex]; // here thIndex refers to index not the actually HandGenerators nID which seems to count forever....
	if (hand->isBeingTracked) {
		return hand;
	}
	return NULL;
}

// Draw all hands
//--------------------------------------------------------------
void ofxHandGenerator::drawHands() {
	for(int i = 0;  i < max_hands; ++i)
		drawHand(i);
}

// Draw one hand
//--------------------------------------------------------------
void ofxHandGenerator::drawHand(int thIndex) {

	ofxTrackedHand *hand = tracked_hands[thIndex]; // here thIndex refers to index not the actually HandGenerators nID which seems to count forever....
	if (hand->isBeingTracked) hand->draw();

}

// Drop all hands
//--------------------------------------------------------------
void ofxHandGenerator::dropHands() {
	hands_generator.StopTrackingAll();
}

//--------------------------------------------------------------
// CALLBACK PIVATES
//--------------------------------------------------------------
void ofxHandGenerator::startHandTracking(const XnPoint3D* pPosition) {
	hands_generator.StartTracking(*pPosition);
}

//--------------------------------------------------------------
void ofxHandGenerator::newHand(XnUserID nID, const XnPoint3D* pPosition)
{
	// Look for a free hand
	for(int i = 0;  i < max_hands; ++i) {
		ofxTrackedHand *hand = tracked_hands[i];
		if (hand->isBeingTracked == false)
		{
			hand->isBeingTracked = true;
			hand->nID = nID;
			hand->update(pPosition, isFiltering, true);
			found_hands++;
			// Stop getting gestures?
			//if (found_hands >= MAX_NUMBER_HANDS)
			//	this->removeGestures();
			return;
		}
	}
}
//--------------------------------------------------------------
void ofxHandGenerator::updateHand(XnUserID nID, const XnPoint3D* pPosition)
{
	for(int i = 0;  i < max_hands; ++i) {
		ofxTrackedHand *hand = tracked_hands[i];
		if (hand->nID == nID)
		{
			if (hand->isBeingTracked)
				hand->update(pPosition, isFiltering);
			return;
		}
	}
}
//--------------------------------------------------------------
void ofxHandGenerator::destroyHand(XnUserID nID)
{
	for(int i = 0;  i < max_hands; ++i) {
		ofxTrackedHand *hand = tracked_hands[i];
		if (hand->nID == nID) 
		{
			// reset position to all 0's -> is this dodgy?
			hand->rawPos.X = 0;
			hand->rawPos.Y = 0;
			hand->rawPos.Z = 0;
			
			hand->isBeingTracked = false;
			found_hands--;
			// Resume grabbing gestures ?
			//this->addGestures();
			return;
		}
	}
}
