// ─── lidarlib_ros ────────────────────────────────────────────────────────────
// Cau noi thu vien C++ lidarlib (liblidarlib.so, OLEI/UDP + SICK/TCP) sang ROS 2.
//
//   lidarlib::make_lidar(cfg) -> ->open() -> ->recv_scan(r, timeout)
//     r.scan (lidarlib::LaserScan) da cung field/don vi voi sensor_msgs/LaserScan
//     => node chi copy gan nhu 1:1 roi publish de RViz hien thi.
//
// HO TRO NHIEU LIDAR CUNG LUC: param 'lidars' la danh sach ten; moi ten co bo
// tham so rieng (<ten>.ip, <ten>.port, ...). Moi lidar chay tren 1 thread rieng
// (giong examples/test_dual.cpp), publish ra topic + frame rieng.

#include <algorithm>
#include <atomic>
#include <chrono>
#include <cstdint>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include "lidarlib/lidarlib.hpp"

// Mot lidar + thread doc + publisher rieng.
struct LidarWorker
{
  std::string name;
  std::string frame_id;
  int         timeout_ms = 1000;
  float       range_min_override = 0.f;
  float       range_max_override = 0.f;
  bool        invert_in_node = false;   // dao chieu scan tai node (brand != OLEI)

  std::unique_ptr<lidarlib::Lidar> lidar;
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub;
  std::thread       worker;
  std::atomic<bool> running{false};
};

// Gom moi field cua ExtraInfo (output #2 tu thu vien) thanh 1 chuoi de in log.
// Field optional khong duoc model cap se hien '-'. error_status giai ma theo bit.
static std::string format_extra_info(const lidarlib::ExtraInfo & e)
{
  std::ostringstream os;
  os << "detected_model=" << e.detected_model
     << " error=0x" << std::hex << std::uppercase
     << static_cast<int>(e.error_status) << std::dec;
  if (e.error_status) {
    os << '[';
    if (e.error_status & 0x01) os << "Monitor ";
    if (e.error_status & 0x02) os << "Voltage ";
    if (e.error_status & 0x04) os << "Temp ";
    os << ']';
  }
  os << " dist_scale_mm=" << static_cast<int>(e.distance_scale_mm);

  // In gia tri optional (hoac '-' neu nullopt); '+' de ep uint8_t thanh so.
  auto opt = [&os](const char * name, const auto & v) {
    os << ' ' << name << '=';
    if (v) { os << +(*v); } else { os << '-'; }
  };
  opt("rotation_raw",       e.rotation_raw);
  opt("distance_ratio_raw", e.distance_ratio_raw);
  opt("scan_freq_raw",      e.scan_frequency_raw);
  opt("input_status",       e.input_status);
  opt("output_status",      e.output_status);
  opt("field_status",       e.field_status);
  opt("status_flags",       e.status_flags);
  return os.str();
}

class LidarlibNode : public rclcpp::Node
{
public:
  LidarlibNode()
  : rclcpp::Node("lidarlib_node")
  {
    // QoS reliability cho tat ca topic scan: "reliable" (mac dinh, khop RViz &
    // 'ros2 topic echo' mac dinh) hoac "best_effort" (nhe hon, hop cam bien toc do cao).
    qos_reliability_ = declare_parameter<std::string>("qos_reliability", "reliable");

    // Danh sach ten lidar can chay. Vd: ["front", "rear"]
    auto names = declare_parameter<std::vector<std::string>>(
      "lidars", std::vector<std::string>{"front"});

    for (const auto & name : names) {
      start_lidar(name);
    }

    if (workers_.empty()) {
      throw std::runtime_error("Khong mo duoc lidar nao (kiem tra tham so 'lidars' va IP/port).");
    }
    RCLCPP_INFO(get_logger(), "Dang chay %zu lidar.", workers_.size());
  }

  ~LidarlibNode() override
  {
    for (auto & w : workers_) {
      w->running.store(false);
    }
    for (auto & w : workers_) {
      if (w->worker.joinable()) {
        w->worker.join();
      }
      if (w->lidar) {
        w->lidar->close();
      }
    }
  }

private:
  void start_lidar(const std::string & name)
  {
    const std::string p = name + ".";   // tien to tham so cho lidar nay

    lidarlib::LidarConfig cfg;
    cfg.name     = name;
    cfg.ip       = declare_parameter<std::string>(p + "ip", "0.0.0.0");
    cfg.port     = static_cast<uint16_t>(declare_parameter<int>(p + "port", 2368));
    cfg.brand    = declare_parameter<std::string>(p + "brand", "OLEI");  // "OLEI" (UDP) | "SICK" (TCP)
    cfg.model    = declare_parameter<std::string>(p + "model", "AUTO");
    cfg.inverted = declare_parameter<bool>(p + "inverted", false);

    // Cua so goc output (do): thu vien remap tuyen tinh goc cua moi scan sang
    // [angle_min_deg, angle_max_deg] - khong bo diem nao, chi doi nhan goc
    // (angle_min/max/increment) trong LaserScan. Vd TiM -45..225 gan lai thanh
    // -135..135 = xoay frame. Mac dinh +/-360 = tat, giu nguyen goc tu thiet bi.
    cfg.angle_min_deg = static_cast<float>(declare_parameter<double>(p + "angle_min_deg", -360.0));
    cfg.angle_max_deg = static_cast<float>(declare_parameter<double>(p + "angle_max_deg",  360.0));

    // 'inverted' cho OLEI do thu vien (liblidarlib) xu ly ben trong. Cac brand
    // khac (vd SICK) thu vien bo qua, nen node tu dao chieu scan luc publish.
    // => tach rieng de tranh dao 2 lan voi OLEI.
    const bool is_olei = (cfg.brand == "OLEI");

    auto w = std::make_unique<LidarWorker>();
    w->name       = name;
    w->frame_id   = declare_parameter<std::string>(p + "frame_id", name);
    w->timeout_ms = declare_parameter<int>(p + "timeout_ms", 1000);
    w->range_min_override = static_cast<float>(declare_parameter<double>(p + "range_min", 0.0));
    w->range_max_override = static_cast<float>(declare_parameter<double>(p + "range_max", 0.0));
    w->invert_in_node = (cfg.inverted && !is_olei);

    const std::string topic = declare_parameter<std::string>(p + "topic", "scan_" + name);

    rclcpp::QoS qos(rclcpp::KeepLast(10));
    if (qos_reliability_ == "best_effort") {
      qos.best_effort();
    } else {
      qos.reliable();
    }
    w->pub = create_publisher<sensor_msgs::msg::LaserScan>(topic, qos);

    // Chi in cua so goc khi user thu hep tu mac dinh +/-360 (co remap).
    std::string angle_note;
    if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
      std::ostringstream os;
      os << " goc[" << cfg.angle_min_deg << ".." << cfg.angle_max_deg << "]deg";
      angle_note = os.str();
    }

    RCLCPP_INFO(get_logger(),
      "[%s] brand=%s model=%s %s:%u inverted=%d%s -> topic '%s' frame '%s'",
      name.c_str(), cfg.brand.c_str(), cfg.model.c_str(),
      cfg.ip.c_str(), cfg.port, cfg.inverted, angle_note.c_str(),
      topic.c_str(), w->frame_id.c_str());

    w->lidar = lidarlib::make_lidar(cfg);   // khong bao gio tra ve nullptr
    if (!w->lidar->open()) {
      RCLCPP_ERROR(get_logger(),
        "[%s] Khong mo duoc lidar (%s:%u) - bo qua con nay. "
        "Kiem tra IP/port, cap mang, hoac port dang bi tien trinh khac giu.",
        name.c_str(), cfg.ip.c_str(), cfg.port);
      return;   // khong lam sap node; cac lidar khac van chay
    }

    w->running.store(true);
    LidarWorker * wp = w.get();
    wp->worker = std::thread([this, wp]() { spin_recv(wp); });
    workers_.push_back(std::move(w));
  }

  void spin_recv(LidarWorker * w)
  {
    lidarlib::ScanResult r;

    // So lan recv that bai lien tiep truoc khi coi la mat ket noi va mo lai.
    // Can cho SICK/TCP: khi socket TCP dut, recv_scan timeout mai mai neu khong
    // close+open lai. OLEI/UDP thi mo lai cung vo hai (chi rebind socket).
    const int reconnect_after = 5;
    int consecutive_failures = 0;

    while (w->running.load() && rclcpp::ok()) {
      if (!w->lidar->recv_scan(r, w->timeout_ms)) {
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
          "[%s] Chua nhan duoc scan (timeout %d ms). Cho goi tu lidar...",
          w->name.c_str(), w->timeout_ms);

        if (++consecutive_failures >= reconnect_after) {
          RCLCPP_WARN(get_logger(),
            "[%s] Mat ket noi (%d lan lien tiep). Dang mo lai...",
            w->name.c_str(), consecutive_failures);
          w->lidar->close();
          std::this_thread::sleep_for(std::chrono::milliseconds(500));  // backoff
          if (w->lidar->open()) {
            RCLCPP_INFO(get_logger(), "[%s] Da mo lai ket noi.", w->name.c_str());
            consecutive_failures = 0;
          } else {
            RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 2000,
              "[%s] Mo lai that bai, se thu lai...", w->name.c_str());
          }
        }
        continue;
      }
      consecutive_failures = 0;
      publish(w, r);
    }
  }

  void publish(LidarWorker * w, const lidarlib::ScanResult & r)
  {
    const auto & s = r.scan;

    sensor_msgs::msg::LaserScan msg;
    msg.header.stamp    = now();
    msg.header.frame_id = w->frame_id;

    msg.angle_min       = s.angle_min;
    msg.angle_max       = s.angle_max;
    msg.angle_increment = s.angle_increment;
    msg.time_increment  = s.time_increment;   // thu vien luon = 0
    msg.scan_time       = s.scan_time;         // thu vien luon = 0
    msg.range_min       = (w->range_min_override > 0.f) ? w->range_min_override : s.range_min;
    msg.range_max       = (w->range_max_override > 0.f) ? w->range_max_override : s.range_max;

    msg.ranges          = s.ranges;
    msg.intensities     = s.intensities;

    // Lidar lap up nguoc (brand != OLEI): dao chieu scan bang cach lat thu tu
    // cac diem. Goc angle_min/max/increment giu nguyen -> tuong duong mirror
    // quanh truc cam bien, khop voi cach thu vien OLEI xu ly 'inverted'.
    if (w->invert_in_node) {
      std::reverse(msg.ranges.begin(), msg.ranges.end());
      std::reverse(msg.intensities.begin(), msg.intensities.end());
    }

    w->pub->publish(msg);

    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
      "[%s] scan: %zu diem, ts=%u ms, FOV[%.1f..%.1f]deg | %s",
      w->name.c_str(), s.ranges.size(), s.timestamp_ms,
      s.angle_min * 180.0f / static_cast<float>(M_PI),
      s.angle_max * 180.0f / static_cast<float>(M_PI),
      format_extra_info(r.info).c_str());
  }

  std::string qos_reliability_ = "reliable";
  std::vector<std::unique_ptr<LidarWorker>> workers_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  try {
    rclcpp::spin(std::make_shared<LidarlibNode>());
  } catch (const std::exception & e) {
    RCLCPP_ERROR(rclcpp::get_logger("lidarlib_node"), "Thoat: %s", e.what());
    rclcpp::shutdown();
    return 1;
  }
  rclcpp::shutdown();
  return 0;
}