#include "diff_wheel_plugin/wheel_plugin.h"
#include <pluginlib/class_list_macros.hpp>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>


namespace diff_wheel_plugin
{

WheelPlugin::WheelPlugin() 
 : command_timeout_(0.5)
{

}

WheelPlugin::WheelPlugin(ros::NodeHandle & nh, const std::string &name)
 : command_timeout_(0.5) 
{
  initialize(nh, name);
}


WheelPlugin::~WheelPlugin() 
{
  if (m_publish_thread_)
  {
    m_publish_thread_->join();
    delete (m_publish_thread_);
    m_publish_thread_ = 0;
  }
}

void WheelPlugin::initialize(ros::NodeHandle & nh, const std::string &name) 
{
  if(!initialized_)
  {
    nh_ = nh;
    name_ = name;
    nh_priv_ = ros::NodeHandle(nh, name); 
    nh_priv_.param("command_timeout", command_timeout_, 0.5);
    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); 
    nh_priv_.param("max_publish_rate", update_rate_, 100.0);

    ROS_INFO_NAMED("WheelPlugin","Initializing on %s with command_timeout %f subscribe_queue_size %d max_publish_rate %f", 
                    name_.c_str(),
                    command_timeout_,
                    m_subscribe_queue_size_,
                    update_rate_
                  );
    std::string mesurement_topic;
    nh_priv_.param("mesurement_topic", mesurement_topic, std::string(""));
    std::string wheel_topic;
    nh_priv_.param("wheel_topic", wheel_topic, name_);

    mesurement_sub_ = nh_.subscribe(mesurement_topic, m_subscribe_queue_size_, &diff_wheel_plugin::WheelPlugin::DLS_Callback, this);
    
    wheel_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float32>(nh_, wheel_topic , 100));
    
    m_publish_thread_ = new boost::thread(&diff_wheel_plugin::WheelPlugin::DoStuff,this);
    command_struct_.stamp = ros::Time::now();
    ROS_INFO_NAMED("WheelPlugin","Initializing on %s is successed", name_.c_str());
    
    initialized_ = true;
  }
}

bool WheelPlugin::Ready()
{
  return is_ready_;
}
 
double WheelPlugin::GetVelocityLimit(unsigned int _index) 
{
  throw std::bad_function_call();
}

void WheelPlugin::SetVelocityLimit(double _velocity) 
{
  throw std::bad_function_call();
}

double WheelPlugin::GetVelocity() 
{
  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
  double result = 0.0;
  if(m_measurement_)
  {
    result = m_measurement_->data;
  }
  return result;
}

void WheelPlugin::SetVelocity(double _vel) 
{
  if (!std::isfinite(_vel))
  {
    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s command. Ignoring.", name_.c_str());
    return;
  }
  command_struct_.vel_wheel = _vel;
  command_struct_.stamp = ros::Time::now();
  command_.writeFromNonRT(command_struct_);
  ROS_DEBUG_STREAM_NAMED(name_,
    "Added values to command."
    << "vel_wheel: " << command_struct_.vel_wheel << ", "
    << "Stamp: " << command_struct_.stamp);

  // ROS_INFO_STREAM_NAMED(name_,
  //   "Added values to command."
  //   << "vel_wheel: " << command_struct_.vel_wheel << ", "
  //   << "Stamp: " << command_struct_.stamp);
}

double WheelPlugin::Position()
{
  throw std::bad_function_call();
}

void WheelPlugin::SetPosition(const double _position)
{
  throw std::bad_function_call();
}

void WheelPlugin::DLS_Callback(const std_msgs::Float32::ConstPtr & msg)
{
  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
  // check that we don't have multiple publishers on the command topic
  if (!allow_multiple_command_publishers_ && mesurement_sub_.getNumPublishers() > 1)
  {
    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << mesurement_sub_.getNumPublishers()
      << " publishers. Only 1 publisher is allowed. Going to brake.");
    this->brake();
    return;
  }

  if (!std::isfinite(msg->data))
  {
    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s encoder. Ignoring.", name_.c_str());
    return;
  }

  m_measurement_ = msg;
}

void WheelPlugin::DoStuff()
{
  ros::Rate rate(update_rate_);
  is_ready_ = true;
  ROS_INFO_NAMED("WheelPlugin","...Threading on %s is ready", name_.c_str());
  bool stoped;
  while (ros::ok())
  {
    WheelCommand curr_cmd =  *(command_.readFromRT());
    ros::Time current_time = ros::Time::now();
    double dt = ros::Duration(current_time - command_struct_.stamp).toSec();
    
    if(dt <= command_timeout_ && fabs(curr_cmd.vel_wheel) > 0.001)
    {
      if (wheel_pub_ && wheel_pub_->trylock())
      {
        wheel_pub_->msg_.data = curr_cmd.vel_wheel;
        wheel_pub_->unlockAndPublish();
      }
      stoped = false;
    }
    else
    {
      if(!stoped)
      {
        for(int i = 0; i < 3; i++)
        {
          if (wheel_pub_ && wheel_pub_->trylock())
          {
            wheel_pub_->msg_.data = 0;
            wheel_pub_->unlockAndPublish();
          }
          ros::Duration(0.1).sleep();
        }
        stoped = true;
      }
    }

    rate.sleep();
    ros::spinOnce();
  }
}

}; //namespace diff_wheel_plugin

PLUGINLIB_EXPORT_CLASS(diff_wheel_plugin::WheelPlugin, models::BaseSteerDrive)