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 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));
        }
    }
}