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

#ifndef VIVE_DATA_CLASS_H
#define VIVE_DATA_CLASS_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <boost/thread/thread.hpp>
#include <mutex>
#include <Eigen/Dense>
#include <queue>

#include <vive_events/EventStamped.h>
#include <vive_events/ViveEvents.h>

namespace vive_data{
	class ViveData {
	public:
        ViveData(std::string &ctrlName, std::string &TrName) : libControllerName(std::move(ctrlName)), libTrackerName(std::move(TrName)){
			this->_init();
		}
        ViveData(std::string &ctrlName, std::string &TrName, int samplesToAvg) : libControllerName(std::move(ctrlName)), libTrackerName(std::move(TrName)){
			this->avgN = samplesToAvg;
			this->_init();
		}
        ViveData(std::string &ctrlName, std::string &TrName, int samplesToAvg, int trackerPosesToAvg) : libControllerName(std::move(ctrlName)), libTrackerName(std::move(TrName)){
			this->avgN = samplesToAvg;
			if (this->avgN < 1){
				this->avgN = 1;
			}
			this->trackerN = trackerPosesToAvg;
			this->_init();
		}
		~ViveData();
		
		geometry_msgs::PoseStamped getData();
		std::list<ViveEvents::EventType> getEvents();
		void setFeedback(vive_events::EventStamped* eventPtr);

	private:
		void _init();

		/** Threads */
		boost::shared_ptr<boost::thread> surviveSubscribingThread;
		boost::shared_ptr<boost::thread> surviveEventThread;

		/** Thread functions */
		void surviveSubscribingThreadf();
		void surviveEventThreadf();
		void joinThreads();

		/** Callback functions */
		void surviveDataCallback(const geometry_msgs::PoseStamped::ConstPtr& msgPtr);
		void surviveEventCallback(const vive_events::EventStamped::ConstPtr& msgPtr);
		void surviveTrackerCallback(const geometry_msgs::PoseStamped::ConstPtr& msgPtr);

		/** Data structures */
		std::queue<geometry_msgs::PoseStamped> averageDataQueue;
		std::list<geometry_msgs::PoseStamped> poseList;
		std::list<geometry_msgs::PoseStamped> trackerPoses;
		std::list<ViveEvents::EventType> eventList;
		std::queue<vive_events::EventStamped*> feedbackEventQueue;


		/** Moving average */
		int avgN = 5;
		int trackerN = 100;
		geometry_msgs::PoseStamped getMovingAverage(std::list<geometry_msgs::PoseStamped> poseList, int avgN);
		

		/** Locks */
		std::mutex averageDataQueueLock;
		std::mutex triggerLock;
		std::mutex eventListLock;
		std::mutex feedbackQueueLock;
		std::mutex timeoutErrorLock;


		geometry_msgs::PoseStamped EMPTY_POSE = geometry_msgs::PoseStamped();
		geometry_msgs::PoseStamped prevPose;
		geometry_msgs::PoseStamped newPose;
		
		/** Flags */
		bool firstPose = false;
		bool triggerPressed = false;
		bool clearPoseList = false;
		bool dataTimeoutError = false;
		bool continuousTimeout = false;
		
		
		ros::Time timeTriggerPressed;
		ros::Time timeTriggerReleased;
		ros::Time dataTime;
		ros::Duration errorTimeoutSec = ros::Duration(1.0);

		vive_events::EventStamped dataTimeoutErrorEvent = ViveEvents::createEventMsg(ViveEvents::EventType::ERROR);
		
		/** Functions */
		bool isDataValid(geometry_msgs::PoseStamped data);
		void handleEvent(vive_events::EventStamped event);
		void handleFeedback(ros::Publisher* pub);
		void setTrackerTransformationMatrix();		
		geometry_msgs::PoseStamped getTrackerPose();
		geometry_msgs::PoseStamped getDifference(geometry_msgs::PoseStamped newPose, geometry_msgs::PoseStamped prevPose);
		geometry_msgs::PoseStamped getPoseDifference();		
		
		
		/** Transformation matrices */
		Eigen::Isometry3d T_tracker_libsurvive;
		Eigen::Isometry3d T_controller_axis;
		
		/** Libsurvive names */
		std::string libTrackerName;
		std::string libControllerName;
		
		

	};
}

#endif