#include <windows.h>
#include <ole2.h>
#include <lsl_cpp.h>
#include <iostream>
#include <string.h>
#include <Leap.h>

using namespace std;
using namespace lsl;
using namespace Leap;

const int JOINT_CHANNELS = 3; // x y z
const int BONE_CHANNELS = (JOINT_CHANNELS * 1); // 2 joints
const int FINGER_CHANNELS = BONE_CHANNELS * 4; // 4 bones per finger
const int HAND_CHANNELS = (FINGER_CHANNELS * 5) + 7; // 5 fingers, 1 palm, p/h/r, 1 confidence
const int ARM_CHANNELS = HAND_CHANNELS + 3; // hands+  1 wrist pos
const int NUM_CHANNELS = ARM_CHANNELS * 2;
const int LEFT = 0; const int RIGHT = 1; const int HANDS = 2;


class LeapListener : public Listener {
public:
	virtual void onInit(const Controller&);
	virtual void onConnect(const Controller&);
	virtual void onDisconnect(const Controller&);
	virtual void onExit(const Controller&);
	virtual void onFrame(const Controller&);
	virtual void onFocusGained(const Controller&);
	virtual void onFocusLost(const Controller&);
	virtual void onDeviceChange(const Controller&);
	virtual void onServiceConnect(const Controller&);
	virtual void onServiceDisconnect(const Controller&);

private:
};

const string fingerNames[] = { "Thumb", "Index", "Middle", "Ring", "Pinky" };
const string boneNames[] = { "Metacarpal", "Proximal", "Middle", "Distal" };
const string stateNames[] = { "STATE_INVALID", "STATE_START", "STATE_UPDATE", "STATE_END" };

void LeapListener::onInit(const Controller& controller) {
	cout << "Initialized" << endl;
}

void LeapListener::onConnect(const Controller& controller) {
	cout << "Connected" << endl;
	controller.enableGesture(Gesture::TYPE_CIRCLE);
	controller.enableGesture(Gesture::TYPE_KEY_TAP);
	controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
	controller.enableGesture(Gesture::TYPE_SWIPE);
}

void LeapListener::onDisconnect(const Controller& controller) {
	// Note: not dispatched when running in a debugger.
	cout << "Disconnected" << endl;
}

void LeapListener::onExit(const Controller& controller) {
	std::cout << "Exited" << std::endl;
}

void LeapListener::onFrame(const Controller& controller) {
	// Get the most recent frame and report some basic information
	const Frame frame = controller.frame();
	std::cout << "Frame id: " << frame.id()
		<< ", timestamp: " << frame.timestamp()
		<< ", hands: " << frame.hands().count()
		<< ", fingers: " << frame.fingers().count()
		<< ", tools: " << frame.tools().count()
		<< ", gestures: " << frame.gestures().count() << std::endl;

	HandList hands = frame.hands();
	for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
		// Get the first hand
		const Hand hand = *hl;
		std::string handType = hand.isLeft() ? "Left hand" : "Right hand";
		std::cout << std::string(2, ' ') << handType << ", id: " << hand.id()
			<< ", palm position: " << hand.palmPosition() << std::endl;
		// Get the hand's normal vector and direction
		const Vector normal = hand.palmNormal();
		const Vector direction = hand.direction();

		// Calculate the hand's pitch, roll, and yaw angles
		std::cout << std::string(2, ' ') << "pitch: " << direction.pitch() * RAD_TO_DEG << " degrees, "
			<< "roll: " << normal.roll() * RAD_TO_DEG << " degrees, "
			<< "yaw: " << direction.yaw() * RAD_TO_DEG << " degrees" << std::endl;

		// Get the Arm bone
		Arm arm = hand.arm();
		std::cout << std::string(2, ' ') << "Arm direction: " << arm.direction()
			<< " wrist position: " << arm.wristPosition()
			<< " elbow position: " << arm.elbowPosition() << std::endl;

		// Get fingers
		const FingerList fingers = hand.fingers();
		for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
			const Finger finger = *fl;
			std::cout << std::string(4, ' ') << fingerNames[finger.type()]
				<< " finger, id: " << finger.id()
				<< ", length: " << finger.length()
				<< "mm, width: " << finger.width() << std::endl;

			// Get finger bones
			for (int b = 0; b < 4; ++b) {
				Bone::Type boneType = static_cast<Bone::Type>(b);
				Bone bone = finger.bone(boneType);
				std::cout << std::string(6, ' ') << boneNames[boneType]
					<< " bone, start: " << bone.prevJoint()
					<< ", end: " << bone.nextJoint()
					<< ", direction: " << bone.direction() << std::endl;
			}
		}
	}

	// Get tools
	const ToolList tools = frame.tools();
	for (ToolList::const_iterator tl = tools.begin(); tl != tools.end(); ++tl) {
		const Tool tool = *tl;
		std::cout << std::string(2, ' ') << "Tool, id: " << tool.id()
			<< ", position: " << tool.tipPosition()
			<< ", direction: " << tool.direction() << std::endl;
	}

	// Get gestures
	const GestureList gestures = frame.gestures();
	for (int g = 0; g < gestures.count(); ++g) {
		Gesture gesture = gestures[g];

		switch (gesture.type()) {
		case Gesture::TYPE_CIRCLE:
		{
			CircleGesture circle = gesture;
			std::string clockwiseness;

			if (circle.pointable().direction().angleTo(circle.normal()) <= PI / 2) {
				clockwiseness = "clockwise";
			}
			else {
				clockwiseness = "counterclockwise";
			}

			// Calculate angle swept since last frame
			float sweptAngle = 0;
			if (circle.state() != Gesture::STATE_START) {
				CircleGesture previousUpdate = CircleGesture(controller.frame(1).gesture(circle.id()));
				sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
			}
			std::cout << std::string(2, ' ')
				<< "Circle id: " << gesture.id()
				<< ", state: " << stateNames[gesture.state()]
				<< ", progress: " << circle.progress()
				<< ", radius: " << circle.radius()
				<< ", angle " << sweptAngle * RAD_TO_DEG
				<< ", " << clockwiseness << std::endl;
			break;
		}
		case Gesture::TYPE_SWIPE:
		{
			SwipeGesture swipe = gesture;
			std::cout << std::string(2, ' ')
				<< "Swipe id: " << gesture.id()
				<< ", state: " << stateNames[gesture.state()]
				<< ", direction: " << swipe.direction()
				<< ", speed: " << swipe.speed() << std::endl;
			break;
		}
		case Gesture::TYPE_KEY_TAP:
		{
			KeyTapGesture tap = gesture;
			std::cout << std::string(2, ' ')
				<< "Key Tap id: " << gesture.id()
				<< ", state: " << stateNames[gesture.state()]
				<< ", position: " << tap.position()
				<< ", direction: " << tap.direction() << std::endl;
			break;
		}
		case Gesture::TYPE_SCREEN_TAP:
		{
			ScreenTapGesture screentap = gesture;
			std::cout << std::string(2, ' ')
				<< "Screen Tap id: " << gesture.id()
				<< ", state: " << stateNames[gesture.state()]
				<< ", position: " << screentap.position()
				<< ", direction: " << screentap.direction() << std::endl;
			break;
		}
		default:
			std::cout << std::string(2, ' ') << "Unknown gesture type." << std::endl;
			break;
		}
	}

	if (!frame.hands().isEmpty() || !gestures.isEmpty()) {
		std::cout << std::endl;
	}
}

void LeapListener::onFocusGained(const Controller& controller) {
	std::cout << "Focus Gained" << std::endl;
}

void LeapListener::onFocusLost(const Controller& controller) {
	std::cout << "Focus Lost" << std::endl;
}

void LeapListener::onDeviceChange(const Controller& controller) {
	std::cout << "Device Changed" << std::endl;
	const DeviceList devices = controller.devices();

	for (int i = 0; i < devices.count(); ++i) {
		std::cout << "id: " << devices[i].toString() << std::endl;
		std::cout << "  isStreaming: " << (devices[i].isStreaming() ? "true" : "false") << std::endl;
	}
}

void LeapListener::onServiceConnect(const Controller& controller) {
	std::cout << "Service Connected" << std::endl;
}

void LeapListener::onServiceDisconnect(const Controller& controller) {
	std::cout << "Service Disconnected" << std::endl;
}


void poll_data(const Controller& controller, bool gesture_on, stream_outlet & outlet, vector<float>& sample) {
	if (gesture_on) {
		controller.enableGesture(Gesture::TYPE_CIRCLE);
		controller.enableGesture(Gesture::TYPE_KEY_TAP);
		controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
		controller.enableGesture(Gesture::TYPE_SWIPE);
	}
	const Frame framedata = controller.frame();
	std::cout << "Frame id: " << framedata.id()
		<< ", timestamp: " << framedata.timestamp()
		<< ", hands: " << framedata.hands().count()
		<< ", fingers: " << framedata.fingers().count()
		<< ", tools: " << framedata.tools().count()
		<< ", gestures: " << framedata.gestures().count() << std::endl;
	
	HandList hands = framedata.hands();
	for (HandList::const_iterator hit = hands.begin(); hit != hands.end(); hit++) {
		const Hand hand = *hit;
		bool is_right = hand.isRight();
		// offset array if right hand (in case just right hand is present)
		int split = sample.capacity() / 2;
		int hand_offset = is_right ? split : 0; // only write to right side if
												// only right hand is present
		std::string handType = hand.isLeft() ? "Left hand" : "Right hand";
		std::cout << std::string(2, ' ') << handType << ", id: " << hand.id()
			<< ", palm position: " << hand.palmPosition() << std::endl;

		Arm current_arm = hand.arm();
		const Vector norm = hand.palmNormal();
		const Vector dir = hand.direction();

		sample[0 + hand_offset] = current_arm.wristPosition().x;
		sample[1 + hand_offset] = current_arm.wristPosition().y;
		sample[2 + hand_offset] = current_arm.wristPosition().z;
		sample[3 + hand_offset] = hand.palmPosition().x;
		sample[4 + hand_offset] = hand.palmPosition().y;
		sample[5 + hand_offset] = hand.palmPosition().z;
		sample[6 + hand_offset] = dir.pitch() * RAD_TO_DEG;
		sample[7 + hand_offset] = norm.yaw() * RAD_TO_DEG;
		sample[8 + hand_offset] = norm.roll() * RAD_TO_DEG;
		sample[9 + hand_offset] = hand.confidence();

		const int start_ind_offset = 10;
		int i_offset = start_ind_offset + hand_offset; // initial index offset

		int f_ind = 0; // finger counter to keep track while using iterator
		const FingerList fingers = hand.fingers();
		for (FingerList::const_iterator fit = fingers.begin(); fit != fingers.end(); fit++) {
			const Finger finger = *fit;
			std::cout << std::string(4, ' ') << fingerNames[finger.type()]
				<< " finger, id: " << finger.id()
				<< ", length: " << finger.length()
				<< "mm, width: " << finger.width() << std::endl;

			for (int b_ind = 0; b_ind < 4; b_ind++) {
				Bone::Type bone_type = static_cast<Bone::Type>(b_ind);
				Bone bone = finger.bone(bone_type);
				sample[i_offset + f_ind * FINGER_CHANNELS + b_ind*BONE_CHANNELS + 0] = bone.nextJoint().x;
				sample[i_offset + f_ind * FINGER_CHANNELS + b_ind*BONE_CHANNELS + 1] = bone.nextJoint().y;
				sample[i_offset + f_ind * FINGER_CHANNELS + b_ind*BONE_CHANNELS + 2] = bone.nextJoint().z;
				std::cout << std::string(6, ' ') << boneNames[bone_type]
					<< " bone, start: " << bone.prevJoint()
					<< ", end: " << bone.nextJoint()
					<< ", direction: " << bone.direction() << std::endl;
			}

			f_ind++;
		}
		
	}


	if (gesture_on) {
		// Get gestures
		const GestureList gestures = framedata.gestures();
		for (int g = 0; g < gestures.count(); ++g) {
			Gesture gesture = gestures[g];

			switch (gesture.type()) {
			case Gesture::TYPE_CIRCLE:
			{
				CircleGesture circle = gesture;
				std::string clockwiseness;

				if (circle.pointable().direction().angleTo(circle.normal()) <= PI / 2) {
					clockwiseness = "clockwise";
				}
				else {
					clockwiseness = "counterclockwise";
				}

				// Calculate angle swept since last frame
				float sweptAngle = 0;
				if (circle.state() != Gesture::STATE_START) {
					CircleGesture previousUpdate = CircleGesture(controller.frame(1).gesture(circle.id()));
					sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
				}
				std::cout << std::string(2, ' ')
					<< "Circle id: " << gesture.id()
					<< ", state: " << stateNames[gesture.state()]
					<< ", progress: " << circle.progress()
					<< ", radius: " << circle.radius()
					<< ", angle " << sweptAngle * RAD_TO_DEG
					<< ", " << clockwiseness << std::endl;
				break;
			}
			case Gesture::TYPE_SWIPE:
			{
				SwipeGesture swipe = gesture;
				std::cout << std::string(2, ' ')
					<< "Swipe id: " << gesture.id()
					<< ", state: " << stateNames[gesture.state()]
					<< ", direction: " << swipe.direction()
					<< ", speed: " << swipe.speed() << std::endl;
				break;
			}
			case Gesture::TYPE_KEY_TAP:
			{
				KeyTapGesture tap = gesture;
				std::cout << std::string(2, ' ')
					<< "Key Tap id: " << gesture.id()
					<< ", state: " << stateNames[gesture.state()]
					<< ", position: " << tap.position()
					<< ", direction: " << tap.direction() << std::endl;
				break;
			}
			case Gesture::TYPE_SCREEN_TAP:
			{
				ScreenTapGesture screentap = gesture;
				std::cout << std::string(2, ' ')
					<< "Screen Tap id: " << gesture.id()
					<< ", state: " << stateNames[gesture.state()]
					<< ", position: " << screentap.position()
					<< ", direction: " << screentap.direction() << std::endl;
				break;
			}
			default:
				std::cout << std::string(2, ' ') << "Unknown gesture type." << std::endl;
				break;
			}
		}
	}

	outlet.push_sample(sample);
	Sleep(15);
}



void stream_setup(const Controller & controller, bool gesture_on) {

	std::cout << "creating stream" << endl;
	stream_info info("LeapMotion", "Mocap", NUM_CHANNELS, IRREGULAR_RATE, cf_float32, "Tracker");
	int twice = 0;
	string side;
	xml_element channels = info.desc().append_child("channels");
	while (twice < HANDS) {
		side = RIGHT ? "Left" : "Right";
		// wrist position
		channels.append_child("channel")
			.append_child_value("label", side + "_Wrist_X")
			.append_child_value("type", "PositionX")
			.append_child_value("unit", "millimeters");
		channels.append_child("channel")
			.append_child_value("label", side + "_Wrist_Y")
			.append_child_value("type", "PositionY")
			.append_child_value("unit", "millimeters");
		channels.append_child("channel")
			.append_child_value("label", side + "_Wrist_Z")
			.append_child_value("type", "PositionZ")
			.append_child_value("unit", "millimeters");
		
		// palm position
		channels.append_child("channel")
			.append_child_value("label", side + "_Palm_X")
			.append_child_value("type", "PositionX")
			.append_child_value("unit", "millimeters");
		channels.append_child("channel")
			.append_child_value("label", side + "_Palm_Y")
			.append_child_value("type", "PositionY")
			.append_child_value("unit", "millimeters");
		channels.append_child("channel")
			.append_child_value("label", side + "_Palm_Z")
			.append_child_value("type", "PositionZ")
			.append_child_value("unit", "millimeters");
		// hand orientation
		channels.append_child("channel")
			.append_child_value("label", side + "_Hand_Pitch")
			.append_child_value("type", "OrientationP")
			.append_child_value("unit", "degrees");
		channels.append_child("channel")
			.append_child_value("label", side + "_Hand_Heading")
			.append_child_value("type", "OrientationH")
			.append_child_value("unit", "degrees");
		channels.append_child("channel")
			.append_child_value("label", side + "_Hand_Roll")
			.append_child_value("type", "OrientationR")
			.append_child_value("unit", "degrees");
		// conf
		channels.append_child("channel")
			.append_child_value("label", side + "_Hand_Confidence")
			.append_child_value("type", "Confidence")
			.append_child_value("unit", "normalized");
		// fingers and bones
		for (int f = 0; f < 5; f++) {
			for (int b = 0; b < 4; b++) {
				channels.append_child("channel")
					.append_child_value("label", side + "_" + fingerNames[f] + boneNames[b] + "_X")
					.append_child_value("type", "PositionX")
					.append_child_value("unit", "millimeters");
				channels.append_child("channel")
					.append_child_value("label", side + "_" + fingerNames[f] + boneNames[b] + "_Y")
					.append_child_value("type", "PositionY")
					.append_child_value("unit", "millimeters");
				channels.append_child("channel")
					.append_child_value("label", side + "_" + fingerNames[f] + boneNames[b] + "_Z")
					.append_child_value("type", "PositionZ")
					.append_child_value("unit", "millimeters");		
			}
		}
		twice++;
	}

	stream_outlet outlet(info);
	vector<float> sample(NUM_CHANNELS);
	while (1) {
		poll_data(controller, gesture_on, outlet, sample);
	}

}

int main(int argc, char** argv) {
	// Create a sample listener and controller
	//LeapListener listener;
	Controller controller;

	// Have the sample listener receive events from the controller
	//controller.addListener(listener);
	controller.setPolicyFlags(Controller::POLICY_BACKGROUND_FRAMES);
	stream_setup(controller, false);

	// Remove the sample listener when done
	//controller.removeListener(listener);
	cout << "Exited." << endl;
	Sleep(1000);
	return 0;
}