#pragma once

#include "fusion_types.h"
#include <vector>

// 避障速度指令 (ENU 坐标系)
struct AvoidanceCmd {
    double vx = 0, vy = 0, vz = 0;   // m/s
    double timestamp = 0;
};

class AvoidanceController {
public:
    explicit AvoidanceController(const FusionConfig& cfg);

    // 计算避障速度, 无需避障时返回 false
    bool compute(const std::vector<FusedThreat>& threats, double now, AvoidanceCmd& cmd);

    bool active() const { return active_; }
    void reset();

private:
    // 计算单个威胁的斥力向量
    void repulsion(const FusedThreat& threat, double speed, double decel_dist,
                   double& rx, double& ry, double& rz) const;

    // 合成多个斥力向量
    void combine(const std::vector<std::pair<AvoidanceCmd, const FusedThreat*>>& vectors,
                 double speed, double& cx, double& cy, double& cz) const;

    FusionConfig cfg_;
    double smooth_vx_ = 0, smooth_vy_ = 0, smooth_vz_ = 0;
    double last_avoidance_time_ = 0;
    bool   active_ = false;
};
