#include "ROSNode.h"

#include <boost/foreach.hpp>

ROSNode::ROSNode() : QUEUE_SIZE(1000) {

}

ROSNode::ROSNode(std::string node_name): QUEUE_SIZE(1000)
{
  init(node_name);
}

ROSNode::ROSNode(std::string node_name, std::string publisher_topic, std::string subscriber_topic) : QUEUE_SIZE(1000) {
	init(node_name);
	advertise(publisher_topic);
	subscribe(subscriber_topic);
}

ROSNode::~ROSNode()
{
	if (n) {
	    delete n;
	}
	if (spinner) {
	    delete spinner;
	}
}

bool ROSNode::init(std::string node_name) {
  int argc = 0;
  char** argv =  NULL;
  ros::init(argc, argv, node_name);

  this->node_name = node_name;

  bool masterIsRunning = this->masterIsRunning();

  if (masterIsRunning) {
	  n = new ros::NodeHandle();
      spinner = new ros::AsyncSpinner(1);
  }

  return masterIsRunning;
}

bool ROSNode::masterIsRunning() {
	return ros::master::check();
}

void ROSNode::advertise(std::string topic_name)
{
	chatter_pub = n->advertise<IIWA::IIWAMsg>(topic_name, QUEUE_SIZE);

	this->pub_topic_name = topic_name;
}

void ROSNode::publishMessage(Eigen::Vector3d& point, Eigen::Quaterniond& orientation)
{
	publishMessage(point, orientation.toRotationMatrix());
}

void ROSNode::publishMessage(Eigen::Vector3d& point, Eigen::Matrix3d& orientation) {
	std::vector<double> cartPosition = std::vector<double>(3);
	std::vector<double> cartOrientation = std::vector<double>(9);

	for (int i = 0; i < 3; i++) {
	   cartPosition[i] = point[i];
	}

	// orientation is a 3x3 matrix which is represented as array
	cartOrientation[0] = orientation(0,0);
	cartOrientation[1] = orientation(0,1);
	cartOrientation[2] = orientation(0,2);
	cartOrientation[3] = orientation(1,0);
	cartOrientation[4] = orientation(1,1);
	cartOrientation[5] = orientation(1,2);
	cartOrientation[6] = orientation(2,0);
	cartOrientation[7] = orientation(2,1);
	cartOrientation[8] = orientation(2,2);

	publishMessage(cartPosition, cartOrientation);
}

void ROSNode::publishMessage(std::vector<double>& point, std::vector<double>& orientation)
{
	IIWA::IIWAMsg msg;

	msg.cartPosition = point;
	msg.cartOrientation = orientation;

	publishMessage(msg);
}

void ROSNode::publishMessage(const IIWA::IIWAMsg::ConstPtr& msgPtr)
{
	chatter_pub.publish(msgPtr);
}

void ROSNode::publishMessage(IIWA::IIWAMsg& msg)
{
	chatter_pub.publish(msg);
}

void ROSNode::subscribe(std::string topic_name)
{
	chatter_sub = n->subscribe(topic_name, QUEUE_SIZE, &ROSNode::chatterCallback, this);

	this->sub_topic_name = topic_name;

    spinner->start();
}

void ROSNode::chatterCallback(const IIWA::IIWAMsg::ConstPtr& msgPtr)
{
	BOOST_FOREACH(boost::function<void (const IIWA::IIWAMsg::ConstPtr)> callback, callbackQueue) {
       callback(msgPtr);
	}
}

void ROSNode::addCallback(boost::function<void (const IIWA::IIWAMsg::ConstPtr)> callback) {
	callbackQueue.push_back(callback);
}

std::string ROSNode::getNodeName() {
	return node_name;
}

std::string ROSNode::getPubName() {
	return pub_topic_name;
}

std::string ROSNode::getSubName() {
	return sub_topic_name;
}