#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.history.size() < 3) return false; // 轨迹点不足，不预测

        // 1. 获取目标当前的 3D 状态 (相对于相机)
        const auto &latest = armor_finder.history.back();
        double dist = latest.pos.z / 100.0; // cm -> m
        
        // 2. 估计目标速度 (简单线性回归或两点差分)
        const auto &first = armor_finder.history.front();
        double dt = latest.time - first.time;
        if (dt <= 0) return false;
        
        cv::Point3f velocity = (latest.pos - first.pos) * (1.0 / dt); // cm/s
        
        // 3. 计算飞行时间 (单位: s)
        double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K);
        double t_total = t_flight + SYSTEM_DELAY / 1000.0; 
        
        // 4. 预测目标未来的位置 (cm)
        cv::Point3f pos_pred = latest.pos + velocity * t_total;
        
        // 5. 将预测位置映射回像素坐标 (用于判断重合)
        double x_pred_pixel = pos_pred.x * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_X;
        double y_pred_pixel = pos_pred.y * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_Y;
        
        // 6. 推断弹着点偏移 (像素级)
        // 理想打击点应该就在相机中心 (自瞄已经对准补差后的位置)
        // 但是我们需要判断当前云台的 "指向误差" 是否足够小
        double dist_to_center = sqrt(pow(x_pred_pixel - IMAGE_CENTER_X, 2) + pow(y_pred_pixel - IMAGE_CENTER_Y, 2));
        
        // 7. 动态阈值判定 (依据目标距离和装甲板大小)
        // 距离越远，允许的像素误差越小
        double threshold = (threshold_deg > 0) ? (threshold_deg * FOCUS_PIXAL * PI / 180.0 / 100.0) : (200.0 / (dist + 0.001)); 
        
        return dist_to_center < threshold;
    }
};

#endif // AUTO_TRIGGER_H