#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;
}
