/*
   ***  此文件用于hero弹道着落点位，通过3D建模以及理论发弹速度以及后期轨道修正手段 ***
   ***  实现对敌方装甲板的精准打击 ***
*/

#ifndef BALLISTIC_PREDICITION_H
#define BALLISTIC_PREDICITION_H

#include <Eigen/Dense>
#include <opencv2/core.hpp>

/**
 * 弹着点计算结果
 */
struct ImpactPoint {
    Eigen::Vector3d xyz_in_camera;  // 弹着点在相机坐标系中的位置 (m)
    double flight_time;             // 弹丸飞行时间 (s)
};

/**
 * @brief 弹道计算类：根据 IMU 当前 pitch 和追踪装甲板距离，
 *        推算弹着点在相机坐标系中的三维坐标。
 *
 * 坐标系约定（与 solver.cpp 一致）：
 *   世界系: x=前, y=左, z=上
 *   相机系: z=前, x=右, y=下（OpenCV 标准）
 */
class BallisticSolver {
public:
    /**
     * @brief 根据水平距离和垂直高度差，迭代求解补偿 pitch 角。
     * @param d  水平距离 (m)
     * @param y  垂直高度差 (m，目标高于枪口为正)
     * @param v0 枪口初速度 (m/s)
     * @param k  空气阻力系数
     * @return 补偿后的 pitch 角 (度)
     */
    static double get_pitch(double d, double y, double v0, double k = 0.1);

    /**
     * @brief 计算弹丸飞行时间。
     * @param d     目标水平距离 (m，由追踪装甲板获得)
     * @param pitch 当前 IMU pitch 角 (度，仰为正)
     * @param v0    枪口初速度 (m/s)
     * @param k     空气阻力系数
     * @return 弹丸飞行时间 (s)
     */
    static double get_flight_time(double d, double pitch, double v0, double k = 0.1);

    /**
     * @brief 根据当前 IMU pitch 角推算弹丸在水平距离 d 处的垂直高度。
     * @param d     目标水平距离 (m)
     * @param pitch 当前 IMU pitch 角 (度，仰为正)
     * @param v0    枪口初速度 (m/s)
     * @param k     空气阻力系数
     * @return 弹着点相对枪口的垂直高度 (m，高于枪口为正)
     */
    static double get_impact_y(double d, double pitch, double v0, double k = 0.1);

    /**
     * [TEST] 用于验证弹道算法正确性：将弹着点反解算到相机坐标系，
     * 再通过 project_to_pixel 投影到像素并用红点显示。
     * 如果实验结果正确，可直接注释调用处，不影响其他结构。
     *
     * 坐标变换链：
     *   xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
     *
     * @param xyz_in_world      目标在世界坐标系中的位置 (m，来自 PnP Solver)
     * @param pitch_imu         当前 IMU pitch 角 (度，仰为正)
     * @param v0                枪口初速度 (m/s)
     * @param R_camera2gimbal   相机到云台的旋转矩阵
     * @param R_gimbal2world    云台到世界的旋转矩阵
     * @param t_camera2gimbal   相机原点在云台系中的平移向量 (m)
     * @param k                 空气阻力系数
     * @return ImpactPoint      { 弹着点相机坐标(m), 飞行时间(s) }
     */
    static ImpactPoint get_impact_in_camera(
        const Eigen::Vector3d& xyz_in_world,
        double pitch_imu,
        double v0,
        const Eigen::Matrix3d& R_camera2gimbal,
        const Eigen::Matrix3d& R_gimbal2world,
        const Eigen::Vector3d& t_camera2gimbal,
        double k = 0.1);

    /**
     * [TEST] 将相机坐标系3D点简化投影到像素坐标（针孔模型，无畲变）。
     * @param xyz_in_camera  弹着点在相机坐标系中的位置 (m)
     * @param camera_matrix  OpenCV 相机内参矩阵 (3x3 CV_64F)
     * @return 像素坐标
     */
    static cv::Point2f project_to_pixel(
        const Eigen::Vector3d& xyz_in_camera,
        const cv::Mat& camera_matrix);
};

#endif // BALLISTIC_PREDICITION_H
