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

namespace CounterDrone.Core.Algorithms
{
    public static class InterceptCalculator
    {
        /// <summary>空基拦截：水平发射(θ=0°)，从当前位置直接抛出。
        /// 炮弹以 vp 水平飞出，下落 tf=√(2Δy/g) 秒后到达目标高度。
        /// 拦截点 = ux + vp·tf。方程: (A-d)/vd = R + expansion + tf。</summary>
        public static (float, float, float) ComputeHorizontal(
            IReadOnlyList<Waypoint> route, float detectArc, float droneSpeedKmh,
            float reactionTime, Vector3 platformPos, float cruiseSpeed,
            float releaseAlt, float targetAlt)
        {
            if (route == null || route.Count < 2 || droneSpeedKmh <= 0 || cruiseSpeed <= 0)
                return (0, 0, 0);

            float vd = droneSpeedKmh / 3.6f;
            float dy = releaseAlt - targetAlt;
            if (dy <= 0) return (0, 0, 0);
            float tf = MathF.Sqrt(2f * dy / 9.81f);

            // 水平发射：炮弹落在 ux + vp*tf 处
            float A = platformPos.X + cruiseSpeed * tf;
            float totalArc = RouteGeometry.TotalLength(route);
            if (A < detectArc || A > totalArc) return (0, 0, 0);

            // 验证时间匹配
            float droneTime = (A - detectArc) / vd;
            float defendTime = reactionTime + tf;
            if (droneTime < defendTime) return (0, 0, 0);

            return (A, droneTime - reactionTime, 0);
        }

        /// <summary>地基拦截：D² + (Δy + ½g·ts²)² = vs²·ts²</summary>
        public static (float InterceptArc, float ShellTime, float LaunchAngle) Compute(
            IReadOnlyList<Waypoint> route, float detectArc, float droneSpeedKmh,
            float reactionTime, Vector3 fireUnitPos, float muzzleVelocity)
        {
            // ... existing code unchanged ...
            if (route == null || route.Count < 2 || droneSpeedKmh <= 0 || muzzleVelocity <= 0)
                return (0, 0, 0);

            float totalArc = RouteGeometry.TotalLength(route);
            if (detectArc >= totalArc) return (0, 0, 0);

            float vd = droneSpeedKmh / 3.6f;
            float vs2 = muzzleVelocity * muzzleVelocity;
            float g = 9.81f;
            float uy = fireUnitPos.Y;
            IReadOnlyList<Waypoint> r = route;

            float F(float a)
            {
                float ts = (a - detectArc) / vd - reactionTime;
                if (ts <= 0) return float.MaxValue;
                var (px, py, pz) = RouteGeometry.PositionAt(r, a);
                float dx = px - fireUnitPos.X, dz = pz - fireUnitPos.Z;
                float D2 = dx * dx + dz * dz;
                float dy = py - uy;
                float term = dy + 0.5f * g * ts * ts;
                return D2 + term * term - vs2 * ts * ts;
            }

            static float Angle(Vector3 unit, float arc, float ts, float vs, IReadOnlyList<Waypoint> wps)
            {
                var (px, py, pz) = RouteGeometry.PositionAt(wps, arc);
                float D = MathF.Sqrt((px - unit.X) * (px - unit.X) + (pz - unit.Z) * (pz - unit.Z));
                float dy = py - unit.Y;
                float sinA = (dy + 0.5f * 9.81f * ts * ts) / (vs * ts);
                float cosA = D / (vs * ts);
                return MathF.Atan2(sinA, cosA);
            }

            // +0.001f 确保 ts > 0，否则 F() 返回 MaxValue 导致搜索失败
            float lo = detectArc + vd * reactionTime + 0.001f;
            if (lo >= totalArc) return (0, 0, 0);

            float fLo = F(lo);
            if (fLo >= float.MaxValue - 1) return (0, 0, 0);

            float step = (totalArc - lo) / 100f;
            if (step < 0.001f) step = 0.001f;
            float hi = lo + step;
            float fHi = 0;
            while (hi <= totalArc)
            {
                fHi = F(hi);
                if (fHi >= float.MaxValue - 1 || fHi <= 0) break;
                lo = hi; fLo = fHi;
                hi += step;
            }
            if (hi > totalArc || fHi >= float.MaxValue - 1) return (0, 0, 0);
            if (fHi > 0 && fLo > 0) return (0, 0, 0);

            for (int i = 0; i < 30; i++)
            {
                float mid = (lo + hi) / 2f;
                float fMid = F(mid);
                if (MathF.Abs(fMid) < 0.001f || hi - lo < 0.001f)
                {
                    float ts = (mid - detectArc) / vd - reactionTime;
                    return (mid, ts, Angle(fireUnitPos, mid, ts, muzzleVelocity, r));
                }
                if (fLo <= 0 && fMid >= 0 || fLo >= 0 && fMid <= 0) { hi = mid; fHi = fMid; }
                else { lo = mid; fLo = fMid; }
            }

            float final = (lo + hi) / 2f;
            float finalTs = (final - detectArc) / vd - reactionTime;
            return (final, finalTs, Angle(fireUnitPos, final, finalTs, muzzleVelocity, r));
        }
    }
}
