#include <show_images/ballistic_predicition.h>
#include <cmath>

/**
 * @brief 迭代法求解补偿 pitch 角。
 * 给定水平距离 d 和目标高度差 y，反算出应出射的 pitch 角（度）。
 */
double BallisticSolver::get_pitch(double d, double y, double v0, double k) {
    const double g = 9.80665;
    double pitch_new = atan2(y, d); // 初始猜测：几何仰角
    for (int i = 0; i < 20; ++i) {
        double cos_p = cos(pitch_new);
        double val   = 1.0 - (k * d) / (v0 * cos_p);
        if (val <= 0) break;
        double t     = -log(val) / k;
        double y_calc = (1.0/k) * (v0*sin(pitch_new) + g/k) * (1.0 - exp(-k*t)) - (g/k)*t;
        double dy    = y - y_calc;
        pitch_new   += atan2(dy, d);
        if (std::abs(dy) < 0.001) break;
    }
    return pitch_new * 180.0 / M_PI;
}

/**
 * @brief 计算弹丸飞行时间。
 * @param d     目标水平距离 (m)
 * @param pitch 当前 IMU pitch 角 (度，仰为正)
 * @param v0    枪口初速度 (m/s)
 * @param k     空气阻力系数
 * @brief 计算弹丸飞行时间（含空气阻力模型）。
 *
 * 运动方程：水平方向  v_x(t) = v0*cos(pitch)*exp(-k*t)
 *            水平位移  x(t)  = (v0*cos(pitch)/k) * (1 - exp(-k*t))
 * 由 x(t) = d 解得 t。
 */
double BallisticSolver::get_flight_time(double d, double pitch, double v0, double k) {
    double cos_pitch = cos(pitch * M_PI / 180.0);
    double val = 1.0 - (k * d) / (v0 * cos_pitch);
    if (val <= 0) return 0.5;   // 超过最大射程，返回兜底时间
    return -log(val) / k;
}

/**
 * @brief 推断给定出射角下的落点高度（弹着点推断）。
 */
double BallisticSolver::get_impact_y(double d, double pitch, double v0, double k) {
    const double g = 9.80665;
    double pitch_rad = pitch * M_PI / 180.0;
    double t = get_flight_time(d, pitch, v0, k);
    return (1.0 / k) * (v0 * sin(pitch_rad) + g / k) * (1.0 - exp(-k * t)) - (g / k) * t;
}

// ──────────────────── [TEST] 以下两个函数用于可视化验证弹道算法正确性 ────────────────────
// 如果测试通过，直接注释掉 armor_finder.cpp 里的调用，不影响其他逻辑。

/**
 * [TEST] 将弹着点从世界坐标系反解算到相机坐标系。
 * 变换链：xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
 */
ImpactPoint BallisticSolver::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)
{
    // 水平距离：世界系 x=前, y=左
    double d = std::sqrt(xyz_in_world.x() * xyz_in_world.x() +
                         xyz_in_world.y() * xyz_in_world.y());

    double impact_z = get_impact_y(d, pitch_imu, v0, k);
    double fly_time = get_flight_time(d, pitch_imu, v0, k);

    // 弹着点世界坐标：水平 (x,y) 与目标相同，z 换为弹道高度
    Eigen::Vector3d impact_world(xyz_in_world.x(), xyz_in_world.y(), impact_z);

    // 世界 → 云台 → 相机
    Eigen::Vector3d impact_gimbal = R_gimbal2world.transpose() * impact_world;
    Eigen::Vector3d impact_camera = R_camera2gimbal.transpose() * (impact_gimbal - t_camera2gimbal);

    return ImpactPoint{ impact_camera, fly_time };
}

/**
 * [TEST] 针孔投影：相机坐标系 3D 点 → 像素坐标（不含畸变，仅用于可视化）。
 */
cv::Point2f BallisticSolver::project_to_pixel(
    const Eigen::Vector3d& xyz_in_camera,
    const cv::Mat& camera_matrix)
{
    if (xyz_in_camera.z() <= 0) return cv::Point2f(-1, -1); // 在相机背后，无效
    double fx = camera_matrix.at<double>(0, 0);
    double fy = camera_matrix.at<double>(1, 1);
    double cx = camera_matrix.at<double>(0, 2);
    double cy = camera_matrix.at<double>(1, 2);
    float u = static_cast<float>(fx * xyz_in_camera.x() / xyz_in_camera.z() + cx);
    float v = static_cast<float>(fy * xyz_in_camera.y() / xyz_in_camera.z() + cy);
    return cv::Point2f(u, v);
}
// ──────────────────────────────── [TEST END] ─────────────────────────────────────────────
