#include <opencv2/opencv.hpp>  // 包含所有 OpenCV 模块

// 创建一个 640x480 的空白图像（黑色）
cv::Mat img = cv::Mat::zeros(480, 640, CV_8UC3);

// 访问某个像素（第 row 行，第 col 列）
cv::Vec3b pixel = img.at<cv::Vec3b>(row, col);
// pixel[0] = B（蓝色通道）
// pixel[1] = G（绿色通道）
// pixel[2] = R（红色通道）

// 修改像素
img.at<cv::Vec3b>(100, 200) = cv::Vec3b(255, 0, 0);  // 设为纯蓝色

// 读取图像（默认 BGR 格式）
cv::Mat img = cv::imread("test.jpg");

// 检查是否读取成功
if (img.empty()) {
    std::cerr << "无法读取图像！" << std::endl;
    return -1;
}

// 显示图像
cv::imshow("窗口名", img);
cv::waitKey(0);  // 等待按键，0 = 无限等待

// 保存图像
cv::imwrite("output.jpg", img);

cv::Mat gray_img;
cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY);

// 彩色图转灰度图
cv::Mat gray_img;
cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY);

cv::Mat binary_img;
cv::threshold(gray_img, binary_img, 80, 255, cv::THRESH_BINARY);

// 进行二值化
cv::Mat binary_img;
cv::threshold(gray_img, binary_img, threshold_, 255, cv::THRESH_BINARY);

threshold_ = yaml["threshold"].as<double>();

// OTSU 自动确定最佳阈值（后面加 | THRESH_OTSU）
double otsu_thresh = cv::threshold(gray_img, binary_img, 0, 255,
                                   cv::THRESH_BINARY | cv::THRESH_OTSU);
// otsu_thresh 就是自动算出的最优阈值
std::cout << "OTSU 自动阈值: " << otsu_thresh << std::endl;

std::vector<std::vector<cv::Point>> contours;
cv::findContours(binary_img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

// 获取轮廓点
std::vector<std::vector<cv::Point>> contours;
cv::findContours(binary_img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

cv::RotatedRect rotated_rect = cv::minAreaRect(contour);

// 在 for 循环中遍历每个轮廓
for (const auto & contour : contours) {
    auto rotated_rect = cv::minAreaRect(contour);  // ← 获取旋转矩形
    auto lightbar = Lightbar(rotated_rect, lightbar_id);
    // ...
}

Lightbar::Lightbar(const cv::RotatedRect & rotated_rect, std::size_t id)
: id(id), rotated_rect(rotated_rect)
{
  // 获取 4 个角点
  std::vector<cv::Point2f> corners(4);
  rotated_rect.points(&corners[0]);

  // 按 Y 坐标排序：Y 小的在上面
  std::sort(corners.begin(), corners.end(), [](const cv::Point2f & a, const cv::Point2f & b) {
    return a.y < b.y;
  });

  // 计算关键几何量
  center = rotated_rect.center;
  top = (corners[0] + corners[1]) / 2;      // 上边中点
  bottom = (corners[2] + corners[3]) / 2;   // 下边中点
  top2bottom = bottom - top;                 // 从上到下的方向向量

  width = cv::norm(corners[0] - corners[1]); // 短边长度
  angle = std::atan2(top2bottom.y, top2bottom.x);  // 自己算角度！
  angle_error = std::abs(angle - CV_PI / 2);       // 与竖直方向的偏差
  length = cv::norm(top2bottom);                    // 长边长度（灯条长度）
  ratio = length / width;                           // 宽高比
}

float ratio = rect.size.width / rect.size.height;

// 灯条的长边（top 到 bottom）和短边（corners 之间）
length = cv::norm(top2bottom);   // 长边
width = cv::norm(corners[0] - corners[1]);  // 短边
ratio = length / width;          // 宽高比 = 长/宽

// top2bottom 是从灯条顶端到底端的方向向量
angle = std::atan2(top2bottom.y, top2bottom.x);

// 与竖直方向（90度 = PI/2 弧度）的偏差
angle_error = std::abs(angle - CV_PI / 2);

bool Detector::check_geometry(const Lightbar & lightbar) const
{
  auto angle_ok = lightbar.angle_error < max_angle_error_;
  auto ratio_ok = lightbar.ratio > min_lightbar_ratio_
               && lightbar.ratio < max_lightbar_ratio_;
  auto length_ok = lightbar.length > min_lightbar_length_;
  return angle_ok && ratio_ok && length_ok;
}

max_angle_error_ = yaml["max_angle_error"].as<double>() / 57.3;  // degree to rad
min_lightbar_ratio_ = yaml["min_lightbar_ratio"].as<double>();
max_lightbar_ratio_ = yaml["max_lightbar_ratio"].as<double>();
min_lightbar_length_ = yaml["min_lightbar_length"].as<double>();

bool Detector::check_geometry(const Armor & armor) const
{
  auto ratio_ok = armor.ratio > min_armor_ratio_
               && armor.ratio < max_armor_ratio_;
  auto side_ratio_ok = armor.side_ratio < max_side_ratio_;
  auto rectangular_error_ok = armor.rectangular_error < max_rectangular_error_;
  return ratio_ok && side_ratio_ok && rectangular_error_ok;
}

auto left2right = right.center - left.center;   // 左灯条到右灯条的向量
auto width = cv::norm(left2right);               // 两灯条中心距离
auto max_lightbar_length = std::max(left.length, right.length);

ratio = width / max_lightbar_length;             // 装甲板宽高比
side_ratio = max_lightbar_length / min_lightbar_length;  // 两侧灯条长度比

// 矩形度误差：灯条应该垂直于连线方向
auto roll = std::atan2(left2right.y, left2right.x);
auto left_rectangular_error = std::abs(left.angle - roll - CV_PI / 2);
auto right_rectangular_error = std::abs(right.angle - roll - CV_PI / 2);
rectangular_error = std::max(left_rectangular_error, right_rectangular_error);

// src: 原图中的 4 个点（倾斜的四边形）
// dst: 目标图中的 4 个点（正视的矩形）
cv::Mat M = cv::getPerspectiveTransform(src, dst);

cv::Mat dst_img;
cv::warpPerspective(src_img, dst_img, M, cv::Size(width, height));

cv::Mat Detector::get_pattern(const cv::Mat & bgr_img, const Armor & armor) const
{
  // 延长灯条获得装甲板角点
  // 1.125 ≈ armor_half_width / lightbar_length = 67.5mm / 55mm ≈ 1.227 (近似值)
  auto tl = armor.left.center - armor.left.top2bottom * 1.125;
  auto bl = armor.left.center + armor.left.top2bottom * 1.125;
  auto tr = armor.right.center - armor.right.top2bottom * 1.125;
  auto br = armor.right.center + armor.right.top2bottom * 1.125;

  // 计算包围盒 ROI
  auto roi_left = std::max<int>(std::min(tl.x, bl.x), 0);
  auto roi_top = std::max<int>(std::min(tl.y, tr.y), 0);
  auto roi_right = std::min<int>(std::max(tr.x, br.x), bgr_img.cols);
  auto roi_bottom = std::min<int>(std::max(bl.y, br.y), bgr_img.rows);
  auto roi = cv::Rect(cv::Point(roi_left, roi_top), cv::Point(roi_right, roi_bottom));

  return bgr_img(roi);  // 用 Rect 裁剪图像
}

cv::Mat gray;
cv::cvtColor(armor.pattern, gray, cv::COLOR_BGR2GRAY);

auto input = cv::Mat(32, 32, CV_8UC1, cv::Scalar(0));
auto x_scale = static_cast<double>(32) / gray.cols;
auto y_scale = static_cast<double>(32) / gray.rows;
auto scale = std::min(x_scale, y_scale);
auto h = static_cast<int>(gray.rows * scale);
auto w = static_cast<int>(gray.cols * scale);

auto roi = cv::Rect(0, 0, w, h);
cv::resize(gray, input(roi), {w, h});

// 假设装甲板四个角点（从图像中检测到）
std::vector<cv::Point2f> src_pts = {
    cv::Point2f(100, 50),   // 左上
    cv::Point2f(200, 40),   // 右上
    cv::Point2f(210, 150),  // 右下
    cv::Point2f(90, 160)    // 左下
};

// 目标矩形（正视图）
std::vector<cv::Point2f> dst_pts = {
    cv::Point2f(0, 0),
    cv::Point2f(100, 0),
    cv::Point2f(100, 100),
    cv::Point2f(0, 100)
};

// 计算变换矩阵
cv::Mat M = cv::getPerspectiveTransform(src_pts, dst_pts);

// 应用变换
cv::Mat warped;
cv::warpPerspective(bgr_img, warped, M, cv::Size(100, 100));
// warped 现在是正视的 100x100 灰度图

cv::Mat eroded;
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::erode(binary_img, eroded, kernel);

cv::Mat dilated;
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::dilate(binary_img, dilated, kernel);

cv::Mat opened;
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::morphologyEx(binary_img, opened, cv::MORPH_OPEN, kernel);

cv::Mat closed;
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::morphologyEx(binary_img, closed, cv::MORPH_CLOSE, kernel);

cv::Mat kernel = cv::getStructuringElement(shape, ksize);

cv::rectangle(img, pt1, pt2, color, thickness);

// 在图像上画一个绿色矩形
cv::rectangle(img, cv::Point(100, 50), cv::Point(200, 150),
              cv::Scalar(0, 255, 0), 2);  // 绿色，线宽 2

cv::circle(img, center, radius, color, thickness);

// 在中心画一个红色实心圆
cv::circle(img, cv::Point(320, 240), 5, cv::Scalar(0, 0, 255), -1);
// -1 表示填充（实心）

cv::putText(img, text, org, fontFace, fontScale, color, thickness);

// 在图像左上角写帧号
cv::putText(img, "Frame: 42", cv::Point(10, 30),
            cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2);

cv::line(img, pt1, pt2, color, thickness);

// 画一条黄色的对角线
cv::line(img, cv::Point(0, 0), cv::Point(640, 480),
         cv::Scalar(0, 255, 255), 1);

cv::drawContours(img, contours, contourIdx, color, thickness);

void Detector::show_result(
  const cv::Mat & binary_img, const cv::Mat & bgr_img,
  const std::list<Lightbar> & lightbars,
  const std::list<Armor> & armors, int frame_count) const
{
  auto detection = bgr_img.clone();  // 克隆原图，避免修改原图

  // 画帧号
  tools::draw_text(detection, fmt::format("[{}]", frame_count),
                   {10, 30}, {255, 255, 255});

  // 画每个灯条
  for (const auto & lightbar : lightbars) {
    auto info = fmt::format("{:.1f} {:.1f} {:.1f} {}",
        lightbar.angle_error * 57.3,  // 角度误差（度）
        lightbar.ratio,               // 宽高比
        lightbar.length,              // 长度
        COLORS[lightbar.color]);      // 颜色名称
    tools::draw_text(detection, info, lightbar.top, {0, 255, 255});    // 黄色文字
    tools::draw_points(detection, lightbar.points, {0, 255, 255}, 3);  // 黄色轮廓
  }

  // 画每个装甲板
  for (const auto & armor : armors) {
    auto info = fmt::format("{:.2f} {:.2f} {:.1f} {:.2f} {} {}",
        armor.ratio, armor.side_ratio,
        armor.rectangular_error * 57.3, armor.confidence,
        ARMOR_NAMES[armor.name], ARMOR_TYPES[armor.type]);
    tools::draw_points(detection, armor.points, {0, 255, 0});       // 绿色轮廓
    tools::draw_text(detection, info, armor.left.bottom, {0, 255, 0}); // 绿色文字
  }

  // 缩小显示（节省屏幕空间）
  cv::Mat binary_img2;
  cv::resize(binary_img, binary_img2, {}, 0.5, 0.5);
  cv::resize(detection, detection, {}, 0.5, 0.5);

  cv::imshow("detection", detection);
}

void draw_point(cv::Mat & img, const cv::Point & point,
                const cv::Scalar & color, int radius)
{
  cv::circle(img, point, radius, color, -1);  // 实心圆
}

void draw_points(cv::Mat & img, const std::vector<cv::Point2f> & points,
                 const cv::Scalar & color, int thickness)
{
  std::vector<cv::Point> int_points(points.begin(), points.end());
  std::vector<std::vector<cv::Point>> contours = {int_points};
  cv::drawContours(img, contours, -1, color, thickness);  // 画轮廓
}

void draw_text(cv::Mat & img, const std::string & text,
               const cv::Point & point, const cv::Scalar & color,
               double font_scale, int thickness)
{
  cv::putText(img, text, point, cv::FONT_HERSHEY_SIMPLEX,
              font_scale, color, thickness);
}

std::vector<cv::Mat> channels;
cv::split(bgr_img, channels);
// channels[0] = B 通道（蓝色）
// channels[1] = G 通道（绿色通道）
// channels[2] = R 通道（红色通道）

cv::Mat merged;
cv::merge(channels, merged);

cv::bitwise_and(img1, img2, dst);   // 位与
cv::bitwise_or(img1, img2, dst);    // 位或
cv::bitwise_xor(img1, img2, dst);   // 位异或
cv::bitwise_not(img, dst);          // 位取反

// 创建一个圆形掩码
cv::Mat mask = cv::Mat::zeros(img.size(), CV_8UC1);
cv::circle(mask, cv::Point(320, 240), 100, cv::Scalar(255), -1);

// 用掩码提取圆形区域
cv::Mat result;
cv::bitwise_and(img, img, result, mask);
// result 中只有圆形区域有内容，其余为黑色

Color Detector::get_color(const cv::Mat & bgr_img,
                          const std::vector<cv::Point> & contour) const
{
  int red_sum = 0, blue_sum = 0;

  for (const auto & point : contour) {
    red_sum += bgr_img.at<cv::Vec3b>(point)[2];   // [2] = R 通道
    blue_sum += bgr_img.at<cv::Vec3b>(point)[0];  // [0] = B 通道
  }

  return blue_sum > red_sum ? Color::blue : Color::red;
}

std::vector<cv::Mat> channels;
cv::split(bgr_img, channels);

// 对灯条区域内的 R 通道和 B 通道分别求均值
cv::Mat lightbar_mask = ...;  // 灯条区域掩码
double red_mean = cv::mean(channels[2], lightbar_mask)[0];
double blue_mean = cv::mean(channels[0], lightbar_mask)[0];

if (blue_mean > red_mean) {
    // 蓝色灯条
}

cv::Mat result;
cv::add(img1, img2, result);        // 像素相加（饱和运算，超过255截断为255）
cv::subtract(img1, img2, result);   // 像素相减
cv::absdiff(img1, img2, result);    // 绝对差值

// 两帧之间的差异
cv::Mat diff;
cv::absdiff(frame1, frame2, diff);

// 转灰度、二值化，找出运动区域
cv::Mat gray_diff;
cv::cvtColor(diff, gray_diff, cv::COLOR_BGR2GRAY);
cv::threshold(gray_diff, motion_mask, 30, 255, cv::THRESH_BINARY);

std::list<Armor> Detector::detect(const cv::Mat & bgr_img, int frame_count)
{
  // ====== 第一步：预处理 ======
  cv::Mat gray_img;
  cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY);    // 灰度转换

  cv::Mat binary_img;
  cv::threshold(gray_img, binary_img, threshold_, 255,
                cv::THRESH_BINARY);                        // 二值化

  // ====== 第二步：轮廓检测 ======
  std::vector<std::vector<cv::Point>> contours;
  cv::findContours(binary_img, contours, cv::RETR_EXTERNAL,
                   cv::CHAIN_APPROX_NONE);                 // 提取轮廓

  // ====== 第三步：灯条检测 ======
  std::list<Lightbar> lightbars;
  for (const auto & contour : contours) {
    auto rotated_rect = cv::minAreaRect(contour);          // 最小外接矩形
    auto lightbar = Lightbar(rotated_rect, lightbar_id);   // 创建灯条对象

    if (!check_geometry(lightbar)) continue;               // 几何筛选

    lightbar.color = get_color(bgr_img, contour);          // 颜色判断
    lightbars.emplace_back(lightbar);
  }

  // ====== 第四步：灯条配对 → 装甲板 ======
  lightbars.sort([](auto & a, auto & b) { return a.center.x < b.center.x; });

  std::list<Armor> armors;
  for (auto left = lightbars.begin(); left != lightbars.end(); left++) {
    for (auto right = std::next(left); right != lightbars.end(); right++) {
      if (left->color != right->color) continue;          // 颜色必须相同

      auto armor = Armor(*left, *right);                   // 创建装甲板
      if (!check_geometry(armor)) continue;                // 装甲板几何筛选

      armor.pattern = get_pattern(bgr_img, armor);         // 裁剪数字图案
      classifier_.classify(armor);                         // 神经网络分类
      if (!check_name(armor)) continue;                    // 置信度筛选

      armor.type = get_type(armor);                        // 判断大小装甲
      armors.emplace_back(armor);
    }
  }

  // ====== 第五步：去重 + 可视化 ======
  // ... 去除共用灯条的重叠装甲板 ...

  if (debug_) show_result(binary_img, bgr_img, lightbars, armors, frame_count);
  return armors;
}