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

namespace CounterDrone.Core.Algorithms
{
    /// <summary>探测计算工具——统一信息网络对威胁的探测能力评估。
    /// 纯函数、无状态，与 Kinematics/RouteGeometry 同范式。
    /// planner（事前规划）基于此估算威胁的最早发现点，而非上帝视角的航路起点。</summary>
    public static class DetectionCalculator
    {
        /// <summary>某探测源在给定能见度下的综合有效探测距离（水平面）。
        /// 取雷达/光电/红外三者的最大有效距离（任一方式发现即算发现）。
        /// 光电受 Visibility 衰减：有效 = 基准 × min(1, Visibility/基准)。
        /// 雷达/红外不受能见度影响（当前范围）。</summary>
        public static float EffectiveRange(float radarRange, float eoRange, float irRange, float visibility)
        {
            float effRadar = radarRange;
            float effEO = eoRange > 0
                ? eoRange * Math.Min(1f, visibility / eoRange)
                : 0f;
            float effIR = irRange;
            return Math.Max(effRadar, Math.Max(effEO, effIR));
        }

        /// <summary>统一信息网络对某威胁的最早探测点。
        /// 沿航路以步长 ≤50m 采样，对每个采样点调 IsInCoverage 做 3D 球冠判定。
        /// 返回首次落入任一探测源球冠的弧长与精度。
        /// 无探测源时返回 (0, float.MaxValue)（回退到航路起点，上帝视角）。
        /// 航路完全不经过任何探测范围时返回 (float.MaxValue, float.MaxValue)。</summary>
        public static (float detectArc, float accuracy) EarliestDetection(
            IReadOnlyList<Waypoint> threatRoute,
            List<DetectionSource> sources,
            float visibility)
        {
            if (sources == null || sources.Count == 0 || threatRoute == null || threatRoute.Count < 2)
                return (0f, float.MaxValue);

            // 计算总弧长，确定采样步长（≤50m）
            float totalArc = 0f;
            for (int i = 0; i < threatRoute.Count - 1; i++)
            {
                float dx = (float)(threatRoute[i + 1].PosX - threatRoute[i].PosX);
                float dy = (float)(threatRoute[i + 1].PosY - threatRoute[i].PosY);
                float dz = (float)(threatRoute[i + 1].PosZ - threatRoute[i].PosZ);
                totalArc += MathF.Sqrt(dx * dx + dy * dy + dz * dz);
            }

            const float sampleStep = 50f;
            int sampleCount = Math.Max(1, (int)(totalArc / sampleStep) + 1);
            float stepArc = totalArc / sampleCount;

            float bestArc = float.MaxValue;
            float bestAccuracy = float.MaxValue;

            // 预计算每个源的 EffectiveRange（能见度衰减）
            var srcCache = new (float range, DetectionSource src)[sources.Count];
            for (int s = 0; s < sources.Count; s++)
            {
                var src = sources[s];
                float range = EffectiveRange(src.RadarRange, src.EORange, src.IRRange, visibility);
                srcCache[s] = (range, src);
            }

            // 沿航路采样
            for (int i = 0; i <= sampleCount; i++)
            {
                float sampleArc = i * stepArc;
                var (sx, sy, sz) = RouteGeometry.PositionAt(threatRoute, sampleArc);
                var pos = new Algorithms.Vector3(sx, sy, sz);

                for (int s = 0; s < srcCache.Length; s++)
                {
                    var (range, src) = srcCache[s];
                    if (range <= 0) continue;
                    if (IsInCoverage(pos, src.Position, range,
                        src.MinElevation, src.MaxElevation,
                        src.MinDetectAlt, src.MaxDetectAlt))
                    {
                        // 最早发现优先；同一点取精度最高
                        if (sampleArc < bestArc)
                        {
                            bestArc = sampleArc;
                            bestAccuracy = src.Accuracy;
                        }
                        else if (MathF.Abs(sampleArc - bestArc) < 0.001f
                            && src.Accuracy < bestAccuracy)
                        {
                            bestAccuracy = src.Accuracy;
                        }
                    }
                }

                // 一旦发现第一个覆盖点即停止（后续采样点距离更远，不可能是"最早"）
                if (bestArc < float.MaxValue)
                    break;
            }

            if (bestArc == float.MaxValue)
                return (float.MaxValue, float.MaxValue);
            return (bestArc, bestAccuracy);
        }

        /// <summary>三维球冠探测判定：目标是否在探测设备的有效探测范围内。
        /// 三项判定：① 水平距离 ≤ effectiveRange；② 俯仰角 ∈ [MinElevation, MaxElevation]；
        /// ③ 目标高度 ∈ [MinDetectAlt, MaxDetectAlt]。
        /// MinElevation/MaxElevation 为 float.MaxValue 时表示无限制（退化球冠→等价 2D 圆）。
        /// MinDetectAlt/MaxDetectAlt 为 float.MaxValue 时同理。
        /// planner 与运行时实时探测共用此判定。</summary>
        public static bool IsInCoverage(
            Algorithms.Vector3 target, Algorithms.Vector3 detector,
            float effectiveRange,
            float minElevation, float maxElevation,
            float minDetectAlt, float maxDetectAlt)
        {
            float dx = target.X - detector.X;
            float dy = target.Y - detector.Y;   // 高度差
            float dz = target.Z - detector.Z;
            float horizDistSq = dx * dx + dz * dz;
            // ① 水平距离
            if (horizDistSq > effectiveRange * effectiveRange) return false;
            // ③ 高度门限
            if (minDetectAlt != float.MaxValue && target.Y < minDetectAlt) return false;
            if (maxDetectAlt != float.MaxValue && target.Y > maxDetectAlt) return false;
            // ② 俯仰角（正顶/正下按角度门限放行）
            if (minElevation == float.MaxValue && maxElevation == float.MaxValue) return true;
            float horizDist = MathF.Sqrt(horizDistSq);
            if (horizDist < 0.0001f) return true;
            float elevation = MathF.Atan2(dy, horizDist) * (180f / MathF.PI);
            if (minElevation != float.MaxValue && elevation < minElevation) return false;
            if (maxElevation != float.MaxValue && elevation > maxElevation) return false;
            return true;
        }

        /// <summary>探测精度换算为抛撒散布半径（m）。
        /// 精度差 → 散布半径大，planner 增加横向覆盖。
        /// 当前模型：散布半径 = 精度值（如精度 100m → 散布 ±100m）。</summary>
        public static float SpreadRadius(float accuracy)
        {
            if (accuracy < 0)
                throw new ArgumentException($"accuracy 必须 ≥ 0，实际: {accuracy}", nameof(accuracy));
            return accuracy;
        }

    }
}
