#include "avoidance_controller.h"
#include <cmath>
#include <algorithm>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

AvoidanceController::AvoidanceController(const FusionConfig& cfg)
    : cfg_(cfg) {}

bool AvoidanceController::compute(
    const std::vector<FusedThreat>& threats, double now, AvoidanceCmd& cmd)
{
    if (!cfg_.avoidance_enabled) return false;

    // 过滤危险威胁
    std::vector<const FusedThreat*> dangerous;
    for (auto& t : threats) {
        if (FusionThreatLevelInfo::is_dangerous(t.level))
            dangerous.push_back(&t);
    }

    if (dangerous.empty()) {
        // 衰减
        if (active_) {
            smooth_vx_ *= 0.8;
            smooth_vy_ *= 0.8;
            smooth_vz_ *= 0.8;
            if (std::fabs(smooth_vx_) < 0.01 && std::fabs(smooth_vy_) < 0.01) {
                active_ = false;
                return false;
            }
            cmd.vx = smooth_vx_;
            cmd.vy = smooth_vy_;
            cmd.vz = smooth_vz_;
            cmd.timestamp = now;
            return true;
        }
        return false;
    }

    // 冷却检查
    if (active_ && (now - last_avoidance_time_) < cfg_.avoidance_cooldown) {
        double elapsed = now - last_avoidance_time_;
        if (elapsed < cfg_.avoidance_hold_time) {
            cmd.vx = smooth_vx_;
            cmd.vy = smooth_vy_;
            cmd.vz = smooth_vz_;
            cmd.timestamp = now;
            return true;
        }
        double decay = std::max(0.0, 1.0 - (elapsed - cfg_.avoidance_hold_time)
                                / (cfg_.avoidance_cooldown - cfg_.avoidance_hold_time));
        cmd.vx = smooth_vx_ * decay;
        cmd.vy = smooth_vy_ * decay;
        cmd.vz = smooth_vz_ * decay;
        cmd.timestamp = now;
        return true;
    }

    // 计算斥力向量
    double speed = cfg_.avoidance_speed;
    double decel = cfg_.avoidance_decel_distance;

    std::vector<std::pair<AvoidanceCmd, const FusedThreat*>> vectors;
    for (auto* t : dangerous) {
        AvoidanceCmd rc;
        repulsion(*t, speed, decel, rc.vx, rc.vy, rc.vz);
        rc.timestamp = now;
        vectors.push_back({rc, t});
    }

    double cx, cy, cz;
    combine(vectors, speed, cx, cy, cz);

    // 平滑
    double alpha = cfg_.avoidance_smoothing;
    smooth_vx_ = alpha * cx + (1 - alpha) * smooth_vx_;
    smooth_vy_ = alpha * cy + (1 - alpha) * smooth_vy_;
    smooth_vz_ = alpha * cz + (1 - alpha) * smooth_vz_;

    active_ = true;
    last_avoidance_time_ = now;

    cmd.vx = smooth_vx_;
    cmd.vy = smooth_vy_;
    cmd.vz = smooth_vz_;
    cmd.timestamp = now;
    return true;
}

void AvoidanceController::repulsion(
    const FusedThreat& threat, double speed, double decel_dist,
    double& rx, double& ry, double& rz) const
{
    // 方位角 → ENU 反方向
    double az_rad = threat.bearing * M_PI / 180.0;
    double avoid_x = -std::sin(az_rad);  // ENU: x=East
    double avoid_y = -std::cos(az_rad);  // ENU: y=North

    // 距离缩放
    double scale;
    if (threat.distance > 0 && threat.distance < decel_dist) {
        scale = std::pow((decel_dist - threat.distance) / decel_dist, 2);
    } else if (threat.distance > 0) {
        scale = 0.0;
    } else {
        scale = 1.0;  // 未知距离, 全力避障
    }

    double mag = std::sqrt(avoid_x * avoid_x + avoid_y * avoid_y);
    if (mag > 0) {
        rx = avoid_x / mag * speed * scale;
        ry = avoid_y / mag * speed * scale;
    } else {
        rx = ry = 0;
    }

    // 垂直避障
    rz = 0;
    if (cfg_.avoidance_vertical_enabled && std::fabs(threat.elevation) > 10) {
        rz = -std::sin(threat.elevation * M_PI / 180.0) * speed * 0.5 * scale;
    }
}

void AvoidanceController::combine(
    const std::vector<std::pair<AvoidanceCmd, const FusedThreat*>>& vectors,
    double speed, double& cx, double& cy, double& cz) const
{
    cx = cy = cz = 0;
    for (auto& [cmd, threat] : vectors) {
        double w = (threat->distance > 0) ? 1.0 / std::max(threat->distance, 1.0) : 2.0;
        w *= threat->confidence;
        cx += cmd.vx * w;
        cy += cmd.vy * w;
        cz += cmd.vz * w;
    }
    double mag = std::sqrt(cx * cx + cy * cy + cz * cz);
    if (mag > speed) {
        double f = speed / mag;
        cx *= f;
        cy *= f;
        cz *= f;
    }
}

void AvoidanceController::reset() {
    smooth_vx_ = 0;
    smooth_vy_ = 0;
    smooth_vz_ = 0;
    active_ = false;
}
