#include <algorithm>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

extern "C" {
#include "libiruvc.h"
#include "libirparse.h"
#include "libirtemp.h"
}

static inline float temp_value_converter(uint16_t temp_val) {
  return static_cast<float>(static_cast<double>(temp_val) / 64.0 - 273.15);
}

class IrCameraNode : public rclcpp::Node {
public:
  IrCameraNode() : Node("ir_camera_node") {
    frame_id_ = this->declare_parameter<std::string>("frame_id", "ir_camera");
    pid_ = this->declare_parameter<int>("pid", 0x5840);
    vid_ = this->declare_parameter<int>("vid", 0x0BDA);
    resolution_index_ = this->declare_parameter<int>("resolution_index", 1);

    enable_temp_correction_ = this->declare_parameter<bool>("enable_temp_correction", false);
    tau_table_path_ = this->declare_parameter<std::string>("tau_table_path", "");
    ems_ = this->declare_parameter<double>("ems", 1.0);
    ta_ = this->declare_parameter<double>("ta", 25.0);
    dist_ = this->declare_parameter<double>("dist", 2.0);
    iter_rounds_ = this->declare_parameter<int>("iter_rounds", 2);

    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/ir/image_raw", 10);
    temp_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/ir/temperature_celsius", 10);
    temp_corrected_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/ir/temperature_celsius_corrected", 10);

    iruvc_log_register(IRUVC_LOG_NO_PRINT);
    irparse_log_register(IRPARSE_LOG_NO_PRINT);
    irtemp_log_register(IRTEMP_LOG_NO_PRINT);

    if (enable_temp_correction_) {
      if (!load_tau_table()) {
        RCLCPP_WARN(this->get_logger(), "enable_temp_correction=true but tau table load failed, fallback to raw temp");
        enable_temp_correction_ = false;
      } else {
        RCLCPP_INFO(this->get_logger(), "Temp correction enabled, ems=%.3f ta=%.2f dist=%.2f iter_rounds=%d",
                    ems_, ta_, dist_, iter_rounds_);
      }
    }

    if (!open_and_start()) {
      throw std::runtime_error("Failed to open/start IR camera stream");
    }

    running_ = true;
    worker_ = std::thread(&IrCameraNode::capture_loop, this);

    RCLCPP_INFO(this->get_logger(), "IR camera node started");
    RCLCPP_INFO(this->get_logger(), "Image topic: /ir/image_raw (bgr8)");
    RCLCPP_INFO(this->get_logger(), "Temp topic: /ir/temperature_celsius (32FC1 raw formula)");
    RCLCPP_INFO(this->get_logger(), "Temp topic: /ir/temperature_celsius_corrected (32FC1 corrected)");
  }

  ~IrCameraNode() override {
    running_ = false;
    if (worker_.joinable()) {
      worker_.join();
    }
    stop_and_close();
  }

private:
  bool load_tau_table() {
    if (tau_table_path_.empty()) {
      RCLCPP_ERROR(this->get_logger(), "tau_table_path is empty");
      return false;
    }

    std::ifstream in(tau_table_path_, std::ios::binary | std::ios::ate);
    if (!in.is_open()) {
      RCLCPP_ERROR(this->get_logger(), "Cannot open tau table: %s", tau_table_path_.c_str());
      return false;
    }

    const std::streamsize file_size = in.tellg();
    if (file_size <= 0 || (file_size % 2) != 0) {
      RCLCPP_ERROR(this->get_logger(), "Invalid tau table file size: %ld", static_cast<long>(file_size));
      return false;
    }

    in.seekg(0, std::ios::beg);
    std::vector<uint8_t> bytes(static_cast<size_t>(file_size));
    if (!in.read(reinterpret_cast<char *>(bytes.data()), file_size)) {
      RCLCPP_ERROR(this->get_logger(), "Failed to read tau table: %s", tau_table_path_.c_str());
      return false;
    }

    correct_table_.resize(bytes.size() / 2);
    std::memcpy(correct_table_.data(), bytes.data(), bytes.size());

    valid_correct_table_ = correct_table_.data();
    valid_correct_table_len_ = static_cast<uint32_t>(correct_table_.size());

    if (correct_table_.size() > 4 && correct_table_[2] == 0xFFFF && correct_table_[3] == 0xFFFF) {
      if (correct_table_.size() <= HEAD_SIZE) {
        RCLCPP_ERROR(this->get_logger(), "Tau table too small for HEAD_SIZE compatibility");
        return false;
      }
      valid_correct_table_ = correct_table_.data() + HEAD_SIZE;
      valid_correct_table_len_ = static_cast<uint32_t>(correct_table_.size() - HEAD_SIZE);
    }

    RCLCPP_INFO(this->get_logger(), "Loaded tau table: %s, total_u16=%zu, valid_u16=%u",
                tau_table_path_.c_str(), correct_table_.size(), valid_correct_table_len_);
    return true;
  }

  bool open_and_start() {
    if (uvc_camera_init() < 0) {
      RCLCPP_ERROR(this->get_logger(), "uvc_camera_init failed");
      return false;
    }

    DevCfg_t devs_cfg[64] = {0};
    if (uvc_camera_list(devs_cfg) < 0) {
      RCLCPP_ERROR(this->get_logger(), "uvc_camera_list failed");
      return false;
    }

    std::vector<int> matched_indices;
    for (int i = 0; i < 64; ++i) {
      if (static_cast<int>(devs_cfg[i].vid) == vid_ && static_cast<int>(devs_cfg[i].pid) == pid_) {
        matched_indices.push_back(i);
        RCLCPP_INFO(this->get_logger(), "Matched device idx=%d name=%s vid=0x%04x pid=0x%04x", i,
                    (devs_cfg[i].name ? devs_cfg[i].name : "null"),
                    static_cast<int>(devs_cfg[i].vid), static_cast<int>(devs_cfg[i].pid));
      }
    }

    if (matched_indices.empty()) {
      RCLCPP_ERROR(this->get_logger(), "Cannot find device vid=0x%04x pid=0x%04x", vid_, pid_);
      return false;
    }

    int dev_index = -1;
    int open_ret = -1;
    for (int idx : matched_indices) {
      open_ret = uvc_camera_open(devs_cfg[idx]);
      if (open_ret >= 0) {
        dev_index = idx;
        break;
      }
      RCLCPP_WARN(this->get_logger(), "uvc_camera_open failed on idx=%d, ret=%d", idx, open_ret);
    }

    if (dev_index < 0 && !matched_indices.empty()) {
      const int base_idx = matched_indices.front();
      for (size_t same_idx = 0; same_idx < matched_indices.size(); ++same_idx) {
        open_ret = uvc_camera_open_same(devs_cfg[base_idx], static_cast<int>(same_idx));
        if (open_ret >= 0) {
          dev_index = base_idx;
          RCLCPP_INFO(this->get_logger(), "uvc_camera_open_same success, same_idx=%zu", same_idx);
          break;
        }
        RCLCPP_WARN(this->get_logger(), "uvc_camera_open_same failed same_idx=%zu ret=%d", same_idx, open_ret);
      }
    }

    if (dev_index < 0) {
      RCLCPP_ERROR(this->get_logger(), "All open attempts failed for vid=0x%04x pid=0x%04x", vid_, pid_);
      return false;
    }

    CameraStreamInfo_t camera_stream_info[32] = {0};
    if (uvc_camera_info_get(devs_cfg[dev_index], camera_stream_info) < 0) {
      RCLCPP_ERROR(this->get_logger(), "uvc_camera_info_get failed");
      return false;
    }

    int max_valid_idx = 0;
    while (max_valid_idx < 31 && camera_stream_info[max_valid_idx].width != 0 && camera_stream_info[max_valid_idx].height != 0) {
      ++max_valid_idx;
    }

    if (max_valid_idx == 0) {
      RCLCPP_ERROR(this->get_logger(), "No valid stream info from camera");
      return false;
    }

    const int idx = std::clamp(resolution_index_, 0, max_valid_idx - 1);

    camera_param_.dev_cfg = devs_cfg[dev_index];
    camera_param_.format = camera_stream_info[idx].format;
    camera_param_.width = camera_stream_info[idx].width;
    camera_param_.height = camera_stream_info[idx].height;
    camera_param_.frame_size = camera_param_.width * camera_param_.height * 2;
    camera_param_.fps = camera_stream_info[idx].fps[0];
    camera_param_.timeout_ms_delay = 1000;

    image_width_ = static_cast<int>(camera_param_.width);
    image_height_ = static_cast<int>(camera_param_.height / 2);
    image_pixel_num_ = image_width_ * image_height_;

    image_byte_size_ = image_pixel_num_ * 2;
    temp_byte_size_ = image_pixel_num_ * 2;

    raw_frame_ = static_cast<uint8_t *>(uvc_frame_buf_create(camera_param_));
    if (raw_frame_ == nullptr) {
      RCLCPP_ERROR(this->get_logger(), "uvc_frame_buf_create failed");
      return false;
    }

    image_frame_.resize(image_byte_size_);
    temp_frame_.resize(temp_byte_size_);
    rgb_frame_.resize(image_pixel_num_ * 3);
    bgr_frame_.resize(image_pixel_num_ * 3);

    if (uvc_camera_stream_start(camera_param_, nullptr) < 0) {
      RCLCPP_ERROR(this->get_logger(), "uvc_camera_stream_start failed");
      return false;
    }

    streaming_ = true;
    RCLCPP_INFO(this->get_logger(), "Stream started: %dx%d@%d", image_width_, image_height_, camera_param_.fps);
    return true;
  }

  void stop_and_close() {
    if (streaming_) {
      uvc_camera_stream_close(KEEP_CAM_SIDE_PREVIEW);
      streaming_ = false;
    }

    if (raw_frame_ != nullptr) {
      uvc_frame_buf_release(raw_frame_);
      raw_frame_ = nullptr;
    }

    uvc_camera_close();
    uvc_camera_release();
  }

  inline float calc_corrected_temp(float org_temp_c) {
    if (!enable_temp_correction_ || valid_correct_table_ == nullptr) {
      return org_temp_c;
    }

    float current = org_temp_c;
    uint16_t tau = 0;

    const int rounds = std::max(1, iter_rounds_);
    for (int k = 0; k < rounds; ++k) {
      if (read_tau_with_target_temp_and_dist(valid_correct_table_, current, static_cast<float>(dist_), &tau) != IRTEMP_SUCCESS) {
        return org_temp_c;
      }
      float next = current;
      if (temp_correct(static_cast<float>(ems_), tau, static_cast<float>(ta_), org_temp_c, &next) != IRTEMP_SUCCESS) {
        return org_temp_c;
      }
      current = next;
    }

    return current;
  }

  void capture_loop() {
    while (rclcpp::ok() && running_) {
      const int ret = uvc_frame_get(raw_frame_);
      if (ret < 0) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "uvc_frame_get timeout/error: %d", ret);
        continue;
      }

      raw_data_cut(raw_frame_, image_byte_size_, temp_byte_size_, image_frame_.data(), temp_frame_.data());

      yuv422_to_rgb(image_frame_.data(), image_pixel_num_, rgb_frame_.data());
      rgb_to_bgr(rgb_frame_.data(), image_pixel_num_, bgr_frame_.data());

      auto stamp = this->now();
      publish_image(stamp);
      publish_temperature(stamp);
      publish_temperature_corrected(stamp);
    }
  }

  void publish_image(const rclcpp::Time & stamp) {
    sensor_msgs::msg::Image msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = frame_id_;
    msg.height = static_cast<uint32_t>(image_height_);
    msg.width = static_cast<uint32_t>(image_width_);
    msg.encoding = "bgr8";
    msg.is_bigendian = 0;
    msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(image_width_ * 3);
    msg.data.assign(bgr_frame_.begin(), bgr_frame_.end());
    image_pub_->publish(msg);
  }

  void publish_temperature(const rclcpp::Time & stamp) {
    sensor_msgs::msg::Image msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = frame_id_;
    msg.height = static_cast<uint32_t>(image_height_);
    msg.width = static_cast<uint32_t>(image_width_);
    msg.encoding = "32FC1";
    msg.is_bigendian = 0;
    msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(image_width_ * sizeof(float));
    msg.data.resize(static_cast<size_t>(image_width_ * image_height_ * sizeof(float)));

    const auto * src = reinterpret_cast<const uint16_t *>(temp_frame_.data());
    auto * dst = reinterpret_cast<float *>(msg.data.data());

    for (int i = 0; i < image_pixel_num_; ++i) {
      dst[i] = temp_value_converter(src[i]);
    }

    temp_pub_->publish(msg);
  }

  void publish_temperature_corrected(const rclcpp::Time & stamp) {
    sensor_msgs::msg::Image msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = frame_id_;
    msg.height = static_cast<uint32_t>(image_height_);
    msg.width = static_cast<uint32_t>(image_width_);
    msg.encoding = "32FC1";
    msg.is_bigendian = 0;
    msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(image_width_ * sizeof(float));
    msg.data.resize(static_cast<size_t>(image_width_ * image_height_ * sizeof(float)));

    const auto * src = reinterpret_cast<const uint16_t *>(temp_frame_.data());
    auto * dst = reinterpret_cast<float *>(msg.data.data());

    for (int i = 0; i < image_pixel_num_; ++i) {
      const float org_temp = temp_value_converter(src[i]);
      dst[i] = calc_corrected_temp(org_temp);
    }

    temp_corrected_pub_->publish(msg);
  }

private:
  std::string frame_id_;
  int pid_{0};
  int vid_{0};
  int resolution_index_{1};

  bool enable_temp_correction_{false};
  std::string tau_table_path_;
  double ems_{1.0};
  double ta_{25.0};
  double dist_{2.0};
  int iter_rounds_{2};

  std::vector<uint16_t> correct_table_;
  uint16_t * valid_correct_table_{nullptr};
  uint32_t valid_correct_table_len_{0};

  CameraParam_t camera_param_{};
  bool streaming_{false};
  bool running_{false};

  uint8_t * raw_frame_{nullptr};
  std::vector<uint8_t> image_frame_;
  std::vector<uint8_t> temp_frame_;
  std::vector<uint8_t> rgb_frame_;
  std::vector<uint8_t> bgr_frame_;

  int image_width_{0};
  int image_height_{0};
  int image_pixel_num_{0};
  int image_byte_size_{0};
  int temp_byte_size_{0};

  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr temp_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr temp_corrected_pub_;

  std::thread worker_;
};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<IrCameraNode>();
    rclcpp::spin(node);
  } catch (const std::exception & e) {
    std::fprintf(stderr, "ir_camera_node exception: %s\n", e.what());
  }
  rclcpp::shutdown();
  return 0;
}

