#include <cmath>
#include <deque>
#include <string>
#include <utility>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>

#include "MvCameraControl.h"
#include "camera_info_manager/camera_info_manager.hpp"
#include "image_transport/image_transport.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/utilities.hpp"

namespace hik_camera_ros2_driver
{

struct TimeStamp {
  int64_t high;
  int64_t low;
};

class HikCameraRos2DriverNode : public rclcpp::Node
{
public:
  explicit HikCameraRos2DriverNode(const rclcpp::NodeOptions & options)
  : Node("hik_camera_ros2_driver", options)
  {
    RCLCPP_INFO(this->get_logger(), "Starting HikCameraRos2DriverNode!");

    initializeCamera();
    declareParameters();
    startCamera();
    initSharedMemory();

    params_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&HikCameraRos2DriverNode::dynamicParametersCallback, this, std::placeholders::_1));

    capture_thread_ = std::thread(&HikCameraRos2DriverNode::captureLoop, this);
  }

  ~HikCameraRos2DriverNode() override
  {
    if (capture_thread_.joinable()) {
      capture_thread_.join();
    }
    if (camera_handle_) {
      MV_CC_StopGrabbing(camera_handle_);
      MV_CC_CloseDevice(camera_handle_);
      MV_CC_DestroyHandle(&camera_handle_);
    }
    if (pointt_ != nullptr && pointt_ != MAP_FAILED) {
      munmap(pointt_, sizeof(TimeStamp));
      pointt_ = nullptr;
    }
    RCLCPP_INFO(this->get_logger(), "HikCameraRos2DriverNode destroyed!");
  }

private:
  bool initializeCamera()
  {
    // serial_number을 먼저 선언하여 카메라 선택에 사용
    serial_number_ = this->declare_parameter("serial_number", std::string(""));

    MV_CC_DEVICE_INFO_LIST device_list;

    // USB + GigE 동시 열거
    while (rclcpp::ok()) {
      n_ret_ = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
      if (n_ret_ != MV_OK) {
        RCLCPP_ERROR(this->get_logger(), "Failed to enumerate devices, retrying...");
        std::this_thread::sleep_for(std::chrono::seconds(1));
      } else if (device_list.nDeviceNum == 0) {
        RCLCPP_ERROR(this->get_logger(), "No camera found, retrying...");
        std::this_thread::sleep_for(std::chrono::seconds(1));
      } else {
        RCLCPP_INFO(this->get_logger(), "Found camera count = %d", device_list.nDeviceNum);
        break;
      }
    }

    // 시리얼 번호 기반 카메라 선택
    unsigned int target_index = 0;
    if (!serial_number_.empty()) {
      bool found = false;
      for (unsigned int i = 0; i < device_list.nDeviceNum; i++) {
        MV_CC_DEVICE_INFO * info = device_list.pDeviceInfo[i];
        std::string serial;
        if (info->nTLayerType == MV_USB_DEVICE) {
          serial = std::string(
            reinterpret_cast<char *>(info->SpecialInfo.stUsb3VInfo.chSerialNumber));
        } else if (info->nTLayerType == MV_GIGE_DEVICE) {
          serial = std::string(
            reinterpret_cast<char *>(info->SpecialInfo.stGigEInfo.chSerialNumber));
        }
        if (serial == serial_number_) {
          target_index = i;
          found = true;
          RCLCPP_INFO(this->get_logger(), "Matched camera serial: %s", serial.c_str());
          break;
        }
      }
      if (!found) {
        RCLCPP_FATAL(
          this->get_logger(), "Camera with serial [%s] not found!", serial_number_.c_str());
        rclcpp::shutdown();
        return false;
      }
    }

    n_ret_ = MV_CC_CreateHandle(&camera_handle_, device_list.pDeviceInfo[target_index]);
    if (n_ret_ != MV_OK) {
      RCLCPP_ERROR(this->get_logger(), "Failed to create camera handle!");
      return false;
    }

    n_ret_ = MV_CC_OpenDevice(camera_handle_);
    if (n_ret_ != MV_OK) {
      RCLCPP_ERROR(this->get_logger(), "Failed to open camera device!");
      return false;
    }

    // Get camera information
    n_ret_ = MV_CC_GetImageInfo(camera_handle_, &img_info_);
    if (n_ret_ != MV_OK) {
      RCLCPP_ERROR(this->get_logger(), "Failed to get camera image info!");
      return false;
    }

    // Init convert param
    image_msg_.data.reserve(img_info_.nHeightMax * img_info_.nWidthMax * 3);
    convert_param_.nWidth = img_info_.nWidthValue;
    convert_param_.nHeight = img_info_.nHeightValue;
    convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;

    return true;
  }

  void declareParameters()
  {
    rcl_interfaces::msg::ParameterDescriptor param_desc;
    MVCC_FLOATVALUE f_value;
    param_desc.integer_range.resize(1);
    param_desc.integer_range[0].step = 1;

    // Trigger mode
    trigger_enable_ = this->declare_parameter("trigger_enable", false);
    use_trigger_timestamp_ = this->declare_parameter("use_trigger_timestamp", false);
    enable_interval_log_ = this->declare_parameter("enable_interval_log", false);
    dev_clock_window_sec_ = this->declare_parameter("dev_clock_window_sec", 10.0);
    dev_clock_recalib_interval_sec_ =
      this->declare_parameter("dev_clock_recalib_interval_sec", 2.0);

    if (trigger_enable_) {
      // 하드웨어 트리거: 프레임률은 트리거 신호가 제어
      MV_CC_SetBoolValue(camera_handle_, "AcquisitionFrameRateEnable", false);
      MV_CC_SetEnumValue(camera_handle_, "TriggerMode", 1);          // On
      MV_CC_SetEnumValue(camera_handle_, "TriggerSource", MV_TRIGGER_SOURCE_LINE0);
      RCLCPP_INFO(this->get_logger(), "Hardware trigger enabled (LINE0)");
    } else {
      MV_CC_SetEnumValue(camera_handle_, "TriggerMode", 0);          // Off

      // Acquisition frame rate (프리런 모드에서만 사용)
      param_desc.description = "Acquisition frame rate in Hz";
      MV_CC_GetFloatValue(camera_handle_, "AcquisitionFrameRate", &f_value);
      param_desc.integer_range[0].from_value = static_cast<int64_t>(f_value.fMin);
      param_desc.integer_range[0].to_value = static_cast<int64_t>(f_value.fMax);
      double acquisition_frame_rate =
        this->declare_parameter("acquisition_frame_rate", 165.0, param_desc);
      MV_CC_SetBoolValue(camera_handle_, "AcquisitionFrameRateEnable", true);
      MV_CC_SetFloatValue(camera_handle_, "AcquisitionFrameRate", acquisition_frame_rate);
      RCLCPP_INFO(this->get_logger(), "Acquisition frame rate: %f", acquisition_frame_rate);
    }

    // Exposure — 모든 파라미터를 항상 선언하여 런타임 모드 전환 지원
    MV_CC_GetFloatValue(camera_handle_, "ExposureTime", &f_value);
    exposure_auto_ = this->declare_parameter("exposure_auto", false);

    param_desc.description = "Auto exposure target brightness (0-255)";
    param_desc.integer_range[0].from_value = 0;
    param_desc.integer_range[0].to_value = 255;
    exposure_auto_target_brightness_ =
      this->declare_parameter("exposure_auto_target_brightness", 128, param_desc);

    param_desc.description = "Auto exposure lower limit in microseconds";
    param_desc.integer_range[0].from_value = static_cast<int64_t>(f_value.fMin);
    param_desc.integer_range[0].to_value = static_cast<int64_t>(f_value.fMax);
    exposure_auto_min_ = this->declare_parameter("exposure_auto_min", 100.0, param_desc);

    param_desc.description = "Auto exposure upper limit in microseconds";
    exposure_auto_max_ = this->declare_parameter("exposure_auto_max", 10000.0, param_desc);

    param_desc.description = "Manual exposure time in microseconds";
    exposure_time_ = this->declare_parameter("exposure_time", 5000, param_desc);

    applyExposureMode();

    // Gain (manual only — GainAuto=Off)
    MV_CC_SetEnumValue(camera_handle_, "GainAuto", 0);
    param_desc.description = "Gain";
    MV_CC_GetFloatValue(camera_handle_, "Gain", &f_value);
    param_desc.integer_range[0].from_value = static_cast<int64_t>(f_value.fMin);
    param_desc.integer_range[0].to_value = static_cast<int64_t>(f_value.fMax);
    double gain = this->declare_parameter("gain", f_value.fCurValue, param_desc);
    MV_CC_SetFloatValue(camera_handle_, "Gain", gain);
    RCLCPP_INFO(this->get_logger(), "Gain: %f", gain);

    // White balance — 기본은 수동(Off) + 고정 R/G/B 비율.
    // 이유: 카메라별로 독립 Continuous AWB를 켜두면 두 카메라가 서로 다른 화각을 보고
    // 각자 다른 색으로 수렴해서, 겹치는 영역의 색감이 안 맞게 된다. 두 카메라에
    // 동일한 balance_ratio_red/green/blue 값을 넣어야 겹치는 부분 색이 일치한다.
    param_desc.description = "Auto white balance (Continuous). false면 수동 R/G/B 비율 사용";
    balance_white_auto_ = this->declare_parameter("balance_white_auto", false);

    MVCC_INTVALUE wb_range;
    MV_CC_GetBalanceRatioRed(camera_handle_, &wb_range);
    param_desc.integer_range[0].from_value = static_cast<int64_t>(wb_range.nMin);
    param_desc.integer_range[0].to_value = static_cast<int64_t>(wb_range.nMax);
    param_desc.description = "Manual white balance ratio - Red (balance_white_auto=false일 때만 적용)";
    int balance_ratio_red = this->declare_parameter("balance_ratio_red", 1024, param_desc);
    param_desc.description = "Manual white balance ratio - Green (balance_white_auto=false일 때만 적용)";
    int balance_ratio_green = this->declare_parameter("balance_ratio_green", 1024, param_desc);
    param_desc.description = "Manual white balance ratio - Blue (balance_white_auto=false일 때만 적용)";
    int balance_ratio_blue = this->declare_parameter("balance_ratio_blue", 1024, param_desc);

    if (balance_white_auto_) {
      MV_CC_SetEnumValue(camera_handle_, "BalanceWhiteAuto", MV_BALANCEWHITE_AUTO_CONTINUOUS);
      RCLCPP_INFO(this->get_logger(), "White balance: Continuous auto");
    } else {
      MV_CC_SetEnumValue(camera_handle_, "BalanceWhiteAuto", MV_BALANCEWHITE_AUTO_OFF);
      MV_CC_SetBalanceRatioRed(camera_handle_, static_cast<unsigned int>(balance_ratio_red));
      MV_CC_SetBalanceRatioGreen(camera_handle_, static_cast<unsigned int>(balance_ratio_green));
      MV_CC_SetBalanceRatioBlue(camera_handle_, static_cast<unsigned int>(balance_ratio_blue));
      RCLCPP_INFO(
        this->get_logger(), "White balance: Manual R=%d G=%d B=%d",
        balance_ratio_red, balance_ratio_green, balance_ratio_blue);
    }

    int status;

    // ADC Bit Depth
    param_desc.description = "ADC Bit Depth";
    param_desc.additional_constraints = "Supported values: Bits_8, Bits_12";
    std::string adc_bit_depth = this->declare_parameter("adc_bit_depth", "Bits_8", param_desc);
    status = MV_CC_SetEnumValueByString(camera_handle_, "ADCBitDepth", adc_bit_depth.c_str());
    if (status == MV_OK) {
      RCLCPP_INFO(this->get_logger(), "ADC Bit Depth set to %s", adc_bit_depth.c_str());
    } else {
      RCLCPP_ERROR(this->get_logger(), "Failed to set ADC Bit Depth, status = %d", status);
    }

    // Pixel format
    param_desc.description = "Pixel Format";
    std::string pixel_format = this->declare_parameter("pixel_format", "RGB8Packed", param_desc);
    status = MV_CC_SetEnumValueByString(camera_handle_, "PixelFormat", pixel_format.c_str());
    if (status == MV_OK) {
      RCLCPP_INFO(this->get_logger(), "Pixel Format set to %s", pixel_format.c_str());
    } else {
      RCLCPP_ERROR(this->get_logger(), "Failed to set Pixel Format, status = %d", status);
    }
  }

  void startCamera()
  {
    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
    camera_name_ = this->declare_parameter("camera_name", "camera");
    frame_id_ = this->declare_parameter("frame_id", camera_name_ + "_optical_frame");
    camera_topic_ = this->declare_parameter("camera_topic", camera_name_ + "/image");

    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
    camera_pub_ = image_transport::create_camera_publisher(this, camera_topic_, qos);

    MV_CC_StartGrabbing(camera_handle_);

    // Load camera info
    camera_info_manager_ =
      std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
    auto camera_info_url = this->declare_parameter(
      "camera_info_url", "package://hik_camera_ros2_driver/config/camera_info.yaml");
    if (camera_info_manager_->validateURL(camera_info_url)) {
      camera_info_manager_->loadCameraInfo(camera_info_url);
      camera_info_msg_ = camera_info_manager_->getCameraInfo();
    } else {
      RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s", camera_info_url.c_str());
    }
  }

  void initSharedMemory()
  {
    if (!use_trigger_timestamp_) {
      return;
    }

    const char * user_name = getlogin();
    if (user_name == nullptr) {
      RCLCPP_WARN(this->get_logger(), "getlogin() failed, falling back to system time");
      return;
    }

    std::string path = "/home/" + std::string(user_name) + "/timeshare";
    int fd = open(path.c_str(), O_RDWR);
    if (fd == -1) {
      RCLCPP_WARN(
        this->get_logger(),
        "Failed to open shared memory file: %s (LiDAR driver may not be running yet)",
        path.c_str());
      return;
    }

    pointt_ = reinterpret_cast<TimeStamp *>(
      mmap(nullptr, sizeof(TimeStamp), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0));
    close(fd);

    if (pointt_ == MAP_FAILED) {
      RCLCPP_WARN(this->get_logger(), "mmap failed, falling back to system time");
      pointt_ = nullptr;
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Shared memory timestamp enabled: %s", path.c_str());
  }

  void applyExposureMode()
  {
    if (exposure_auto_) {
      MV_CC_SetEnumValue(camera_handle_, "ExposureAuto", 2);  // Continuous
      MV_CC_SetIntValue(camera_handle_, "AutoTargetBrightness",
        static_cast<unsigned int>(exposure_auto_target_brightness_));
      MV_CC_SetIntValue(camera_handle_, "AutoExposureTimeLowerLimit",
        static_cast<unsigned int>(exposure_auto_min_));
      MV_CC_SetIntValue(camera_handle_, "AutoExposureTimeUpperLimit",
        static_cast<unsigned int>(exposure_auto_max_));
      RCLCPP_INFO(this->get_logger(),
        "Auto exposure: ON, brightness=%d, range=[%.0f, %.0f] us",
        exposure_auto_target_brightness_, exposure_auto_min_, exposure_auto_max_);
    } else {
      MV_CC_SetEnumValue(camera_handle_, "ExposureAuto", 0);  // Off
      MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
      RCLCPP_INFO(this->get_logger(), "Manual exposure: %d us", exposure_time_);
    }
  }

  // 2026-07-29: nTriggerIndex는 이 카메라(MV-CS016-10UC USB3)에서 항상 0으로 고정되어
  // 실사용 불가로 확인됨 (실기기 로그로 검증). 대신 nDevTimeStampHigh/Low(카메라 자체
  // 자유구동 하드웨어 클럭)는 프레임마다 정상적으로 증가하는 것을 로그로 확인했다.
  //
  // 처음엔 시작 후 고정 3초 구간만 보고 회귀를 얼려서(freeze) 썼는데, 실기기 이동
  // 테스트(bag: trigger_test__2_moving)에서 카메라-라이다 오프셋이 27초 동안
  // -12ms → -16ms로 선형 드리프트하는 것이 확인됨 — 3초짜리 창만으로는 카메라
  // 클럭의 실제 틱 주파수를 ~150ppm 정도의 오차로만 추정하게 되고, 이 오차가 세션이
  // 길어질수록 그대로 누적된다. 그래서 한 번 얼리는 대신, 최근
  // dev_clock_window_sec_ 구간의 슬라이딩 윈도우로 dev_clock_recalib_interval_sec_
  // 마다 회귀를 반복 갱신한다 — 오차가 세션 길이에 비례해 누적되지 않고 창 하나의
  // 오차 수준으로 항상 bounded 된다.
  void recalibrateDevClock()
  {
    double sum_x = 0.0, sum_y = 0.0;
    for (const auto & s : dev_clock_samples_) {
      sum_x += s.first;
      sum_y += s.second;
    }
    double n = static_cast<double>(dev_clock_samples_.size());
    double mean_x = sum_x / n;
    double mean_y = sum_y / n;

    double num = 0.0, den = 0.0;
    for (const auto & s : dev_clock_samples_) {
      double dx = s.first - mean_x;
      num += dx * (s.second - mean_y);
      den += dx * dx;
    }

    double slope_ns_per_tick = (den > 0.0) ? (num / den) : 0.0;

    if (!(den > 0.0) || !std::isfinite(slope_ns_per_tick) || slope_ns_per_tick <= 0.0) {
      dev_clock_calib_fail_count_++;
      RCLCPP_ERROR(
        this->get_logger(),
        "Device-clock timestamp (re)calibration failed (slope=%.6f, %zu samples, fail #%d).",
        slope_ns_per_tick, dev_clock_samples_.size(), dev_clock_calib_fail_count_);
      if (dev_clock_calib_fail_count_ >= 3) {
        dev_clock_permanently_disabled_ = true;
        RCLCPP_ERROR(
          this->get_logger(),
          "Device-clock timestamp calibration failed %d times in a row — this camera/SDK does "
          "not usably populate nDevTimeStamp either. Falling back to per-frame shared-memory "
          "timestamp for the rest of this session.",
          dev_clock_calib_fail_count_);
      }
      return;
    }
    dev_clock_calib_fail_count_ = 0;

    bool first_calibration = !dev_clock_usable_;
    dev_clock_slope_ns_per_tick_ = slope_ns_per_tick;
    dev_clock_mean_ticks_ = mean_x;
    dev_clock_mean_rel_ns_ = mean_y;
    dev_clock_usable_ = true;

    if (first_calibration || enable_interval_log_) {
      RCLCPP_INFO(
        this->get_logger(),
        "Device-clock timestamp %scalibrated: %.6f ns/tick (%.3f MHz), %zu samples over %.2f s",
        first_calibration ? "" : "re",
        slope_ns_per_tick, 1000.0 / slope_ns_per_tick, dev_clock_samples_.size(),
        (dev_clock_samples_.back().second - dev_clock_samples_.front().second) / 1e9);
    }
  }

  rclcpp::Time getTimestamp(const MV_FRAME_OUT_INFO_EX & frame_info)
  {
    bool have_shared_ts = pointt_ != nullptr && pointt_ != MAP_FAILED && pointt_->low != 0;

    if (trigger_enable_ && use_trigger_timestamp_ && !dev_clock_permanently_disabled_) {
      double dev_ticks = static_cast<double>(
        (static_cast<uint64_t>(frame_info.nDevTimeStampHigh) << 32) |
        frame_info.nDevTimeStampLow);

      if (have_shared_ts) {
        if (!dev_clock_ref_set_) {
          dev_clock_ref_ns_ = pointt_->low;
          dev_clock_ref_set_ = true;
        }
        double rel_ns = static_cast<double>(pointt_->low - dev_clock_ref_ns_);
        dev_clock_samples_.emplace_back(dev_ticks, rel_ns);

        // 창 크기 판정을 먼저 하고, 실제로 사용한 뒤에 오래된 샘플을 잘라낸다.
        // (이전 버그: pruning을 먼저 하면 창이 window_sec를 절대 넘을 수 없어서
        // "10초 이상 쌓였는가" 조건이 영원히 참이 될 수 없었다 — 2026-07-29
        // [DEVCLOCK-HB] 로그로 window_span_sec이 9.9~10.0에서 멈춰있는 것으로 확인.)
        double window_span_sec =
          (dev_clock_samples_.back().second - dev_clock_samples_.front().second) / 1e9;
        bool warmed_up = window_span_sec >= dev_clock_window_sec_ &&
          dev_clock_samples_.size() >= 10;
        bool due = (rel_ns - dev_clock_last_recalib_rel_ns_) >=
          dev_clock_recalib_interval_sec_ * 1e9;

        if (++dev_clock_heartbeat_count_ % 50 == 0) {
          RCLCPP_INFO(
            this->get_logger(),
            "[DEVCLOCK-HB] rel_ns=%.0f window_span_sec=%.3f samples=%zu usable=%d "
            "permanently_disabled=%d",
            rel_ns, window_span_sec, dev_clock_samples_.size(),
            dev_clock_usable_, dev_clock_permanently_disabled_);
        }

        if (warmed_up && (!dev_clock_usable_ || due)) {
          recalibrateDevClock();
          dev_clock_last_recalib_rel_ns_ = rel_ns;
        }

        while (!dev_clock_samples_.empty() &&
          (rel_ns - dev_clock_samples_.front().second) > dev_clock_window_sec_ * 1e9)
        {
          dev_clock_samples_.pop_front();
        }
      }

      if (dev_clock_usable_) {
        double rel_ns = dev_clock_mean_rel_ns_ +
          (dev_ticks - dev_clock_mean_ticks_) * dev_clock_slope_ns_per_tick_;
        int64_t final_ns = dev_clock_ref_ns_ + static_cast<int64_t>(std::llround(rel_ns));
        return rclcpp::Time(final_ns);
      }
    }

    if (enable_interval_log_) {
      RCLCPP_INFO(
        this->get_logger(),
        "[TS-DEBUG] nFrameNum=%u nTriggerIndex=%u nDevTimeStampHigh=%u nDevTimeStampLow=%u "
        "nHostTimeStamp=%ld",
        frame_info.nFrameNum, frame_info.nTriggerIndex, frame_info.nDevTimeStampHigh,
        frame_info.nDevTimeStampLow, frame_info.nHostTimeStamp);
    }

    if (have_shared_ts) {
      return rclcpp::Time(pointt_->low);
    }
    return this->now();
  }

  void captureLoop()
  {
    MV_FRAME_OUT out_frame;
    RCLCPP_INFO(this->get_logger(), "Publishing image!");

    image_msg_.header.frame_id = frame_id_;
    image_msg_.encoding = "rgb8";

    // [DEBUG] 타임스탬프 간격 측정용 임시 변수
    rclcpp::Time last_stamp{0, 0, RCL_ROS_TIME};
    double interval_min_ms = 1e9, interval_max_ms = 0.0, interval_sum_ms = 0.0;
    int interval_count = 0;
    static constexpr int kLogEveryN = 10;

    while (rclcpp::ok()) {
      n_ret_ = MV_CC_GetImageBuffer(camera_handle_, &out_frame, 1000);
      if (MV_OK == n_ret_) {
        convert_param_.pDstBuffer = image_msg_.data.data();
        convert_param_.nDstBufferSize = image_msg_.data.size();
        convert_param_.pSrcData = out_frame.pBufAddr;
        convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
        convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;

        MV_CC_ConvertPixelType(camera_handle_, &convert_param_);

        image_msg_.header.stamp = getTimestamp(out_frame.stFrameInfo);
        image_msg_.height = out_frame.stFrameInfo.nHeight;
        image_msg_.width = out_frame.stFrameInfo.nWidth;
        image_msg_.step = out_frame.stFrameInfo.nWidth * 3;
        image_msg_.data.resize(image_msg_.width * image_msg_.height * 3);

        if (enable_interval_log_) {
          rclcpp::Time cur_stamp = image_msg_.header.stamp;
          if (last_stamp.nanoseconds() != 0) {
            double interval_ms = (cur_stamp - last_stamp).seconds() * 1000.0;
            if (interval_ms > 0.0) {
              interval_min_ms = std::min(interval_min_ms, interval_ms);
              interval_max_ms = std::max(interval_max_ms, interval_ms);
              interval_sum_ms += interval_ms;
              interval_count++;
            }
            RCLCPP_INFO(this->get_logger(),
              "[TS] interval=%.3f ms", interval_ms);
            if (interval_count > 0 && interval_count % kLogEveryN == 0) {
              RCLCPP_INFO(this->get_logger(),
                "[TS] last %d frames: avg=%.3f ms  min=%.3f ms  max=%.3f ms  jitter=%.3f ms",
                kLogEveryN,
                interval_sum_ms / interval_count,
                interval_min_ms, interval_max_ms,
                interval_max_ms - interval_min_ms);
              interval_min_ms = 1e9; interval_max_ms = 0.0;
              interval_sum_ms = 0.0; interval_count = 0;
            }
          }
          last_stamp = cur_stamp;
        }

        camera_info_msg_.header = image_msg_.header;
        camera_pub_.publish(image_msg_, camera_info_msg_);

        MV_CC_FreeImageBuffer(camera_handle_, &out_frame);

        static auto last_log_time = std::chrono::steady_clock::now();
        auto now = std::chrono::steady_clock::now();
        if (std::chrono::duration_cast<std::chrono::seconds>(now - last_log_time).count() >= 3) {
          MVCC_FLOATVALUE f_value;
          MV_CC_GetFloatValue(camera_handle_, "ResultingFrameRate", &f_value);
          RCLCPP_DEBUG(this->get_logger(), "ResultingFrameRate: %f Hz", f_value.fCurValue);

          if (balance_white_auto_) {
            // Continuous AWB로 수렴 중인 R/G/B 값을 주기적으로 노출 — 여러 카메라의 색을
            // 맞추려면 이 값을 읽어서 balance_white_auto=false + 동일 값으로 고정시키면 됨.
            MVCC_INTVALUE wb_r, wb_g, wb_b;
            MV_CC_GetBalanceRatioRed(camera_handle_, &wb_r);
            MV_CC_GetBalanceRatioGreen(camera_handle_, &wb_g);
            MV_CC_GetBalanceRatioBlue(camera_handle_, &wb_b);
            RCLCPP_INFO(
              this->get_logger(), "White balance (auto, converging): R=%u G=%u B=%u",
              wb_r.nCurValue, wb_g.nCurValue, wb_b.nCurValue);
          }
          last_log_time = now;
        }

      } else {
        RCLCPP_WARN(this->get_logger(), "Get buffer failed! nRet: [%x]", n_ret_);
        MV_CC_StopGrabbing(camera_handle_);
        MV_CC_StartGrabbing(camera_handle_);
        fail_count_++;
      }

      if (fail_count_ > 5) {
        RCLCPP_FATAL(this->get_logger(), "Camera failed!");
        rclcpp::shutdown();
      }
    }
  }

  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
    const std::vector<rclcpp::Parameter> & parameters)
  {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;

    for (const auto & param : parameters) {
      const auto & type = param.get_type();
      const auto & name = param.get_name();
      int status = MV_OK;

      if (type == rclcpp::ParameterType::PARAMETER_BOOL) {
        if (name == "enable_interval_log") {
          enable_interval_log_ = param.as_bool();
        } else if (name == "exposure_auto") {
          exposure_auto_ = param.as_bool();
          applyExposureMode();
        } else {
          result.successful = false;
          result.reason = "Unknown parameter: " + name;
          continue;
        }
      } else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) {
        if (name == "gain") {
          status = MV_CC_SetFloatValue(camera_handle_, "Gain", param.as_double());
        } else if (name == "exposure_auto_min") {
          exposure_auto_min_ = param.as_double();
          if (exposure_auto_) {
            status = MV_CC_SetIntValue(camera_handle_, "AutoExposureTimeLowerLimit",
              static_cast<unsigned int>(exposure_auto_min_));
          }
        } else if (name == "exposure_auto_max") {
          exposure_auto_max_ = param.as_double();
          if (exposure_auto_) {
            status = MV_CC_SetIntValue(camera_handle_, "AutoExposureTimeUpperLimit",
              static_cast<unsigned int>(exposure_auto_max_));
          }
        } else {
          result.successful = false;
          result.reason = "Unknown parameter: " + name;
          continue;
        }
      } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) {
        if (name == "exposure_time") {
          exposure_time_ = static_cast<int>(param.as_int());
          if (!exposure_auto_) {
            status = MV_CC_SetFloatValue(camera_handle_, "ExposureTime",
              static_cast<float>(exposure_time_));
          }
        } else if (name == "exposure_auto_target_brightness") {
          exposure_auto_target_brightness_ = static_cast<int>(param.as_int());
          if (exposure_auto_) {
            status = MV_CC_SetIntValue(camera_handle_, "AutoTargetBrightness",
              static_cast<unsigned int>(exposure_auto_target_brightness_));
          }
        } else {
          result.successful = false;
          result.reason = "Unknown parameter: " + name;
          continue;
        }
      } else {
        result.successful = false;
        result.reason = "Unsupported parameter type for: " + name;
        continue;
      }

      if (status != MV_OK) {
        result.successful = false;
        result.reason = "Failed to set " + name + ", status = " + std::to_string(status);
      }
    }

    return result;
  }

  // Camera
  void * camera_handle_ = nullptr;
  int n_ret_ = MV_OK;
  MV_IMAGE_BASIC_INFO img_info_;
  MV_CC_PIXEL_CONVERT_PARAM convert_param_;

  // ROS
  sensor_msgs::msg::Image image_msg_;
  sensor_msgs::msg::CameraInfo camera_info_msg_;
  image_transport::CameraPublisher camera_pub_;
  std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;

  std::string camera_name_;
  std::string frame_id_;
  std::string camera_topic_;

  std::thread capture_thread_;
  int fail_count_ = 0;

  // Trigger & shared memory
  bool trigger_enable_ = false;
  bool use_trigger_timestamp_ = false;
  bool enable_interval_log_ = false;

  // Device-clock-based timestamp (see recalibrateDevClock() for design notes).
  // Shared-memory nanosecond values (~1.8e18, Unix epoch) exceed double's 2^53 exact-
  // integer range, so calibration samples store shared_mem_ns relative to
  // dev_clock_ref_ns_ (the first sample ever seen, kept as an exact int64_t) rather
  // than the raw epoch value.
  double dev_clock_window_sec_ = 10.0;
  double dev_clock_recalib_interval_sec_ = 2.0;
  bool dev_clock_usable_ = false;
  bool dev_clock_ref_set_ = false;
  bool dev_clock_permanently_disabled_ = false;
  int dev_clock_calib_fail_count_ = 0;
  int64_t dev_clock_ref_ns_ = 0;
  double dev_clock_last_recalib_rel_ns_ = -1e30;
  int dev_clock_heartbeat_count_ = 0;
  std::deque<std::pair<double, double>> dev_clock_samples_;  // (dev_ticks, rel_shared_mem_ns)
  double dev_clock_slope_ns_per_tick_ = 0.0;
  double dev_clock_mean_ticks_ = 0.0;
  double dev_clock_mean_rel_ns_ = 0.0;

  // Exposure
  bool exposure_auto_ = false;
  int exposure_auto_target_brightness_ = 128;
  double exposure_auto_min_ = 100.0;
  double exposure_auto_max_ = 10000.0;
  int exposure_time_ = 5000;

  // White balance
  bool balance_white_auto_ = false;

  std::string serial_number_;
  TimeStamp * pointt_ = nullptr;
};
}  // namespace hik_camera_ros2_driver

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(hik_camera_ros2_driver::HikCameraRos2DriverNode)
