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

namespace CounterDrone.Core.Tests
{
    public class InterceptCalculatorTests
    {
        private static List<Waypoint> FlatRoute(float length, float speedMs)
            => new() {
                new() { PosX = 0, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
                new() { PosX = length, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
            };

        [Fact]
        public void NoHeightDiff_vs100_vd1()
        {
            // 航路 0→20, vd=1, unit@(10,0,0), vs=100, R=0
            // (10-A)² + 0.25·g²·A⁴ = 10000·A²
            // 近似解 A≈0.099 (重力项可忽略)
            var route = FlatRoute(20, 1);
            var unit = new Vector3(10, 0, 0);
            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 100);

            Assert.True(arc > 0.05f && arc < 0.15f, $"arc={arc:F4}");
        }

        [Fact]
        public void NoHeightDiff_vs100_vd1_R2()
        {
            // R=2s, ts=A-2, 方程: (10-A)²+0.25g²(A-2)⁴ = 10000(A-2)²
            var route = FlatRoute(20, 1);
            var unit = new Vector3(10, 0, 0);
            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 2, unit, 100);

            Assert.True(arc > 1.5f && arc < 3f, $"arc={arc:F3}");
        }

        [Fact]
        public void NoHeightDiff_vs50_vd1_UnitFar()
        {
            // unit@(50,0,0), vs=50, vd=1, R=0, route 0→100
            var route = FlatRoute(100, 1);
            var unit = new Vector3(50, 0, 0);
            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 50);

            Assert.True(arc > 0.3f && arc < 1.5f, $"arc={arc:F4}");
        }

        [Fact]
        public void UpwardBranch_ShellGoingUp()
        {
            // Y=500, unit Y=0, vs=800, vd=55.6(200km/h), R=5, route 0→10000
            // 核实求出的 ts 对应上升段
            var route = StraightRoute(0, 10000, 500, 200);
            var unit = new Vector3(5000, 0, 50);
            var (arc, ts) = InterceptCalculator.Compute(route, 0, 200, 5, unit, 800);

            Assert.True(arc > 0, $"arc={arc:F0}");
            var (rx, ry, rz) = RouteGeometry.PositionAt(route, arc);
            float d = MathF.Sqrt((rx - 5000) * (rx - 5000) + (rz - 50) * (rz - 50));
            float cosTheta = d / (800 * ts);
            float sinTheta = (ry - 0 + 0.5f * 9.81f * ts * ts) / (800 * ts);
            // 上升段: vy > 0，即 vs*sinθ - g*ts > 0
            float vy = 800 * sinTheta - 9.81f * ts;
            Assert.True(vy > 0, $"vy={vy:F1} should be >0 (upward branch), θ={MathF.Atan2(sinTheta, cosTheta)*180/MathF.PI:F1}°");
        }

        private static List<Waypoint> StraightRoute(float x0, float x1, float alt, float speed)
            => new() {
                new() { PosX = x0, PosY = alt, PosZ = 0, Speed = speed },
                new() { PosX = x1, PosY = alt, PosZ = 0, Speed = speed },
            };
    }
}