using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace EMPlannerVerificationHost;

internal static class CoordinatorChecks
{
    public static void Run()
    {
        VerifiesCallerSuppliedSchedulingDecision();
        VerifiesCompletedCycleDoesNotPoisonNextCancellationSource();
        VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
        VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics();
        VerifiesSafePreviousTrajectoryHandoffs();
    }

    public static void RunRollingEndToEnd()
    {
        VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
        VerifiesSafePreviousTrajectoryHandoffs();
        VerifiesPublishedForwardAndReverseCommands();
        VerifiesFailureLeavesPublishedZeroSpeedTailExecutable();
    }

    private static void VerifiesCallerSuppliedSchedulingDecision()
    {
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);
        DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(100d);
        PlanningCycleInput input = CreateInput(CreateMap(1), "schedule-reference", 7L, "prior", 0, "schedule", start);

        Verification.True(coordinator.ShouldStartCycle(start), "first cycle is due");
        Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
        service.WaitUntilStarted("schedule");
        service.Complete("schedule");
        Verification.Equal(EmPlanningStatus.Success, cycle.GetAwaiter().GetResult().Result.Status, "schedule setup result");

        Verification.True(!coordinator.ShouldStartCycle(start.AddSeconds(0.199d)), "cycle remains deferred before period");
        Verification.True(coordinator.ShouldStartCycle(start.AddSeconds(0.20d)), "cycle becomes due at exact period");
    }

    private static void VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults()
    {
        PlanningGridMap firstMap = CreateMap(10);
        PlanningGridMap secondMap = CreateMap(11);
        DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(200d);

        VerifySuperseded("later-version", CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-A", now),
            CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-B", now.AddSeconds(0.20d)));
        VerifySuperseded("map", CreateInput(firstMap, "reference", 7L, "prior", 0, "map-A", now),
            CreateInput(secondMap, "reference", 7L, "prior", 0, "map-B", now.AddSeconds(0.20d)));
        VerifySuperseded("reference", CreateInput(firstMap, "reference-A", 7L, "prior", 0, "reference-A", now),
            CreateInput(firstMap, "reference-B", 7L, "prior", 0, "reference-B", now.AddSeconds(0.20d)));
        VerifySuperseded("state", CreateInput(firstMap, "reference", 7L, "prior", 0, "state-A", now),
            CreateInput(firstMap, "reference", 8L, "prior", 0, "state-B", now.AddSeconds(0.20d)));
        VerifySuperseded("segment", CreateInput(firstMap, "reference", 7L, "prior", 0, "segment-A", now),
            CreateInput(firstMap, "reference", 7L, "prior", 1, "segment-B", now.AddSeconds(0.20d)));
        VerifySuperseded("previous", CreateInput(firstMap, "reference", 7L, "prior-A", 0, "previous-A", now),
            CreateInput(firstMap, "reference", 7L, "prior-B", 0, "previous-B", now.AddSeconds(0.20d)));
    }

    private static void VerifiesCompletedCycleDoesNotPoisonNextCancellationSource()
    {
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);
        DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(150d);

        PlanningCycleInput first = CreateInput(CreateMap(5), "completed-reference", 4L, "prior", 0,
            "completed-first", now);
        Task<PlanningCycleResult> firstCycle = coordinator.PlanLatestAsync(first, CancellationToken.None);
        service.WaitUntilStarted(first.Request.OutputTrajectoryId);
        service.Complete(first.Request.OutputTrajectoryId);
        Verification.Equal(EmPlanningStatus.Success, firstCycle.GetAwaiter().GetResult().Result.Status,
            "first completed cycle succeeds");

        PlanningCycleInput second = CreateInput(CreateMap(6), "completed-reference", 5L, "completed-first", 0,
            "completed-second", now.AddSeconds(0.20d));
        Task<PlanningCycleResult> secondCycle = coordinator.PlanLatestAsync(second, CancellationToken.None);
        service.WaitUntilStarted(second.Request.OutputTrajectoryId);
        service.Complete(second.Request.OutputTrajectoryId);

        PlanningCycleResult result = secondCycle.GetAwaiter().GetResult();
        Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "next cycle after completion succeeds");
        Verification.True(result.Published, "next cycle after completion publishes");
    }

    private static void VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics()
    {
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service, new ThrowingCycleSink());
        PlanningCycleInput input = CreateInput(CreateMap(20), "sink-reference", 9L, "prior", 0, "sink", DateTimeOffset.UnixEpoch);

        Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
        service.WaitUntilStarted("sink");
        service.Complete("sink");
        PlanningCycleResult result = cycle.GetAwaiter().GetResult();

        Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "sink failure preserves planning result");
        Verification.True(result.Diagnostic.IndexOf("cycle sink", StringComparison.OrdinalIgnoreCase) >= 0,
            "sink failure is reported in diagnostic");
    }

    private static void VerifiesSafePreviousTrajectoryHandoffs()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(300d);
        EmPlannerConfiguration configuration = CreateHandoffConfiguration();
        var selector = new TrajectoryHandoffSelector();
        EmTrajectory forward = CreateHandoffTrajectory("handoff-forward", TravelDirection.Forward, 2, effectiveAt,
            EmBoundaryType.RollingSafetyStop, false);
        VehicleMotionState forwardMeasured = CreateMeasuredState(0.01d, 0d, 3.1666666666666665d, 0.10d,
            effectiveAt.AddSeconds(0.10d), 40L);

        TrajectoryHandoffSelection accepted = selector.Select(forward, forwardMeasured, 2, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.10d), configuration);
        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, accepted.Source, "forward handoff source");
        Verification.Equal(TrajectoryHandoffRejectionReason.None, accepted.RejectionReason, "forward handoff reason");
        Verification.True(object.ReferenceEquals(forward, accepted.PreviousTrajectory), "forward handoff seed");
        Verification.NearlyEqual(0.04d, accepted.StartState.Pose.X, "forward handoff position interpolation");
        Verification.NearlyEqual(3.3666666666666667d, accepted.StartState.Pose.Heading,
            "forward handoff yaw remains unwrapped");
        Verification.NearlyEqual(0.10d, accepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
            "forward handoff signed speed interpolation");
        Verification.Equal(40L, accepted.StartState.SequenceId, "handoff retains measured-state identity");

        EmTrajectory reverse = CreateHandoffTrajectory("handoff-reverse", TravelDirection.Reverse, 2, effectiveAt,
            EmBoundaryType.RollingSafetyStop, false);
        VehicleMotionState reverseMeasured = CreateMeasuredState(-0.01d, 0d, 3.1666666666666665d, -0.10d,
            effectiveAt.AddSeconds(0.10d), 41L);
        TrajectoryHandoffSelection reverseAccepted = selector.Select(reverse, reverseMeasured, 2, TravelDirection.Reverse,
            effectiveAt.AddSeconds(0.10d), configuration);
        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, reverseAccepted.Source,
            "reverse same-segment handoff source");
        Verification.NearlyEqual(-0.04d, reverseAccepted.StartState.Pose.X, "reverse handoff position interpolation");
        Verification.NearlyEqual(-0.10d, reverseAccepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
            "reverse handoff signed speed interpolation");

        AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.21d), configuration, TrajectoryHandoffRejectionReason.TrajectoryTooOld,
            "stale trajectory handoff");
        configuration.Scheduling.MaximumVehicleStateAgeSeconds = 0.60d;
        AssertHandoffRejected(selector, forward,
            CreateMeasuredState(1d, 0d, 3.1666666666666665d, 0.10d, effectiveAt.AddSeconds(0.10d), 42L), 2,
            TravelDirection.Forward, effectiveAt.AddSeconds(0.10d), configuration,
            TrajectoryHandoffRejectionReason.TrackingErrorExceeded, "large tracking-error handoff");
        VehicleMotionState terminalMeasured = CreateMeasuredState(0.03d, 0d, 3.30d, 0.10d,
            effectiveAt.AddSeconds(0.30d), 43L);
        AssertHandoffRejected(selector, forward, terminalMeasured, 2, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.30d), configuration, TrajectoryHandoffRejectionReason.TerminalBoundary,
            "terminal-proximity handoff");
        AssertHandoffRejected(selector, forward, forwardMeasured, 3, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.SegmentMismatch,
            "segment-mismatch handoff");
        AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Reverse,
            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.DirectionMismatch,
            "direction-mismatch handoff");
        VehicleMotionState beyondMeasured = CreateMeasuredState(0.031d, 0d, 3.3066666666666666d, 0.10d,
            effectiveAt.AddSeconds(0.31d), 44L);
        AssertHandoffRejected(selector, forward, beyondMeasured, 2, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.31d), configuration, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory,
            "beyond-trajectory handoff");

        EmTrajectory gearBoundary = CreateHandoffTrajectory("handoff-gear", TravelDirection.Forward, 2, effectiveAt,
            EmBoundaryType.RollingSafetyStop, true);
        AssertHandoffRejected(selector, gearBoundary, forwardMeasured, 2, TravelDirection.Forward,
            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.GearBoundary,
            "gear-boundary handoff");
        Verification.True(!new TrajectorySampler().TrySample(gearBoundary, 0.15d, out _),
            "sampler never interpolates across different boundary types");

        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);
        PlanningCycleInput coordinatorInput = CreateInput(CreateMap(22), "handoff-reference", 40L, "handoff-forward",
            2, "handoff-publish", effectiveAt.AddSeconds(0.10d), configuration, forwardMeasured);
        Task<PlanningCycleResult> publication = coordinator.PlanLatestAsync(coordinatorInput, CancellationToken.None);
        service.WaitUntilStarted(coordinatorInput.Request.OutputTrajectoryId);
        service.Complete(coordinatorInput.Request.OutputTrajectoryId, forward);
        publication.GetAwaiter().GetResult();
        TrajectoryHandoffSelection coordinatorSelection = coordinator.SelectHandoff(coordinatorInput, TravelDirection.Forward);
        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, coordinatorSelection.Source,
            "coordinator consumes only its published immutable trajectory");
    }

    private static void VerifiesPublishedForwardAndReverseCommands()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(800d);
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);
        var executor = new TrajectoryExecutor();

        PlanningCycleInput forwardInput = CreateInput(CreateMap(30), "rolling-reference", 90L, string.Empty, 6,
            "rolling-forward", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 90L));
        EmTrajectory forward = CreateExecutionTrajectory("rolling-forward", TravelDirection.Forward, effectiveAt,
            EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
        PlanningCycleResult forwardResult = Publish(service, coordinator, forwardInput, forward);
        Verification.True(forwardResult.Published, "first forward rolling cycle publishes");
        TrajectoryControlCommand forwardCommand = executor.UpdateCommand(effectiveAt,
            forwardInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Forward,
            TravelDirection.Forward, false);
        AssertPublishedMotion(forwardCommand, coordinator.PublishedTrajectory.Points[0], "forward rolling command");

        PlanningCycleInput forwardRepeat = CreateInput(CreateMap(31), "rolling-reference", 91L, "rolling-forward", 6,
            "rolling-forward-repeat", effectiveAt.AddSeconds(0.20d), null,
            CreateMeasuredState(0.02d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.20d), 91L));
        EmTrajectory repeatedForward = CreateExecutionTrajectory("rolling-forward-repeat", TravelDirection.Forward,
            effectiveAt.AddSeconds(0.20d), EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
        Publish(service, coordinator, forwardRepeat, repeatedForward);
        Verification.Equal("rolling-forward-repeat", coordinator.PublishedTrajectory.Metadata.TrajectoryId,
            "repeated forward replan replaces only the published trajectory");

        PlanningCycleInput reverseInput = CreateInput(CreateMap(32), "rolling-reference", 92L,
            "rolling-forward-repeat", 7, "rolling-reverse", effectiveAt.AddSeconds(0.40d), null,
            CreateMeasuredState(0d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.40d), 92L));
        EmTrajectory reverse = CreateExecutionTrajectory("rolling-reverse", TravelDirection.Reverse,
            effectiveAt.AddSeconds(0.40d), EmBoundaryType.Goal, EmTerminalType.Goal);
        PlanningCycleResult reverseResult = Publish(service, coordinator, reverseInput, reverse);
        Verification.True(reverseResult.Published, "reverse rolling cycle publishes");
        TrajectoryControlCommand reverseCommand = executor.UpdateCommand(effectiveAt.AddSeconds(0.40d),
            reverseInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Reverse,
            TravelDirection.Reverse, false);
        AssertPublishedMotion(reverseCommand, coordinator.PublishedTrajectory.Points[0], "reverse rolling command");
        Verification.True(reverseCommand.SignedLongitudinalVelocity < 0d,
            "reverse rolling command preserves negative longitudinal velocity");

        PlanningCycleInput unsafeTracking = CreateInput(CreateMap(32), "rolling-reference", 93L, "rolling-reverse", 7,
            "unsafe-tracking", effectiveAt.AddSeconds(0.41d), CreateHandoffConfiguration(),
            CreateMeasuredState(5d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.41d), 93L));
        TrajectoryHandoffSelection reset = coordinator.SelectHandoff(unsafeTracking, TravelDirection.Reverse);
        Verification.Equal(TrajectoryHandoffSource.MeasuredState, reset.Source,
            "unsafe tracking resets rolling handoff to caller measurement");
        Verification.True(object.ReferenceEquals(unsafeTracking.Request.VehicleState, reset.StartState),
            "unsafe tracking preserves the supplied measurement snapshot");
    }

    private static void VerifiesFailureLeavesPublishedZeroSpeedTailExecutable()
    {
        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(900d);
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);
        var executor = new TrajectoryExecutor();
        PlanningCycleInput successfulInput = CreateInput(CreateMap(40), "tail-reference", 100L, string.Empty, 8,
            "tail-published", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 100L));
        EmTrajectory published = CreateExecutionTrajectory("tail-published", TravelDirection.Forward, effectiveAt,
            EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
        Publish(service, coordinator, successfulInput, published);

        for (int attempt = 1; attempt <= 2; attempt++)
        {
            DateTimeOffset now = effectiveAt.AddSeconds(0.20d * attempt);
            PlanningCycleInput failedInput = CreateInput(CreateMap(40 + attempt), "tail-reference", 100L + attempt,
                "tail-published", 8, "tail-failed-" + attempt, now, null,
                CreateMeasuredState(0.02d, 0d, 0d, 0.10d, now, 100L + attempt));
            Task<PlanningCycleResult> failedCycle = coordinator.PlanLatestAsync(failedInput, CancellationToken.None);
            service.WaitUntilStarted(failedInput.Request.OutputTrajectoryId);
            service.Fail(failedInput.Request.OutputTrajectoryId);
            PlanningCycleResult failure = failedCycle.GetAwaiter().GetResult();
            Verification.Equal(EmPlanningStatus.Failed, failure.Result.Status, "failed replan status " + attempt);
            Verification.True(!failure.Published, "failed replan cannot publish " + attempt);
            Verification.True(object.ReferenceEquals(published, coordinator.PublishedTrajectory),
                "failed replan retains the complete prior trajectory " + attempt);
        }

        TrajectoryControlCommand beforeTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.10d),
            CreateMeasuredState(0.10d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.10d), 103L),
            coordinator.PublishedTrajectory, TravelDirection.Forward, TravelDirection.Forward, false);
        Verification.True(beforeTail.SignedLongitudinalVelocity > 0d,
            "old trajectory remains executable before its safety tail");

        TrajectoryControlCommand atTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
            CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(0.30d), 104L), coordinator.PublishedTrajectory,
            TravelDirection.Forward, TravelDirection.Forward, false);
        TrajectoryControlCommand afterTail = executor.UpdateCommand(effectiveAt.AddSeconds(10d),
            CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(10d), 105L), coordinator.PublishedTrajectory,
            TravelDirection.Forward, TravelDirection.Forward, false);
        AssertZeroTail(atTail, "exact rolling safety-stop terminal");
        AssertZeroTail(afterTail, "after rolling safety-stop terminal");
    }

    private static PlanningCycleResult Publish(ControlledPlanningService service, EmPlanningCoordinator coordinator,
        PlanningCycleInput input, EmTrajectory trajectory)
    {
        Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
        service.WaitUntilStarted(input.Request.OutputTrajectoryId);
        service.Complete(input.Request.OutputTrajectoryId, trajectory);
        return cycle.GetAwaiter().GetResult();
    }

    private static EmTrajectory CreateExecutionTrajectory(string trajectoryId, TravelDirection direction,
        DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, EmTerminalType terminalType)
    {
        double signedVelocity = direction == TravelDirection.Forward ? 0.10d : -0.10d;
        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L,
            string.Empty, 8, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop
                ? EmLongitudinalMode.RollingContinuation
                : EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction,
                EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(signedVelocity * 3d, 0d, 0d, 0d, 0.30d, 0.20d, 8, 0.03d, 0.03d, direction,
                terminalBoundary, 0d, 0d),
        });
    }

    private static void AssertPublishedMotion(TrajectoryControlCommand command, EmTrajectoryPoint point, string name)
    {
        Verification.NearlyEqual(point.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
            name + " originates from the currently published trajectory");
        Verification.NearlyEqual(point.YawRate, command.YawRate,
            name + " preserves the currently published trajectory yaw rate");
        Verification.True(!command.HoldBrake && !command.IsTrajectoryComplete,
            name + " is neither a brake hold nor an extrapolated completion command");
    }

    private static void AssertZeroTail(TrajectoryControlCommand command, string name)
    {
        Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity, name + " signed velocity");
        Verification.NearlyEqual(0d, command.YawRate, name + " yaw rate");
        Verification.True(command.HoldBrake && command.IsTrajectoryComplete, name + " holds a completed trajectory");
    }

    private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory,
        VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now,
        EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name)
    {
        TrajectoryHandoffSelection selection = selector.Select(trajectory, measuredState, segmentIndex, direction, now,
            configuration);
        Verification.Equal(TrajectoryHandoffSource.MeasuredState, selection.Source, name + " source");
        Verification.Equal(reason, selection.RejectionReason, name + " reason");
        Verification.True(selection.PreviousTrajectory == null, name + " has no seed");
        Verification.True(object.ReferenceEquals(measuredState, selection.StartState), name + " returns measured state");
    }

    private static EmPlannerConfiguration CreateHandoffConfiguration()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Validation.SpatialToleranceMeters = 0.05d;
        configuration.Validation.KinematicTolerance = 0.05d;
        return configuration;
    }

    private static VehicleMotionState CreateMeasuredState(double x, double y, double yaw, double signedSpeed,
        DateTimeOffset capturedAt, long sequenceId)
    {
        return new VehicleMotionState(new Pose2D(x, y, yaw), signedSpeed, 0d, capturedAt, sequenceId);
    }

    private static EmTrajectory CreateHandoffTrajectory(string trajectoryId, TravelDirection direction, int segmentIndex,
        DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, bool includeGearBoundary)
    {
        double sign = direction == TravelDirection.Forward ? 1d : -1d;
        EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None;
        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L,
            string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.01d, 0d, 3.1666666666666665d, sign * 0.10d, 0.10d, 0.25d,
                segmentIndex, 0.01d, 0.01d, direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.03d, 0d, 3.30d, sign * 0.10d, 0.30d, 0.25d, segmentIndex, 0.03d,
                0.03d, direction, middleBoundary, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.055d, 0d, 3.4666666666666668d, sign * 0.10d, 0.55d, 0.25d,
                segmentIndex, 0.055d, 0.055d, direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.06d, 0d, 3.50d, 0d, 0.60d, 0.25d, segmentIndex, 0.06d, 0.06d,
                direction, terminalBoundary, 0d, 0d),
        });
    }

    private static void VerifySuperseded(string name, PlanningCycleInput older, PlanningCycleInput newer)
    {
        var service = new ControlledPlanningService();
        var coordinator = new EmPlanningCoordinator(service);

        Task<PlanningCycleResult> slow = coordinator.PlanLatestAsync(older, CancellationToken.None);
        service.WaitUntilStarted(older.Request.OutputTrajectoryId);
        Task<PlanningCycleResult> current = coordinator.PlanLatestAsync(newer, CancellationToken.None);
        service.WaitUntilStarted(newer.Request.OutputTrajectoryId);

        Verification.True(service.WasCancelled(older.Request.OutputTrajectoryId), name + " cancels older cycle token");
        service.Complete(newer.Request.OutputTrajectoryId);
        PlanningCycleResult currentResult = current.GetAwaiter().GetResult();
        service.Complete(older.Request.OutputTrajectoryId);
        PlanningCycleResult staleResult = slow.GetAwaiter().GetResult();

        Verification.Equal(EmPlanningStatus.Success, currentResult.Result.Status, name + " current result succeeds");
        Verification.True(currentResult.Published, name + " current result publishes");
        Verification.Equal(EmPlanningStatus.Superseded, staleResult.Result.Status, name + " old result is superseded");
        Verification.True(staleResult.Result.Trajectory == null, name + " stale result exposes no trajectory");
        Verification.Equal(newer.Request.OutputTrajectoryId, coordinator.PublishedTrajectory.Metadata.TrajectoryId,
            name + " only latest trajectory is published");
    }

    private static PlanningCycleInput CreateInput(PlanningGridMap map, string referencePathId, long stateSequenceId,
        string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now,
        EmPlannerConfiguration? configuration = null, VehicleMotionState? state = null)
    {
        configuration ??= EmPlannerConfiguration.CreateDefault();
        state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
        var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now,
            outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse,
            EmPlanningScope.RollingHorizon);
        return new PlanningCycleInput(request, now);
    }

    private static PlanningGridMap CreateMap(int widthOffset)
    {
        PlanningMapBuildResult build = new PlanningMapFactory().Create(new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(-1000f, 1000f + widthOffset * 20f, -1000f, 1000f),
            ResolutionMm = 20f,
            ObstacleSources = Array.Empty<IMapObstacleSource>(),
            AllowExplicitEmptyMap = true,
        });
        Verification.True(build.Succeeded && build.Map != null && build.Map.PlanningReady, "coordinator map builds");
        return build.Map!;
    }

    private sealed class ControlledPlanningService : IEmPlanningService
    {
        private readonly object gate = new object();
        private readonly Dictionary<string, PendingCycle> pending = new Dictionary<string, PendingCycle>();

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            var cycle = new PendingCycle(request, cancellationToken);
            lock (gate)
            {
                pending.Add(request.OutputTrajectoryId, cycle);
                Monitor.PulseAll(gate);
            }
            return cycle.Completion.Task.GetAwaiter().GetResult();
        }

        public void WaitUntilStarted(string outputTrajectoryId)
        {
            DateTimeOffset timeout = DateTimeOffset.UtcNow.AddSeconds(5d);
            lock (gate)
            {
                while (!pending.ContainsKey(outputTrajectoryId))
                {
                    TimeSpan remaining = timeout - DateTimeOffset.UtcNow;
                    if (remaining <= TimeSpan.Zero || !Monitor.Wait(gate, remaining))
                        throw new InvalidOperationException("Planner cycle did not start: " + outputTrajectoryId);
                }
            }
        }

        public bool WasCancelled(string outputTrajectoryId)
        {
            lock (gate)
                return pending[outputTrajectoryId].CancellationToken.IsCancellationRequested;
        }

        public void Complete(string outputTrajectoryId, EmTrajectory? trajectory = null)
        {
            PendingCycle cycle;
            lock (gate)
                cycle = pending[outputTrajectoryId];
            EmPlanningResult result = trajectory == null
                ? CreateSuccess(cycle.Request)
                : new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
            cycle.Completion.TrySetResult(result);
        }

        public void Fail(string outputTrajectoryId)
        {
            PendingCycle cycle;
            lock (gate)
                cycle = pending[outputTrajectoryId];
            cycle.Completion.TrySetResult(new EmPlanningResult(EmPlanningStatus.Failed, null, "scripted rolling failure"));
        }

        private static EmPlanningResult CreateSuccess(EmPlanningRequest request)
        {
            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
                EmLongitudinalMode.RollingContinuation, request.PlanningScope);
            var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d,
                TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d);
            return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty);
        }

        private sealed class PendingCycle
        {
            public PendingCycle(EmPlanningRequest request, CancellationToken cancellationToken)
            {
                Request = request;
                CancellationToken = cancellationToken;
                Completion = new TaskCompletionSource<EmPlanningResult>(TaskCreationOptions.RunContinuationsAsynchronously);
            }

            public EmPlanningRequest Request { get; }
            public CancellationToken CancellationToken { get; }
            public TaskCompletionSource<EmPlanningResult> Completion { get; }
        }
    }

    private sealed class ThrowingCycleSink : IEmPlanningCycleSink
    {
        public void OnCycleCompleted(PlanningCycleResult result)
        {
            throw new InvalidOperationException("cycle sink failure");
        }
    }
}
