#include <algorithm>
#include <chrono>
#include <cmath>
#include <filesystem>
#include <fstream>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <thread>
#include <vector>

#include <dirent.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include <opencv2/opencv.hpp>

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

#include "mipi_vflow_backend.hpp"

namespace fs = std::filesystem;

struct CameraDeviceInfo {
  std::string video_dev;
  std::string card_name;
  std::string bus_type;
  std::vector<std::string> stable_ids;
};

struct FrameSize {
  int width;
  int height;
};

static std::string read_text_file(const std::string & path) {
  std::ifstream in(path);
  if (!in.is_open()) {
    return "";
  }
  std::string line;
  std::getline(in, line);
  return line;
}

static std::vector<std::string> list_dir_names(const std::string & dir) {
  std::vector<std::string> out;
  DIR * d = opendir(dir.c_str());
  if (!d) {
    return out;
  }
  struct dirent * ent = nullptr;
  while ((ent = readdir(d)) != nullptr) {
    std::string name = ent->d_name;
    if (name == "." || name == "..") {
      continue;
    }
    out.push_back(name);
  }
  closedir(d);
  std::sort(out.begin(), out.end());
  return out;
}

static std::string detect_bus_type(const std::string & video_name) {
  const std::string dev_link = "/sys/class/video4linux/" + video_name + "/device";
  std::error_code ec;
  const auto real = fs::weakly_canonical(dev_link, ec).string();
  if (ec || real.empty()) {
    return "unknown";
  }
  if (real.find("usb") != std::string::npos) {
    return "usb";
  }
  if (real.find("platform") != std::string::npos || real.find("csi") != std::string::npos || real.find("mipi") != std::string::npos) {
    return "mipi";
  }
  return "unknown";
}

static std::map<std::string, std::vector<std::string>> build_stable_id_map() {
  std::map<std::string, std::vector<std::string>> m;
  const std::vector<std::string> stable_dirs = {"/dev/v4l/by-id", "/dev/v4l/by-path"};
  for (const auto & dir : stable_dirs) {
    std::error_code ec;
    if (!fs::exists(dir, ec)) {
      continue;
    }
    for (const auto & entry : fs::directory_iterator(dir, ec)) {
      if (ec) {
        continue;
      }
      const std::string link = entry.path().string();
      std::error_code ec2;
      const auto target = fs::weakly_canonical(link, ec2);
      if (ec2) {
        continue;
      }
      const std::string dev_name = target.filename().string();
      m["/dev/" + dev_name].push_back(entry.path().filename().string());
    }
  }
  return m;
}

static std::vector<CameraDeviceInfo> list_camera_devices() {
  std::vector<CameraDeviceInfo> result;
  auto stable_map = build_stable_id_map();

  for (const auto & video_name : list_dir_names("/sys/class/video4linux")) {
    if (video_name.rfind("video", 0) != 0) {
      continue;
    }

    CameraDeviceInfo info;
    info.video_dev = "/dev/" + video_name;
    info.card_name = read_text_file("/sys/class/video4linux/" + video_name + "/name");
    info.bus_type = detect_bus_type(video_name);

    auto it = stable_map.find(info.video_dev);
    if (it != stable_map.end()) {
      info.stable_ids = it->second;
    }
    result.push_back(info);
  }

  std::sort(result.begin(), result.end(), [](const CameraDeviceInfo & a, const CameraDeviceInfo & b) {
    return a.video_dev < b.video_dev;
  });
  return result;
}

static bool contains_substr(const std::string & s, const std::string & token) {
  if (token.empty()) {
    return true;
  }
  return s.find(token) != std::string::npos;
}

static std::optional<CameraDeviceInfo> select_camera(
  const std::vector<CameraDeviceInfo> & devices,
  const std::string & bus_type,
  const std::string & camera_name,
  const std::string & stable_id,
  const std::string & explicit_device)
{
  if (!explicit_device.empty()) {
    for (const auto & d : devices) {
      if (d.video_dev == explicit_device) {
        return d;
      }
    }
    return std::nullopt;
  }

  for (const auto & d : devices) {
    if (bus_type != "auto" && d.bus_type != bus_type) {
      continue;
    }
    if (!contains_substr(d.card_name, camera_name)) {
      continue;
    }

    bool stable_ok = stable_id.empty();
    for (const auto & sid : d.stable_ids) {
      if (contains_substr(sid, stable_id)) {
        stable_ok = true;
        break;
      }
    }
    if (!stable_ok) {
      continue;
    }

    return d;
  }
  return std::nullopt;
}

static std::vector<FrameSize> enum_mjpeg_framesizes(const std::string & dev_path) {
  std::vector<FrameSize> sizes;

  int fd = open(dev_path.c_str(), O_RDONLY);
  if (fd < 0) {
    return sizes;
  }

  v4l2_frmsizeenum frmsize{};
  frmsize.pixel_format = V4L2_PIX_FMT_MJPEG;

  for (frmsize.index = 0; ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0; ++frmsize.index) {
    if (frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
      sizes.push_back({static_cast<int>(frmsize.discrete.width), static_cast<int>(frmsize.discrete.height)});
    }
  }

  close(fd);

  std::sort(sizes.begin(), sizes.end(), [](const FrameSize & a, const FrameSize & b) {
    if (a.width == b.width) {
      return a.height < b.height;
    }
    return a.width < b.width;
  });
  sizes.erase(std::unique(sizes.begin(), sizes.end(), [](const FrameSize & a, const FrameSize & b) {
    return a.width == b.width && a.height == b.height;
  }), sizes.end());
  return sizes;
}

static FrameSize choose_best_size(const std::vector<FrameSize> & sizes, int target_width, int target_height) {
  if (sizes.empty()) {
    return {target_width, target_height > 0 ? target_height : 480};
  }

  FrameSize best = sizes.front();
  int best_score = std::numeric_limits<int>::max();

  for (const auto & s : sizes) {
    const int w_score = std::abs(s.width - target_width) * 100;
    const int h_ref = target_height > 0 ? target_height : s.height;
    const int h_score = std::abs(s.height - h_ref);
    const int score = w_score + h_score;
    if (score < best_score) {
      best_score = score;
      best = s;
    }
  }

  return best;
}

class CameraNode : public rclcpp::Node {
public:
  CameraNode()
  : Node(std::string("color_camera_node_") + std::to_string(getpid())) {
    frame_id_ = declare_parameter<std::string>("frame_id", "camera_link");
    bus_type_ = declare_parameter<std::string>("bus_type", "auto");
    camera_name_ = declare_parameter<std::string>("camera_name", "");
    stable_id_ = declare_parameter<std::string>("stable_id", "");
    device_path_ = declare_parameter<std::string>("device_path", "");

    width_ = declare_parameter<int>("width", 640);
    height_hint_ = declare_parameter<int>("height", 0);
    fps_ = declare_parameter<int>("fps", 30);

    pixel_format_ = declare_parameter<std::string>("pixel_format", "MJPG");
    output_topic_ = declare_parameter<std::string>("output_topic", "/camera/image_raw");
    topic_name_ = declare_parameter<std::string>("topic_name", "");
    reliability_ = declare_parameter<std::string>("reliability", "best_effort");
    reconnect_on_failure_ = declare_parameter<bool>("reconnect_on_failure", true);
    reconnect_interval_ms_ = declare_parameter<int>("reconnect_interval_ms", 500);
    max_fail_count_before_reconnect_ = declare_parameter<int>("max_fail_count_before_reconnect", 10);

    // MIPI(vflow) backend parameters.
    mipi_host_ = declare_parameter<int>("mipi_host", 0);      // 0/1
    mipi_channel_ = declare_parameter<int>("mipi_channel", 0); // 0/1 for output
    mipi_sensor_id_ = declare_parameter<int>("mipi_sensor_id", 0);
    mipi_timeout_ms_ = declare_parameter<int>("mipi_timeout_ms", 2000);

    if (!topic_name_.empty()) {
      output_topic_ = topic_name_;
    }

    if (fps_ <= 0) {
      RCLCPP_WARN(get_logger(), "fps=%d invalid, fallback to 30", fps_);
      fps_ = 30;
    }
    if (reconnect_interval_ms_ < 50) {
      reconnect_interval_ms_ = 50;
    }
    if (max_fail_count_before_reconnect_ < 1) {
      max_fail_count_before_reconnect_ = 1;
    }

    // Use SensorDataQoS for high-frequency image publishing
    rclcpp::QoS qos = rclcpp::SensorDataQoS();
    if (reliability_ == "reliable") {
      qos.reliable();
    } else {
      qos.best_effort();
    }
    image_pub_ = create_publisher<sensor_msgs::msg::Image>(output_topic_, qos);

    if (bus_type_ == "mipi") {
      mipi_backend_ = std::make_unique<MipiVflowBackend>();
      if (!mipi_backend_->open(mipi_host_, mipi_channel_, mipi_sensor_id_)) {
        throw std::runtime_error("failed to open mipi vflow pipeline");
      }
    } else {
      if (!open_camera()) {
        throw std::runtime_error("failed to open camera");
      }
    }

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

  ~CameraNode() override {
    running_ = false;
    if (worker_.joinable()) {
      worker_.join();
    }
    if (cap_.isOpened()) {
      cap_.release();
    }
    if (mipi_backend_) {
      mipi_backend_->close();
    }
  }

private:
  bool open_mipi() {
    if (!mipi_backend_) {
      mipi_backend_ = std::make_unique<MipiVflowBackend>();
    }
    return mipi_backend_->open(mipi_host_, mipi_channel_, mipi_sensor_id_);
  }

  bool reopen_mipi() {
    if (mipi_backend_) {
      mipi_backend_->close();
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
    return open_mipi();
  }

  bool open_camera() {
    const auto devices = list_camera_devices();
    RCLCPP_INFO(get_logger(), "Detected %zu video devices", devices.size());
    for (const auto & d : devices) {
      std::string ids;
      for (size_t i = 0; i < d.stable_ids.size(); ++i) {
        ids += d.stable_ids[i];
        if (i + 1 < d.stable_ids.size()) {
          ids += ",";
        }
      }
      RCLCPP_INFO(get_logger(), "  %s name='%s' bus=%s stable=[%s]",
                  d.video_dev.c_str(), d.card_name.c_str(), d.bus_type.c_str(), ids.c_str());
    }

    const auto selected = select_camera(devices, bus_type_, camera_name_, stable_id_, device_path_);
    if (!selected.has_value()) {
      RCLCPP_ERROR(get_logger(), "No camera matched selection conditions");
      return false;
    }
    selected_device_ = *selected;

    const auto sizes = enum_mjpeg_framesizes(selected_device_.video_dev);
    const FrameSize chosen = choose_best_size(sizes, width_, height_hint_);

    RCLCPP_INFO(get_logger(), "Selected camera: %s name='%s' bus=%s",
                selected_device_.video_dev.c_str(), selected_device_.card_name.c_str(), selected_device_.bus_type.c_str());
    RCLCPP_INFO(get_logger(), "Requested width=%d height_hint=%d, chosen=%dx%d",
                width_, height_hint_, chosen.width, chosen.height);

    cap_.open(selected_device_.video_dev, cv::CAP_V4L2);
    if (!cap_.isOpened()) {
      RCLCPP_ERROR(get_logger(), "Failed to open %s", selected_device_.video_dev.c_str());
      return false;
    }


    if (pixel_format_ == "MJPG" || pixel_format_ == "MJPEG") {
      cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    }

    cap_.set(cv::CAP_PROP_FRAME_WIDTH, chosen.width);
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, chosen.height);
    // Do not force FPS via OpenCV on embedded platforms (can cause instability)
    // cap_.set(cv::CAP_PROP_FPS, fps_);

    const int actual_w = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
    const int actual_h = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
    const int actual_fps = static_cast<int>(cap_.get(cv::CAP_PROP_FPS));
    RCLCPP_INFO(get_logger(), "Camera opened, actual=%dx%d@%d, topic=%s, reliability=%s",
                actual_w, actual_h, actual_fps, output_topic_.c_str(), reliability_.c_str());

    return true;
  }

  bool reopen_camera() {
    if (cap_.isOpened()) {
      cap_.release();
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
    return open_camera();
  }

  void capture_loop() {
    if (bus_type_ == "mipi") {
      capture_loop_mipi();
      return;
    }

    cv::Mat frame;
    int fail_count = 0;

    while (rclcpp::ok() && running_) {
      if (!cap_.isOpened()) {
        if (reconnect_on_failure_) {
          RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "camera closed, trying reopen");
          if (!reopen_camera()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
            continue;
          }
          fail_count = 0;
        } else {
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
          continue;
        }
      }

      if (!cap_.read(frame) || frame.empty()) {
        fail_count++;
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
                             "capture failed, fail_count=%d", fail_count);

        if (reconnect_on_failure_ && fail_count >= max_fail_count_before_reconnect_) {
          RCLCPP_WARN(get_logger(), "capture continuous failures, reopen camera");
          reopen_camera();
          fail_count = 0;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        continue;
      }

      fail_count = 0;

      if (!frame.isContinuous()) {
        frame = frame.clone();
      }

      // Simple frame rate limiter
      static auto last_pub_time = std::chrono::steady_clock::now();
      auto now_tp = std::chrono::steady_clock::now();
      auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now_tp - last_pub_time).count();
      int target_interval_ms = 1000 / fps_;
      if (elapsed < target_interval_ms) {
        std::this_thread::sleep_for(std::chrono::milliseconds(target_interval_ms - elapsed));
      }
      last_pub_time = std::chrono::steady_clock::now();

      sensor_msgs::msg::Image msg;
      msg.header.stamp = now();
      msg.header.frame_id = frame_id_;
      msg.height = static_cast<uint32_t>(frame.rows);
      msg.width = static_cast<uint32_t>(frame.cols);
      msg.encoding = "bgr8";
      msg.is_bigendian = 0;
      msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.cols * frame.elemSize());

      const size_t bytes = frame.total() * frame.elemSize();
      msg.data.assign(frame.data, frame.data + bytes);

      image_pub_->publish(std::move(msg));
    }
  }

  void capture_loop_mipi() {
    int fail_count = 0;
    auto last_pub_time = std::chrono::steady_clock::now();

    while (rclcpp::ok() && running_) {
      if (!mipi_backend_ || !mipi_backend_->is_open()) {
        if (reconnect_on_failure_) {
          RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "mipi backend closed, trying reopen");
          if (!reopen_mipi()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
            continue;
          }
          fail_count = 0;
        } else {
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
          continue;
        }
      }

      Nv12Frame nv12;
      if (!mipi_backend_->get_frame(nv12, mipi_timeout_ms_) || nv12.data.empty()) {
        fail_count++;
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "mipi capture failed, fail_count=%d", fail_count);

        if (reconnect_on_failure_ && fail_count >= max_fail_count_before_reconnect_) {
          RCLCPP_WARN(get_logger(), "mipi capture continuous failures, reopen pipeline");
          reopen_mipi();
          fail_count = 0;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        continue;
      }

      fail_count = 0;

      // Simple frame rate limiter (works as a throttle on top of timeout-based capture).
      auto now_tp = std::chrono::steady_clock::now();
      auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now_tp - last_pub_time).count();
      int target_interval_ms = 1000 / fps_;
      if (elapsed < target_interval_ms) {
        std::this_thread::sleep_for(std::chrono::milliseconds(target_interval_ms - elapsed));
      }
      last_pub_time = std::chrono::steady_clock::now();

      if (nv12.width == 0 || nv12.height == 0) {
        continue;
      }

      // NV12 -> BGR (for compatibility with your existing downstream).
      cv::Mat yuv_nv12(static_cast<int>(nv12.height + nv12.height / 2),
                       static_cast<int>(nv12.width),
                       CV_8UC1,
                       nv12.data.data());
      cv::Mat bgr;
      cv::cvtColor(yuv_nv12, bgr, cv::COLOR_YUV2BGR_NV12);
      if (!bgr.isContinuous()) {
        bgr = bgr.clone();
      }

      sensor_msgs::msg::Image msg;
      msg.header.stamp = now();
      msg.header.frame_id = frame_id_;
      msg.height = static_cast<uint32_t>(bgr.rows);
      msg.width = static_cast<uint32_t>(bgr.cols);
      msg.encoding = "bgr8";
      msg.is_bigendian = 0;
      msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(bgr.cols * bgr.elemSize());

      const size_t bytes = bgr.total() * bgr.elemSize();
      msg.data.assign(bgr.data, bgr.data + bytes);

      image_pub_->publish(std::move(msg));
    }
  }

private:
  std::string frame_id_;
  std::string bus_type_;
  std::string camera_name_;
  std::string stable_id_;
  std::string device_path_;
  int width_{640};
  int height_hint_{0};
  int fps_{30};
  std::string pixel_format_;
  std::string output_topic_;
  std::string topic_name_;
  std::string reliability_;
  bool reconnect_on_failure_{true};
  int reconnect_interval_ms_{500};
  int max_fail_count_before_reconnect_{10};

  // MIPI backend params/state.
  int mipi_host_{0};
  int mipi_channel_{0};
  int mipi_sensor_id_{0};
  int mipi_timeout_ms_{2000};
  std::unique_ptr<MipiVflowBackend> mipi_backend_;

  CameraDeviceInfo selected_device_;

  cv::VideoCapture cap_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;

  bool running_{false};
  std::thread worker_;
};

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

