#include <iostream>
#include <algorithm>

#include "vrpn_Tracker.h"
#include "quat.h"

#include "vrpn_tracker.hpp"

namespace microcart
{
	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);

	Tracker::Tracker(std::string server) : Tracker(server.c_str())
	{
	}

	Tracker::Tracker(const char * server) :
		remote(server),
		stop_flag(0),
		trackerData({
			.x = 0.0,
			.y = 0.0,
			.z = 0.0,
			.pitch = 0.0,
			.roll = 0.0,
			.yaw = 0.0,
			.fps = 0.0,
			.timestamp = {
				.tv_sec = 0,
				.tv_usec = 0
			}
		})

	{
		remote.register_change_handler(this, vrpn_cb);

		stop_flag = 0;
		vrpn_thread = std::thread(&Tracker::vrpn_loop, this);
	}

	Tracker::~Tracker()
	{
		{
			std::lock_guard<std::mutex> guard(vrpn_mutex);
			stop_flag = 1;
		}
		vrpn_thread.join();
	}

	const struct TrackerData Tracker::getData(void)
	{
		std::lock_guard<std::mutex> guard(vrpn_mutex);
		return trackerData;
	}

	void Tracker::vrpn_loop(void)
	{
		while (1) {
			remote.mainloop();

			{
				std::lock_guard<std::mutex> guard(vrpn_mutex);

				if (stop_flag) {
					return;
				}
			}
		}
	}

	void Tracker::callback(const vrpn_TRACKERCB t)
	{
		std::lock_guard<std::mutex> guard(vrpn_mutex);

		q_vec_type euler;
		q_to_euler(euler, t.quat);

		trackerData.x = (double) t.pos[0];
		trackerData.y = (double) t.pos[1];
		trackerData.z = (double) t.pos[2];

		trackerData.roll  = (double) euler[2];
		trackerData.pitch = (double) euler[1];
		trackerData.yaw   = (double) euler[0];

		timeval elapsed_time;
		timersub(&t.msg_time, &trackerData.timestamp, &elapsed_time);
		trackerData.timestamp = t.msg_time;

		double elapsed_time_usec = (double) elapsed_time.tv_usec + 
			((1000000.0) * (double) elapsed_time.tv_sec);

		trackerData.fps = 1.0 / elapsed_time_usec;

		auto td = trackerData;
		std::for_each(cb_vector.begin(), cb_vector.end(),
			[&td](std::function<void(const TrackerData &)> &fn){
				fn(td);
		});
	}

	void Tracker::addCallback(std::function<void(const TrackerData&)> cb) 
	{
		cb_vector.push_back(cb);
	}

	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t)
	{
		Tracker * inst = (Tracker *)(param);
		inst->callback(t);
	}
}

/* C Interface stuff */
struct ucart_vrpn_tracker {
	microcart::Tracker * t;
};

extern "C"
{
	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
			const char * server)
	{
		try {
			auto inst = new struct ucart_vrpn_tracker;
			inst->t = new microcart::Tracker(server);
		} catch(...) {
			return NULL;
		}
	}

	void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst)
	{
		delete inst->t;
		delete inst;
	}

	int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
			void (*cb)(struct ucart_vrpn_TrackerData *))
	{
		try {
			inst->t->addCallback([cb](const microcart::TrackerData & td) {
						struct ucart_vrpn_TrackerData data;
						data.x = td.x;
						data.y = td.y;
						data.z = td.z;
						data.pitch = td.pitch;
						data.roll = td.roll;
						data.yaw = td.yaw;
						(*cb)(&data);
					});
		} catch(...) {
			return -1;
		}
		return 0;
	}

	int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker *inst,
			struct ucart_vrpn_TrackerData *td)
	{
		try {
			auto data = inst->t->getData();
			td->x = data.x;
			td->y = data.y;
			td->z = data.z;
			td->pitch = data.pitch;
			td->roll = data.roll;
			td->yaw = data.yaw;

			return 0;
		} catch(...) {
			return -1;
		}
	}
}