//#include <functional>
#include <boost/bind.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Point.h>

//--------------------------------------------------------------------------------
//  class Robot tracks the robot and provides callback functions
//--------------------------------------------------------------------------------
class Robot
{
public:
  Robot(ros::NodeHandle nh, const std::string& topic_name, uint32_t queue_size);
  void onTimer(const ros::TimerEvent& e);
  void onConnect(const ros::SingleSubscriberPublisher& pub);
  void onDisconnect(const ros::SingleSubscriberPublisher& pub);
private:
  ros::NodeHandle nh_;
  geometry_msgs::Point location_;
  ros::Publisher pub_;
  ros::Timer timer_;
  unsigned int numSubscribers_;
};

//--------------------------------------------------------------------------------
// Implementation of constructor
//--------------------------------------------------------------------------------

Robot::Robot(ros::NodeHandle nh, const std::string& topic_name, uint32_t queue_size) :
  nh_(nh), numSubscribers_(0)
{
  /* Setup callbacks - using boost::bind */
  pub_ = nh_.advertise<geometry_msgs::Point>("robot_location", 10,
                                             boost::bind(&Robot::onConnect, this, _1),
                                             boost::bind(&Robot::onDisconnect, this, _1));


  /* Setup callbacks - using C++11 lambda
  pub_ = nh_.advertise<geometry_msgs::Point>(
    "robot_location", 10,
    [this](const ros::SingleSubscriberPublisher& pub) { this->onConnect(pub); },
    [this](const ros::SingleSubscriberPublisher& pub) { this->onDisconnect(pub); });
*/

  location_.x = 10;
  location_.y = 10;
  location_.z = 10;
}

//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
void Robot::onConnect(const ros::SingleSubscriberPublisher& pub)
{
  ++numSubscribers_;

  if (numSubscribers_ > 0)
  {
    ROS_INFO_STREAM("Received subscriber connection: start publishing");
    timer_ = nh_.createTimer(ros::Duration(1.0), &Robot::onTimer, this);
  }
}

//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
void Robot::onDisconnect(const ros::SingleSubscriberPublisher& pub)
{
  --numSubscribers_;
  if (numSubscribers_ == 0)
  {
    ROS_INFO_STREAM("No more subscribers: stop publishing");
    timer_.stop();
  }
}

//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
void Robot::onTimer(const ros::TimerEvent& e)
{
  ROS_INFO_STREAM("Publishing robot location: " << location_.x << ", " << location_.y);
  pub_.publish(location_);

  // Simulate a move is some direction
  location_.x += 1;
  location_.y += 1;
}


//--------------------------------------------------------------------------------
// Main
//--------------------------------------------------------------------------------

int main(int argc, char **argv)
{
  ros::init(argc, argv, "robot_location_publisher");
  ros::NodeHandle nh;
  Robot robot(nh, "robot_location", 10);

  ros::spin();
}


