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

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

namespace CounterDrone.Core.Tests
{
    public class RouteGeometryTests
    {
        private static List<Waypoint> Line(float x0, float z0, float x1, float z1, float y = 500f)
            => new()
            {
                new Waypoint { PosX = x0, PosY = y, PosZ = z0 },
                new Waypoint { PosX = x1, PosY = y, PosZ = z1 },
            };

        private static List<Waypoint> LShape()
            => new()
            {
                new Waypoint { PosX = 0,   PosY = 500, PosZ = 0   },   // 起点
                new Waypoint { PosX = 100, PosY = 500, PosZ = 0   },   // 拐点（X 段 100m）
                new Waypoint { PosX = 100, PosY = 500, PosZ = 100 },   // 终点（Z 段 100m）
            };

        // ═══════════════════════════════════════
        // TotalLength
        // ═══════════════════════════════════════

        [Fact]
        public void TotalLength_StraightLine()
        {
            var wps = Line(0, 0, 3000, 4000);  // 3-4-5 → 5000m
            Assert.Equal(5000f, RouteGeometry.TotalLength(wps), 1);
        }

        [Fact]
        public void TotalLength_LShape()
        {
            var wps = LShape();  // 100 + 100 = 200m
            Assert.Equal(200f, RouteGeometry.TotalLength(wps), 1);
        }

        [Fact]
        public void TotalLength_SingleWaypoint_Zero()
        {
            var wps = new List<Waypoint> { new Waypoint { PosX = 5, PosY = 5, PosZ = 5 } };
            Assert.Equal(0f, RouteGeometry.TotalLength(wps));
        }

        // ═══════════════════════════════════════
        // PositionAt
        // ═══════════════════════════════════════

        [Fact]
        public void PositionAt_Start_ReturnsFirstWaypoint()
        {
            var wps = Line(0, 0, 1000, 0);
            var (x, y, z) = RouteGeometry.PositionAt(wps, 0f);
            Assert.Equal(0f, x, 1);
            Assert.Equal(0f, z, 1);
        }

        [Fact]
        public void PositionAt_Midpoint_LinearInterp()
        {
            var wps = Line(0, 0, 1000, 0);
            var (x, _, _) = RouteGeometry.PositionAt(wps, 500f);  // 中点
            Assert.Equal(500f, x, 1);
        }

        [Fact]
        public void PositionAt_LShape_Corner()
        {
            var wps = LShape();  // 0→(100,0)→(100,100)
            // 弧长 100 = 拐点
            var (x, _, z) = RouteGeometry.PositionAt(wps, 100f);
            Assert.Equal(100f, x, 1);
            Assert.Equal(0f, z, 1);
            // 弧长 150 = Z 段中点
            var (x2, _, z2) = RouteGeometry.PositionAt(wps, 150f);
            Assert.Equal(100f, x2, 1);
            Assert.Equal(50f, z2, 1);
        }

        [Fact]
        public void PositionAt_BeyondEnd_ReturnsLastWaypoint()
        {
            var wps = Line(0, 0, 1000, 0);
            var (x, _, _) = RouteGeometry.PositionAt(wps, 99999f);
            Assert.Equal(1000f, x, 1);
        }

        // ═══════════════════════════════════════
        // ArcLengthNearestTo
        // ═══════════════════════════════════════

        [Fact]
        public void ArcLengthNearestTo_PointOnLine()
        {
            var wps = Line(0, 0, 1000, 0);  // X 轴上
            // 点 (300, 0) 在航路上，弧长应=300
            Assert.Equal(300f, RouteGeometry.ArcLengthNearestTo(wps, 300f, 0f), 1);
        }

        [Fact]
        public void ArcLengthNearestTo_PointOffLine_Projects()
        {
            var wps = Line(0, 0, 1000, 0);  // X 轴
            // 点 (300, 50) 投影回 X 轴弧长=300
            Assert.Equal(300f, RouteGeometry.ArcLengthNearestTo(wps, 300f, 50f), 1);
        }

        [Fact]
        public void ArcLengthNearestTo_LShape_CorrectSegment()
        {
            var wps = LShape();  // 0→(100,0)→(100,100)
            // 点 (50, 0) 在第一段，弧长=50
            Assert.Equal(50f, RouteGeometry.ArcLengthNearestTo(wps, 50f, 0f), 1);
            // 点 (100, 50) 在第二段（X 段 100 + Z 段 50），弧长=150
            Assert.Equal(150f, RouteGeometry.ArcLengthNearestTo(wps, 100f, 50f), 1);
        }

        // ═══════════════════════════════════════
        // TravelTimeTo
        // ═══════════════════════════════════════

        [Fact]
        public void TravelTimeTo_UniformSpeed()
        {
            // 弧长 1000m，速度 36km/h=10m/s → 100s
            Assert.Equal(100f, RouteGeometry.TravelTimeTo(Line(0, 0, 1000, 0), 1000f, 36f), 1);
        }

        [Fact]
        public void TravelTimeTo_ZeroSpeed_Throws()
        {
            Assert.Throws<System.ArgumentException>(
                () => RouteGeometry.TravelTimeTo(Line(0, 0, 1000, 0), 1000f, 0f));
        }

        // ═══════════════════════════════════════
        // TangentAt
        // ═══════════════════════════════════════

        [Fact]
        public void TangentAt_XDirection_IsUnitX()
        {
            var wps = Line(0, 0, 1000, 0);  // +X 方向
            var (dx, dz) = RouteGeometry.TangentAt(wps, 500f);
            Assert.Equal(1f, dx, 3);
            Assert.Equal(0f, dz, 3);
        }

        [Fact]
        public void TangentAt_ZDirection_IsUnitZ()
        {
            var wps = Line(0, 0, 0, 1000);  // +Z 方向
            var (dx, dz) = RouteGeometry.TangentAt(wps, 500f);
            Assert.Equal(0f, dx, 3);
            Assert.Equal(1f, dz, 3);
        }

        [Fact]
        public void TangentAt_LShape_CornerTransition()
        {
            var wps = LShape();
            // 第一段（弧长 50）：+X
            var (dx1, dz1) = RouteGeometry.TangentAt(wps, 50f);
            Assert.Equal(1f, dx1, 3);
            Assert.Equal(0f, dz1, 3);
            // 第二段（弧长 150）：+Z
            var (dx2, dz2) = RouteGeometry.TangentAt(wps, 150f);
            Assert.Equal(0f, dx2, 3);
            Assert.Equal(1f, dz2, 3);
        }

        [Fact]
        public void TangentAt_AlwaysUnitLength()
        {
            // 任意方向直线，切向量必须是单位向量
            var wps = Line(0, 0, 3000, 4000);  // 3-4-5 方向
            var (dx, dz) = RouteGeometry.TangentAt(wps, 100f);
            float mag = (float)System.Math.Sqrt(dx * dx + dz * dz);
            Assert.Equal(1f, mag, 3);
        }
    }
}