#include <ros/ros.h>
#include <boost/thread.hpp>
#include "delta_modbus/delta_modbus_tcp.h"
#include <std_msgs/Float32.h>

double combine_uint16_to_double(uint16_t high, uint16_t low) {
  // Chuyển đổi high thành double và dịch nó sang trái 16 bit
  double result = (double)high * 65536.0;  // 65536 = 2^16
  
  // Thêm low vào kết quả
  result += (double)low;
  
  return result;
}

/**********************************************************************
 *                   		MAIN 
***********************************************************************/
int main(int argc, char** argv) {
  ros::init(argc, argv, "diff_encoder_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_node("~");
  std::string node_name = ros::this_node::getName();
  ROS_INFO("node_name: %s", node_name.c_str());
  
  std::string ip_address; 
  int port;

  if(!nh_node.getParam("ip_address", ip_address)) 
  {
    ROS_ERROR("Could not find 'ip_address' in node handle");
    return 1;
  }
  if(!nh_node.getParam("port", port)) 
  {
    ROS_ERROR("Could not find 'port' in node handle");
    return 1;
  }

  int id;
  nh_node.param("id", id, 1);
  double rate;
  nh_node.param("publish_rate", rate, 20.0);
  
  int start, end;
  nh_node.param("start_bit", start, 101);
  nh_node.param("end_bit", end, 103);

  ROS_INFO("%s: start_bit: %d end_bit: %d ", node_name.c_str(), start, end);

  ros::Publisher left_encoder_pub   = nh.advertise<std_msgs::Float32>("left_encoder", 1);
  ros::Publisher right_encoder_pub  = nh.advertise<std_msgs::Float32>("right_encoder", 1);
  DELTA_NAMESPACE::PLC *plc = NULL;
  plc = new DELTA_NAMESPACE::PLC(ip_address, port, id);
  ros::Rate r(rate);
  uint16_t dataD[4];
  float left_encoder, righ_encoder;
  while (ros::ok())
  {   
    plc->connect();
    while (ros::ok() && plc->checkConnected())
    {
      plc->mulGetD(start, end, dataD);
      std::memcpy(reinterpret_cast<void*>(&righ_encoder), reinterpret_cast<void const*>(&dataD[0]), sizeof left_encoder);
      std::memcpy(reinterpret_cast<void*>(&left_encoder), reinterpret_cast<void const*>(&dataD[2]), sizeof righ_encoder);
      std_msgs::Float32 left_encoder_msg, righ_encoder_msg;
      left_encoder_msg.data = left_encoder * 2 * M_PI;
      righ_encoder_msg.data = righ_encoder * 2 * M_PI;
      left_encoder_pub.publish(left_encoder_msg);
      right_encoder_pub.publish(righ_encoder_msg);
      r.sleep();
      ros::spinOnce();
    }
    ros::spinOnce();
  }
  
  ros::spin();  
  plc = NULL;  
  delete(plc);
  return 0;
}