/*
	@author: Dmitrij Sojma <sojmadmi@fel.cvut.cz>
	@institution: CTU in Prague

	Based on libsurvive/tools/ros_publisher/main.cc under the MIT license, available at https://github.com/cntools/libsurvive

	MIT license:
	Permission is hereby granted, free of charge, to any person obtaining a copy
	of this software and associated documentation files (the "Software"), to deal
	in the Software without restriction, including without limitation the rights
	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
	copies of the Software, and to permit persons to whom the Software is
	furnished to do so, subject to the following conditions:

	The above copyright notice and this permission notice shall be included in all
	copies or substantial portions of the Software.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
	SOFTWARE.

*/

#include <vive_survive_publisher/vive_survive_publisher.h>

static volatile int keepRunning = 1;

#ifdef __linux__

#include <assert.h>
#include <signal.h>
#include <stdlib.h>

void intHandler(int dummy) {
	if (keepRunning == 0)
		exit(-1);
	keepRunning = 0;
}

#endif

struct SurviveExternalObject {
	SurvivePose pose;
	SurviveVelocity velocity;
};

struct SurviveLighthouseData {
	int lighthouse;
	char serial_number[16];
};

struct SurviveSimpleObject {
	struct SurviveSimpleContext *actx;

	enum SurviveSimpleObject_type {
		SurviveSimpleObject_LIGHTHOUSE,
		SurviveSimpleObject_OBJECT,
		SurviveSimpleObject_EXTERNAL
	} type;

	union {
		struct SurviveLighthouseData lh;
		struct SurviveObject *so;
		struct SurviveExternalObject seo;
	} data;

	char name[32];
	bool has_update;
};

#define MAX_EVENT_SIZE 64
struct SurviveSimpleContext {
	SurviveContext* ctx; 
	
	bool running;
	og_thread_t thread;
	og_mutex_t poll_mutex;

	size_t events_cnt;
	size_t event_next_write;
	struct SurviveSimpleEvent events[MAX_EVENT_SIZE];

	size_t external_object_ct;
	struct SurviveSimpleObject *external_objects;

	size_t object_ct;
	struct SurviveSimpleObject objects[];
};


void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
{
	survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val);
	printf( "ID: %"             PRIu8       ", Event type: %"       PRIu8
			", axis1_id: %"     PRIu8       ", axis1_val: %"        PRIu16
			", axis2_id: %"     PRIu8       ", axis2_val: %"        PRIu16
			", object_name: %s\n",
			buttonId,         eventType,
			axis1Id,      axis2Id,
			axis1Val,       axis2Val,
			so->codename
	);

	survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); //not working for usb
}

int main(int argc, char** argv) {
#ifdef __linux__
	signal(SIGINT, intHandler);
	signal(SIGTERM, intHandler);
	signal(SIGKILL, intHandler);
#endif

	std::map<std::string, ros::Publisher> publishers;

	ros::init(argc, argv, "libsurvive");
	ros::NodeHandle n;

	SurviveSimpleContext* actx = survive_simple_init(argc, argv);
	if (actx == 0) // implies -help or similiar
		return 0;

	survive_simple_start_thread(actx);

	//survive_install_button_fn(actx->ctx, testprog_button_process);

	auto get_publisher = [&](const char* name) {
		auto it = publishers.find(name);
		if (it != publishers.end())
			return it->second;
		std::cerr << "adding " << name << std::endl;
		return publishers[name] =
			n.advertise<geometry_msgs::PoseStamped>("/survive/" + std::string(name) + "_pose", 1, strpbrk(name, "LH") != 0);
	};

	// events publisher
	ros::Publisher survive_event_pub = n.advertise<vive_events::EventStamped>("/survive/events", 1);

	// subscriber for incoming events eg. Haptic	
	ros::Subscriber sub = n.subscribe("/survive/feedback", 1, surviveFeedbackCB);

	/*
	uint16_t pulseHigh = 0xff00;
	uint16_t pulseLow =	0xff00;
	uint16_t desired_duration_in_seconds = 1;
	uint16_t desired_frequency = 1000 * 1000 / (pulseHigh + pulseLow);
	uint16_t repeatCount = desired_duration_in_seconds * desired_frequency;
	*/


	SurviveSimpleEvent* event = new SurviveSimpleEvent;

	while (survive_simple_is_running(actx) && ros::ok() && keepRunning) { // old version
	//while (survive_simple_wait_for_update(actx) && ros::ok() && keepRunning) { // newer version
		ros::spinOnce();
		
		//TODO: haptic feedback when implemented
		//survive_haptic(survive_simple_get_survive_object(buttonEvent->object), 0, pulseHigh, pulseLow, repeatCount);

		SurvivePose pose;
		//SurviveVelocity velocity;

		survive_simple_next_event(actx, event);
		const SurviveSimpleButtonEvent* buttonEvent = survive_simple_get_button_event(event);

		if (buttonEvent != 0) { //0 means not a button event			
			const char *name = survive_simple_object_name(buttonEvent->object);
			bool isWired = (strpbrk(name, "M") == 0) || (strpbrk(name, "T") != 0);
			bool isTracker = strpbrk(name, "T") != 0; // only works when wired, Tracker vs. Controller recognition is only in latest versions
			vive_events::EventStamped event_msg = handleButtonEvent(buttonEvent, isWired, isTracker);

			if (event_msg.event_type != ViveEvents::EventType::NUM_EVENTS) {
				survive_event_pub.publish(event_msg);
			}
		}

		for (const SurviveSimpleObject* it = survive_simple_get_next_updated(actx); it != 0;
			it = survive_simple_get_next_updated(actx)) {

			uint32_t timecode = survive_simple_object_get_latest_pose(it, &pose);
			const char* name = survive_simple_object_name(it);
			geometry_msgs::PoseStamped pose_msg = createPoseMsg(pose);
			get_publisher(survive_simple_object_name(it)).publish(pose_msg);

			//timecode = survive_simple_object_get_latest_velocity(it, &velocity);
		}
	}

	delete event;
	survive_simple_close(actx);
	return 0;
}


vive_events::EventStamped handleButtonEvent(const SurviveSimpleButtonEvent* buttonEvent, bool isWired, bool isTracker) {
	static int lastTrackpadAxis1Val = 0;
	static int lastTrackpadAxis2Val = 0;

	vive_events::EventStamped event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::NUM_EVENTS);

	uint8_t menuID = BUTTON_MENU;
	uint8_t systemID = BUTTON_SYSTEM;
	uint8_t gripID = BUTTON_GRIP;

	if(isWired){
		menuID = BUTTON_MENU_WIRED;
		systemID = BUTTON_SYSTEM_WIRED;
		gripID = BUTTON_GRIP_WIRED;
	}
	
	if(isTracker){
		if(buttonEvent->event_type == BUTTON_EVENT_PRESSED){
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKER_PRESSED);
		}
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKER_RELEASED);
		}
		return event_msg;
	}

	/* ------------------- TRACKPAD & TRIGGER AXIS ------------------- */
	if (buttonEvent->event_type == BUTTON_EVENT_AXIS){
		if (buttonEvent->axis_ids[0] == TRACKPAD_AXIS1 && buttonEvent->axis_ids[1] == TRACKPAD_AXIS2) {
			lastTrackpadAxis1Val = buttonEvent->axis_val[0];
			lastTrackpadAxis2Val = buttonEvent->axis_val[1];
			double data[2] = {(double)lastTrackpadAxis1Val, (double)lastTrackpadAxis2Val};
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKPAD_TOUCH, 2, data);
		}
		else { // buttonEvent->axis_ids[0] == TRIGGER_AXIS1
			double data[1] = {(double)buttonEvent->axis_val[0]};
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRIGGER, 1, data);
		}
	}

	/* -------------------------- TRACKPAD -------------------------- */
	else if (buttonEvent->button_id == BUTTON_TRACKPAD_TOUCH) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKPAD_TOUCH_START);
		} 
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKPAD_TOUCH_END);
		}
	}

	else if ((buttonEvent->button_id == BUTTON_TRACKPAD_PRESS) && (!isWired)) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			// When pressing on the very edge axis values go to 0, this messes up the direction, so we ignore it.
			if ((lastTrackpadAxis1Val != 0) || (lastTrackpadAxis2Val != 0)) {
				ViveEvents::EventType event_type = getTrackerEventType(lastTrackpadAxis1Val, lastTrackpadAxis2Val);
				event_msg = ViveEvents::createEventMsg(event_type);
			}

			// uncomment to use TRACKPAD_PRESSED instead of UP/DOWN/LEFT/RIGHT
			//event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKPAD_PRESSED);
		}
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRACKPAD_RELEASED);
		}
	}
	
	/* -------------------------- TRIGGER -------------------------- */
	else if (buttonEvent->button_id == BUTTON_TRIGGER) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRIGGER_PRESSED);
		} 
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::TRIGGER_RELEASED);
		}
	}

	/* -------------------------- MENU -------------------------- */
	else if (buttonEvent->button_id == menuID) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::MENU_PRESSED);
		} 
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::MENU_RELEASED);
		}
	}

	/* -------------------------- SYSTEM -------------------------- */
	else if (buttonEvent->button_id == systemID) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::SYSTEM_PRESSED);
		} 
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::SYSTEM_RELEASED);
		}
	}

	/* -------------------------- GRIP -------------------------- */
	else if (buttonEvent->button_id == gripID) {
		if (buttonEvent->event_type == BUTTON_EVENT_PRESSED) {
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::GRIP_PRESSED);
		} 
		else { //buttonEvent->event_type == BUTTON_EVENT_RELEASED
			event_msg = ViveEvents::createEventMsg(ViveEvents::EventType::GRIP_RELEASED);
		}
	}

	return event_msg;
}

ViveEvents::EventType getTrackerEventType(int lastTrackpadAxis1Val, int lastTrackpadAxis2Val) {
	if (lastTrackpadAxis1Val <= HALF_TRACKPAD) {
		if (lastTrackpadAxis2Val <= HALF_TRACKPAD) {
			if (lastTrackpadAxis1Val <= lastTrackpadAxis2Val) {
				return ViveEvents::EventType::TRACKPAD_UP;
			}
			else {
				return ViveEvents::EventType::TRACKPAD_RIGHT;
			}
		}
		else {
			if ((lastTrackpadAxis2Val - (HALF_TRACKPAD+1)) <= (HALF_TRACKPAD - lastTrackpadAxis1Val)) {
				return ViveEvents::EventType::TRACKPAD_DOWN;
			}
			else {
				return ViveEvents::EventType::TRACKPAD_RIGHT;
			}
		}
	}
	else {
		if (lastTrackpadAxis2Val <= HALF_TRACKPAD) {
			if ((lastTrackpadAxis1Val - (HALF_TRACKPAD+1)) <= (HALF_TRACKPAD - lastTrackpadAxis2Val)) {
				return ViveEvents::EventType::TRACKPAD_LEFT;
			}
			else {
				return ViveEvents::EventType::TRACKPAD_UP;
			}
		}
		else {
			if (lastTrackpadAxis1Val <= lastTrackpadAxis2Val) {
				return ViveEvents::EventType::TRACKPAD_LEFT;
			}
			else {
				return ViveEvents::EventType::TRACKPAD_DOWN;
			}
		}
	}
}
geometry_msgs::PoseStamped createPoseMsg(SurvivePose pose) {
	static uint32_t seq = 0;

	geometry_msgs::PoseStamped pose_msg = {};
	pose_msg.header.seq = seq++;
	pose_msg.header.stamp = ros::Time::now();
	pose_msg.header.frame_id = "libsurvive_world";
	pose_msg.pose.position.x = pose.Pos[0];
	pose_msg.pose.position.y = pose.Pos[1];
	pose_msg.pose.position.z = pose.Pos[2];
	pose_msg.pose.orientation.w = pose.Rot[0];
	pose_msg.pose.orientation.x = pose.Rot[1];
	pose_msg.pose.orientation.y = pose.Rot[2];
	pose_msg.pose.orientation.z = pose.Rot[3];

	return pose_msg;
}

void surviveFeedbackCB(const vive_events::EventStamped::ConstPtr& msg_ptr) {
	ViveEvents::EventType event_type = (ViveEvents::EventType)(msg_ptr->event_type);
	if (event_type == ViveEvents::EventType::HAPTIC){
		//haptic = true;
		//hapticFrequency = (unsigned int)msg_ptr->data[1];
	}
}