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

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

namespace CounterDrone.Core.Algorithms
{
    /// <summary>防御规划器 — 五步流水线，全部调用与引擎共享的物理工具类</summary>
    public class DefensePlanner : IDefensePlanner
    {
        private readonly List<AmmunitionSpec> _ammoCatalog;
        private readonly IDamageModel _damageModel;
        private readonly PlannerConfig _config;

        public DefensePlanner(List<AmmunitionSpec> ammoCatalog, PlannerConfig config,
            IDamageModel? damageModel = null)
        {
            _ammoCatalog = ammoCatalog ?? throw new ArgumentNullException(nameof(ammoCatalog));
            _config = config ?? throw new ArgumentNullException(nameof(config));
            _damageModel = damageModel ?? new DamageModelRouter();
            if (_ammoCatalog.Count == 0)
                throw new ArgumentException("弹药规格目录不能为空");
        }

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

        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(_config, t.Target);
            }
            var sorted = threats.OrderByDescending(t => t.Priority).ToList();

            // Step 2-4: 贪心分配求解
            // 预先检查：所有需要的弹药类型都在目录中
            var neededTypes = sorted
                .Select(t => MatchAmmo(_config, (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(PlannerConfig config, TargetConfig target)
        {
            float typeCoef = config.TypeCoefficient.GetValueOrDefault((TargetType)target.TargetType, 1f);
            float speedCoef = (float)target.TypicalSpeed / 60f;
            return typeCoef * speedCoef;
        }

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

        private static AerosolType MatchAmmo(PlannerConfig config, PowerType power)
        {
            return config.AmmoMatch.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(_config, (PowerType)threat.Target.PowerType);
                var ammo = _ammoCatalog.First(a => a.AerosolType == (int)neededAmmo);

                // 单机需求
                var (effectiveRadius, expansionTime, turbulentRadius) = ComputeEffectiveRadius(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 车道
                // 云团间距 = 2R × (1 - 重叠比例)，重叠由配置驱动，保证有效区互相覆盖
                float spacing = 2f * turbulentRadius * (1f - _config.CloudOverlapRatio);
                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;
                    // 车道第一个单元设基准时间，后续单元以此为准保证间距均匀
                    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;
                            // TargetX/Z 保留 GenerateFireEventsAt 算出的风偏补偿后抛撒点 cloudGenX/Z
                            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(_config, (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(_config, (PowerType)threat.Target.PowerType);
            var ammo = _ammoCatalog.First(a => a.AerosolType == (int)neededAmmo);

            var (ammoEff, _, _) = ComputeEffectiveRadius(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 = _damageModel.RequiredExposureSeconds((TargetType)threat.Target.TargetType, (PowerType)threat.Target.PowerType, ammoType);
            float actualExposure = effectiveR * 2f / avgSpeed;
            float prob = Math.Min(_config.MaxInterceptProbability, actualExposure / neededExposure);

            return new InterceptCandidate
            {
                Unit = unit,
                AmmoType = ammoType,
                EarliestInterceptTime = interceptTime,
                KillProbability = prob,
            };
        }

        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 = _damageModel.RequiredExposureSeconds((TargetType)threat.Target.TargetType, (PowerType)threat.Target.PowerType, ammoType);
            float actualExposure = effectiveR * 2f / avgSpeed;
            float prob = Math.Min(_config.MaxInterceptProbability, actualExposure / neededExposure);

            return new InterceptCandidate
            {
                Unit = unit,
                AmmoType = ammoType,
                EarliestInterceptTime = interceptTime,
                KillProbability = prob,
            };
        }

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

        private (float effectiveRadius, float expansionTime, float rPhase2) ComputeEffectiveRadius(
            AmmunitionSpec ammo, CombatScene env)
        {
            var model = new CloudExpansionModel(ammo, env);
            float rPhase2 = model.RadiusAt(30f);
            float expansionTime = model.TimeToReach(rPhase2);
            // 云团被穿过时的真实年龄 = expansionTime（云团生成后膨胀到有效半径的时长）。
            // 这是云团自身的物理量，与无人机飞行时间无关。
            float effectiveR = model.RadiusAt(expansionTime);
            return (effectiveR, expansionTime, rPhase2);
        }

        private int CalcRoundsNeeded(DroneGroup threat, AmmunitionSpec ammo,
            CombatScene env, bool isAirBased, float turbulentRadius)
        {
            var cloudModel = new CloudExpansionModel(ammo, env);
            float avgSpeed = (float)threat.Target.TypicalSpeed / 3.6f;
            float neededExposure = _damageModel.RequiredExposureSeconds(
                (TargetType)threat.Target.TargetType, (PowerType)threat.Target.PowerType, (AerosolType)ammo.AerosolType);
            float requiredCoverage = neededExposure * avgSpeed;
            // 间距由配置的重叠比例驱动，公式在 CloudExpansionModel（共享）
            float spacing = 2f * turbulentRadius * (1f - _config.CloudOverlapRatio);
            return cloudModel.RoundsNeeded(requiredCoverage, spacing);
        }

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

            var aerosolType = (AerosolType)ammo.AerosolType;
            float neededExposure = _damageModel.RequiredExposureSeconds((TargetType)threat.Target.TargetType, (PowerType)threat.Target.PowerType, aerosolType);

            return Math.Min(_config.MaxInterceptProbability, 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 wps = threat.Waypoints;
            if (wps == null || wps.Count < 2) return events;

            var cloudModel = new CloudExpansionModel(ammo, env);
            // 云龄 = expansionTime（云团生成后膨胀到有效半径的时长）
            float effectiveAge = cloudModel.TimeToReach(cloudModel.TurbulentRadius) * 0.9f;
            float effectiveR = cloudModel.RadiusAt(effectiveAge);
            float expansionTime = cloudModel.TimeToReach(effectiveR);

            // 密度检查：云团在膨胀时间后密度是否仍达标
            float densityAtPassage = cloudModel.DensityAt(expansionTime);
            if (densityAtPassage < (float)ammo.EffectiveConcentration)
                return events; // 云团到达时已稀释失效

            float typicalSpeed = (float)threat.Target.TypicalSpeed;
            if (typicalSpeed <= 0) return events;

            // 航路感知布局：穿越点弧长 = 航路中点弧长 + 沿航路偏移
            // targetOffset 现在是沿航路切向的弧长偏移（不再是 X 分量），支持任意方向航路
            var mid = ThreatMidpoint(threat);
            float midArc = RouteGeometry.ArcLengthNearestTo(wps, mid.X, mid.Z);
            float crossArc = midArc + targetOffset;

            // 编队横向偏移：按车道分布在航路法向上
            // 法向 = 切向旋转 90°，用 RouteGeometry.TangentAt 取穿越点处航路方向
            var (tanX, tanZ) = RouteGeometry.TangentAt(wps, crossArc);
            float normalX = -tanZ, normalZ = tanX; // 切向逆时针 90° = 左侧法向
            float laneOffset = yLanes > 1 ? (yLane - (yLanes - 1) / 2f) * (formationWidth / Math.Max(1, yLanes - 1)) : 0f;

            // 穿越点 = 航路上 crossArc 处 + 横向车道偏移
            var (routeX, routeY, routeZ) = RouteGeometry.PositionAt(wps, crossArc);
            float tx = routeX + normalX * laneOffset;
            float ty = routeY;
            float tz = routeZ + normalZ * laneOffset;

            // 无人机到达穿越点的时间：沿航路匀速飞行 crossArc 弧长
            float txArrival = RouteGeometry.TravelTimeTo(wps, crossArc >= 0 ? crossArc : 0, typicalSpeed);
            float recommendedTiming = txArrival - expansionTime;
            if (recommendedTiming <= 0f) return events;

            // 风偏补偿：云团生成后会被风吹偏，补偿时长 = 从生成到被穿过。
            // 每朵云的生成时刻不同（受发射/飞行时间影响），但 planner 此处用 expansionTime
            // 作为云团从生成到被穿过的时长（recommendedTiming 已对齐），各发独立用各自的 expansionTime。
            var (wx, _, wz) = Kinematics.WindToVector((WindDirection)env.WindDirection, (float)env.WindSpeed);
            float cloudGenX = tx - wx * expansionTime;
            float cloudGenZ = tz - wz * expansionTime;

            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,
                    cloudGenX, releaseAlt, cloudGenZ);
                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 = cloudGenX - unit.Position.X;
                float dz = cloudGenZ - unit.Position.Z;
                float dist = (float)Math.Sqrt(dx * dx + dz * dz);
                float heightDiff = (float)threat.Target.TypicalAltitude - unit.Position.Y;
                float shellTime = Kinematics.ParabolicShellTime(dist, heightDiff, mv);
                fireTime = recommendedTiming - shellTime;
            }

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

            events.Add(new FireEvent
            {
                FireTime = fireTime,
                PlatformIndex = 0,
                TargetX = cloudGenX,
                TargetY = ty,
                TargetZ = cloudGenZ,
                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 < _config.CriticalProbabilityThreshold) 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 = _config.CriticalProbabilityThreshold;
            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);
        }
    }
}