ParkingRobot/MultiWheelC.Tests/FleetLayoutCaptureTests.cs

using System;
using MultiWheelC.Fleet;
using MyParking.Shared;

namespace MultiWheelC.Tests
{
    internal static class FleetLayoutCaptureTests
    {
        private const double Tolerance = 1e-9;

        public static void Run()
        {
            VerifySymmetricTailToTailLayout();
            VerifyAsymmetricLayoutAndPoseReconstruction();
            VerifyInputOrderDoesNotChangeResult();
            VerifyEmptyInputIsRejected();
            VerifyDuplicateVehicleIdIsRejected();
            VerifyMissingLeaderIsRejected();

            Console.WriteLine(
                "FleetLayoutCapture布局建立测试通过。共6个场景。");
        }

        private static void VerifySymmetricTailToTailLayout()
        {
            var result = FleetLayoutCapture.Capture(
                new[]
                {
                    new FleetMemberPose(
                        1,
                        new Pose2D(-1.2, 0.0, 0.0)),
                    new FleetMemberPose(
                        2,
                        new Pose2D(1.2, 0.0, Math.PI))
                },
                leaderVehicleId: 1);

            AssertPose(
                result.FleetPoseInWorld,
                Pose2D.Identity,
                "对称双车中心");
            AssertVehicleLayout(
                result.Layout,
                1,
                new Pose2D(-1.2, 0.0, 0.0),
                "对称双车主车布局");
            AssertVehicleLayout(
                result.Layout,
                2,
                new Pose2D(1.2, 0.0, Math.PI),
                "对称双车从车布局");
        }

        private static void VerifyAsymmetricLayoutAndPoseReconstruction()
        {
            var members = new[]
            {
                new FleetMemberPose(
                    3,
                    new Pose2D(1.0, 1.0, 0.4)),
                new FleetMemberPose(
                    1,
                    new Pose2D(4.0, 1.0, 0.4)),
                new FleetMemberPose(
                    2,
                    new Pose2D(1.0, 4.0, -0.8))
            };
            var result = FleetLayoutCapture.Capture(
                members,
                leaderVehicleId: 1);

            AssertPose(
                result.FleetPoseInWorld,
                new Pose2D(2.0, 2.0, 0.4),
                "非对称三车中心");

            for (var index = 0;
                 index < members.Length;
                 index++)
            {
                if (!result.Layout.TryGetVehicle(
                        members[index].VehicleId,
                        out var vehicleLayout))
                {
                    throw new InvalidOperationException(
                        "非对称布局缺少成员车。" +
                        members[index].VehicleId);
                }

                var reconstructedPoseInWorld =
                    FrameTransform2D.Compose(
                        result.FleetPoseInWorld,
                        vehicleLayout.PoseInFleet);
                AssertPose(
                    reconstructedPoseInWorld,
                    members[index].PoseInWorld,
                    "非对称布局世界位姿还原");
            }
        }

        private static void VerifyInputOrderDoesNotChangeResult()
        {
            var first = FleetLayoutCapture.Capture(
                new[]
                {
                    new FleetMemberPose(
                        1,
                        new Pose2D(2.0, 3.0, 0.6)),
                    new FleetMemberPose(
                        2,
                        new Pose2D(4.0, 5.0, -1.0))
                },
                leaderVehicleId: 1);
            var second = FleetLayoutCapture.Capture(
                new[]
                {
                    new FleetMemberPose(
                        2,
                        new Pose2D(4.0, 5.0, -1.0)),
                    new FleetMemberPose(
                        1,
                        new Pose2D(2.0, 3.0, 0.6))
                },
                leaderVehicleId: 1);

            AssertPose(
                first.FleetPoseInWorld,
                second.FleetPoseInWorld,
                "输入顺序不变中心");
            AssertSameLayout(first.Layout, second.Layout);
        }

        private static void VerifyEmptyInputIsRejected()
        {
            ExpectException<ArgumentException>(
                () => FleetLayoutCapture.Capture(
                    Array.Empty<FleetMemberPose>(),
                    leaderVehicleId: 1),
                "空成员集合");
        }

        private static void VerifyDuplicateVehicleIdIsRejected()
        {
            ExpectException<ArgumentException>(
                () => FleetLayoutCapture.Capture(
                    new[]
                    {
                        new FleetMemberPose(
                            1,
                            Pose2D.Identity),
                        new FleetMemberPose(
                            1,
                            new Pose2D(1.0, 0.0, 0.0))
                    },
                    leaderVehicleId: 1),
                "重复车号");
        }

        private static void VerifyMissingLeaderIsRejected()
        {
            ExpectException<ArgumentException>(
                () => FleetLayoutCapture.Capture(
                    new[]
                    {
                        new FleetMemberPose(
                            2,
                            Pose2D.Identity)
                    },
                    leaderVehicleId: 1),
                "缺少主车");
        }

        private static void AssertSameLayout(
            FleetLayout first,
            FleetLayout second)
        {
            if (first.VehicleCount != second.VehicleCount)
            {
                throw new InvalidOperationException(
                    "输入顺序变化后成员数量发生变化。");
            }

            for (var index = 0;
                 index < first.Vehicles.Count;
                 index++)
            {
                var vehicle = first.Vehicles[index];
                AssertVehicleLayout(
                    second,
                    vehicle.VehicleId,
                    vehicle.PoseInFleet,
                    "输入顺序不变布局");
            }
        }

        private static void AssertVehicleLayout(
            FleetLayout layout,
            int vehicleId,
            Pose2D expectedPoseInFleet,
            string scenario)
        {
            if (!layout.TryGetVehicle(
                    vehicleId,
                    out var vehicle))
            {
                throw new InvalidOperationException(
                    $"{scenario}缺少车辆{vehicleId}。");
            }

            AssertPose(
                vehicle.PoseInFleet,
                expectedPoseInFleet,
                scenario);
        }

        private static void AssertPose(
            Pose2D actual,
            Pose2D expected,
            string scenario)
        {
            AssertNear(
                actual.XMeters,
                expected.XMeters,
                scenario + " X");
            AssertNear(
                actual.YMeters,
                expected.YMeters,
                scenario + " Y");
            AssertNear(
                AngleMath.NormalizeRadians(
                    actual.YawRadians -
                    expected.YawRadians),
                0.0,
                scenario + " Yaw");
        }

        private static void AssertNear(
            double actual,
            double expected,
            string valueName)
        {
            if (Math.Abs(actual - expected) > Tolerance)
            {
                throw new InvalidOperationException(
                    $"{valueName}错误：" +
                    $"actual={actual:F9}, expected={expected:F9}。");
            }
        }

        private static void ExpectException<TException>(
            Action action,
            string scenario)
            where TException : Exception
        {
            try
            {
                action();
            }
            catch (TException)
            {
                return;
            }

            throw new InvalidOperationException(
                $"{scenario}没有抛出{typeof(TException).Name}。");
        }
    }
}