#ifndef AUTO_TRIGGER_H
#define AUTO_TRIGGER_H

#include <armor_finder/armor_finder.h>
#include <show_images/ballistic_predicition.h>
#include <config/setconfig.h>
#include <vector>
#include <numeric>

/**
 * @brief 自动扳机逻辑类，负责发射时机决策。
 */
class AutoTrigger {
public:
    /**
     * @brief 自动扳机决策函数
     * @param armor_finder 当前自瞄主对象 (用于获取历史轨迹)
     * @param v0 弹丸初速度
     * @param gimbal_yaw 当前云台实时自瞄角偏移 (Yaw, 度)
     * @param gimbal_pitch 当前云台实时自瞄角偏移 (Pitch, 度)
     * @return true 目标与预测弹着点重合，建议击发
     */
    static bool should_fire(const ArmorFinder &armor_finder, double v0, double gimbal_yaw, double gimbal_pitch, double threshold_deg = -1.0) {
        if (armor_finder.last_box.rect == cv::Rect2d()) return false;
        
        cv::Point2f curr_center = armor_finder.target_box.getCenter();
        cv::Point2f last_center = armor_finder.last_box.getCenter();
        cv::Point2f area_center = armor_finder.target_area_center;

        // 横向速度估计 (采用帧差分，工业相机一般 ~100 fps)
        double vx = curr_center.x - last_center.x; 
        if (std::abs(vx) < 1.0) return false;

        // 计算这块装甲板滑到大框中心所需的预测帧数
        double frames_to_pass = (area_center.x - curr_center.x) / vx;

        // 若不是朝着中心滑动 (已偏离)，不激发
        if (frames_to_pass < 0) return false;

        // 弹道时间预判
        double dist = armor_finder.z_min_filtered / 100.0; // cm -> m
        double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K);
        double t_total = t_flight + SYSTEM_DELAY / 1000.0;

        // 将总延迟时间换算为帧数偏移 (以 100FPS)
        double delay_frames = t_total * 100.0;

        // 如果剩余到达中心时间 = 飞弹+机械延迟时间 左右，开火！
        return std::abs(frames_to_pass - delay_frames) < 3.0; // 容差 +/- 3帧
    }
};

#endif // AUTO_TRIGGER_H