CounterDroneBackend/src/CounterDrone.Core/Algorithms/DefaultDefensePlanner.cs

using System;
using System.Collections.Generic;
using System.Linq;
using CounterDrone.Core.Models;

namespace CounterDrone.Core.Algorithms
{
    /// <summary>默认防御规划器 — 五步流水线，全部使用真实物理计算</summary>
    public class DefaultDefensePlanner : IDefensePlanner
    {
        private readonly List<AmmunitionSpec> _ammoCatalog;

        public DefaultDefensePlanner(List<AmmunitionSpec> ammoCatalog)
        {
            _ammoCatalog = ammoCatalog ?? throw new ArgumentNullException(nameof(ammoCatalog));
            if (_ammoCatalog.Count == 0)
                throw new ArgumentException("弹药规格目录不能为空");
        }

        private static readonly Dictionary<PowerType, AerosolType> MatchTable = new()
        {
            { PowerType.Electric, AerosolType.InertGas },
            { PowerType.Piston,   AerosolType.InertGas },
            { PowerType.Jet,      AerosolType.ActiveMaterial },
        };

        private static readonly Dictionary<TargetType, float> TypeCoefficient = new()
        {
            { TargetType.HighSpeed,  4f },
            { TargetType.FixedWing,  2f },
            { TargetType.Piston,     2f },
            { TargetType.Rotor,      1f },
            { TargetType.Electric,   1f },
        };

        // ═══════════════════════════════════════════════
        // 五步流水线
        // ═══════════════════════════════════════════════

        public PlannerResult Plan(List<FireUnit> fireUnits, List<DroneGroup> threats, CombatScene environment)
        {
            var result = new PlannerResult();

            if (threats.Count == 0)
            {
                result.Best.Summary = "无威胁目标";
                return result;
            }
            if (fireUnits.Count == 0)
            {
                result.Best.Summary = "无可用火力单元";
                result.Best.ThreatsUnengaged = threats.Count;
                return result;
            }

            // Step 1: 威胁排序
            foreach (var t in threats)
            {
                t.ArrivalTime = t.GetArrivalTime();
                t.ThreatIndex = CalcThreatIndex(t.Target);
            }
            var sorted = threats.OrderByDescending(t => t.Priority).ToList();

            // Step 2-4: 贪心分配求解
            // 预先检查：所有需要的弹药类型都在目录中
            var neededTypes = sorted
                .Select(t => MatchAmmo((PowerType)t.Target.PowerType))
                .Distinct()
                .ToList();
            foreach (var t in neededTypes)
            {
                if (!_ammoCatalog.Any(a => a.AerosolType == (int)t))
                    throw new InvalidOperationException($"弹药规格目录中缺少类型: {t}");
            }

            result.Best = Solve(sorted, fireUnits, environment);

            // Step 5: 临界方案
            result.Critical = DeriveCritical(result.Best);

            return result;
        }

        // ═══════════════════════════════════════════════
        // Step 1: 威胁指数
        // ═══════════════════════════════════════════════

        private static float CalcThreatIndex(TargetConfig target)
        {
            float typeCoef = TypeCoefficient.GetValueOrDefault((TargetType)target.TargetType, 1f);
            float speedCoef = (float)target.TypicalSpeed / 60f;
            return typeCoef * speedCoef;
        }

        // ═══════════════════════════════════════════════
        // Step 2: 弹药匹配
        // ═══════════════════════════════════════════════

        private static AerosolType MatchAmmo(PowerType power)
        {
            return MatchTable.GetValueOrDefault(power, AerosolType.InertGas);
        }

        // ═══════════════════════════════════════════════
        // Step 3-4: 候选生成 → 贪心分配
        // ═══════════════════════════════════════════════

        private DefensePlan Solve(List<DroneGroup> sortedThreats,
            List<FireUnit> fireUnits, CombatScene env)
        {
            var plan = new DefensePlan();
            var remainingMunitions = new Dictionary<string, int>();
            foreach (var u in fireUnits)
                remainingMunitions[u.Id] = u.TotalMunitions;

            foreach (var threat in sortedThreats)
            {
                var available = fireUnits
                    .Where(u => remainingMunitions.GetValueOrDefault(u.Id, 0) > 0)
                    .ToList();

                var candidates = GenerateCandidates(threat, available, env);
                if (candidates.Count == 0)
                {
                    plan.ThreatsUnengaged++;
                    continue;
                }

                // 计算总弹药需求
                var neededAmmo = MatchAmmo((PowerType)threat.Target.PowerType);
                var ammo = _ammoCatalog.First(a => a.AerosolType == (int)neededAmmo);

                // 单机需求
                var (effectiveRadius, expansionTime, turbulentRadius) = ComputeEffectiveRadius(threat, ammo, env);
                int singleNeeded = CalcRoundsNeeded(threat, ammo, env, false, turbulentRadius);

                // 横向编队：每种 Width = Quantity 个独立车道
                int yLanes = 1;
                float formationWidth = 0f;
                if (threat.Target.Quantity > 1 && threat.Route != null && (FormationMode)threat.Route.FormationMode == FormationMode.Formation)
                {
                    yLanes = threat.Target.Quantity;
                    formationWidth = (threat.Target.Quantity - 1) * (float)threat.Route.LateralSpacing;
                }
                int totalRoundsNeeded = singleNeeded * yLanes;

                // 逐单元分配：每个单元锁定一个 Y 车道
                float spacing = 2f * turbulentRadius;
                int[] laneNeeded = new int[yLanes];
                float[] laneBaseTime = new float[yLanes];
                bool[] laneBaseSet = new bool[yLanes];
                for (int l = 0; l < yLanes; l++) laneNeeded[l] = singleNeeded;
                int currentLane = 0;
                int roundsCollected = 0;
                int eventIdx = 0;
                var assignedUnits = new List<(FireUnit unit, int rounds, List<FireEvent> events)>();

                foreach (var c in candidates.OrderBy(c => c.EarliestInterceptTime)
                    .ThenByDescending(c => c.KillProbability))
                {
                    if (roundsCollected >= totalRoundsNeeded) break;
                    int remaining = remainingMunitions.GetValueOrDefault(c.Unit.Id, 0);
                    if (remaining <= 0) continue;

                    while (currentLane < yLanes && laneNeeded[currentLane] <= 0) currentLane++;
                    if (currentLane >= yLanes) break;

                    int toTake = Math.Min(Math.Min(c.Unit.TotalChannels, remaining), laneNeeded[currentLane]);
                    if (toTake <= 0) continue;

                    int yLane = currentLane;
                    var mid = ThreatMidpoint(threat);
                    // 车道第一个单元设基准时间，后续单元以此为准保证间距均匀
                    if (!laneBaseSet[yLane])
                    {
                        var refEvt = GenerateFireEventsAt(threat, c.Unit, c.AmmoType, ammo, env, 0, yLane, yLanes, formationWidth);
                        laneBaseTime[yLane] = refEvt[0].FireTime;
                        laneBaseSet[yLane] = true;
                    }
                    float stagger = Kinematics.CloudCoverInterval(turbulentRadius * 2f, (float)threat.Target.TypicalSpeed, c.Unit.ChannelInterval);

                    var fevents = new List<FireEvent>();
                    int unitIdx = fireUnits.IndexOf(c.Unit);
                    for (int ch = 0; ch < toTake; ch++)
                    {
                        float offset = (eventIdx - (totalRoundsNeeded - 1) / 2f) * spacing;
                        var fe = GenerateFireEventsAt(threat, c.Unit, c.AmmoType, ammo, env, offset, yLane, yLanes, formationWidth);
                        int baseRoundInLane = singleNeeded - laneNeeded[currentLane];
                        foreach (var e in fe)
                        {
                            e.FireTime = laneBaseTime[yLane] + (baseRoundInLane + ch) * stagger;
                            e.TargetX = mid.X + offset;
                            e.PlatformIndex = unitIdx * c.Unit.TotalChannels + ch;
                        }
                        fevents.AddRange(fe);
                        eventIdx++;
                    }

                    assignedUnits.Add((c.Unit, toTake, fevents));
                    remainingMunitions[c.Unit.Id] -= toTake;
                    laneNeeded[currentLane] -= toTake;
                    roundsCollected += toTake;
                }

                if (roundsCollected <= 0) { plan.ThreatsUnengaged++; continue; }

                foreach (var (unit, rounds, fireEvents) in assignedUnits)
                {
                    plan.Assignments.Add(new UnitAssignment
                    {
                        FireUnitId = unit.Id,
                        DroneGroupId = threat.GroupId,
                        AmmoType = neededAmmo,
                        RoundsFired = rounds,
                        FirstFireTime = fireEvents.Count > 0 ? fireEvents[0].FireTime : 0,
                        FireEvents = fireEvents,
                    });
                    plan.MergedSchedule.AddRange(fireEvents);
                }

                plan.ThreatsEngaged++;
            }

            plan.MergedSchedule.Sort((a, b) => a.FireTime.CompareTo(b.FireTime));

            // 按威胁汇总概率
            var threatProbs = new List<float>();
            foreach (var threat in sortedThreats)
            {
                var a2 = _ammoCatalog.FirstOrDefault(s =>
                    s.AerosolType == (int)MatchAmmo((PowerType)threat.Target.PowerType));
                int totalRounds = plan.Assignments
                    .Where(a => a.DroneGroupId == threat.GroupId)
                    .Sum(a => a.RoundsFired);
                if (totalRounds > 0)
                    threatProbs.Add(ComputeInterceptProbability(threat, a2, totalRounds, env));
            }
            plan.OverallProbability = threatProbs.Count > 0 ? threatProbs.Average() : 0f;
            plan.Summary = plan.ThreatsEngaged > 0
                ? $"分配 {plan.ThreatsEngaged} 个威胁，{plan.ThreatsUnengaged} 个无方案"
                : "无威胁被分配拦截方案";

            return plan;
        }

        // ═══════════════════════════════════════════════
        // 候选生成（使用真实物理）
        // ═══════════════════════════════════════════════

        private List<InterceptCandidate> GenerateCandidates(DroneGroup threat,
            List<FireUnit> availableUnits, CombatScene env)
        {
            var candidates = new List<InterceptCandidate>();
            var neededAmmo = MatchAmmo((PowerType)threat.Target.PowerType);
            var ammo = _ammoCatalog.First(a => a.AerosolType == (int)neededAmmo);

            var (ammoEff, _, _) = ComputeEffectiveRadius(threat, ammo, env);

            foreach (var unit in availableUnits)
            {
                if (!unit.AmmoTypes.Contains(neededAmmo)) continue;

                if (unit.Type == PlatformType.AirBased)
                {
                    var c = BuildAirBasedCandidate(threat, unit, neededAmmo, ammo, ammoEff, env);
                    if (c != null) candidates.Add(c);
                }
                else
                {
                    var c = BuildGroundBasedCandidate(threat, unit, neededAmmo, ammo, ammoEff, env);
                    if (c != null) candidates.Add(c);
                }
            }

            return candidates;
        }

        private InterceptCandidate? BuildGroundBasedCandidate(DroneGroup threat,
            FireUnit unit, AerosolType ammoType, AmmunitionSpec ammo,
            float effectiveR, CombatScene env)
        {
            var mid = ThreatMidpoint(threat);
            float dx = mid.X - unit.Position.X;
            float dz = mid.Z - unit.Position.Z;
            float dist = (float)Math.Sqrt(dx * dx + dz * dz);

            if (unit.MuzzleVelocity <= 0) return null;
            float maxRange = unit.MuzzleVelocity * unit.MuzzleVelocity / 9.81f;
            if (dist > maxRange) return null;

            float shellTime = dist / unit.MuzzleVelocity;
            float interceptTime = threat.ArrivalTime;

            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            float neededExposure = ammoType == AerosolType.ActiveMaterial ? 2f : 6f;
            float actualExposure = effectiveR * 2f / avgSpeed;
            float prob = Math.Min(0.95f, actualExposure / neededExposure);

            return new InterceptCandidate
            {
                Unit = unit,
                AmmoType = ammoType,
                EarliestInterceptTime = interceptTime,
                CoverageDuration = effectiveR * 2f,
                KillProbability = prob,
                FlightTime = shellTime,
                ShellFlightTime = shellTime,
            };
        }

        private InterceptCandidate? BuildAirBasedCandidate(DroneGroup threat,
            FireUnit unit, AerosolType ammoType, AmmunitionSpec ammo,
            float effectiveR, CombatScene env)
        {
            if (unit.ReleaseAltitude <= 0 || unit.CruiseSpeed <= 0) return null;
            var mid = ThreatMidpoint(threat);
            float releaseAlt = unit.ReleaseAltitude;
            float flightDist = Kinematics.Distance3D(
                unit.Position.X, unit.Position.Y, unit.Position.Z,
                mid.X, releaseAlt, mid.Z);
            float flightTime = flightDist / unit.CruiseSpeed;
            float fallTime = Kinematics.AirDropFallTime(releaseAlt, (float)threat.Target.TypicalAltitude);
            float totalTime = flightTime + fallTime;

            float interceptTime = threat.ArrivalTime;
            if (totalTime > interceptTime) return null;

            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            float neededExposure = ammoType == AerosolType.ActiveMaterial ? 2f : 6f;
            float actualExposure = effectiveR * 2f / avgSpeed;
            float prob = Math.Min(0.95f, actualExposure / neededExposure);

            return new InterceptCandidate
            {
                Unit = unit,
                AmmoType = ammoType,
                EarliestInterceptTime = interceptTime,
                CoverageDuration = effectiveR * 2f,
                KillProbability = prob,
                FlightTime = totalTime,
                ShellFlightTime = fallTime,
            };
        }

        // ═══════════════════════════════════════════════
        // 弹药计算（使用 AmmunitionSpec + 环境参数）
        // ═══════════════════════════════════════════════

        private (float effectiveRadius, float expansionTime, float rPhase2) ComputeEffectiveRadius(
            DroneGroup threat, AmmunitionSpec ammo, CombatScene env)
        {
            float totalFlightTime = threat.ArrivalTime * 2f;
            float halfTime = totalFlightTime / 2f;
            float windSpeed = (float)env.WindSpeed;
            var weather = (WeatherType)env.WeatherType;
            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;

            // Phase 1 初始半径（基于爆发药 TNT 当量）
            float r0 = 3.3f * (float)Math.Pow(Math.Max(0.01, (float)ammo.BurstChargeKg), 0.32);
            float k = (float)ammo.TurbulentExpansionK;
            float maxDur = (float)ammo.MaxDuration;

            // Phase 2 湍流膨胀后的半径
            float phase2Time = Math.Min(halfTime, 30f);
            float turbulentRadius = r0 + k * (float)Math.Sqrt(Math.Max(0, phase2Time));

            float expansionTime = (float)Math.Pow((turbulentRadius - r0) / Math.Max(k, 0.01f), 2);
            float effectiveR = turbulentRadius;

            // Phase 3 高斯扩散
            if (halfTime > 30f)
            {
                float x = Math.Max(1f, windSpeed * (halfTime - 30f));
                var cls = Kinematics.GetStabilityClass(weather, windSpeed);
                float sY = Kinematics.SigmaY(cls, x);
                float sZ = Kinematics.SigmaZ(cls, x);
                float peakC = Kinematics.GaussianPeakConcentration((float)ammo.SourceStrength, sY, sZ);
                float threshold = (float)ammo.EffectiveConcentration;
                if (peakC > threshold)
                {
                    float sigma = (sY + sZ) / 2f;
                    effectiveR = turbulentRadius + sigma * (float)Math.Sqrt(2f * Math.Log(peakC / threshold));
                }
            }

            return (effectiveR, expansionTime, turbulentRadius);
        }

        private int CalcRoundsNeeded(DroneGroup threat, AmmunitionSpec ammo,
            CombatScene env, bool isAirBased, float turbulentRadius)
        {
            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            float spacing = 2f * turbulentRadius;

            var aerosolType = (AerosolType)ammo.AerosolType;
            float neededExposure = aerosolType == AerosolType.ActiveMaterial ? 2f : 6f;
            float singleCoverage = neededExposure * avgSpeed;
            float requiredCoverage = singleCoverage;

            return Math.Max(1,
                (int)Math.Ceiling((requiredCoverage - 2f * turbulentRadius) / spacing) + 1);
        }

        private float ComputeInterceptProbability(DroneGroup threat,
            AmmunitionSpec ammo, int rounds, CombatScene env)
        {
            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            var (effectiveR, _, _) = ComputeEffectiveRadius(threat, ammo, env);
            float spacing = 2f * effectiveR;
            float actualCoverage = spacing * (rounds - 1) + 2f * effectiveR;
            float actualExposure = actualCoverage / avgSpeed;

            var aerosolType = (AerosolType)ammo.AerosolType;
            float neededExposure = aerosolType == AerosolType.ActiveMaterial ? 2f : 6f;

            return Math.Min(0.95f, actualExposure / neededExposure);
        }

        // ═══════════════════════════════════════════════
        // 发射事件生成（真实物理）
        // ═══════════════════════════════════════════════

        private List<FireEvent> GenerateFireEventsAt(DroneGroup threat, FireUnit unit,
            AerosolType ammoType, AmmunitionSpec ammo, CombatScene env, float targetOffset,
            int yLane, int yLanes, float formationWidth)
        {
            var events = new List<FireEvent>();

            var mid = ThreatMidpoint(threat);
            var (effectiveR, expansionTime, _) = ComputeEffectiveRadius(threat, ammo, env);

            // 编队 Y 偏移：按车道分布
            float laneSpacingZ = yLanes > 1 ? formationWidth / (yLanes - 1) : 0f;
            float tz = mid.Z + yLane * laneSpacingZ;

            float tx = mid.X + targetOffset;

            // 按目标点位置计算无人机到达时间，非中点
            float droneSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            if (droneSpeed <= 0) return events;
            float startX = (float)threat.Waypoints[0].PosX;
            float startZ = (float)threat.Waypoints[0].PosZ;
            float endX = (float)threat.Waypoints[^1].PosX;
            float endZ = (float)threat.Waypoints[^1].PosZ;
            float totalDist = Kinematics.Distance2D(startX, startZ, endX, endZ);
            if (totalDist <= 0) return events;
            float distToTx = Kinematics.Distance2D(startX, startZ, tx, tz);
            distToTx = Math.Max(0, Math.Min(totalDist, distToTx));
            float txArrival = distToTx / droneSpeed;
            float recommendedTiming = txArrival - expansionTime;
            // 来不及：无人机到达时云团未形成 → 跳过此目标点
            if (recommendedTiming <= 0f) return events;

            float fireTime;
            if (unit.Type == PlatformType.AirBased)
            {
                if (unit.ReleaseAltitude <= 0 || unit.CruiseSpeed <= 0)
                    throw new InvalidOperationException($"空基单元 {unit.Id}: ReleaseAltitude={unit.ReleaseAltitude}, CruiseSpeed={unit.CruiseSpeed} 必须>0");
                float releaseAlt = unit.ReleaseAltitude;
                float cruiseSpd = unit.CruiseSpeed;
                float fallTime = Kinematics.AirDropFallTime(releaseAlt, (float)threat.Target.TypicalAltitude);
                // 载具飞向云端方向，漂移距离 = 巡航速度 × 落体时间
                float driftDist = cruiseSpd * fallTime;
                float distToCloud = Kinematics.Distance3D(
                    unit.Position.X, unit.Position.Y, unit.Position.Z,
                    tx, releaseAlt, tz);
                // 投放点间距 = 总距离 - 漂移，投后弹药滑翔至预期云位
                float flightDist = Math.Max(0f, distToCloud - driftDist);
                float flightTime = flightDist / cruiseSpd;
                fireTime = recommendedTiming - flightTime - fallTime;
            }
            else
            {
                if (unit.MuzzleVelocity <= 0)
                    throw new InvalidOperationException($"地基单元 {unit.Id}: MuzzleVelocity={unit.MuzzleVelocity} 必须>0");
                float mv = unit.MuzzleVelocity;
                float dx = tx - unit.Position.X;
                float dz = tz - unit.Position.Z;
                float dist = (float)Math.Sqrt(dx * dx + dz * dz);
                float shellTime = dist / mv;
                fireTime = recommendedTiming - shellTime;
            }

            if (fireTime <= 0f) return events; // 平台来不及到达

            events.Add(new FireEvent
            {
                FireTime = fireTime,
                PlatformIndex = 0,
                TargetX = tx,
                TargetY = mid.Y,
                TargetZ = tz,
                MuzzleVelocity = unit.Type == PlatformType.AirBased ? 0f : unit.MuzzleVelocity,
            });

            return events;
        }

        // ═══════════════════════════════════════════════
        // Step 5: 临界方案（概率阈值 50%）
        // ═══════════════════════════════════════════════

        private DefensePlan DeriveCritical(DefensePlan best)
        {
            var critical = new DefensePlan
            {
                ThreatsEngaged = best.ThreatsEngaged,
                ThreatsUnengaged = best.ThreatsUnengaged,
            };

            foreach (var assignment in best.Assignments)
            {
                int rounds = assignment.RoundsFired;
                while (rounds > 1)
                {
                    // 简化：弹药减半 → 概率减半
                    if ((float)rounds / assignment.RoundsFired < 0.5f) break;
                    rounds--;
                }

                var reduced = new UnitAssignment
                {
                    FireUnitId = assignment.FireUnitId,
                    DroneGroupId = assignment.DroneGroupId,
                    AmmoType = assignment.AmmoType,
                    RoundsFired = rounds,
                    FirstFireTime = assignment.FirstFireTime,
                    FireEvents = assignment.FireEvents.Take(rounds).ToList(),
                };
                critical.Assignments.Add(reduced);
                critical.MergedSchedule.AddRange(reduced.FireEvents);
            }

            critical.MergedSchedule.Sort((a, b) => a.FireTime.CompareTo(b.FireTime));
            critical.OverallProbability = 0.5f;
            critical.Summary = $"临界方案：刚好满足 50% 拦截概率";
            return critical;
        }

        // ═══════════════════════════════════════════════
        // 辅助
        // ═══════════════════════════════════════════════

        private static Vector3 ThreatMidpoint(DroneGroup threat)
        {
            if (threat.Waypoints.Count < 2)
                return new Vector3(0, (float)threat.Target.TypicalAltitude, 0);
            var s = threat.Waypoints[0];
            var e = threat.Waypoints[^1];
            return new Vector3(
                (float)(s.PosX + e.PosX) / 2f,
                (float)(s.PosY + e.PosY) / 2f,
                (float)(s.PosZ + e.PosZ) / 2f);
        }
    }
}