#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <string>
#include <vector>
#include <thread>
#include <chrono>

// 假设使用的是通过串口通信的LIDAR
// 实际应用中需要替换为具体LIDAR的SDK或通信库
#include <serial/serial.h>

class LidarDriverNode : public rclcpp::Node
{
public:
  LidarDriverNode() : Node("lidar_driver_node")
  {
    // 声明并获取参数
    this->declare_parameter<std::string>("port", "/dev/ttyUSB0");
    this->declare_parameter<int>("baudrate", 115200);
    this->declare_parameter<std::string>("frame_id", "laser_frame");
    this->declare_parameter<double>("frequency", 10.0);
    
    port_ = this->get_parameter("port").as_string();
    baudrate_ = this->get_parameter("baudrate").as_int();
    frame_id_ = this->get_parameter("frame_id").as_string();
    frequency_ = this->get_parameter("frequency").as_double();
    
    // 创建激光扫描消息发布者
    scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
    
    // 初始化LIDAR
    if (!initLidar()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize LIDAR");
      return;
    }
    
    // 创建数据采集线程
    data_thread_ = std::thread(&LidarDriverNode::dataCollectionLoop, this);
    RCLCPP_INFO(this->get_logger(), "LIDAR driver node initialized");
  }
  
  ~LidarDriverNode()
  {
    // 停止数据采集线程
    running_ = false;
    if (data_thread_.joinable()) {
      data_thread_.join();
    }
    
    // 关闭串口
    if (serial_port_.isOpen()) {
      serial_port_.close();
    }
  }

private:
  bool initLidar()
  {
    try {
      // 配置串口
      serial_port_.setPort(port_);
      serial_port_.setBaudrate(baudrate_);
      serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
      serial_port_.setTimeout(timeout);
      
      // 打开串口
      serial_port_.open();
      
      if (serial_port_.isOpen()) {
        RCLCPP_INFO(this->get_logger(), "Successfully connected to LIDAR on port %s", port_.c_str());
        
        // 发送初始化命令（根据具体LIDAR型号调整）
        std::string init_cmd = "start_scan\n";
        serial_port_.write(init_cmd);
        
        // 等待LIDAR初始化
        std::this_thread::sleep_for(std::chrono::seconds(1));
        return true;
      } else {
        RCLCPP_ERROR(this->get_logger(), "Unable to open port %s", port_.c_str());
        return false;
      }
    } catch (serial::SerialException& e) {
      RCLCPP_ERROR(this->get_logger(), "Serial exception: %s", e.what());
      return false;
    }
  }
  
  void dataCollectionLoop()
  {
    running_ = true;
    rclcpp::Rate rate(frequency_);
    
    // 激光扫描消息初始化
    auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
    scan_msg->header.frame_id = frame_id_;
    
    // 假设LIDAR参数（根据实际LIDAR型号调整）
    scan_msg->angle_min = -3.141592;  // -180度
    scan_msg->angle_max = 3.141592;   // 180度
    scan_msg->angle_increment = 0.0174533;  // 约1度
    scan_msg->time_increment = 0.0;
    scan_msg->scan_time = 1.0 / frequency_;
    scan_msg->range_min = 0.1;  // 最小检测距离10厘米
    scan_msg->range_max = 30.0; // 最大检测距离30米
    
    // 存储激光点数量
    size_t num_points = static_cast<size_t>((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment) + 1;
    scan_msg->ranges.resize(num_points);
    scan_msg->intensities.resize(num_points);
    
    while (running_ && rclcpp::ok()) {
      try {
        // 读取LIDAR数据（根据具体LIDAR协议解析）
        if (serial_port_.available()) {
          std::string data = serial_port_.read(serial_port_.available());
          
          // 解析数据（示例，实际需根据LIDAR协议实现）
          if (parseLidarData(data, scan_msg->ranges, scan_msg->intensities)) {
            // 设置时间戳
            scan_msg->header.stamp = this->now();
            
            // 发布激光扫描消息
            scan_pub_->publish(std::move(scan_msg));
            
            // 为下一次发布创建新消息
            scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
            scan_msg->header.frame_id = frame_id_;
            scan_msg->angle_min = -3.141592;
            scan_msg->angle_max = 3.141592;
            scan_msg->angle_increment = 0.0174533;
            scan_msg->time_increment = 0.0;
            scan_msg->scan_time = 1.0 / frequency_;
            scan_msg->range_min = 0.1;
            scan_msg->range_max = 30.0;
            scan_msg->ranges.resize(num_points);
            scan_msg->intensities.resize(num_points);
          }
        }
      } catch (serial::SerialException& e) {
        RCLCPP_ERROR(this->get_logger(), "Serial exception during data collection: %s", e.what());
        running_ = false;
      }
      
      rate.sleep();
    }
  }
  
  bool parseLidarData(const std::string& data, std::vector<float>& ranges, std::vector<float>& intensities)
  {
    // 这里需要根据具体LIDAR的数据协议实现解析逻辑
    // 示例代码仅作为框架，实际应用中需要替换为真实的解析逻辑
    
    // 简单示例：假设数据格式为"RANGE1,INTENSITY1;RANGE2,INTENSITY2;..."
    // 注意：这只是一个示例，实际LIDAR协议会复杂得多
    
    size_t pos = 0;
    size_t point_index = 0;
    std::string token;
    
    while ((pos = data.find(';')) != std::string::npos && point_index < ranges.size()) {
      token = data.substr(0, pos);
      
      // 解析距离和强度
      size_t comma_pos = token.find(',');
      if (comma_pos != std::string::npos) {
        try {
          float range = std::stof(token.substr(0, comma_pos));
          float intensity = std::stof(token.substr(comma_pos + 1));
          
          // 检查范围
          if (range >= 0.1 && range <= 30.0) {
            ranges[point_index] = range;
            intensities[point_index] = intensity;
          } else {
            ranges[point_index] = std::numeric_limits<float>::infinity();
            intensities[point_index] = 0.0;
          }
        } catch (const std::invalid_argument&) {
          // 解析失败，设置为无穷大
          ranges[point_index] = std::numeric_limits<float>::infinity();
          intensities[point_index] = 0.0;
        }
      }
      
      data.erase(0, pos + 1);
      point_index++;
    }
    
    // 如果解析到了足够的点，返回true
    return point_index > ranges.size() / 2;
  }
  
  // 串口通信对象
  serial::Serial serial_port_;
  
  // 参数
  std::string port_;
  int baudrate_;
  std::string frame_id_;
  double frequency_;
  
  // 发布者
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
  
  // 数据采集线程
  std::thread data_thread_;
  bool running_{false};
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LidarDriverNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}