#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "SceneGraphRoot.h"
#include "MainLoop.h"

#include <XnOpenNI.h>
#include <XnCppWrapper.h>
#include <XnVNite.h>

#include "kinect.h"

#define INIT_XML_PATH "./kinect.xml"

void checkRC(const XnStatus &rc, const char *what) {
	if (rc != XN_STATUS_OK) {
		printf("%s faild: %s\n", what, xnGetStatusString(rc));
	}
}

void checkErrors(XnStatus &rc, xn::EnumerationErrors &errors, const char *what) {
	if (rc == XN_STATUS_NO_NODE_PRESENT) {
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
	}
}

SessionState NIState::gSessionState = NOT_IN_SESSION;
XnBool NIState::gBDrawDepthMap = true;
xn::Context NIState::gContext;
XnVSessionManager *NIState::gPSessionManager;
XnVPointDrawer *NIState::gPDrawer;

void NIState::XN_CALLBACK_TYPE sessionStarting(const XnPoint3D &ptPosition, void *userCxt) {
	printf("Session start: (%f, %f, %f)\n", ptPosition.X,  ptPosition.Y,  ptPosition.Z);
	gSessionState = IN_SESSION;
}
void NIState::XN_CALLBACK_TYPE sessionEnding(void *userCxt) {
	printf("Session end\n");
	gSessionState = NOT_IN_SESSION;
}
void NIState::XN_CALLBACK_TYPE focusProgress(const XnChar *strFocus, const XnPoint3D &ptPosition,
										   XnFloat fProgress, void *userCxt) {
	printf("Focus progress: %s @(%f, %f, %f): %f\n", strFocus, ptPosition.X,  ptPosition.Y,  ptPosition.Z, fProgress);
}
void NIState::XN_CALLBACK_TYPE noHands(void *UserCxt) {
	if (gSessionState != NOT_IN_SESSION) {
		printf("Quick refocus\n");
		gSessionState = QUICK_REFOCUS;
	}
}

// prototype
static void ball_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h);
static void null_collision(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h, SceneGraphPtr tree);

static void
ball_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
	NIState::gContext.WaitNoneUpdateAll();
	NIState::gPSessionManager->Update(&NIState::gContext);
	float x, y, z;
	int error = NIState::gPDrawer->getPosition(x, y, z);
	if (error > 0) {
		node->xyz[0] = x/640 * screen_w;
		node->xyz[1] = y/480 * screen_h;
		node->xyz[2] = z - 500;
	}
}

static void
null_collision(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h,
			   SceneGraphPtr tree)
{
}

MainLoopPtr 
Kinect::init(Viewer *sgroot, int screen_w, int screen_h)
{
    SceneGraphPtr ball;

    srandom(100);

    sgroot->createFromXMLfile("xml_file/Ball.xml");

    ball = sgroot->createSceneGraph("Ball");
    ball->set_move_collision(ball_move, null_collision);

    ball->xyz[0] = screen_w/2;
    ball->xyz[1] = screen_h/2;
    ball->xyz[2] = 550.0f;

    sgroot->setSceneData(ball);

	xn::EnumerationErrors errors;
	XnStatus rc = NIState::gContext.InitFromXmlFile(INIT_XML_PATH, &errors);
	checkErrors(rc, errors, "InitFromXMLFile");
	checkRC(rc, "InitFromXMLFile");

	xn::DepthGenerator gDepthGenerator;
	rc = NIState::gContext.FindExistingNode(XN_NODE_TYPE_DEPTH, gDepthGenerator);
	checkRC(rc, "Find depth generator");

	xn::HandsGenerator gHandsGenerator;
	rc = NIState::gContext.FindExistingNode(XN_NODE_TYPE_HANDS, gHandsGenerator);
	checkRC(rc, "Find hands generator");
	NIState::gPSessionManager = new XnVSessionManager();
	rc = NIState::gPSessionManager->Initialize(&NIState::gContext, "Click,Wave", "RaiseHand");
	checkRC(rc, "SessionManager::Initialize");

	NIState::gPSessionManager->RegisterSession(NULL, NIState::sessionStarting, NIState::sessionEnding, NIState::focusProgress);

	NIState::gPDrawer = new XnVPointDrawer(20, gDepthGenerator);
	XnVFlowRouter *gPFlowRouter = new XnVFlowRouter;
	gPFlowRouter->SetActive(NIState::gPDrawer);

	NIState::gPSessionManager->AddListener(gPFlowRouter);

	NIState::gPDrawer->RegisterNoPoints(NULL, NIState::noHands);
	NIState::gPDrawer->setDepthMap(NIState::gBDrawDepthMap);
	
	rc = NIState::gContext.StartGeneratingAll();
	checkRC(rc, "StartGenerating");
	
    return sgroot;
}

extern Application *
application() {
    return new Kinect();
}

const char *usr_help_str = "Usage: ./kinect [OPTION]\n";

extern int init(TaskManager *manager, int argc, char *argv[]);
extern void task_initialize();
static void TMend(TaskManager *manager);

int
TMmain(TaskManager *manager, int argc, char *argv[])
{
    task_initialize();
    manager->set_TMend(TMend);
    return init(manager, argc, argv);
}

void
TMend(TaskManager *manager)
{
    printf("kinect end\n");
}

/* end */