#ifndef _ROS_NODE_H
#define _ROS_NODE_H

#include <ros/ros.h>
#include <IIWA/IIWAMsg.h>
#include <ImFusion/Base/Mat.h>

#include <string>
#include <list>

/* Initializes a ROS node which can send and receive messages. */
class ROSNode {
public:
	/* Create a new instance without initializing ROS node*/
	ROSNode();
	/* Create a new instance and initialize ROS node */
	ROSNode(std::string node_name);
	/* Create a new instance and initialize ROS node with publisher and subscriber */
	ROSNode(std::string node_name, std::string publisher_topic, std::string subscriber_topic);
	/* Causes the ROS node to be shut down */
	~ROSNode();
	/* Initialize the ROS node */
	bool init(std::string node_name);
	/* Check if ROS core is ready to accept connections */
	bool masterIsRunning();
	/* Advertise on the given topic name*/
	void advertise(std::string topic_name);
	/* Publish message */
	void publishMessage(const IIWA::IIWAMsg::ConstPtr& msgPtr);
	void publishMessage(IIWA::IIWAMsg& msg);
	void publishMessage(Eigen::Vector3d& point, Eigen::Quaterniond& orientation);
	void publishMessage(Eigen::Vector3d& point, Eigen::Matrix3d& orientation);
	void publishMessage(std::vector<double>& point, std::vector<double>& orientation);
	/* Subscribe to the given topic name*/
	void subscribe(std::string topic_name);
	/* Callback function that propagates messages to other functions */
	void chatterCallback(const IIWA::IIWAMsg::ConstPtr& msgPtr);
	/* Add another callback function for handling messages */
	void addCallback(boost::function<void (const IIWA::IIWAMsg::ConstPtr)> callback);
	/* Get name of the node */
	std::string getNodeName();
	/* Get name of the publisher */
	std::string getPubName();
	/* Get name of the subscriber */
	std::string getSubName();
protected:
	/* Handle for the internal ROS node */
	ros::NodeHandle* n;
	/* Publisher for advertising messages */
	ros::Publisher chatter_pub;
	/* Subscriber for listening for messages */
	ros::Subscriber chatter_sub;
	/* Non-blocking thread that accepts incoming messages */
	ros::AsyncSpinner* spinner;
	/* Stores callback functions that get called by main subsciber callback function */
	std::list<boost::function<void (const IIWA::IIWAMsg::ConstPtr)>> callbackQueue;
	/* Size of message queues for publisher and subscriber */
	const int QUEUE_SIZE;
	/* Name of node */
	std::string node_name;
	/* Publisher topic */
	std::string pub_topic_name;
	/* Subscriber topic*/
	std::string sub_topic_name;
};
#endif // !_ROS_NODE_H