#include <ros/ros.h>
#include <XmlRpcValue.h>
#include <boost/thread/thread.hpp>
#include "md_controller/md_controller.h"
#include "md_controller/diagnostic.h"
#include "md_controller/md_controller_subscriber.h"
#include "libserial/udevadm_info.h"
#include <XmlRpcException.h>

/* define struct */
typedef struct device{
  std::string _port;
  int _baud;
}device_t;


bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) {  
	ROS_INFO("loadParam() - node_name: %s",  node_name.c_str());
	if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud)) 
	{
    if(!nh->getParam("baudrate", dv->_baud)) 
      return false;
  }

  ROS_INFO("product");
	std::string product;    //port name
  if(!nh->param(node_name + "/product", product, product)) {}
  else if(!nh->getParam("product", product)) {}
  else
  {
    udevadmInfo *ludev = new udevadmInfo(product.c_str());
    if(ludev->init() == EXIT_FAILURE) {}
    if(ludev->scanDevices())
    {
      strcpy(dv->_port.data(), ludev->port);
      return true;
    }
  }
  
  ROS_INFO("port_name");
  if(!nh->param(node_name + "/port_name", dv->_port, dv->_port))
  {
    if(!nh->getParam("port_name", dv->_port))
      return false;
  }

  ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str());
  return true;

}

/**
 * @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)
  {
    ROS_WARN_STREAM( "'"<<member << "' "<< e.getMessage() << '\n');
    return defaultvalue;
  }
}
/**********************************************************************
* MAIN 
***********************************************************************/
int main(int argc,char **argv)
{
  /* create ros ndoe */
  ros::init(argc, argv, "MD");
  ros::NodeHandlePtr nh = boost::make_shared<ros::NodeHandle>();
  std::string node_name = ros::this_node::getName();
  ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str());

  /* connect with the server */
  std::string diagnostic_topic = "MD_diagnostic";
  MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller");


  XmlRpc::XmlRpcValue drivers;
  if(!nh->getParam("drivers", drivers) || drivers.size() < 1)
  {
    ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
    MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file");
    return 2;
  }

  std::vector<boost::shared_ptr<MD::Subscriber>> p_md_controller_subscriber;
  for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter)
  {
    std::string topic_name    = readMember<std::string>(object_iter->second, "toppic", "");
    int subscribe_queue_size  = readMember<int>(object_iter->second, "subscribe_queue_size", 1);
    ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size);
    p_md_controller_subscriber.push_back(boost::make_shared<MD::Subscriber>(*nh, object_iter));
  }

  device_t dv;
	if(findDevice(nh, node_name, &dv))
  {
		ROS_INFO("%s: found device successfull", node_name.c_str());
	}
  else 
  {
    ROS_ERROR("%s: Error when find device", node_name.c_str());
    return 2;
  }
  
  uint8_t rate = 20; // Hz
  ros::Rate loop_rate(rate); 
  while (ros::ok())
  {
    ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud);
    boost::shared_ptr<MD::MdController> md_controller = boost::make_shared<MD::MdController>(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1);
    while(ros::ok() && md_controller->_connected)
    {
      for(auto &p_sub : p_md_controller_subscriber)
      {
        if(p_sub->isWheelTriggered()) 
        {
          double rotate_speed_wheel;
          {
            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
            rotate_speed_wheel = p_sub->speed;
          }
          if(abs(rotate_speed_wheel) > 0)
            md_controller->pid_vel_cmd(p_sub->slave_id, rotate_speed_wheel);
          else 
            md_controller->pid_break(p_sub->slave_id);
          p_sub->scheduleWheelController(false);
        }
      }
      loop_rate.sleep();
      ros::spinOnce();
    }
    ros::spinOnce();
    md_controller->close_port();
  }
  ros::spin();
  return 0;
}

