#include "any_node_example/ExampleNode.hpp"

namespace any_node_example {

bool ExampleNode::init() {
  // called on startup
  // add new workers (threads) like this. The workers are automatically stopped before the cleanup() function is called
  constexpr double defaultWorkerTimeStep = 3.0;
  constexpr int priority = 10;
  auto workerTimeStep = param<double>("time_step", defaultWorkerTimeStep);
  addWorker("exampleNode::updateWorker", workerTimeStep, &ExampleNode::update, this, priority);

  // to create publishers/subscribers/service clients/servers or read params, use the functions provided by the any_node::Node base class
  // the name of the publishers/subscribers/.. has to be consistent
  // with the names specified in the yaml file (see description of Topic.hpp above)
  constexpr unsigned int defaultQueueSize = 1;
  ros::Publisher my_publisher = advertise<std_msgs::String>("my_publisher_name", "/default_publisher_topic", defaultQueueSize);
  ros::Subscriber my_subscriber =
      subscribe("my_subscriber_name", "/default_subscriber_topic", defaultQueueSize, &ExampleNode::subscriberCallback, this);

  MELO_INFO("init called");

  // if you encounter an error in the init function and wish to shut down the node, you can return false
  return true;
}

void ExampleNode::cleanup() {
  // this function is called when the node is requested to shut down, _after_ the ros spinners and workers were stopped
  // no need to stop workers which are started with addWorker(..) function
  MELO_INFO("cleanup called");
}

bool ExampleNode::update(const any_worker::WorkerEvent& event) {
  // called by the worker which is automatically set up if rosparam standalone == True.
  // The frequency is defined in the time_step rosparam.
  MELO_INFO("update called");
  return true;
}

void ExampleNode::preCleanup() {
  // this function is called when the node is requested to shut down, _before_ the ros spinners and workers are beeing stopped
  MELO_INFO("preCleanup called");
}

void ExampleNode::subscriberCallback(const std_msgs::StringConstPtr& msg) {
  // called asynchrounously when ros messages arrive for the subscriber created in init() function
  MELO_INFO("received ros message: %s", msg->data.c_str());
}

}  // namespace any_node_example