#include <camera_info_manager/camera_info_manager.hpp>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <filesystem>
#include <image_transport/image_transport.hpp>
#include <memory>
#include <mutex>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <string>
#include <thread>

#include "MvCameraControl.h"
#include "hik_camera/recorder.hpp"
#include "rm_utils/heartbeat.hpp"
#include "rm_utils/logger/log.hpp"

namespace fyt::camera_driver
{
class HikCameraNode : public rclcpp::Node
{
public:
  explicit HikCameraNode(const rclcpp::NodeOptions & options) : Node("camera_driver", options)
  {
    FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
    FYT_INFO("camera_driver", "Starting HikCameraNode!");

    camera_name_ = this->declare_parameter("camera_name", "narrow_stereo");
    camera_info_url_ =
      this->declare_parameter("camera_info_url", "package://hik_camera/config/camera_info.yaml");
    frame_id_ = this->declare_parameter("camera_frame_id", "camera_optical_frame");
    pixel_format_ = this->declare_parameter("pixel_format", "rgb8");
    frame_rate_ = this->declare_parameter("frame_rate", 210);
    exposure_time_ = this->declare_parameter("exposure_time", 5000);
    gain_ = this->declare_parameter("gain", 12.0);

    resolution_width_ = this->declare_parameter("resolution_width", 1280);
    resolution_height_ = this->declare_parameter("resolution_height", 1024);
    offset_x_ = this->declare_parameter("offsetX", 0);
    offset_y_ = this->declare_parameter("offsetY", 0);

    image_msg_.header.frame_id = frame_id_;
    image_msg_.encoding = pixel_format_;

    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
    camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);

    camera_info_manager_ =
      std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
    if (camera_info_manager_->validateURL(camera_info_url_)) {
      camera_info_manager_->loadCameraInfo(camera_info_url_);
      camera_info_msg_ = camera_info_manager_->getCameraInfo();
    } else {
      camera_info_manager_->setCameraName(camera_name_);
      sensor_msgs::msg::CameraInfo camera_info;
      camera_info_manager_->setCameraInfo(camera_info);
      FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
    }

    // Heartbeat
    heartbeat_ = fyt::HeartBeatPublisher::create(this);

    // Param set callback
    params_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));

    // Recorder
    bool enable_recorder = this->declare_parameter("recording", false);
    if (enable_recorder) {
      std::string home = std::getenv("HOME") ? std::getenv("HOME") : std::string(".");
      namespace fs = std::filesystem;
      std::filesystem::path video_path = fs::path(home) / "fyt2024-log/video/" /
                                         std::string(std::to_string(std::time(nullptr)) + ".avi");
      recorder_ = std::make_unique<Recorder>(
        video_path, frame_rate_, cv::Size(resolution_width_, resolution_height_));
      recorder_->start();
      FYT_INFO("camera_driver", "Recorder started! Video file: {}", video_path.string());
    }

    // Watch dog timer (also handles initial open)
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this));

    // Start capture thread
    capture_thread_ = std::thread([this]() { this->captureLoop(); });

    FYT_INFO("camera_driver", "HikCameraNode has been initialized!");
  }

  ~HikCameraNode() override
  {
    close();
    if (capture_thread_.joinable()) {
      capture_thread_.join();
    }
    if (recorder_ != nullptr) {
      recorder_->stop();
      FYT_INFO(
        "camera_driver", "Recorder stopped! Video file {} has been saved",
        recorder_->path.string());
    }
    FYT_INFO("camera_driver", "HikCameraNode has been destroyed!");
  }

private:
  static int64_t clampToInc(int64_t value, int64_t min, int64_t max, int64_t inc)
  {
    if (inc <= 0) {
      inc = 1;
    }
    if (value < min) {
      value = min;
    }
    if (value > max) {
      value = max;
    }
    const int64_t delta = value - min;
    return min + (delta / inc) * inc;
  }

  bool getIntNodeLocked(const char * name, MVCC_INTVALUE & out)
  {
    if (camera_handle_ == nullptr) {
      return false;
    }
    return MV_CC_GetIntValue(camera_handle_, name, &out) == MV_OK;
  }

  bool setIntNodeLocked(const char * name, int64_t value)
  {
    if (camera_handle_ == nullptr) {
      return false;
    }
    return MV_CC_SetIntValue(camera_handle_, name, value) == MV_OK;
  }

  void applyRoiLocked()
  {
    // Best-effort: not all cameras support all nodes
    MVCC_INTVALUE width_info;
    MVCC_INTVALUE height_info;
    MVCC_INTVALUE offx_info;
    MVCC_INTVALUE offy_info;

    const bool has_width = getIntNodeLocked("Width", width_info);
    const bool has_height = getIntNodeLocked("Height", height_info);
    const bool has_offx = getIntNodeLocked("OffsetX", offx_info);
    const bool has_offy = getIntNodeLocked("OffsetY", offy_info);

    if (!has_width || !has_height) {
      FYT_WARN("camera_driver", "Camera does not expose Width/Height nodes; ROI params ignored");
      return;
    }

    int64_t target_w =
      clampToInc(resolution_width_, width_info.nMin, width_info.nMax, width_info.nInc);
    int64_t target_h =
      clampToInc(resolution_height_, height_info.nMin, height_info.nMax, height_info.nInc);

    int64_t target_x = 0;
    int64_t target_y = 0;
    if (has_offx) {
      target_x = clampToInc(offset_x_, offx_info.nMin, offx_info.nMax, offx_info.nInc);
    }
    if (has_offy) {
      target_y = clampToInc(offset_y_, offy_info.nMin, offy_info.nMax, offy_info.nInc);
    }

    // Some cameras constrain width/height based on offset; apply in safe order
    if (has_offx) {
      (void)setIntNodeLocked("OffsetX", target_x);
    }
    if (has_offy) {
      (void)setIntNodeLocked("OffsetY", target_y);
    }
    (void)setIntNodeLocked("Width", target_w);
    (void)setIntNodeLocked("Height", target_h);

    resolution_width_ = static_cast<int>(target_w);
    resolution_height_ = static_cast<int>(target_h);
    offset_x_ = static_cast<int>(target_x);
    offset_y_ = static_cast<int>(target_y);

    FYT_INFO(
      "camera_driver", "ROI applied: {}x{} offset({},{})", resolution_width_, resolution_height_,
      offset_x_, offset_y_);
  }

  void timerCallback()
  {
    // Open camera
    while (!is_open_ && rclcpp::ok()) {
      if (!open()) {
        FYT_ERROR("camera_driver", "open() failed");
        close();
        return;
      }
    }

    // Watchdog: no frames for too long => reconnect
    double dt = (this->now() - latest_frame_stamp_).seconds();
    if (dt > 5.0) {
      FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt);
      close();
    }
  }

  bool open()
  {
    if (is_open_) {
      close();
    }

    FYT_INFO("camera_driver", "Opening Hik Camera Device!");

    MV_CC_DEVICE_INFO_LIST device_list;
    int ret = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
    if (ret != MV_OK || device_list.nDeviceNum == 0) {
      FYT_WARN("camera_driver", "No camera found! Enum state: [0x{:x}]", ret);
      return false;
    }
    FYT_INFO("camera_driver", "Found {} devices", device_list.nDeviceNum);

    ret = MV_CC_CreateHandle(&camera_handle_, device_list.pDeviceInfo[0]);
    if (ret != MV_OK) {
      FYT_ERROR("camera_driver", "CreateHandle failed: [0x{:x}]", ret);
      camera_handle_ = nullptr;
      return false;
    }

    ret = MV_CC_OpenDevice(camera_handle_);
    if (ret != MV_OK) {
      FYT_ERROR("camera_driver", "OpenDevice failed: [0x{:x}]", ret);
      MV_CC_DestroyHandle(&camera_handle_);
      camera_handle_ = nullptr;
      return false;
    }

    {
      std::lock_guard<std::mutex> lock(camera_mutex_);
      // Trigger off (continuous)
      (void)MV_CC_SetTriggerMode(camera_handle_, MV_TRIGGER_MODE_OFF);

      // Apply ROI/offset before querying image info
      applyRoiLocked();

      // Set exposure / gain
      (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
      (void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
    }

    // Get image info
    {
      std::lock_guard<std::mutex> lock(camera_mutex_);
      ret = MV_CC_GetImageInfo(camera_handle_, &img_info_);
    }
    if (ret != MV_OK) {
      FYT_ERROR("camera_driver", "GetImageInfo failed: [0x{:x}]", ret);
      close();
      return false;
    }

    // Init convert param
    convert_param_ = {};
    convert_param_.nWidth = img_info_.nWidthValue;
    convert_param_.nHeight = img_info_.nHeightValue;
    convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;

    // Use actual output size from camera
    resolution_width_ = static_cast<int>(img_info_.nWidthValue);
    resolution_height_ = static_cast<int>(img_info_.nHeightValue);

    image_msg_.height = resolution_height_;
    image_msg_.width = resolution_width_;
    image_msg_.step = resolution_width_ * 3;
    image_msg_.data.resize(static_cast<size_t>(image_msg_.height) * image_msg_.step);

    camera_info_msg_.header.frame_id = frame_id_;
    camera_info_msg_.width = image_msg_.width;
    camera_info_msg_.height = image_msg_.height;
    latest_frame_stamp_ = this->now();

    {
      std::lock_guard<std::mutex> lock(camera_mutex_);
      ret = MV_CC_StartGrabbing(camera_handle_);
    }
    if (ret != MV_OK) {
      FYT_ERROR("camera_driver", "StartGrabbing failed: [0x{:x}]", ret);
      close();
      return false;
    }

    is_open_ = true;
    FYT_INFO("camera_driver", "Hik Camera Device Open Success!");
    return true;
  }

  void close()
  {
    if (!is_open_) {
      return;
    }
    FYT_INFO("camera_driver", "Closing Hik Camera Device!");
    is_open_ = false;

    std::lock_guard<std::mutex> lock(camera_mutex_);
    if (camera_handle_) {
      (void)MV_CC_StopGrabbing(camera_handle_);
      (void)MV_CC_CloseDevice(camera_handle_);
      (void)MV_CC_DestroyHandle(&camera_handle_);
      camera_handle_ = nullptr;
    }
  }

  void captureLoop()
  {
    MV_FRAME_OUT out_frame;
    while (rclcpp::ok()) {
      if (!is_open_ || camera_handle_ == nullptr) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        continue;
      }

      int ret;
      {
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (!is_open_ || camera_handle_ == nullptr) {
          continue;
        }
        ret = MV_CC_GetImageBuffer(camera_handle_, &out_frame, 1000);
      }
      if (ret != MV_OK) {
        FYT_WARN("camera_driver", "GetImageBuffer failed: [0x{:x}]", ret);
        fail_count_++;
        if (fail_count_ > 5) {
          close();
        }
        continue;
      }

      // Ensure output buffer
      const size_t need_size =
        static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
      if (image_msg_.data.size() != need_size) {
        image_msg_.data.resize(need_size);
      }

      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;

      {
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (!is_open_ || camera_handle_ == nullptr) {
          continue;
        }
        ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_);
      }
      if (ret != MV_OK) {
        FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}]", ret);
        {
          std::lock_guard<std::mutex> lock(camera_mutex_);
          if (camera_handle_ != nullptr) {
            (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
          }
        }
        fail_count_++;
        continue;
      }

      auto stamp = this->now();
      image_msg_.header.stamp = stamp;
      image_msg_.header.frame_id = frame_id_;
      image_msg_.height = out_frame.stFrameInfo.nHeight;
      image_msg_.width = out_frame.stFrameInfo.nWidth;
      image_msg_.step = out_frame.stFrameInfo.nWidth * 3;

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

      latest_frame_stamp_ = stamp;
      fail_count_ = 0;

      if (recorder_ != nullptr) {
        recorder_->addFrame(image_msg_.data);
      }

      {
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
        }
      }
    }
  }

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

    for (const auto & param : parameters) {
      if (param.get_name() == "exposure_time") {
        exposure_time_ = param.as_int();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          int status =
            MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
          if (status != MV_OK) {
            result.successful = false;
            result.reason = "Failed to set exposure time, status = " + std::to_string(status);
          } else {
            FYT_INFO("camera_driver", "Set exposure_time: {}", exposure_time_);
          }
        }
      } else if (param.get_name() == "gain") {
        gain_ = param.as_double();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          int status = MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
          if (status != MV_OK) {
            result.successful = false;
            result.reason = "Failed to set gain, status = " + std::to_string(status);
          } else {
            FYT_INFO("camera_driver", "Set gain: {}", gain_);
          }
        }
      } else if (param.get_name() == "resolution_width") {
        resolution_width_ = param.as_int();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          applyRoiLocked();
        }
      } else if (param.get_name() == "resolution_height") {
        resolution_height_ = param.as_int();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          applyRoiLocked();
        }
      } else if (param.get_name() == "offsetX") {
        offset_x_ = param.as_int();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          applyRoiLocked();
        }
      } else if (param.get_name() == "offsetY") {
        offset_y_ = param.as_int();
        std::lock_guard<std::mutex> lock(camera_mutex_);
        if (camera_handle_ != nullptr) {
          applyRoiLocked();
        }
      }
    }

    return result;
  }

  // ROS pub
  sensor_msgs::msg::Image image_msg_;
  image_transport::CameraPublisher camera_pub_;

  // camera info
  std::string camera_name_;
  std::string camera_info_url_;
  std::string frame_id_;
  std::string pixel_format_;
  std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
  sensor_msgs::msg::CameraInfo camera_info_msg_;

  // Watch dog
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Time latest_frame_stamp_{0, 0, RCL_ROS_TIME};

  // Heartbeat
  fyt::HeartBeatPublisher::SharedPtr heartbeat_;

  // Params
  int frame_rate_{210};
  int exposure_time_{5000};
  double gain_{12.0};
  int resolution_width_{1280};
  int resolution_height_{1024};
  int offset_x_{0};
  int offset_y_{0};

  // Recorder
  std::unique_ptr<Recorder> recorder_;

  // Camera SDK
  void * camera_handle_{nullptr};
  MV_IMAGE_BASIC_INFO img_info_{};
  MV_CC_PIXEL_CONVERT_PARAM convert_param_{};

  std::mutex camera_mutex_;

  // State
  std::atomic<bool> is_open_{false};
  int fail_count_{0};
  std::thread capture_thread_;
  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
};
}  // namespace fyt::camera_driver

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::HikCameraNode)
