/*******************************************************************************
 * Copyright (c) 2023 Orbbec 3D Technology, Inc
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *******************************************************************************/

#include "orbbec_camera/ob_camera_node_driver.h"
#include "orbbec_camera/utils.h"
#include <fcntl.h>
#include <semaphore.h>
#include <sys/shm.h>
#include <ament_index_cpp/get_package_prefix.hpp>
#if __has_include(<ament_index_cpp/get_package_share_path.hpp>)
#include <ament_index_cpp/get_package_share_path.hpp>
#define ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
#else
#include <ament_index_cpp/get_package_share_directory.hpp>
#endif
#include <rclcpp_components/register_node_macro.hpp>
#if __has_include(<rclcpp/version.h>)
#include <rclcpp/version.h>
#define ORBBEC_RCLCPP_HANDLES_SIGTERM \
  ((RCLCPP_VERSION_MAJOR > 13) || (RCLCPP_VERSION_MAJOR == 13 && RCLCPP_VERSION_MINOR >= 1))
#else
#define ORBBEC_RCLCPP_HANDLES_SIGTERM 0
#endif
#include <rcutils/logging.h>
#include <csignal>
#include <sys/mman.h>
#include <unistd.h>
#include <filesystem>
#include <atomic>

#include <fstream>
#include <iomanip>  // For std::put_time
#include <malloc.h>
#include <sstream>

std::string g_camera_name = "orbbec_camera";  // Assuming this is declared elsewhere
std::string g_time_domain = "global";         // Assuming this is declared elsewhere
namespace {
constexpr auto kStreamStartDelayAfterReconnect = std::chrono::seconds(5);

std::filesystem::path getPackageSharePath(const std::string &package_name) {
#ifdef ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
  return ament_index_cpp::get_package_share_path(package_name);
#else
  return ament_index_cpp::get_package_share_directory(package_name);
#endif
}

std::filesystem::path getPackagePrefixPath(const std::string &package_name) {
#ifdef ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
  std::filesystem::path package_prefix;
  ament_index_cpp::get_package_prefix(package_name, package_prefix);
  return package_prefix;
#else
  return ament_index_cpp::get_package_prefix(package_name);
#endif
}

std::string getLogDirectoryForCamera(const std::string &camera_name) {
  const char *log_dir_override = std::getenv("ORBBEC_LOG_DIR");
  if (log_dir_override && log_dir_override[0] != '\0') {
    return (std::filesystem::path(log_dir_override) / "Log" / camera_name).string();
  }
  std::string home_dir = std::getenv("HOME") ? std::getenv("HOME") : "";
  return (std::filesystem::path(home_dir) / ".ros" / "Log" / camera_name).string();
}

std::string getDefaultBagRecordFilePath() {
  const std::time_t now_time = std::time(nullptr);
  std::tm tm{};
  localtime_r(&now_time, &tm);

  std::ostringstream time_stream;
  time_stream << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S");

  return (std::filesystem::current_path() / ("orbbec_record_" + time_stream.str() + ".bag"))
      .string();
}

std::string makeDefaultSdkLogFileName() {
  const std::time_t now_time = std::time(nullptr);
  std::tm tm{};
  localtime_r(&now_time, &tm);

  std::ostringstream file_name;
  file_name << "OrbbecSDK_" << std::put_time(&tm, "%Y%m%d_%H%M%S") << ".log";
  return file_name.str();
}
}  // namespace

void crashSignalHandler(int sig) {
  // Prevent recursive crash signal handling.
  static std::atomic<bool> in_signal_handler{false};
  if (in_signal_handler.exchange(true)) {
    _exit(sig);
  }

  std::cerr << "Received signal: " << sig << std::endl;
  std::filesystem::path log_dir = getLogDirectoryForCamera(g_camera_name);

  // get current time
  std::time_t now = std::time(nullptr);
  std::tm *local_time = std::localtime(&now);

  // format date and time, format "2024_05_20_12_34_56"
  std::ostringstream time_stream;
  time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");

  // generate log file name
  std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
  std::filesystem::path log_file_path = log_dir / log_file_name;

  if (!std::filesystem::exists(log_dir)) {
    std::filesystem::create_directories(log_dir);
  }

  std::cerr << "Log crash stack trace to " << log_file_path.string() << std::endl;
  std::ofstream log_file(log_file_path, std::ios::app);

  if (log_file.is_open()) {
    log_file << "Received signal: " << sig << std::endl;

    backward::StackTrace st;
    st.load_here(32);  // Capture stack
    backward::Printer p;
    p.print(st, log_file);  // Print stack to log file
  }

  log_file.close();
  _exit(sig);  // Use _exit instead of exit to avoid cleanup that may crash
}

#if !ORBBEC_RCLCPP_HANDLES_SIGTERM
void forwardSigtermToRclcpp(int) {
  // Older rclcpp versions such as Foxy's only handle SIGINT. Forward SIGTERM to that signal-safe
  // shutdown path instead of calling rclcpp::shutdown() directly from this signal handler.
  kill(getpid(), SIGINT);
}
#endif

namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh;

namespace {
int rosLogSeverityFromString(const std::string_view &log_level) {
  if (log_level == "debug") {
    return RCUTILS_LOG_SEVERITY_DEBUG;
  } else if (log_level == "info") {
    return RCUTILS_LOG_SEVERITY_INFO;
  } else if (log_level == "warn") {
    return RCUTILS_LOG_SEVERITY_WARN;
  } else if (log_level == "error") {
    return RCUTILS_LOG_SEVERITY_ERROR;
  } else if (log_level == "fatal") {
    return RCUTILS_LOG_SEVERITY_FATAL;
  }
  return RCUTILS_LOG_SEVERITY_UNSET;
}
}  // namespace

OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
    : Node("orbbec_camera_node", "/", node_options),
      node_options_(node_options),
      config_path_(
          (getPackageSharePath("orbbec_camera") / "config" / "OrbbecSDKConfig_v2.0.xml").string()),
      logger_(this->get_logger()),
      extension_path_((getPackagePrefixPath("orbbec_camera") / "lib" / "extensions").string()) {
  node_name_ = "orbbec_camera_node";
  init();
}

OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::string &ns,
                                       const rclcpp::NodeOptions &node_options)
    : Node(node_name, ns, node_options),
      node_options_(node_options),
      config_path_(
          (getPackageSharePath("orbbec_camera") / "config" / "OrbbecSDKConfig_v2.0.xml").string()),
      logger_(this->get_logger()),
      extension_path_((getPackagePrefixPath("orbbec_camera") / "lib" / "extensions").string()) {
  node_name_ = node_name;
  init();
}

OBCameraNodeDriver::~OBCameraNodeDriver() {
  is_alive_.store(false);

  // Finalize bag recording before the pipeline is torn down, otherwise the
  // bag file can end up truncated/corrupted.
  if (record_device_) {
    record_device_.reset();
    RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
  }

  // First stop the camera node cleanly before stopping threads
  if (ob_camera_node_) {
    try {
      ob_camera_node_->clean();
    } catch (...) {
      RCLCPP_WARN_STREAM(logger_, "Exception during camera node cleanup in destructor");
    }
    ob_camera_node_->stopGmslTrigger();
  }

  // Stop timers that might access the device
  if (sync_host_time_timer_) {
    try {
      sync_host_time_timer_->cancel();
      sync_host_time_timer_.reset();
    } catch (...) {
      RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup in destructor");
    }
  }

  if (check_connect_timer_) {
    try {
      check_connect_timer_->cancel();
      check_connect_timer_.reset();
    } catch (...) {
      RCLCPP_WARN_STREAM(logger_, "Exception during check connect timer cleanup in destructor");
    }
  }

  if (device_status_timer_) {
    try {
      device_status_timer_->cancel();
      device_status_timer_.reset();
    } catch (...) {
      RCLCPP_WARN_STREAM(logger_, "Exception during device status timer cleanup in destructor");
    }
  }

  // Now stop threads
  if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
    device_count_update_thread_->join();
  }
  if (query_thread_ && query_thread_->joinable()) {
    query_thread_->join();
  }
  if (reset_device_thread_ && reset_device_thread_->joinable()) {
    reset_device_cond_.notify_all();
    reset_device_thread_->join();
  }
  if (ob_camera_node_) {
    ob_camera_node_->stopGmslTrigger();
  }

  // Unregister device changed callback before destroying context
  if (ctx_ && device_changed_callback_id_ != INVALID_CALLBACK_ID) {
    try {
      ctx_->unregisterDeviceChangedCallback(device_changed_callback_id_);
      device_changed_callback_id_ = INVALID_CALLBACK_ID;
    } catch (...) {
      RCLCPP_WARN_STREAM(logger_,
                         "Exception during device changed callback unregister in destructor");
    }
  }

  if (orb_device_lock_shm_fd_ != -1) {
    close(orb_device_lock_shm_fd_);
    orb_device_lock_shm_fd_ = -1;
  }
  shm_unlink(ORB_DEFAULT_LOCK_NAME.c_str());
}

void OBCameraNodeDriver::init() {
  // Keep shutdown signals managed by rclcpp. Overriding them from a composable node bypasses its
  // deferred signal handling and can leave SDK streaming threads running after the ROS context has
  // already been shut down.
#if !ORBBEC_RCLCPP_HANDLES_SIGTERM
  // Older rclcpp versions such as Foxy's predate native SIGTERM handling, so translate it to the
  // SIGINT path that rclcpp does manage. Newer distributions handle both signals themselves.
  signal(SIGTERM, forwardSigtermToRclcpp);
#endif
  signal(SIGSEGV, crashSignalHandler);  // segment fault
  signal(SIGABRT, crashSignalHandler);  // abort
  signal(SIGFPE, crashSignalHandler);   // float point exception
  signal(SIGILL, crashSignalHandler);   // illegal instruction
  ob::Context::setExtensionsDirectory(extension_path_.c_str());
  g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
  auto log_level_str = declare_parameter<std::string>("log_level", "info");
  auto log_level = obLogSeverityFromString(log_level_str);
  auto ros_log_level = rosLogSeverityFromString(log_level_str);
  auto log_file_name = declare_parameter<std::string>("log_file_name", "");
  std::string log_path = getLogDirectoryForCamera(g_camera_name);
  // Set logger to console
  ob::Context::setLoggerToConsole(log_level);
  ob::Context::setLoggerToFile(log_level, log_path.c_str());
  if (ros_log_level != RCUTILS_LOG_SEVERITY_UNSET) {
    auto ret = rcutils_logging_set_logger_level(this->get_logger().get_name(), ros_log_level);
    if (ret != RCUTILS_RET_OK) {
      RCLCPP_WARN_STREAM(logger_, "Failed to set ROS log level to " << log_level_str);
    }
  }
  if (log_file_name.empty()) {
    log_file_name = makeDefaultSdkLogFileName();
  }
  try {
    ob::Context::setLoggerFileName(log_file_name);
    RCLCPP_INFO_STREAM(logger_, "SDK log file path set to: "
                                    << (std::filesystem::path(log_path) / log_file_name).string());
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(
        logger_, "Failed to set SDK log file name: " << orbbec_camera::formatObErrorWithStatus(e));
  }
  // Bag file playback mode: load a previously recorded .bag file as a virtual
  // device instead of enumerating real hardware. Must be checked before the
  // ob::Context / device discovery machinery is set up below.
  device_type_ = declare_parameter<std::string>("device_type", "camera");
  bag_filename_ = declare_parameter<std::string>("bag_filename", "");
  bag_loop_ = declare_parameter<bool>("bag_loop", false);
  if (!bag_filename_.empty()) {
    is_alive_.store(true);
    parameters_ = std::make_shared<Parameters>(this);
    initializeBagPlayback();
    return;
  }

  // Force IP
  force_ip_enable_ = declare_parameter<bool>("force_ip_enable", false);
  force_ip_mac_ = declare_parameter<std::string>("force_ip_mac", "");
  force_ip_address_ = declare_parameter<std::string>("force_ip_address", "192.168.1.10");
  force_ip_subnet_mask_ = declare_parameter<std::string>("force_ip_subnet_mask", "255.255.255.0");
  force_ip_gateway_ = declare_parameter<std::string>("force_ip_gateway", "192.168.1.1");
  if (config_path_.empty()) {
    ctx_ = std::make_unique<ob::Context>();
  } else {
    ctx_ = std::make_unique<ob::Context>(config_path_.c_str());
  }
  timestamp_clock_type_str_ = declare_parameter<std::string>("timestamp_clock_type", "");
  if (!timestamp_clock_type_str_.empty()) {
    auto timestamp_clock_type = timestampClockTypeFromString(timestamp_clock_type_str_);
    try {
      ctx_->setTimestampClockType(timestamp_clock_type);
      auto actual_timestamp_clock_type = ctx_->getTimestampClockType();
      RCLCPP_INFO_STREAM(logger_, "Set timestamp clock type to "
                                      << timestampClockTypeToString(actual_timestamp_clock_type));
    } catch (const ob::Error &e) {
      RCLCPP_WARN_STREAM(logger_, "Failed to set SDK timestamp clock type: "
                                      << orbbec_camera::formatObErrorWithStatus(e));
    }
  }
  applyForceIpConfig();

  connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
  enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
  double time_sync_period = declare_parameter<double>("time_sync_period", 60.0);
  time_sync_period_ = std::chrono::milliseconds((int)(time_sync_period * 1000));
  upgrade_firmware_ = declare_parameter<std::string>("upgrade_firmware", "");
  g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
  preset_firmware_path_ =
      declare_parameter<std::string>("preset_firmware_path", preset_firmware_path_);
  orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666);
  if (orb_device_lock_shm_fd_ < 0) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to open shared memory " << ORB_DEFAULT_LOCK_NAME);
    return;
  }
  int ret = ftruncate(orb_device_lock_shm_fd_, sizeof(pthread_mutex_t));
  if (ret < 0) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to truncate shared memory " << ORB_DEFAULT_LOCK_NAME);
    return;
  }
  orb_device_lock_shm_addr_ =
      static_cast<uint8_t *>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
                                  orb_device_lock_shm_fd_, 0));
  if (orb_device_lock_shm_addr_ == MAP_FAILED) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
    return;
  }
  reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
      "reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
                                 std::placeholders::_1, std::placeholders::_2));
  set_bag_recording_srv_ = this->create_service<orbbec_camera_msgs::srv::SetBagRecording>(
      "set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this,
                                     std::placeholders::_1, std::placeholders::_2));
  pthread_mutexattr_init(&orb_device_lock_attr_);
  pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
  orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
  pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
  is_alive_.store(true);
  // Initialize the reset device completion time to allow immediate device connection on startup
  last_reset_device_completion_time_ = std::chrono::steady_clock::now() - std::chrono::seconds(10);
  parameters_ = std::make_shared<Parameters>(this);
  serial_number_ = declare_parameter<std::string>("serial_number", "");
  bag_record_filename_ = declare_parameter<std::string>("bag_record_filename", "");
  bag_record_compression_ = declare_parameter<bool>("bag_record_compression", true);
  device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
  usb_port_ = declare_parameter<std::string>("usb_port", "");
  net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
  net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
  enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
  uvc_backend_ = declare_parameter<std::string>("uvc_backend", "libuvc");
  device_access_mode_str_ = declare_parameter<std::string>("device_access_mode", "Default");
  device_access_mode_ = stringToAccessMode(device_access_mode_str_);
  RCLCPP_INFO_STREAM(logger_, "Device access mode: " << device_access_mode_str_ << " ("
                                                     << device_access_mode_ << ")");
  if (uvc_backend_ == "libuvc") {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
    RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
  } else if (uvc_backend_ == "v4l2") {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_V4L2);
    RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
  } else {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
    RCLCPP_WARN_STREAM(logger_,
                       "Unsupported uvc_backend '" << uvc_backend_ << "', using default libuvc");
  }
  ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
  device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
      [this](const std::shared_ptr<ob::DeviceList> &removed_list,
             const std::shared_ptr<ob::DeviceList> &added_list) {
        onDeviceDisconnected(removed_list);
        onDeviceConnected(added_list);
      });
  check_connect_timer_ =
      this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
  CHECK_NOTNULL(check_connect_timer_);
  if (device_type_ == "camera") {
    device_status_timer_ =
        this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz),
                                [this]() { deviceStatusTimer(); });
    auto qos = rclcpp::QoS(1).transient_local();
    if (node_options_.use_intra_process_comms()) {
      qos = rclcpp::QoS(1);
    }
    device_status_pub_ =
        this->create_publisher<orbbec_camera_msgs::msg::DeviceStatus>("device_status", qos);
  }
  query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
  reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
}

void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
  CHECK_NOTNULL(device_list);
  {
    RCLCPP_INFO_STREAM(logger_, "Device connected callback triggered");
    std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
    if (reset_device_flag_) {
      RCLCPP_INFO_STREAM(logger_, "Device reset in progress, waiting before connecting");
      reset_device_cond_.wait(
          reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
      if (!is_alive_) {
        return;
      }
      RCLCPP_INFO_STREAM(logger_, "Device reset completed, continuing connection");
    }
  }
  if (device_list->getCount() == 0) {
    return;
  }

  // Check if device is already connected or connecting
  if (device_connected_.load() || device_connecting_.load()) {
    RCLCPP_DEBUG_STREAM(logger_, "onDeviceConnected: device already connected or connecting");
    return;
  }

  if (!device_) {
    startDevice(device_list);
  }
}

void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
  CHECK_NOTNULL(device_list);
  if (device_list->getCount() == 0) {
    return;
  }
  RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected called");

  // Check if device connection/initialization is in progress
  if (device_connecting_.load()) {
    RCLCPP_INFO_STREAM(logger_,
                       "onDeviceDisconnected: device connection/initialization in progress, "
                       "ignoring disconnect event");
    return;
  }

  std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);

  std::lock_guard<decltype(device_lock_)> lock(device_lock_);

  if (!device_connected_.load()) {
    RCLCPP_DEBUG_STREAM(logger_, "onDeviceDisconnected: device already disconnected");
    return;
  }

  for (size_t i = 0; i < device_list->getCount(); i++) {
    std::string uid = device_list->getUid(i);
    std::string serial_number = device_list->getSerialNumber(i);
    RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
    if (uid == device_unique_id_ || serial_number_ == serial_number) {
      RCLCPP_INFO_STREAM(logger_,
                         "device with " << uid << " disconnected, notify reset device thread");
      delay_stream_start_after_reconnect_ = true;
      reset_device_flag_ = true;
      reset_device_cond_.notify_all();
      break;
    }
  }
}

OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view &log_level) {
  if (log_level == "debug") {
    return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
  } else if (log_level == "info") {
    return OBLogSeverity::OB_LOG_SEVERITY_INFO;
  } else if (log_level == "warn") {
    return OBLogSeverity::OB_LOG_SEVERITY_WARN;
  } else if (log_level == "error") {
    return OBLogSeverity::OB_LOG_SEVERITY_ERROR;
  } else if (log_level == "fatal") {
    return OBLogSeverity::OB_LOG_SEVERITY_FATAL;
  } else {
    return OBLogSeverity::OB_LOG_SEVERITY_NONE;
  }
}

void OBCameraNodeDriver::checkConnectTimer() {
  if (!device_connected_.load()) {
    RCLCPP_DEBUG_STREAM(logger_,
                        "checkConnectTimer: device " << serial_number_ << " not connected");
    return;
  } else if (!ob_camera_node_ && !ob_lidar_node_) {
    device_connected_.store(false);
  }
}

void OBCameraNodeDriver::queryDevice() {
  while (is_alive_ && rclcpp::ok()) {
    // Check if device reset is in progress before attempting to connect
    {
      std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
      if (reset_device_flag_) {
        RCLCPP_INFO_STREAM(logger_, "queryDevice: device reset in progress, waiting...");
        reset_device_cond_.wait(
            reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
        if (!is_alive_ || !rclcpp::ok()) {
          return;
        }
        RCLCPP_INFO_STREAM(logger_, "queryDevice: device reset completed, continuing connection");
      }
    }

    // Check if connection is already in progress
    if (device_connecting_.load()) {
      RCLCPP_DEBUG_STREAM(logger_,
                          "queryDevice: device connection already in progress, waiting...");
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      continue;
    }

    // If device is already connected, skip connection attempt
    if (!device_connected_.load()) {
      // Check if sufficient time has passed since last reset device completion
      auto now = std::chrono::steady_clock::now();
      auto time_since_last_reset = std::chrono::duration_cast<std::chrono::seconds>(
          now - last_reset_device_completion_time_);

      if (time_since_last_reset.count() < 10) {
        RCLCPP_DEBUG_STREAM(
            logger_,
            "queryDevice: Only "
                << time_since_last_reset.count()
                << " seconds since last reset completion, waiting before starting device...");
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        continue;
      }

      if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
        connectNetDevice(net_device_ip_, net_device_port_);
      } else {
        auto device_list = ctx_->queryDeviceList();
        if (device_list->getCount() != 0) {
          startDevice(device_list);
        }
      }
    }
    // Add a delay to prevent tight loop
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }
}

void OBCameraNodeDriver::resetDevice() {
  while (is_alive_ && rclcpp::ok()) {
    {
      std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
      // Use a timeout to make the wait interruptible
      auto timeout = std::chrono::milliseconds(1000);
      bool notified = reset_device_cond_.wait_for(
          lock, timeout, [this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; });

      // Check if we should exit due to shutdown
      if (!is_alive_ || !rclcpp::ok()) {
        break;
      }

      // If not notified by reset flag, continue waiting
      if (!notified || !reset_device_flag_) {
        continue;
      }

      // Stop sync timer to prevent it from accessing the device during reset
      if (sync_host_time_timer_) {
        try {
          sync_host_time_timer_->cancel();
          sync_host_time_timer_.reset();
        } catch (...) {
          RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup during reset");
        }
      }

      RCLCPP_INFO_STREAM(logger_, "Resetting device UID: " << device_unique_id_);
      std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
      {
        // Mark device as disconnected immediately to prevent other threads from accessing it
        device_connected_ = false;
        device_connecting_ = false;  // Clear connecting flag

        // Stop recording before tearing down the pipeline so the bag file is finalized
        if (record_device_) {
          record_device_.reset();
          RCLCPP_WARN_STREAM(logger_, "Device disconnected, bag recording stopped");
        }

        // Reset objects in order, with additional safety checks
        if (ob_camera_node_) {
          ob_camera_node_.reset();
        } else if (ob_lidar_node_) {
          ob_lidar_node_.reset();
        }

        // Allow more time for internal SDK cleanup
        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        if (device_) {
          try {
            RCLCPP_INFO_STREAM(logger_, "Resetting device handle");
            // Force free any idle memory before device reset
            if (ctx_) {
              try {
                ctx_->freeIdleMemory();
              } catch (...) {
                // Ignore exceptions during memory cleanup
              }
            }
            device_.reset();
            RCLCPP_INFO_STREAM(logger_, "Device handle reset complete");
          } catch (const ob::Error &e) {
            RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
                                            << orbbec_camera::formatObErrorWithStatus(e));
          } catch (const std::exception &e) {
            RCLCPP_WARN_STREAM(logger_, "Standard exception during device reset: " << e.what());
          } catch (...) {
            RCLCPP_WARN_STREAM(logger_, "Unknown exception during device reset");
          }
        }

        if (device_info_) {
          try {
            RCLCPP_INFO_STREAM(logger_, "Resetting device info");
            device_info_.reset();
            RCLCPP_INFO_STREAM(logger_, "Device info reset complete");
          } catch (...) {
            RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
          }
        }

        device_unique_id_.clear();
      }
      reset_device_flag_ = false;
      last_reset_device_completion_time_ = std::chrono::steady_clock::now();
    }
    reset_device_cond_.notify_all();
    malloc_trim(0);
    RCLCPP_INFO_STREAM(logger_, "Device reset complete");
  }
}

void OBCameraNodeDriver::deviceStatusTimer() {
  // Always publish device status regardless of device connection state
  orbbec_camera_msgs::msg::DeviceStatus status_msg;
  status_msg.header.stamp = this->now();
  status_msg.device_online = device_connected_.load();
  status_msg.header.frame_id = node_name_;

  // Initialize default values for when device is not connected
  status_msg.connection_type = "";
  status_msg.calibration_from_factory = false;
  status_msg.calibration_from_launch_param = false;
  status_msg.customer_calibration_ready = false;

  // Flag to track if device communication error occurs
  bool device_communication_error = false;

  // Only try to get device information if device is connected and stable
  if (device_connected_.load() && !device_connecting_.load()) {
    // Check if reset is in progress
    std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_,
                                                               std::try_to_lock);
    if (reset_lock.owns_lock() && !reset_device_flag_) {
      // Only get device-specific info if we have a valid camera node and device
      if (ob_camera_node_) {
        // Safely get color and depth status - these may access device
        try {
          ob_camera_node_->getColorStatus(status_msg);
          ob_camera_node_->getDepthStatus(status_msg);
        } catch (const ob::Error &e) {
          std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
          if (error_msg.find("Device is deactivated") != std::string::npos ||
              error_msg.find("disconnected") != std::string::npos ||
              error_msg.find("Send control transfer failed") != std::string::npos) {
            RCLCPP_WARN(
                logger_,
                "Device communication error in %s at line %d: %s - Device may be disconnected",
                __FUNCTION__, __LINE__, error_msg.c_str());
            device_communication_error = true;
          } else {
            RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
                         error_msg.c_str());
          }
        } catch (const std::exception &e) {
          RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
        } catch (...) {
          RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
        }

        // These should be safe as they don't directly access hardware
        status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
      }

      // Safely get connection type
      try {
        if (device_info_) {
          status_msg.connection_type = device_info_->getConnectionType();
        }
      } catch (const ob::Error &e) {
        std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
        if (error_msg.find("Device is deactivated") != std::string::npos ||
            error_msg.find("disconnected") != std::string::npos ||
            error_msg.find("Send control transfer failed") != std::string::npos) {
          RCLCPP_WARN(
              logger_,
              "Device communication error in %s at line %d: %s - Device may be disconnected",
              __FUNCTION__, __LINE__, error_msg.c_str());
          device_communication_error = true;
        } else {
          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
                       error_msg.c_str());
        }
      } catch (const std::exception &e) {
        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
      } catch (...) {
        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
      }

      // Safely get calibration info
      try {
        if (device_) {
          auto camera_params = device_->getCalibrationCameraParamList();
          bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
          status_msg.calibration_from_factory = calibration_from_factory;
        }
      } catch (const ob::Error &e) {
        std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
        if (error_msg.find("Device is deactivated") != std::string::npos ||
            error_msg.find("disconnected") != std::string::npos ||
            error_msg.find("Send control transfer failed") != std::string::npos) {
          RCLCPP_WARN(
              logger_,
              "Device communication error in %s at line %d: %s - Device may be disconnected",
              __FUNCTION__, __LINE__, error_msg.c_str());
          device_communication_error = true;
        } else {
          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
                       error_msg.c_str());
        }
      } catch (const std::exception &e) {
        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
      } catch (...) {
        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
      }

      // Safely check user calibration readiness - this may also access device
      // Only execute for 435LE devices (check by device name for network devices)
      try {
        if (device_info_ && ob_camera_node_) {
          std::string device_name = device_info_->getName();
          if (device_name.find("435Le") != std::string::npos ||
              device_name.find("435LE") != std::string::npos) {
            if (!ob_camera_node_->checkUserCalibrationReady()) {
              status_msg.customer_calibration_ready = false;
            } else {
              status_msg.customer_calibration_ready = true;
            }
          } else {
            // For non-435LE devices, set a default value or skip this check
            status_msg.customer_calibration_ready = false;
          }
        } else {
          status_msg.customer_calibration_ready = false;
        }
      } catch (const ob::Error &e) {
        std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
        if (error_msg.find("Device is deactivated") != std::string::npos ||
            error_msg.find("disconnected") != std::string::npos ||
            error_msg.find("Send control transfer failed") != std::string::npos) {
          RCLCPP_WARN(
              logger_,
              "Device communication error in %s at line %d: %s - Device may be disconnected",
              __FUNCTION__, __LINE__, error_msg.c_str());
          device_communication_error = true;
        } else {
          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
                       error_msg.c_str());
        }
      } catch (const std::exception &e) {
        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
      } catch (...) {
        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
      }
    }
  }

  // If device communication error occurred, set device_online to false
  if (device_communication_error) {
    status_msg.device_online = false;
  }

  // if status_msg.connection_type is empty, set it to "unknown"
  if (status_msg.connection_type.empty()) {
    status_msg.connection_type = "unknown";
    status_msg.device_online = false;
  }

  // Always publish the status message, regardless of device state
  if (device_status_pub_) {
    device_status_pub_->publish(status_msg);
  }
  // RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
}

void OBCameraNodeDriver::setBagRecordingCallback(
    const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
    std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response) {
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);

  if (!device_) {
    response->success = false;
    response->message = "No device connected";
    return;
  }

  if (!request->enable) {
    if (!record_device_) {
      response->success = true;
      response->message = "Bag recording is not running";
      return;
    }
    record_device_.reset();
    RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
    response->success = true;
    response->message = "Bag recording stopped";
    return;
  }

  std::string file_path =
      request->file_path.empty() ? getDefaultBagRecordFilePath() : request->file_path;

  if (record_device_) {
    record_device_.reset();
    RCLCPP_INFO_STREAM(logger_, "Bag recording stopped before starting a new recording");
  }

  try {
    exportBagPresetJson(file_path);
    record_device_ =
        std::make_shared<ob::RecordDevice>(device_, file_path, bag_record_compression_);
  } catch (const ob::Error &e) {
    response->success = false;
    response->message = "Failed to start recording: " + orbbec_camera::formatObErrorWithStatus(e);
    RCLCPP_ERROR_STREAM(logger_, response->message);
    return;
  }

  RCLCPP_INFO_STREAM(logger_, "Recording to " << file_path);

  response->success = true;
  response->message = "Recording started: " + file_path;
}

void OBCameraNodeDriver::rebootDeviceCallback(
    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
    std::shared_ptr<std_srvs::srv::Empty::Response> response) {
  (void)request;
  (void)response;

  malloc_trim(0);
  RCLCPP_INFO(logger_, "Reboot device service called");

  struct timespec timeout;
  clock_gettime(CLOCK_REALTIME, &timeout);
  timeout.tv_sec += 15;

  int lock_result = pthread_mutex_timedlock(orb_device_lock_, &timeout);
  if (lock_result != 0) {
    RCLCPP_WARN(logger_, "Failed to acquire process lock for reboot: %s", strerror(lock_result));
    return;
  }

  std::shared_ptr<int> process_lock_guard(
      nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });

  try {
    std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
    reset_device_flag_ = true;
    {
      std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);

      if (!device_connected_ || (!ob_camera_node_ && !ob_lidar_node_)) {
        RCLCPP_INFO(logger_, "Device not connected");
        reset_device_flag_ = false;
      } else {
        std::string current_device_uid = device_unique_id_;
        RCLCPP_INFO_STREAM(logger_, "Rebooting device with UID: " << current_device_uid);
        delay_stream_start_after_reconnect_ = true;
        if (ob_lidar_node_) {
          ob_lidar_node_->rebootDevice();
        } else if (ob_camera_node_) {
          ob_camera_node_->rebootDevice();
        }
      }
    }
    if (reset_device_flag_) {
      RCLCPP_INFO(logger_, "Device reboot initiated, waiting for reconnection");
    }

  } catch (std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to reboot device: " << e.what());
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to reboot device: unknown error");
  }
  process_lock_guard.reset();
  if (reset_device_flag_) {
    reset_device_cond_.notify_all();
  }
  malloc_trim(0);
  return;
}

std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
    const std::shared_ptr<ob::DeviceList> &list) {
  std::shared_ptr<ob::Device> device = nullptr;
  if (!net_device_ip_.empty() && net_device_port_ != 0) {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                "Connecting to device with net ip: " << net_device_ip_);
    device = selectDeviceByNetIP(list, net_device_ip_);
  } else if (!serial_number_.empty()) {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                "Connecting to device with serial number: " << serial_number_);
    device = selectDeviceBySerialNumber(list, serial_number_);
  } else if (!usb_port_.empty()) {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                "Connecting to device with usb port: " << usb_port_);
    device = selectDeviceByUSBPort(list, usb_port_);
  } else if (device_num_ == 1) {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Connecting to the default device");
    return list->getDevice(0, device_access_mode_);
  }
  if (device == nullptr) {
    RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 5000, "Device with serial number %s not found",
                         serial_number_.c_str());
    device_connected_ = false;
    return nullptr;
  }
  return device;
}

std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
    const std::shared_ptr<ob::DeviceList> &list, const std::string &serial_number) {
  std::string lower_sn;
  std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
                 [](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
  for (size_t i = 0; i < list->getCount(); i++) {
    RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                 "Selecting device by serial number: " << serial_number);
    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
    try {
      auto pid = list->getPid(i);
      if (isOpenNIDevice(pid)) {
        // openNI device
        auto device = list->getDevice(i);
        auto device_info = device->getDeviceInfo();
        if (device_info->getSerialNumber() == serial_number) {
          RCLCPP_INFO_STREAM_THROTTLE(
              logger_, *get_clock(), 5000,
              "Matched device serial number: " << device_info->getSerialNumber());
          return device;
        }
      } else {
        std::string sn = list->getSerialNumber(i);
        RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                     "Checking device serial number: " << sn);
        if (sn == serial_number) {
          RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                      "Matched device serial number: " << sn);
          return list->getDevice(i, device_access_mode_);
        }
      }
    } catch (ob::Error &e) {
      RCLCPP_ERROR_STREAM_THROTTLE(
          logger_, *get_clock(), 5000,
          "Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
    } catch (std::exception &e) {
      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                   "Failed to get device info " << e.what());
    } catch (...) {
      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Failed to get device info");
    }
  }
  return nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
    const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
  try {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                "Selecting device by USB port: " << usb_port);
    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
    auto device = list->getDeviceByUid(usb_port.c_str(), device_access_mode_);
    if (device) {
      RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                  "getDeviceByUid device usb port " << usb_port << " done");
    } else {
      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                   "getDeviceByUid device usb port " << usb_port << " failed");
      RCLCPP_ERROR_STREAM_THROTTLE(
          logger_, *get_clock(), 5000,
          "Please use script to get usb port: ros2 run orbbec_camera list_devices_node");
    }
    return device;
  } catch (ob::Error &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        logger_, *get_clock(), 5000,
        "Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (std::exception &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                 "Failed to get device info " << e.what());
  } catch (...) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Failed to get device info");
  }

  return nullptr;
}

std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
    const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
  RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                               "Selecting device by network IP: " << net_ip);
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
  std::shared_ptr<ob::Device> device = nullptr;
  for (size_t i = 0; i < list->getCount(); i++) {
    try {
      if (std::string(list->getConnectionType(i)) != "Ethernet") {
        continue;
      }
      if (list->getIpAddress(i) == nullptr) {
        continue;
      }
      RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                   "FindDeviceByNetIP device net ip " << list->getIpAddress(i));
      if (std::string(list->getIpAddress(i)) == net_ip) {
        RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                    "getDeviceByNetIP device net ip " << net_ip << " done");
        return list->getDevice(i, device_access_mode_);
      }
    } catch (ob::Error &e) {
      RCLCPP_ERROR_STREAM_THROTTLE(
          logger_, *get_clock(), 5000,
          "Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
      continue;
    } catch (std::exception &e) {
      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                   "Failed to get device info " << e.what());
      continue;
    } catch (...) {
      RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Failed to get device info");
      continue;
    }
  }

  return nullptr;
}

void OBCameraNodeDriver::exportBagPresetJson(const std::string &bag_path) {
  if (!device_ || bag_path.empty() || playback_device_) {
    return;
  }

  auto json_path = std::filesystem::path(bag_path);
  if (json_path.extension() == ".bag") {
    json_path.replace_extension(".json");
  } else {
    json_path += ".json";
  }

  const auto json_path_str = json_path.string();
  try {
    const auto parent_path = json_path.parent_path();
    if (!parent_path.empty()) {
      std::filesystem::create_directories(parent_path);
    }
    device_->exportSettingsAsPresetJsonFile(json_path_str.c_str());
    RCLCPP_INFO_STREAM(logger_, "Exported bag preset JSON: " << json_path_str);
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_, "Failed to export bag preset JSON "
                                    << json_path_str << ": "
                                    << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(logger_,
                       "Failed to export bag preset JSON " << json_path_str << ": " << e.what());
  }
}

void OBCameraNodeDriver::initializeBagPlayback() {
  RCLCPP_INFO_STREAM(logger_, "Starting bag file playback: " << bag_filename_);
  try {
    playback_device_ = std::make_shared<ob::PlaybackDevice>(bag_filename_);
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Failed to open bag file: " << orbbec_camera::formatObErrorWithStatus(e));
    return;
  }

  if (bag_loop_) {
    playback_device_->setPlaybackStatusChangeCallback([this](OBPlaybackStatus status) {
      if (status == OB_PLAYBACK_STOPPED && is_alive_) {
        RCLCPP_INFO_STREAM(logger_, "Bag playback completed, restarting from beginning...");
        try {
          playback_device_->seek(0);
          if (ob_camera_node_) {
            ob_camera_node_->restartPlaybackStreams();
          }
        } catch (const ob::Error &e) {
          RCLCPP_WARN_STREAM(logger_, "Failed to restart bag playback: "
                                          << orbbec_camera::formatObErrorWithStatus(e));
        }
      }
    });
  }

  std::shared_ptr<ob::Device> device = playback_device_;
  initializeDevice(device);
}

void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
  device_ = device;
  updatePresetFirmware(preset_firmware_path_);
  CHECK_NOTNULL(device_);
  CHECK_NOTNULL(device_.get());
  if (ob_camera_node_) {
    ob_camera_node_.reset();
  } else if (ob_lidar_node_) {
    ob_lidar_node_.reset();
  }
  int retry_count = 0;
  constexpr int max_retries = 3;
  bool initialized = false;
  device_info_ = device_->getDeviceInfo();
  RCLCPP_DEBUG_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());

  while (retry_count < max_retries && !initialized) {
    try {
      if (device_type_ == "camera") {
        ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
                                                         node_options_.use_intra_process_comms(),
                                                         playback_device_ != nullptr);
      } else if (device_type_ == "lidar") {
        ob_lidar_node_ = std::make_unique<orbbec_lidar::OBLidarNode>(
            this, device_, parameters_, node_options_.use_intra_process_comms());
      }

      initialized = true;
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
                                       << retry_count + 1 << " of " << max_retries
                                       << "): " << orbbec_camera::formatObErrorWithStatus(e));
    } catch (const std::exception &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
                                                                           << " of " << max_retries
                                                                           << "): " << e.what());
    } catch (...) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
                                       << retry_count + 1 << " of " << max_retries << ")");
    }
    retry_count++;
  }

  if (!initialized) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Device initialization failed after " << max_retries << " attempts.");
    throw std::runtime_error("Device initialization failed after " + std::to_string(max_retries) +
                             " attempts.");
  }

  device_connected_ = true;
  device_info_ = device_->getDeviceInfo();
  serial_number_ = device_info_->getSerialNumber();
  CHECK_NOTNULL(device_info_.get());
  device_unique_id_ = device_info_->getUid();

  if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
      !playback_device_) {
    TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
    if (g_time_domain != "global") {
      device_->enableGlobalTimestamp(false);
      sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
        // Multiple safety checks before attempting time sync
        if (!device_) {
          RCLCPP_DEBUG_STREAM(logger_, "sync_host_time_timer_: device is null, skip time sync");
          return;
        }

        // Check device connection status
        if (!device_connected_.load()) {
          RCLCPP_DEBUG_STREAM(logger_,
                              "sync_host_time_timer_: device not connected, skip time sync");
          return;
        }

        // Check if device is in connecting state
        if (device_connecting_.load()) {
          RCLCPP_DEBUG_STREAM(logger_, "sync_host_time_timer_: device connecting, skip time sync");
          return;
        }

        // Check if reset is in progress
        {
          std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_,
                                                                     std::try_to_lock);
          if (!reset_lock.owns_lock() || reset_device_flag_) {
            RCLCPP_DEBUG_STREAM(logger_,
                                "sync_host_time_timer_: device reset in progress, skip time sync");
            return;
          }
        }

        // Additional safety check with device lock
        std::unique_lock<decltype(device_lock_)> device_lock(device_lock_, std::try_to_lock);
        if (!device_lock.owns_lock()) {
          RCLCPP_DEBUG_STREAM(logger_,
                              "sync_host_time_timer_: cannot acquire device lock, skip time sync");
          return;
        }

        // Verify device is still valid after acquiring lock
        if (!device_) {
          RCLCPP_DEBUG_STREAM(
              logger_, "sync_host_time_timer_: device became null after lock, skip time sync");
          return;
        }

        TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
      });
      RCLCPP_INFO_STREAM(logger_, "Enabled timer sync with host with period "
                                      << time_sync_period_.count() << " ms");
    }
  }

  // Safely log device information - these calls can throw if device disconnects
  TRY_EXECUTE_BLOCK({
    RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");
    RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->getSerialNumber());
    RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->getFirmwareVersion());
    RCLCPP_INFO_STREAM(logger_, "ROS Wrapper version: " << OB_ROS_VERSION_STR);
    RCLCPP_INFO_STREAM(logger_, "SDK version: " << getObSDKVersion());
    RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->getHardwareVersion());
    try {
      std::string isp_fw_version = device_->getExtensionInfo("IspFwVer");
      if (!isp_fw_version.empty()) {
        RCLCPP_INFO_STREAM(logger_, "ISP firmware version: " << isp_fw_version);
      }
      std::string isp_need_version = device_->getExtensionInfo("IspNeedVer");
      if (!isp_need_version.empty()) {
        RCLCPP_INFO_STREAM(logger_, "ISP needed version: " << isp_need_version);
      }
    } catch (ob::Error &e) {
      // Some devices don't support ISP firmware version query
      RCLCPP_DEBUG_STREAM(logger_, "Current device not support ISP firmware version query: "
                                       << orbbec_camera::formatObErrorWithStatus(e));
    }
    RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
  });
  RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
  RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
  auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::high_resolution_clock::now() - start_time_);
  RCLCPP_DEBUG_STREAM(logger_, "Start device cost: " << time_cost.count() << " ms");

  if (!upgrade_firmware_.empty()) {
    // Check if this is a second update (reupdate scenario)
    bool is_second_update = is_reupdating_.load();

    if (is_second_update) {
      RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update");
    } else {
      RCLCPP_INFO(logger_, "Starting firmware update from file: %s", upgrade_firmware_.c_str());
    }

    firmware_update_success_ = false;
    need_reupdate_ = false;

    if (ob_camera_node_) {
      TRY_EXECUTE_BLOCK({
        ob_camera_node_->withDeviceLock([&]() {
          device_->updateFirmware(
              upgrade_firmware_.c_str(),
              std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
                        std::placeholders::_2, std::placeholders::_3),
              false);
        });
      });
    } else if (ob_lidar_node_) {
      device_->updateFirmware(
          upgrade_firmware_.c_str(),
          std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
                    std::placeholders::_2, std::placeholders::_3),
          false);
    }

    if (need_reupdate_) {
      // Some devices require a second update after reboot
      RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically");
      // Set flag to indicate we're waiting for device to reboot for second update
      is_reupdating_ = true;
      // Keep upgrade_firmware_ path and wait for device to reconnect
      // The second update will be triggered automatically when device reconnects
      return;
    }

    if (firmware_update_success_) {
      if (is_second_update) {
        RCLCPP_INFO(logger_, "Second firmware update completed successfully");
        is_reupdating_ = false;
      } else {
        RCLCPP_INFO(logger_, "Firmware update completed successfully");
      }
      return;
    }
  }

  const bool should_delay_stream_start = delay_stream_start_after_reconnect_.exchange(false) &&
                                         isGemini305SeriesPID(device_info_->getPid());
  if (should_delay_stream_start) {
    std::this_thread::sleep_for(kStreamStartDelayAfterReconnect);
  }

  if (ob_camera_node_) {
    ob_camera_node_->startIMU();
    ob_camera_node_->startStreams();
  } else if (ob_lidar_node_) {
    ob_lidar_node_->startStreams();
    ob_lidar_node_->startIMU();
  } else {
    RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
  }

  if (!bag_record_filename_.empty() && !record_device_) {
    try {
      exportBagPresetJson(bag_record_filename_);
      record_device_ = std::make_shared<ob::RecordDevice>(device_, bag_record_filename_,
                                                          bag_record_compression_);
      RCLCPP_INFO_STREAM(logger_, "Recording to bag file: " << bag_record_filename_);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(
          logger_, "Failed to start recording: " << orbbec_camera::formatObErrorWithStatus(e));
    }
  }

}  // namespace orbbec_camera

bool OBCameraNodeDriver::applyForceIpConfig() {
  if (!force_ip_enable_) {
    RCLCPP_DEBUG(logger_, "[ForceIP] Disabled, skip config");
    return false;
  }
  if (force_ip_success_) {
    RCLCPP_DEBUG(logger_, "[ForceIP] Already applied, skip");
    return false;
  }

  OBNetIpConfig config{};
  config.dhcp = force_ip_dhcp_ ? 1 : 0;

  if (config.dhcp == 0) {
    RCLCPP_INFO(logger_, "[ForceIP] Static config mode");
    auto strToIp = [&](const std::string &s, uint8_t out[4]) -> bool {
      std::stringstream ss(s);
      std::string item;
      int i = 0;
      while (std::getline(ss, item, '.') && i < 4) {
        int val = std::stoi(item);
        if (val < 0 || val > 255) return false;
        out[i++] = static_cast<uint8_t>(val);
      }
      return i == 4;
    };
    uint8_t ip[4], mask[4], gw[4];
    if (!strToIp(force_ip_address_, ip)) {
      RCLCPP_ERROR(logger_, "[ForceIP] Invalid IP: %s", force_ip_address_.c_str());
      return false;
    }
    if (!strToIp(force_ip_subnet_mask_, mask)) {
      RCLCPP_ERROR(logger_, "[ForceIP] Invalid Mask: %s", force_ip_subnet_mask_.c_str());
      return false;
    }
    if (!strToIp(force_ip_gateway_, gw)) {
      RCLCPP_ERROR(logger_, "[ForceIP] Invalid Gateway: %s", force_ip_gateway_.c_str());
      return false;
    }
    std::memcpy(config.address, ip, 4);
    std::memcpy(config.mask, mask, 4);
    std::memcpy(config.gateway, gw, 4);
  }

  force_ip_success_ = false;
  try {
    auto device_list = ctx_->queryDeviceList();
    uint32_t index = 0;
    std::string mac;
    if (!force_ip_mac_.empty()) {
      mac = force_ip_mac_;
    } else if (device_list->getCount() == 1) {
      mac = device_list->getUid(index);
    } else {
      RCLCPP_ERROR(logger_, "[ForceIP] MAC address is empty");
      return false;
    }

    if (ctx_->forceIp(mac.c_str(), config)) {
      RCLCPP_INFO(logger_, "[ForceIP] Config applied. dhcp=%d ip=%s mask=%s gw=%s", config.dhcp,
                  force_ip_address_.c_str(), force_ip_subnet_mask_.c_str(),
                  force_ip_gateway_.c_str());
      force_ip_success_ = true;
    } else {
      RCLCPP_ERROR(logger_, "[ForceIP] Failed to apply config (SDK returned false)");
    }
  } catch (const ob::Error &e) {
    RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s",
                 orbbec_camera::formatObErrorWithStatus(e).c_str());
  } catch (const std::exception &e) {
    RCLCPP_ERROR(logger_, "[ForceIP] std::exception: %s", e.what());
  } catch (...) {
    RCLCPP_ERROR(logger_, "[ForceIP] Unknown error");
  }

  return force_ip_success_;
}

void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
  if (net_device_ip.empty() || net_device_port == 0) {
    RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
    return;
  }

  // Check if already connecting
  bool expected = false;
  if (!device_connecting_.compare_exchange_strong(expected, true)) {
    RCLCPP_DEBUG_STREAM(logger_, "connectNetDevice: connection already in progress");
    return;
  }

  // Use RAII to ensure connecting flag is cleared
  std::shared_ptr<int> connecting_guard(nullptr,
                                        [this](int *) { device_connecting_.store(false); });

  std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
  auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port, device_access_mode_);
  if (device == nullptr) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to connect to net device " << net_device_ip);
    return;
  }
  try {
    initializeDevice(device);
    if (!device_connected_) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize net device " << net_device_ip);
    }
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Exception during net device initialization: " << e.what());
    device_connected_ = false;
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Unknown exception during net device initialization");
    device_connected_ = false;
  }
}

void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
  if (device_connected_.load()) {
    return;
  }

  // Try to set connecting flag atomically
  bool expected = false;
  if (!device_connecting_.compare_exchange_strong(expected, true)) {
    RCLCPP_DEBUG_STREAM(logger_, "startDevice: connection already in progress by another thread");
    return;
  }

  // Use RAII to ensure connecting flag is cleared
  std::shared_ptr<int> connecting_guard(nullptr, [this](int *) {
    device_connecting_.store(false);
    RCLCPP_DEBUG_STREAM(logger_, "startDevice: connecting flag cleared");
  });

  if (list->getCount() == 0) {
    RCLCPP_WARN(logger_, "No device found");
    return;
  }

  RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 5000, "startDevice called");

  start_time_ = std::chrono::high_resolution_clock::now();
  if (device_) {
    device_.reset();
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
  int try_lock_count = 0;
  int max_try_lock_count = 5;

  while (try_lock_count < max_try_lock_count) {
    int try_lock_result = pthread_mutex_trylock(orb_device_lock_);

    if (try_lock_result == 0) {
      // success get lock,break
      break;
    } else if (try_lock_result == EBUSY) {
      RCLCPP_WARN_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } else {
      RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
      return;  // Not EBUSY, return
    }

    try_lock_count++;
  }
  if (try_lock_count >= max_try_lock_count) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
    return;
  }

  std::shared_ptr<int> lock_holder(nullptr,
                                   [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
  bool start_device_failed = false;
  try {
    auto start_time = std::chrono::high_resolution_clock::now();
    auto device = selectDevice(list);
    if (device == nullptr) {
      device_connected_ = false;
      return;
    }
    auto end_time = std::chrono::high_resolution_clock::now();
    auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
    RCLCPP_DEBUG_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
    start_time = std::chrono::high_resolution_clock::now();
    initializeDevice(device);
    end_time = std::chrono::high_resolution_clock::now();
    time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
    RCLCPP_INFO_STREAM(logger_, "Initialize device cost: " << time_cost.count() << " ms");

    if (firmware_update_success_) {
      firmware_update_success_ = false;
      device_connected_ = false;
      {
        std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
        reset_device_flag_ = true;
      }
      reset_device_cond_.notify_all();
      return;
    }

    auto pid = device->getDeviceInfo()->getPid();
    if (GEMINI_335LG_PID == pid || GEMINI_338LG_PID == pid) {
      ob_camera_node_->startGmslTrigger();
    }
    // if (isGemini305SeriesPID(pid)) {
    //   // Fixing 305 series hot-swap not outputting power
    //   ob_camera_node_->startStreams();
    // }
  } catch (ob::Error &e) {
    RCLCPP_ERROR_STREAM(
        logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e));
    start_device_failed = true;
  } catch (std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
    start_device_failed = true;
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device");
    start_device_failed = true;
  }
  if (start_device_failed) {
    device_connected_ = false;
    {
      std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
      reset_device_flag_ = true;
    }
    reset_device_cond_.notify_all();
  }
}
void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
  if (path.empty()) {
    return;
  } else {
    std::stringstream ss(path);
    std::string path_segment;
    std::vector<std::string> paths;
    OBFwUpdateState updateState = STAT_START;
    bool firstCall = true;

    while (std::getline(ss, path_segment, ',')) {
      paths.push_back(path_segment);
    }
    uint8_t index = 0;
    uint8_t count = static_cast<uint8_t>(paths.size());
    char(*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX];
    RCLCPP_INFO_STREAM(this->get_logger(), "paths.cout : " << (uint32_t)count);
    for (const auto &p : paths) {
      strcpy(filePaths[index], p.c_str());
      RCLCPP_INFO_STREAM(this->get_logger(),
                         "path: " << (uint32_t)index << ":" << filePaths[index]);
      index++;
    }
    RCLCPP_INFO_STREAM(this->get_logger(),
                       "Start to update optional depth preset, please wait a moment...");
    try {
      device_->updateOptionalDepthPresets(
          filePaths, count,
          [this, &updateState, &firstCall](OBFwUpdateState state, const char *message,
                                           uint8_t percent) {
            updateState = state;
            presetUpdateCallback(firstCall, state, message, percent);
            // firstCall = false;
          });

      delete[] filePaths;
      filePaths = nullptr;
      if (updateState == STAT_DONE || updateState == STAT_DONE_WITH_DUPLICATES) {
        RCLCPP_INFO_STREAM(this->get_logger(), "After updating the preset: ");
        auto presetList = device_->getAvailablePresetList();
        RCLCPP_INFO_STREAM(this->get_logger(), "Preset count: " << presetList->getCount());
        for (uint32_t i = 0; i < presetList->getCount(); ++i) {
          RCLCPP_INFO_STREAM(this->get_logger(), "  - " << presetList->getName(i));
        }
        RCLCPP_INFO_STREAM(this->get_logger(),
                           "Current preset: " << device_->getCurrentPresetName());
        std::string key = "PresetVer";
        if (device_->isExtensionInfoExist(key)) {
          std::string value = device_->getExtensionInfo(key);
          RCLCPP_INFO_STREAM(this->get_logger(), "Preset version: " << value);
        } else {
          RCLCPP_INFO_STREAM(this->get_logger(), "PresetVer: ");
        }
      }
    } catch (ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware "
                                       << orbbec_camera::formatObErrorWithStatus(e));
    } catch (std::exception &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what());
    } catch (...) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware");
    }
  }
}
void OBCameraNodeDriver::presetUpdateCallback(bool firstCall, OBFwUpdateState state,
                                              const char *message, uint8_t percent) {
  if (!firstCall) {
    std::cout << "\033[3F";
  }

  std::cout << "\033[K";
  std::cout << "Progress: " << static_cast<uint32_t>(percent) << "%" << std::endl;

  std::cout << "\033[K";
  std::cout << "Status  : ";
  switch (state) {
    case STAT_VERIFY_SUCCESS:
      std::cout << "Image file verification success" << std::endl;
      break;
    case STAT_FILE_TRANSFER:
      std::cout << "File transfer in progress" << std::endl;
      break;
    case STAT_DONE:
      std::cout << "Update completed" << std::endl;
      break;
    case STAT_DONE_REBOOT_AND_REUPDATE:
      std::cout << "Update completed, requires reboot and reupdate" << std::endl;
      break;
    case STAT_DONE_WITH_DUPLICATES:
      std::cout << "Update completed, duplicated presets have been ignored" << std::endl;
      break;
    case STAT_IN_PROGRESS:
      std::cout << "Update in progress" << std::endl;
      break;
    case STAT_START:
      std::cout << "Starting the update" << std::endl;
      break;
    case STAT_VERIFY_IMAGE:
      std::cout << "Verifying image file" << std::endl;
      break;
    default:
      std::cout << "Unknown status or error" << std::endl;
      break;
  }

  std::cout << "\033[K";
  std::cout << "Message : " << message << std::endl << std::flush;
}
void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const char *message,
                                                uint8_t percent) {
  std::cout << "\033[K";  // Clear the current line
  std::cout << "Progress: " << static_cast<uint32_t>(percent) << "%" << std::endl;

  std::cout << "\033[K";
  std::cout << "Status  : ";
  switch (state) {
    case STAT_VERIFY_SUCCESS:
      std::cout << "Image file verification success" << std::endl;
      break;
    case STAT_FILE_TRANSFER:
      std::cout << "File transfer in progress" << std::endl;
      break;
    case STAT_DONE:
      std::cout << "Update completed" << std::endl;
      break;
    case STAT_DONE_REBOOT_AND_REUPDATE:
      need_reupdate_ = true;
      std::cout << "Update completed (requires reboot and reupdate)" << std::endl;
      break;
    case STAT_IN_PROGRESS:
      std::cout << "Upgrade in progress" << std::endl;
      break;
    case STAT_START:
      std::cout << "Starting the upgrade" << std::endl;
      break;
    case STAT_VERIFY_IMAGE:
      std::cout << "Verifying image file" << std::endl;
      break;
    default:
      std::cout << "Unknown status or error" << std::endl;
      break;
  }

  std::cout << "\033[K";
  std::cout << "Message : " << message << std::endl << std::flush;
  if (state == STAT_DONE || state == STAT_DONE_REBOOT_AND_REUPDATE) {
    RCLCPP_INFO(logger_, "Reboot device");

    if (ob_camera_node_) {
      // Don't call clean() here to avoid deadlock - just stop timers and reboot
      // The resetDevice thread will handle proper cleanup when device disconnects
      if (sync_host_time_timer_) {
        try {
          sync_host_time_timer_->cancel();
          sync_host_time_timer_.reset();
        } catch (...) {
          RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup in firmware update");
        }
      }
      delay_stream_start_after_reconnect_ = true;
      device_->reboot();
    } else if (ob_lidar_node_) {
      ob_lidar_node_.reset();
    }
    device_connected_ = false;
    firmware_update_success_ = true;
    if (state == STAT_DONE_REBOOT_AND_REUPDATE) {
      // Keep upgrade_firmware_ path for second update
      RCLCPP_INFO(logger_, "Firmware update requires a second update after reboot");
    } else {
      upgrade_firmware_ = "";
    }
  }
}

OBDeviceAccessMode OBCameraNodeDriver::stringToAccessMode(const std::string &mode_str) {
  std::string lower_mode;
  std::transform(mode_str.begin(), mode_str.end(), std::back_inserter(lower_mode),
                 [](auto ch) { return tolower(ch); });

  if (lower_mode == "ea") {
    return OB_DEVICE_EXCLUSIVE_ACCESS;
  } else if (lower_mode == "ca") {
    return OB_DEVICE_CONTROL_ACCESS;
  } else if (lower_mode == "mr") {
    return OB_DEVICE_MONITOR_ACCESS;
  } else if (lower_mode == "default") {
    return OB_DEVICE_DEFAULT_ACCESS;
  } else {
    RCLCPP_WARN_STREAM(logger_, "Unknown access mode: " << mode_str << ", using default");
    return OB_DEVICE_DEFAULT_ACCESS;
  }
}

OBClockType OBCameraNodeDriver::timestampClockTypeFromString(const std::string &clock_type_str) {
  std::string lower_type;
  std::transform(clock_type_str.begin(), clock_type_str.end(), std::back_inserter(lower_type),
                 [](auto ch) { return tolower(ch); });

  if (lower_type == "realtime") {
    return OB_CLOCK_TYPE_REALTIME;
  }
  if (lower_type == "monotonic") {
    return OB_CLOCK_TYPE_MONOTONIC;
  }

  RCLCPP_WARN_STREAM(logger_,
                     "Unknown timestamp_clock_type: " << clock_type_str << ", using realtime");
  return OB_CLOCK_TYPE_REALTIME;
}

std::string OBCameraNodeDriver::timestampClockTypeToString(OBClockType clock_type) {
  switch (clock_type) {
    case OB_CLOCK_TYPE_MONOTONIC:
      return "monotonic";
    case OB_CLOCK_TYPE_REALTIME:
    default:
      return "realtime";
  }
}

std::string OBCameraNodeDriver::accessModeToString(OBDeviceAccessMode mode) {
  switch (mode) {
    case OB_DEVICE_EXCLUSIVE_ACCESS:
      return "EA";
    case OB_DEVICE_CONTROL_ACCESS:
      return "CA";
    case OB_DEVICE_MONITOR_ACCESS:
      return "MR";
    case OB_DEVICE_ACCESS_DENIED:
      return "Denied";
    case OB_DEVICE_DEFAULT_ACCESS:
    default:
      return "Default";
  }
}
}  // namespace orbbec_camera

RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeDriver)