ParkingRobot/MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs

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

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

        public static void Run()
        {
            VerifyLeaderYawExample();
            VerifyTailToTailUsesEquivalentAxis();
            VerifyAllMembersMustBeReady();
            VerifyStaleStatusIsIgnored();
            VerifyMemberFaultIsLatched();
            VerifyFaultAfterAuthorizationIsLatched();
            VerifyPrematureActiveIsRejected();
            VerifyCancelClearsPlan();

            Console.WriteLine(
                "FleetPreparationCoordinator测试通过。共8个场景。");
        }

        private static void VerifyLeaderYawExample()
        {
            var capture = FleetLayoutCapture.Capture(
                new[]
                {
                    new FleetMemberPose(
                        1,
                        new Pose2D(
                            -1.0,
                            0.0,
                            AngleMath.DegreesToRadians(20.0))),
                    new FleetMemberPose(
                        2,
                        new Pose2D(1.0, 0.0, 0.0))
                },
                leaderVehicleId: 1);
            var coordinator =
                new FleetPreparationCoordinator();

            coordinator.StartRollingPreparation(
                planId: 1,
                capture.Layout,
                motionDirectionInFleetRadians: 0.0);

            AssertTargetDegrees(coordinator, 1, 0.0);
            AssertTargetDegrees(coordinator, 2, 20.0);
        }

        private static void VerifyTailToTailUsesEquivalentAxis()
        {
            var coordinator =
                new FleetPreparationCoordinator();
            coordinator.StartRollingPreparation(
                planId: 2,
                CreateTailToTailLayout(),
                motionDirectionInFleetRadians: 0.0);

            AssertTargetDegrees(coordinator, 1, 0.0);
            AssertTargetDegrees(coordinator, 2, 0.0);
        }

        private static void VerifyAllMembersMustBeReady()
        {
            var coordinator = CreateStartedCoordinator(3);

            AssertState(
                coordinator.ReportMemberStatus(
                    CreateStatus(
                        3,
                        1,
                        FleetMemberAgentState.Ready)),
                FleetPreparationCoordinatorState
                    .WaitingForMembers,
                "仅一辆车Ready");
            AssertState(
                coordinator.ReportMemberStatus(
                    CreateStatus(
                        3,
                        2,
                        FleetMemberAgentState.Ready)),
                FleetPreparationCoordinatorState
                    .ReadyToActivate,
                "全部成员Ready");
            AssertState(
                coordinator.ReportMemberStatus(
                    CreateStatus(
                        3,
                        1,
                        FleetMemberAgentState.Preparing)),
                FleetPreparationCoordinatorState
                    .WaitingForMembers,
                "成员失去Ready");
            AssertState(
                coordinator.ReportMemberStatus(
                    CreateStatus(
                        3,
                        1,
                        FleetMemberAgentState.Ready)),
                FleetPreparationCoordinatorState
                    .ReadyToActivate,
                "成员重新Ready");

            if (coordinator.TryAuthorizeActivation(4))
            {
                throw new InvalidOperationException(
                    "错误任务编号不应获得激活授权。");
            }

            if (!coordinator.TryAuthorizeActivation(3))
            {
                throw new InvalidOperationException(
                    "全部成员Ready后没有获得激活授权。");
            }

            AssertState(
                coordinator.State,
                FleetPreparationCoordinatorState
                    .ActivationAuthorized,
                "统一激活授权");
        }

        private static void VerifyStaleStatusIsIgnored()
        {
            var coordinator = CreateStartedCoordinator(5);
            var state = coordinator.ReportMemberStatus(
                CreateStatus(
                    4,
                    1,
                    FleetMemberAgentState.Ready));

            AssertState(
                state,
                FleetPreparationCoordinatorState
                    .WaitingForMembers,
                "旧任务状态报告");
        }

        private static void VerifyMemberFaultIsLatched()
        {
            var coordinator = CreateStartedCoordinator(6);
            var state = coordinator.ReportMemberStatus(
                CreateStatus(
                    6,
                    2,
                    FleetMemberAgentState.Faulted,
                    "舵轮未到位"));

            AssertState(
                state,
                FleetPreparationCoordinatorState.Faulted,
                "成员准备故障");
            if (string.IsNullOrWhiteSpace(
                    coordinator.LastFailureReason))
            {
                throw new InvalidOperationException(
                    "成员准备故障没有保存原因。");
            }
        }

        private static void VerifyFaultAfterAuthorizationIsLatched()
        {
            var coordinator = CreateStartedCoordinator(9);
            coordinator.ReportMemberStatus(
                CreateStatus(
                    9,
                    1,
                    FleetMemberAgentState.Ready));
            coordinator.ReportMemberStatus(
                CreateStatus(
                    9,
                    2,
                    FleetMemberAgentState.Ready));
            coordinator.TryAuthorizeActivation(9);

            var state = coordinator.ReportMemberStatus(
                CreateStatus(
                    9,
                    2,
                    FleetMemberAgentState.Faulted,
                    "激活失败"));

            AssertState(
                state,
                FleetPreparationCoordinatorState.Faulted,
                "授权后的成员故障");
        }

        private static void VerifyPrematureActiveIsRejected()
        {
            var coordinator = CreateStartedCoordinator(7);
            var state = coordinator.ReportMemberStatus(
                CreateStatus(
                    7,
                    1,
                    FleetMemberAgentState.Active));

            AssertState(
                state,
                FleetPreparationCoordinatorState.Faulted,
                "成员提前运动");
        }

        private static void VerifyCancelClearsPlan()
        {
            var coordinator = CreateStartedCoordinator(8);
            coordinator.Cancel();

            AssertState(
                coordinator.State,
                FleetPreparationCoordinatorState.Idle,
                "取消准备任务");
            if (coordinator.CurrentPlanId != 0 ||
                coordinator.Targets.Count != 0)
            {
                throw new InvalidOperationException(
                    "取消后没有清除准备任务数据。");
            }
        }

        private static FleetPreparationCoordinator
            CreateStartedCoordinator(long planId)
        {
            var coordinator =
                new FleetPreparationCoordinator();
            coordinator.StartRollingPreparation(
                planId,
                CreateTailToTailLayout(),
                motionDirectionInFleetRadians: 0.0);
            return coordinator;
        }

        private static FleetLayout CreateTailToTailLayout()
        {
            return new FleetLayout(
                new[]
                {
                    new VehicleLayout(
                        1,
                        new Pose2D(-1.0, 0.0, 0.0)),
                    new VehicleLayout(
                        2,
                        new Pose2D(1.0, 0.0, Math.PI))
                });
        }

        private static FleetMemberPreparationStatus CreateStatus(
            long planId,
            int vehicleId,
            FleetMemberAgentState state,
            string failureReason = "")
        {
            return new FleetMemberPreparationStatus(
                planId,
                vehicleId,
                state,
                failureReason);
        }

        private static void AssertTargetDegrees(
            FleetPreparationCoordinator coordinator,
            int vehicleId,
            double expectedDegrees)
        {
            if (!coordinator.TryGetTarget(
                    vehicleId,
                    out var target))
            {
                throw new InvalidOperationException(
                    $"没有找到车辆{vehicleId}的准备目标。");
            }

            AssertNear(
                AngleMath.RadiansToDegrees(
                    target.MotionDirectionInBodyRadians),
                expectedDegrees,
                $"车辆{vehicleId}本地β");
        }

        private static void AssertState(
            FleetPreparationCoordinatorState actual,
            FleetPreparationCoordinatorState expected,
            string scenario)
        {
            if (actual != expected)
            {
                throw new InvalidOperationException(
                    $"{scenario}状态错误：" +
                    $"actual={actual}, expected={expected}。");
            }
        }

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