#ifndef CAMERA_CALIB_HPP
#define CAMERA_CALIB_HPP

/**
 * @file  camera_calib.hpp
 * @brief 相机标定数据（内参/畸变）的加载与使用 —— 27RM
 *
 * 配置来源：configs/camera.yaml（由 ./calibrate 生成）
 *
 * 典型用法：
 *   CameraCalib calib;
 *   if (!calib.load("configs/camera.yaml")) return -1;
 *
 *   // 1) 去畸变（每帧）
 *   cv::Mat undist;
 *   calib.undistort(frame, undist);
 *
 *   // 2) 装甲板四角点 → 位姿（角点顺序：左下、左上、右上、右下）
 *   std::array<cv::Point2f, 4> corners{lb, lt, rt, rb};
 *   if (auto pose = calib.solvePose(corners, ArmorSize::small())) {
 *       double yaw, pitch;
 *       CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch);   // 云台需要的角度
 *       printf("距离 %.2f m, yaw %.1f°, pitch %.1f°, 重投影 %.2f px\n",
 *              pose->distance_mm / 1000.0, yaw * 180 / CV_PI,
 *              pitch * 180 / CV_PI, pose->reproj_error_px);
 *   }
 */

#include <opencv2/opencv.hpp>

#include <array>
#include <optional>
#include <string>

/** 装甲板物理尺寸（毫米）：width = 两灯条中心间距，height = 灯条长度 */
struct ArmorSize {
    double width_mm;
    double height_mm;

    /** 大装甲板：英雄（一号）230mm 间距 */
    static ArmorSize big() { return ArmorSize{230.0, 56.0}; }
    /** 小装甲板：2/3/4/5 号、哨兵 135mm 间距 */
    static ArmorSize small() { return ArmorSize{135.0, 56.0}; }
};

/** 位姿解算结果（相机坐标系，单位 mm） */
struct ArmorPose {
    cv::Vec3d rvec{0, 0, 0};        // 旋转向量
    cv::Vec3d tvec{0, 0, 0};        // 装甲板中心位置
    double distance_mm = 0.0;       // 到装甲板中心的距离
    double reproj_error_px = 0.0;   // 重投影误差（质量指标）
};

class CameraCalib {
public:
    CameraCalib() = default;
    explicit CameraCalib(const std::string &yaml_path) { load(yaml_path); }

    /** 加载标定文件；成功返回 true（失败会打印原因） */
    bool load(const std::string &yaml_path);

    bool valid() const { return valid_; }
    const cv::Mat &K() const { return camera_matrix_; }
    const cv::Mat &D() const { return dist_coeffs_; }
    cv::Size imageSize() const { return image_size_; }

    /** 每帧去畸变（内部缓存映射表，尺寸不变时不用重建） */
    void undistort(const cv::Mat &src, cv::Mat &dst);

    /** 重投影误差门限（像素）：超过则视为无效解，默认 8.0 */
    void setMaxReprojError(double px) { max_reproj_err_px_ = px; }

    /**
     * 四个角点 → 装甲板位姿
     * 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 →
     * 依次筛掉「z<=0（相机后方）」「重投影超差」→ 取重投影最小者。
     * 注：角点在解算前先做去畸变（IPPE+畸变+板内旋转组合有数值坑）。
     * @param corners 图像坐标（原图，含畸变），顺序：左下、左上、右上、右下
     * @param size    装甲板物理尺寸
     * @return 成功返回位姿；全部候选被筛掉时返回 std::nullopt
     */
    std::optional<ArmorPose> solvePose(const std::array<cv::Point2f, 4> &corners,
                                       const ArmorSize &size) const;

    /** 位置 → 云台角度（弧度）：yaw 向右为正、pitch 向上为正 */
    static void poseToYawPitch(const cv::Vec3d &tvec, double &yaw, double &pitch) {
        yaw = std::atan2(tvec[0], tvec[2]);
        pitch = std::atan2(-tvec[1], std::sqrt(tvec[0] * tvec[0] +
                                               tvec[2] * tvec[2]));
    }

private:
    cv::Mat camera_matrix_;
    cv::Mat dist_coeffs_;
    cv::Size image_size_{0, 0};
    bool valid_ = false;
    double max_reproj_err_px_ = 8.0;

    cv::Mat map1_, map2_;   // 去畸变映射表缓存
};

#endif // CAMERA_CALIB_HPP