#include "md_controller/md_controller_subscriber.h"
#include <XmlRpcException.h>

/**
 * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
 */
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
  try
  {
    if(value.hasMember(member))
      return value[member];
    return defaultvalue;
  }
  catch(const XmlRpc::XmlRpcException& e)
  {
    return defaultvalue;
  }
  
}

MD::Subscriber::Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size, double schedule_delay, double schedule_delay_lastest)
 :  nh_(nh), slave_id(0), 
    topic_name_(ros_topic), 
    m_subscribe_queue_size_(subscribe_queue_size),
    schedule_delay_(ros::Duration(schedule_delay)),
    schedule_lastest_delay_(ros::Duration(schedule_delay_lastest)),
    publish_time_(ros::Time(0)),
    publish_lastest_time_(ros::Time(0))
{
  control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}

MD::Subscriber::Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object)
 : nh_(nh), slave_id(0),
  publish_time_(ros::Time(0)),
  publish_lastest_time_(ros::Time(0))
{
  topic_name_               = readMember<std::string>(object->second, "toppic", "");
  m_subscribe_queue_size_   = readMember<int>(object->second, "subscribe_queue_size", 1);
  slave_id                  = (uint8_t)readMember<int>(object->second, "id", 1);
  ratio                     = (double)readMember<int>(object->second, "ratio", 0.0);
  ratio                     = readMember<double>(object->second, "ratio", ratio);
  double schedule_delay     = readMember<double>(object->second, "schedule_delay", 0.05);
  double schedule_delay_lastest = readMember<double>(object->second, "schedule_delay_lastest", 0.5);

  this->schedule_delay_ = ros::Duration(schedule_delay);
  this->schedule_lastest_delay_ = ros::Duration(schedule_delay_lastest);
  control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}

MD::Subscriber::~Subscriber() = default;


void MD::Subscriber::wheelCallback(const std_msgs::Float32::Ptr & msg)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(this->m_measurement_mutex);
  // check that we don't have multiple publishers on the command topic
  if (control_wheel_sub_.getNumPublishers() > 1)
  {
    ROS_ERROR_STREAM_THROTTLE(1.0, "Detected " << control_wheel_sub_.getNumPublishers()
      << " publishers " << topic_name_ << ". Only 1 publisher is allowed. Going to brake.");
    speed = 0; 
    return;
  }

  if (!std::isfinite(msg->data))
  {
    ROS_WARN_THROTTLE(1.0, "Received NaN in %s command. Ignoring.", topic_name_.c_str());
    speed = 0;
    return;
  }
  speed = (int16_t)(msg->data * ratio * 60 / 2 / M_PI);
  this->scheduleWheelController(true);
}

bool MD::Subscriber::isWheelTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}

bool MD::Subscriber::isWheelLastestTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}

void MD::Subscriber::scheduleWheelController(bool schedule)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  if(schedule && publish_time_.isZero())
  {
    publish_time_ = ros::Time::now() + schedule_delay_;
    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
  }
  if(!schedule && !publish_time_.isZero())
  {
    publish_time_ = ros::Time(0);
    publish_lastest_time_ = ros::Time(0);
  }
}