CounterDroneBackend/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs

using CounterDrone.Core.Algorithms;
using CounterDrone.Core.Models;
using Xunit;

namespace CounterDrone.Core.Tests
{
    public class KinematicsTests
    {
        [Fact]
        public void WindToVector_East_CorrectDirection()
        {
            var (x, y, z) = Kinematics.WindToVector(WindDirection.E, 10f);
            Assert.True(x > 9f, "东风 → X 正方向");
            Assert.True(System.Math.Abs(z) < 1f, "东风 → Z 接近 0");
            Assert.Equal(0f, y, 3);
        }

        [Fact]
        public void WindToVector_North_CorrectDirection()
        {
            var (x, y, z) = Kinematics.WindToVector(WindDirection.N, 10f);
            Assert.True(System.Math.Abs(x) < 1f);
            Assert.True(z > 9f, "北风 → Z 正方向");
        }

        [Fact]
        public void CalculateLaunchAngle_ReturnsValidAngle()
        {
            var angle = Kinematics.CalculateLaunchAngle(5000f, 500f, 300f);
            Assert.True(angle > 0);
            Assert.True(angle < System.Math.PI / 2); // < 90°
        }

        [Fact]
        public void Parabolic_45Degree_FlatGround_ReachesExpectedRange()
        {
            // 标准公式：45° 时射程 R = v²/g
            float v0 = 300f, g = 9.81f;
            float expectedR = v0 * v0 / g;
            float angle45 = (float)(System.Math.PI / 4);
            float tof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
            var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle45, (float)(System.Math.PI / 2), v0, tof);
            Assert.True(System.Math.Abs(x - expectedR) < 1f, $"X={x:F1} expected={expectedR:F1}");
            Assert.True(System.Math.Abs(y) < 1f, $"Y={y:F1} expected=0");
        }

        [Fact]
        public void Parabolic_45Degree_ApexAtHalfTime()
        {
            float v0 = 300f, g = 9.81f;
            float angle45 = (float)(System.Math.PI / 4);
            float tof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
            float halfT = tof / 2f;
            var (_, y, _) = Kinematics.ParabolicPosition(0, 0, 0, angle45, (float)(System.Math.PI / 2), v0, halfT);
            float expectedApex = v0 * v0 * (float)System.Math.Sin(angle45) * (float)System.Math.Sin(angle45) / (2f * g);
            Assert.True(System.Math.Abs(y - expectedApex) < 1f, $"Y apex={y:F1} expected={expectedApex:F1}");
        }

        [Fact]
        public void CalculateLaunchAngle_45Degree_Returns45ForMaxRange()
        {
            // 给定 v=300，最大射程 R=v²/g≈9174，需要角度=45°
            float v0 = 300f, g = 9.81f;
            float maxR = v0 * v0 / g;
            float angle = Kinematics.CalculateLaunchAngle(maxR, v0, 0f);
            float expected = (float)(System.Math.PI / 4);
            Assert.True(System.Math.Abs(angle - expected) < 0.01f, $"angle={angle*180/System.Math.PI:F2}° expected=45°");
        }

        [Fact]
        public void ParabolicShellTime_45Degree_MatchesStandardFormula()
        {
            float v0 = 300f, g = 9.81f;
            float R = v0 * v0 / g; // 最大射程
            float time = Kinematics.ParabolicShellTime(R, 0f, v0);
            float angle45 = (float)(System.Math.PI / 4);
            float expectedTof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
            Assert.True(System.Math.Abs(time - expectedTof) < 0.5f, $"time={time:F3} expected={expectedTof:F3}");
        }

        [Fact]
        public void ParabolicShellTime_MatchesTimeOfFlight()
        {
            float R = 5000f, v0 = 800f, H = 500f;
            float time = Kinematics.ParabolicShellTime(R, H, v0);
            float angle = Kinematics.CalculateLaunchAngle(R, v0, H);
            float tof = Kinematics.ParabolicTimeOfFlight(R, angle, v0);
            Assert.True(System.Math.Abs(time - tof) < 0.5f, $"ParabolicShellTime={time:F3} ParabolicTimeOfFlight={tof:F3}");
        }

        [Fact]
        public void Parabolic_RealScenario_5000mRange_500mHeight()
        {
            // 实际场景：单元(5000,0,50) → 目标(10000,500,0)
            float dx = 5000f, dz = -50f;
            float R = (float)System.Math.Sqrt(dx * dx + dz * dz); // ~5000.25
            float H = 500f, v0 = 800f;
            float angle = Kinematics.CalculateLaunchAngle(R, v0, H);
            float time = Kinematics.ParabolicShellTime(R, H, v0);
            float azimuth = (float)System.Math.Atan2(dx, dz);
            var (x, y, z) = Kinematics.ParabolicPosition(5000, 0, 50, angle, azimuth, v0, time);
            Assert.True(System.Math.Abs(x - 10000) < 10f, $"X={x:F1} 应≈10000");
            Assert.True(System.Math.Abs(y - 500) < 10f, $"Y={y:F1} 应≈500");
            Assert.True(System.Math.Abs(z - 0) < 10f, $"Z={z:F1} 应≈0");
        }

        [Fact]
        public void CalculateLaunchAngle_CloseRange_UsesHeightConstraint()
        {
            // 近距离，高度约束应主导
            var angle = Kinematics.CalculateLaunchAngle(1000f, 300f, 300f);
            Assert.True(angle > 0.05f, "近距离也有合理发射角");
        }

        [Fact]
        public void ParabolicPosition_T0_IsAtStart()
        {
            var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, 0.5f, 0, 500f, 0);
            Assert.Equal(0f, x, 1);
            Assert.Equal(0f, y, 1);
            Assert.Equal(0f, z, 1);
        }

        // ═══════════════════════════════════════════════
        // 输入验证：非法参数必须抛出异常
        // ═══════════════════════════════════════════════

        [Fact]
        public void CalculateLaunchAngle_ThrowsOnInvalidInputs()
        {
            Assert.Throws<ArgumentException>(() => Kinematics.CalculateLaunchAngle(0, 300, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.CalculateLaunchAngle(-1, 300, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.CalculateLaunchAngle(5000, 0, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.CalculateLaunchAngle(5000, -10, 0));
        }

        [Fact]
        public void CalculateLaunchAngle_ThrowsOnUnreachableTarget()
        {
            // 300m/s 初速不可能击中 10000m 外 5000m 高的目标
            Assert.Throws<ArgumentException>(() => Kinematics.CalculateLaunchAngle(10000f, 300f, 5000f));
        }

        [Fact]
        public void ParabolicShellTime_ThrowsOnInvalidInputs()
        {
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicShellTime(0, 0, 300));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicShellTime(5000, 0, 0));
        }

        [Fact]
        public void ParabolicShellTime_ThrowsOnUnreachableTarget()
        {
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicShellTime(10000f, 5000f, 300f));
        }

        [Fact]
        public void ParabolicTimeOfFlight_ThrowsOnInvalidInputs()
        {
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicTimeOfFlight(0, 0.5f, 300));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicTimeOfFlight(-1, 0.5f, 300));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicTimeOfFlight(5000, 0.5f, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicTimeOfFlight(5000, 0.5f, -10));
        }

        [Fact]
        public void ComputeParabolicRange_ThrowsOnInvalidInputs()
        {
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(0, 0.5f, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(-10, 0.5f, 0));
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(300, (float)(-System.Math.PI / 2), 0));
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(300, (float)(System.Math.PI / 2), 0));
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(300, (float)System.Math.PI, 0));
        }

        [Fact]
        public void ComputeParabolicRange_ThrowsOnUnreachableHeight()
        {
            // 300m/s 45° 不可能达到 5000m 高度的目标
            float angle45 = (float)(System.Math.PI / 4);
            Assert.Throws<ArgumentException>(() => Kinematics.ComputeParabolicRange(300f, angle45, 5000f));
        }

        // ═══════════════════════════════════════════════
        // 顶点计算
        // ═══════════════════════════════════════════════

        [Fact]
        public void ParabolicApex_45Degree_MatchesStandardFormula()
        {
            float v0 = 300f, g = 9.81f;
            float angle45 = (float)(System.Math.PI / 4);
            float expectedH = v0 * v0 * (float)System.Math.Sin(angle45) * (float)System.Math.Sin(angle45) / (2f * g);
            float expectedT = v0 * (float)System.Math.Sin(angle45) / g;

            var (apexH, apexT) = Kinematics.ParabolicApex(v0, angle45);
            Assert.True(System.Math.Abs(apexH - expectedH) < 0.1f, $"H={apexH:F2} expected={expectedH:F2}");
            Assert.True(System.Math.Abs(apexT - expectedT) < 0.001f, $"T={apexT:F4} expected={expectedT:F4}");
        }

        [Fact]
        public void ParabolicApex_30Degree_Then_ParabolicPosition_AtApexTime_ReturnsApexHeight()
        {
            float v0 = 500f;
            float angle = 30f * (float)(System.Math.PI / 180f);

            var (apexH, apexT) = Kinematics.ParabolicApex(v0, angle);

            // ParabolicPosition 在顶点时刻的高度应 = apexH
            var (_, y, _) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, apexT);
            Assert.True(System.Math.Abs(y - apexH) < 0.1f, $"y={y:F2} apex={apexH:F2}");
        }

        [Fact]
        public void ParabolicApex_ThrowsOnInvalidInputs()
        {
            float angle = 0.5f;
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicApex(0, angle));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicApex(-10, angle));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicApex(300, (float)(-System.Math.PI / 2)));
            Assert.Throws<ArgumentException>(() => Kinematics.ParabolicApex(300, (float)(System.Math.PI / 2)));
        }

        // ═══════════════════════════════════════════════
        // 正问题：给定初速和发射角 → 计算射程和飞行时间
        // ═══════════════════════════════════════════════

        [Fact]
        public void ComputeParabolicRange_FlatGround_MatchesStandardFormula()
        {
            // 45° 平地: R = v²/g, T = 2v·sin45°/g
            float v0 = 300f, g = 9.81f;
            float angle45 = (float)(System.Math.PI / 4);
            float expectedR = v0 * v0 / g;
            float expectedT = 2f * v0 * (float)System.Math.Sin(angle45) / g;

            var (range, time) = Kinematics.ComputeParabolicRange(v0, angle45, 0f);
            Assert.True(System.Math.Abs(range - expectedR) < 1f, $"R={range:F1} expected={expectedR:F1}");
            Assert.True(System.Math.Abs(time - expectedT) < 0.1f, $"T={time:F3} expected={expectedT:F3}");
        }

        [Fact]
        public void ComputeParabolicRange_30Degree_FlatGround()
        {
            float v0 = 500f, g = 9.81f;
            float angle30 = (float)(System.Math.PI / 6);
            // R = v²·sin(2θ)/g, T = 2v·sinθ/g
            float expectedR = v0 * v0 * (float)System.Math.Sin(2 * angle30) / g;
            float expectedT = 2f * v0 * (float)System.Math.Sin(angle30) / g;

            var (range, time) = Kinematics.ComputeParabolicRange(v0, angle30, 0f);
            Assert.True(System.Math.Abs(range - expectedR) < 1f, $"R={range:F1} expected={expectedR:F1}");
            Assert.True(System.Math.Abs(time - expectedT) < 0.1f, $"T={time:F3} expected={expectedT:F3}");
        }

        [Fact]
        public void ComputeParabolicRange_ElevatedTarget_VerifiableByPosition()
        {
            // v₀=800, θ=10°, 目标在发射点上方 500m
            float v0 = 800f, heightDiff = 500f;
            float angle = 10f * (float)System.Math.PI / 180f;

            var (range, time) = Kinematics.ComputeParabolicRange(v0, angle, heightDiff);

            // 用 ParabolicPosition 验证：在 time 时刻应到达 (range, heightDiff)
            var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, time);
            // X 方向（azimuth=0 → cos(0) → +Z 方向）
            // 实际水平距离 = sqrt(x²+z²) = z（因为 azimuth=0）
            float actualDist = (float)System.Math.Sqrt(x * x + z * z);
            Assert.True(System.Math.Abs(actualDist - range) < 5f,
                $"水平距离={actualDist:F1} expected={range:F1}");
            Assert.True(System.Math.Abs(y - heightDiff) < 5f,
                $"高度={y:F1} expected={heightDiff:F1}");
        }

        [Fact]
        public void ComputeParabolicRange_DepressedTarget_VerifiableByPosition()
        {
            // v₀=500, θ=5°, 目标在发射点下方 200m
            float v0 = 500f, heightDiff = -200f;
            float angle = 5f * (float)System.Math.PI / 180f;

            var (range, time) = Kinematics.ComputeParabolicRange(v0, angle, heightDiff);

            // 验证到达时间时位置
            var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, time);
            float actualDist = (float)System.Math.Sqrt(x * x + z * z);
            Assert.True(System.Math.Abs(actualDist - range) < 5f,
                $"水平距离={actualDist:F1} expected={range:F1}");
            Assert.True(System.Math.Abs(y - heightDiff) < 5f,
                $"高度={y:F1} expected={heightDiff:F1}");
        }

        [Fact]
        public void ComputeParabolicRange_5DegreeLongRange()
        {
            // v₀=800, θ=5°, 水平射程
            float v0 = 800f, g = 9.81f;
            float angle = 5f * (float)System.Math.PI / 180f;
            float cosA = (float)System.Math.Cos(angle);
            float sinA = (float)System.Math.Sin(angle);

            // 平坦地面: R = v²sin(2θ)/g
            float expectedR = v0 * v0 * (float)System.Math.Sin(2 * angle) / g;
            float expectedT = 2f * v0 * sinA / g;

            var (range, time) = Kinematics.ComputeParabolicRange(v0, angle, 0f);
            Assert.True(System.Math.Abs(range - expectedR) < 1f);
            Assert.True(System.Math.Abs(time - expectedT) < 0.1f);
        }

        // ═══════════════════════════════════════════════
        // 往返验证：正问题 → 逆问题 应还原一致
        // ═══════════════════════════════════════════════

        [Fact]
        public void Roundtrip_ComputeRange_Then_CalculateAngle_ReturnsSameAngle()
        {
            float v0 = 500f, hDiff = 100f;
            float originalAngle = 15f * (float)System.Math.PI / 180f;

            // 正问题：给定角度，计算射程
            var (range, _) = Kinematics.ComputeParabolicRange(v0, originalAngle, hDiff);

            // 逆问题：给定射程，反算角度
            float recoveredAngle = Kinematics.CalculateLaunchAngle(range, v0, hDiff);

            float diffDeg = System.Math.Abs(originalAngle - recoveredAngle) * 180f / (float)System.Math.PI;
            Assert.True(diffDeg < 0.1f,
                $"原始={originalAngle * 180 / System.Math.PI:F3}° 还原={recoveredAngle * 180 / System.Math.PI:F3}° 差={diffDeg:F4}°");
        }

        [Fact]
        public void Roundtrip_CalculateAngle_Then_ComputeRange_ReturnsSameRange()
        {
            // 用平地场景：低弹道解在平地也走下行段，往返一致
            float R = 5000f, v0 = 300f, hDiff = 0f;

            float angle = Kinematics.CalculateLaunchAngle(R, v0, hDiff);
            var (recoveredRange, _) = Kinematics.ComputeParabolicRange(v0, angle, hDiff);

            Assert.True(System.Math.Abs(recoveredRange - R) < 1f,
                $"原始 R={R:F1} 还原 R={recoveredRange:F1}");
        }

        [Fact]
        public void Roundtrip_DepressedTarget_ReturnsSameRange()
        {
            // 俯射往返：v=80, Δy=-500, R=286（空基典型值）
            float R = 286f, v0 = 80f, hDiff = -500f;
            float angle = Kinematics.CalculateLaunchAngle(R, v0, hDiff);
            Assert.True(angle < 0, $"俯射角度为负，实际={angle * 180 / Math.PI:F2}°");
            var (r, _) = Kinematics.ComputeParabolicRange(v0, angle, hDiff);
            Assert.True(System.Math.Abs(r - R) < 1f, $"{R}→angle→range={r:F0}，应={R}");
        }

        [Fact]
        public void Roundtrip_ElevatedTarget_ReturnsSameRange()
        {
            // 高靶：低弹道在上升段命中，往返应一致
            float R = 5000f, v0 = 800f, hDiff = 500f;
            float angle = Kinematics.CalculateLaunchAngle(R, v0, hDiff);
            var (r, _) = Kinematics.ComputeParabolicRange(v0, angle, hDiff);
            Assert.True(System.Math.Abs(r - R) < 5f, $"{R}→angle→range={r:F0}，应={R}");
        }

        [Fact]
        public void ComputeParabolicRange_Then_ParabolicPosition_IsOnTrajectory()
        {
            // 验证整条轨迹：多时间点采样，确保 ParabolicPosition 与 ComputeParabolicRange 一致
            float v0 = 400f;
            float angle = 25f * (float)System.Math.PI / 180f;
            float cosA = (float)System.Math.Cos(angle);
            float sinA = (float)System.Math.Sin(angle);
            float g = 9.81f;

            var (totalRange, totalTime) = Kinematics.ComputeParabolicRange(v0, angle, 0f);

            // 在 10 个等间隔时间点采样
            for (int i = 0; i <= 10; i++)
            {
                float t = totalTime * i / 10f;
                var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, t);

                float expectedX = 0;
                float expectedZ = v0 * cosA * t;
                float expectedY = v0 * sinA * t - 0.5f * g * t * t;

                Assert.True(System.Math.Abs(x - expectedX) < 0.1f, $"t={t:F2}: X");
                Assert.True(System.Math.Abs(z - expectedZ) < 0.1f, $"t={t:F2}: Z");
                Assert.True(System.Math.Abs(y - expectedY) < 0.1f, $"t={t:F2}: Y");
            }
        }

        [Fact]
        public void PointInPolygon_Inside_ReturnsTrue()
        {
            var vertices = new (float X, float Z)[] { (0, 0), (10, 0), (10, 10), (0, 10) };
            Assert.True(Kinematics.PointInPolygon(5, 5, vertices));
        }

        [Fact]
        public void PointInPolygon_Outside_ReturnsFalse()
        {
            var vertices = new (float X, float Z)[] { (0, 0), (10, 0), (10, 10), (0, 10) };
            Assert.False(Kinematics.PointInPolygon(20, 5, vertices));
        }
        [Fact]
        public void PathInSphere_CenterPass_ReturnsFullDiameter()
        {
            // 无人机穿过球心: 路径长度 = 2R
            float len = DamageAssessment.PathInSphere(0, 0, 0, 40, 0, 0, 20, 0, 0, 20);
            Assert.Equal(40, len, 1);
        }

        [Fact]
        public void PathInSphere_EdgePass_ReturnsCorrectChord()
        {
            // 从(0)→(40), 球心(30), R=20: 进入t=0.25(x=10), 段尾在球内(t=1,x=40), 路径=30m
            float len = DamageAssessment.PathInSphere(0, 0, 0, 40, 0, 0, 30, 0, 0, 20);
            Assert.True(System.Math.Abs(len - 30f) < 1f, $"len={len:F2}");
        }

        [Fact]
        public void PathInSphere_Miss_ReturnsZero()
        {
            float len = DamageAssessment.PathInSphere(0, 0, 0, 10, 0, 0, 50, 0, 0, 20);
            Assert.Equal(0, len, 1);
        }

        [Fact]
        public void PathInSphere_PartialEntry_ReturnsCorrectSegment()
        {
            // 从球外(0)到球内(20), R=20, 球心(30): 进入点在10, 段长=10
            float len = DamageAssessment.PathInSphere(0, 0, 0, 20, 0, 0, 30, 0, 0, 20);
            Assert.True(System.Math.Abs(len - 10f) < 1f, $"len={len:F2}");
        }
        [Fact]
        public void PathInSphere_TwoClouds_AccumulatesCorrectly()
        {
            // 一段经过两个云: (0→100), 云1(20,R20), 云2(80,R20)
            float len1 = DamageAssessment.PathInSphere(0, 0, 0, 100, 0, 0, 20, 0, 0, 20);
            float len2 = DamageAssessment.PathInSphere(0, 0, 0, 100, 0, 0, 80, 0, 0, 20);
            Assert.True(System.Math.Abs(len1 - 40f) < 1f, $"cloud1 len={len1:F2}");
            Assert.True(System.Math.Abs(len2 - 40f) < 1f, $"cloud2 len={len2:F2}");
        }
        [Fact]
        public void PathInSphere_AdjacentClouds_SumCorrectly()
        {
            // 无人机从云1中心(0)到云2中心(40)，两云R=20，间距=40（边碰边）
            // 两个云在段内路径：云1=20m(中心→右边缘), 云2=20m(左边缘→中心)
            float len1 = DamageAssessment.PathInSphere(0, 0, 0, 40, 0, 0, 0, 0, 0, 20);
            float len2 = DamageAssessment.PathInSphere(0, 0, 0, 40, 0, 0, 40, 0, 0, 20);
            float total = len1 + len2;
            Assert.True(System.Math.Abs(total - 40f) < 1f, $"total={total:F2} (cloud1={len1:F2} + cloud2={len2:F2})");
        }
    }
}