using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using TrajectoryPlanningVisualization;

namespace EMPlannerVerificationHost;

internal static class TrajectoryObservationChecks
{
    public static void Run()
    {
        TrajectoryObservationSettingsChecks.Run();
        TrajectoryObservationSegmentChecks.Run();
        VerifiesTrajectoryObservationReportWriterCreatesTxt();
        VerifiesPainterPoseGeometryReplacesLargeArrow();
        TrajectoryObservationVisualizationChecks.Run();
        VerifiesObservationSourceHasNoActuatorCalls();
        VerifiesObservationSourceUsesRequiredOperatorText();
        VerifiesOperatorDocumentationUsesExactUiEntry();
        VerifiesMovementTestVehicleInputsAndSettingsSnapshot();
        VerifiesMovementTestKeepsNativePainterLazyAndConditional();
        VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration();
        RejectsInvalidObservationSettings();
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
        VerifiesValidNonEmptyObstacleSource();
        RejectsObstacleOutsideConfiguredBounds();
        VerifiesObserverTicksWhilePlanningIsDelayed();
        VerifiesFullDirectionSegmentPlansOncePerActiveSegment();
        VerifiesFullDirectionRequestCarriesValidatedScope();
        VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation();
        VerifiesDeterministicSingleForwardObservationSimulation();
        VerifiesFailedFullPlanDoesNotAutoRetry();
        VerifiesStaticSnapshotExportsPlanningScope();
        VerifiesSessionLayerCleanupDecisions();
        VerifiesGearSwitchWaitStateForWorldPresentation();
        VerifiesLsAndStUsePublishedTrajectoryData();
        VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
        VerifiesPlanningDiagnosticsKeepRawFailureReason();
        VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
        VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk();
        VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
        VerifiesPainterStAxesUsePathSAndTimeUnits();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
        VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories();
        VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed();
        FreezesBootstrapVehicleForRollingRequests();
        VerifiesEmptyMapBootstrapProducesPublishableReference();
    }

    private static void VerifiesObservationSourceHasNoActuatorCalls()
    {
        string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest");
        string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs");
        Verification.True(File.Exists(movementTestSource), "observation MovementTest source exists");

        string[] forbiddenTokens =
        {
            ".SendXYThSpeed(", ".SendMotion(", ".SendTh(", ".AccumulateSpeed(",
            ".ComputeWheelsGeometrically(", ".DriveStop(", ".PredefinedDriveStop("
        };
        string[] runtimeSources = Directory.GetFiles(sourceDirectory, "*.cs", SearchOption.TopDirectoryOnly);
        for (int sourceIndex = 0; sourceIndex < runtimeSources.Length; sourceIndex++)
        {
            string source = File.ReadAllText(runtimeSources[sourceIndex]);
            for (int tokenIndex = 0; tokenIndex < forbiddenTokens.Length; tokenIndex++)
            {
                Verification.True(source.IndexOf(forbiddenTokens[tokenIndex], StringComparison.Ordinal) < 0,
                    "observation runtime source excludes actuator token " + forbiddenTokens[tokenIndex]);
            }
        }
    }

    private static void VerifiesTrajectoryObservationReportWriterCreatesTxt()
    {
        string reportDirectory = Path.Combine(Path.GetTempPath(),
            "em-trajectory-report-" + Guid.NewGuid().ToString("N"));
        try
        {
            var writer = new TrajectoryObservationReportWriter(17L, reportDirectory);
            writer.Append("cycle=1 status=SolverTimedOut primal=140.918");
            string reportPath = writer.Save();

            Verification.True(File.Exists(reportPath), "trajectory observation txt report exists");
            string text = File.ReadAllText(reportPath);
            Verification.True(text.Contains("session=17"), "report includes session identity");
            Verification.True(text.Contains("OBSERVE_ONLY"), "report records observation-only boundary");
            Verification.True(text.Contains("SolverTimedOut"), "report includes cycle failure");
        }
        finally
        {
            if (Directory.Exists(reportDirectory))
                Directory.Delete(reportDirectory, true);
        }
    }

    private static void VerifiesObservationSourceUsesRequiredOperatorText()
    {
        string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));

        Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
            "observation MovementTest uses required Chinese display name");
        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段",
            TrajectoryObservationRuntimeState.GearSwitchWaitingNotice,
            "observation runtime uses required gear-switch notice");
        Verification.True(source.Contains("Console.WriteLine(\"[TrajectoryObserver] \" + text);"),
            "observer status is mirrored to the host terminal");
    }

    private static void VerifiesOperatorDocumentationUsesExactUiEntry()
    {
        string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest");
        string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs");
        string readmePath = Path.Combine(sourceDirectory, "README.md");
        string movementTestText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
        string readmeText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(readmePath));
        const string uiEntry = "EM轨迹规划观察闭环测试";

        Verification.True(movementTestText.Contains("[MovementTest(name = \"" + uiEntry + "\")]"),
            "observation MovementTest has the exact UI entry");
        Verification.True(readmeText.Contains(uiEntry),
            "observation README has the exact UI entry");
    }

    private static void VerifiesMovementTestVehicleInputsAndSettingsSnapshot()
    {
        string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
        Verification.True(source.Contains("public double VehicleLengthMeters = 0.80d;"),
            "observer MovementTest exposes vehicle length with Task-1 default");
        Verification.True(source.Contains("public double VehicleWidthMeters = 0.60d;"),
            "observer MovementTest exposes vehicle width with Task-1 default");
        Verification.True(source.Contains("public double SafetyMarginMeters = 0.05d;"),
            "observer MovementTest exposes safety margin with Task-1 default");
        Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"),
            "observer MovementTest exposes maximum curvature with Task-1 default");
        Verification.True(source.Contains("public double SolverTimeoutSeconds ="),
            "observer MovementTest exposes a configurable solver timeout");
        Verification.True(source.Contains("public int MaximumOsqpIterations ="),
            "observer MovementTest exposes a configurable OSQP iteration limit");
        Verification.True(source.Contains("public double TimeHorizonSeconds ="),
            "observer MovementTest exposes a configurable ST time horizon");
        Verification.True(source.Contains("public double OutputTimeStepSeconds ="),
            "observer MovementTest exposes a configurable ST timestamp spacing");
        Verification.True(source.Contains("不等于观察循环周期"),
            "observer MovementTest documents the distinction between output timestamps and observer ticks");
        Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"),
            "observer MovementTest copies vehicle length into settings");
        Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"),
            "observer MovementTest copies vehicle width into settings");
        Verification.True(source.Contains("SafetyMarginMeters = SafetyMarginMeters,"),
            "observer MovementTest copies safety margin into settings");
        Verification.True(source.Contains("MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,"),
            "observer MovementTest copies maximum curvature into settings");
        Verification.True(source.Contains("SolverTimeoutSeconds = SolverTimeoutSeconds,"),
            "observer MovementTest copies solver timeout into settings");
        Verification.True(source.Contains("MaximumOsqpIterations = MaximumOsqpIterations,"),
            "observer MovementTest copies OSQP iteration limit into settings");
        Verification.True(source.Contains("TimeHorizonSeconds = TimeHorizonSeconds,"),
            "observer MovementTest copies ST time horizon into settings");
        Verification.True(source.Contains("OutputTimeStepSeconds = OutputTimeStepSeconds,"),
            "observer MovementTest copies ST timestamp spacing into settings");
        string normalizedSource = source.Replace("\r\n", "\n");
        Verification.True(normalizedSource.Contains(
                "VehicleMotionState state = ReadVehicleState();\n            DateTimeOffset now = state.CapturedAtUtc;"),
            "observer host uses the fresh state snapshot time for each observation tick");
        Verification.True(source.Contains("effectiveConfiguration.Frenet.MaximumProjectionDistanceMeters"),
            "native observation charts use the EM Frenet projection tolerance rather than map padding");
        Verification.True(source.Contains("tick.SegmentState.Phase == TrajectoryObservationSegmentPhase.Completed"),
            "observer session terminates after the final direction segment completes");
        Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
            "observer settings use the ST timestamp-spacing default");

        var configured = new TrajectoryObservationSettings
        {
            VehicleLengthMeters = 1.10d,
            VehicleWidthMeters = 0.70d,
            SafetyMarginMeters = 0.08d,
            MaximumCurvaturePerMeter = 0.55d,
            SolverTimeoutSeconds = 0.42d,
            MaximumOsqpIterations = 9000,
            TimeHorizonSeconds = 4d,
            OutputTimeStepSeconds = 0.20d,
        };
        TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot();
        configured.VehicleLengthMeters = 9.10d;
        configured.VehicleWidthMeters = 9.20d;
        configured.SafetyMarginMeters = 9.30d;
        configured.MaximumCurvaturePerMeter = 9.40d;
        configured.SolverTimeoutSeconds = 9.50d;
        configured.MaximumOsqpIterations = 9500;
        configured.TimeHorizonSeconds = 5d;
        configured.OutputTimeStepSeconds = 0.25d;

        Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters,
            "observer settings snapshot freezes vehicle length");
        Verification.NearlyEqual(0.70d, snapshot.VehicleWidthMeters,
            "observer settings snapshot freezes vehicle width");
        Verification.NearlyEqual(0.08d, snapshot.SafetyMarginMeters,
            "observer settings snapshot freezes safety margin");
        Verification.NearlyEqual(0.55d, snapshot.MaximumCurvaturePerMeter,
            "observer settings snapshot freezes maximum curvature");
        Verification.NearlyEqual(0.42d, snapshot.SolverTimeoutSeconds,
            "observer settings snapshot freezes solver timeout");
        Verification.Equal(9000, snapshot.MaximumOsqpIterations,
            "observer settings snapshot freezes OSQP iteration limit");
        Verification.NearlyEqual(4d, snapshot.TimeHorizonSeconds,
            "observer settings snapshot freezes ST time horizon");
        Verification.NearlyEqual(0.20d, snapshot.OutputTimeStepSeconds,
            "observer settings snapshot freezes ST timestamp spacing");

        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        AssertVehicleSnapshot(job.Vehicle, 1.10d, 0.70d, 0.08d, 0.55d, null,
            "observer configured bootstrap vehicle");
    }

    private static void VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            ReplanPeriodSeconds = 0.25d,
            SolverTimeoutSeconds = 1.25d,
            MaximumOsqpIterations = 54321,
            TimeHorizonSeconds = 3.5d,
            OutputTimeStepSeconds = 0.10d,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer configuration-diagnostic bootstrap succeeds");
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic");

        string text = controller.CreateConfigurationDiagnostic().Text;
        MethodInfo effectiveConfigurationMethod = typeof(TrajectoryObservationController).GetMethod(
            "CreateEffectiveConfigurationSnapshot", BindingFlags.Instance | BindingFlags.NonPublic)!;
        var effectiveConfiguration = (EmPlannerConfiguration)effectiveConfigurationMethod.Invoke(
            controller, Array.Empty<object>())!;
        string distanceHorizonText = "distanceHorizon=" +
            effectiveConfiguration.Scheduling.DistanceHorizonMeters.ToString("F2", CultureInfo.InvariantCulture) + "m";
        foreach (string expected in new[]
        {
            "planning configuration:", "timeHorizon=3.50s", distanceHorizonText, "outputTimeStep=0.10s",
            "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321",
            "solverTimeout=1.25s", "replanPeriod=0.25s", "maximumJerkLimitedStopDistance=",
            "maximumJerkLimitedStopDuration=", "requiredDistanceHorizon=",
        })
        {
            Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected);
        }

        string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
        string runnerSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
        int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal);
        Verification.True(firstCall >= 0, "runner prints configuration diagnostic");
        Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1,
            StringComparison.Ordinal), "runner prints configuration diagnostic once");
    }

    private static void RejectsInvalidObservationSettings()
    {
        AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding");
        AssertInvalidSetting(settings => settings.MapResolutionMillimeters = float.NaN, "map resolution");
        AssertInvalidSetting(settings => settings.ReplanPeriodSeconds = 0d, "replan period");
        AssertInvalidSetting(settings => settings.ObserverPeriodSeconds = double.PositiveInfinity,
            "observer period");
        AssertInvalidSetting(settings => settings.VehicleLengthMeters = 0d, "vehicle length");
        AssertInvalidSetting(settings => settings.VehicleWidthMeters = double.NaN, "vehicle width");
        AssertInvalidSetting(settings => settings.SafetyMarginMeters = 0d, "safety margin");
        AssertInvalidSetting(settings => settings.MaximumCurvaturePerMeter = double.PositiveInfinity,
            "maximum curvature");
        AssertInvalidSetting(settings => settings.SolverTimeoutSeconds = 0d, "solver timeout");
        AssertInvalidSetting(settings => settings.MaximumOsqpIterations = 0, "OSQP iteration limit");
        AssertInvalidSetting(settings => settings.MaximumOsqpIterations = -1, "negative OSQP iteration limit");
        AssertInvalidSetting(settings => settings.TimeHorizonSeconds = 0d, "ST time horizon");
        AssertInvalidSetting(settings => settings.OutputTimeStepSeconds = 0d, "ST timestamp spacing");
        AssertInvalidSetting(settings =>
        {
            settings.TimeHorizonSeconds = 1d;
            settings.OutputTimeStepSeconds = 1.01d;
        }, "ST timestamp spacing larger than the horizon");
    }

    private static void AssertInvalidSetting(Action<TrajectoryObservationSettings> mutate, string name)
    {
        var settings = new TrajectoryObservationSettings();
        mutate(settings);
        Verification.True(Throws(settings.Validate), "observer rejects invalid " + name);
    }

    private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
    {
        var settings = new TrajectoryObservationSettings
        {
            MapPaddingMeters = 2d,
            MapResolutionMillimeters = 50f,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 17L);

        Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
        Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
        Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
        Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
        Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
    }

    private static void VerifiesValidNonEmptyObstacleSource()
    {
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
            new[] { TrajectoryObservationObstacle.Circle(500d, 500d, 100d) }, 23L);

        Verification.True(!job.MapRequest.AllowExplicitEmptyMap,
            "observer non-empty obstacle map is not explicitly empty");
        Verification.Equal(1, job.MapRequest.ObstacleSources.Count,
            "observer non-empty map has exactly one obstacle source");
        IMapObstacleSource source = job.MapRequest.ObstacleSources[0];
        Verification.Equal("trajectory-observer-manual", source.SourceId,
            "observer manual obstacle source ID");
        Verification.Equal(23L, source.SourceVersion,
            "observer manual obstacle source version");
        Verification.True(source.IsRequired, "observer manual obstacle source is required");
        ObstacleProjectionResult projection = source.ProjectToWorld();
        Verification.Equal(ObstacleSourceStatus.Applied, projection.Status,
            "observer valid manual obstacle source applies");
        Verification.Equal(1, projection.Obstacles.Count,
            "observer valid manual obstacle source preserves geometry");
    }

    private static void RejectsObstacleOutsideConfiguredBounds()
    {
        Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
            new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)),
            "observer obstacle outside configured bounds");
    }

    private static void VerifiesObserverTicksWhilePlanningIsDelayed()
    {
        DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 4, 3, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer delayed-planner bootstrap succeeds");

        EmTrajectory published = CreatePublishedTrajectory(startedAt);
        var planningService = new DelayedTrajectoryPlanningService(published, 2);
        try
        {
            var controller = new TrajectoryObservationController(
                bootstrap, settings, planningService, "cadence-check");
            var loop = new TrajectoryObservationLoop(controller);
            var firstState = new VehicleMotionState(
                new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 10L);

            TrajectoryObservationLoopTick firstTick = loop.Tick(startedAt, firstState, CancellationToken.None);

            Verification.True(firstTick.PlanningStarted, "observer first tick reports a planning-cycle start");
            Verification.True(!firstTick.PlanningCompleted, "observer first tick has no completed cycle");
            Verification.True(firstTick.ShouldLog,
                "observer first cadence tick is eligible for session-guarded logging");
            Verification.Equal(10L, firstTick.Observation.VehicleState.SequenceId,
                "observer first tick uses first fresh state");

            TrajectoryObservationLoopTick? initialPublicationTick = null;
            bool initiallyPublished = SpinWait.SpinUntil(() =>
            {
                var currentState = new VehicleMotionState(
                    new Pose2D(0.15d, 0d, 0d), 0.15d, null,
                    startedAt.AddSeconds(0.10d), 11L);
                initialPublicationTick = loop.Tick(startedAt.AddSeconds(0.10d), currentState,
                    CancellationToken.None);
                return !initialPublicationTick.PlanningInFlight;
            }, TimeSpan.FromSeconds(5d));
            Verification.True(initiallyPublished && initialPublicationTick != null &&
                    ReferenceEquals(published, initialPublicationTick.Observation.PublishedTrajectory),
                "observer establishes a published trajectory before delayed rolling planning");

            DateTimeOffset replanAt = startedAt.AddSeconds(settings.ReplanPeriodSeconds);
            var replanState = new VehicleMotionState(
                new Pose2D(0.20d, 0d, 0d), 0.20d, null, replanAt, 12L);
            TrajectoryObservationLoopTick replanTick = loop.Tick(replanAt, replanState,
                CancellationToken.None);
            Verification.True(replanTick.PlanningInFlight,
                "observer delayed rolling planning remains in flight after replan tick");
            Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)),
                "observer delayed planner enters planning service");

            DateTimeOffset secondAt = replanAt.AddSeconds(settings.ObserverPeriodSeconds);
            var secondState = new VehicleMotionState(
                new Pose2D(0.25d, 0d, 0d), 0.25d, null, secondAt, 13L);
            TrajectoryObservationLoopTick secondTick = loop.Tick(secondAt, secondState, CancellationToken.None);

            Verification.True(secondTick.PlanningInFlight,
                "observer second tick does not wait for delayed planning");
            Verification.True(secondTick.ShouldLog,
                "observer second cadence tick is eligible for session-guarded logging");
            Verification.True(ReferenceEquals(published, secondTick.Observation.PublishedTrajectory),
                "observer keeps observing the existing publication during delayed rolling planning");
            Verification.Equal(13L, secondTick.Observation.VehicleState.SequenceId,
                "observer second tick uses second fresh state");
            Verification.Equal(secondAt, secondTick.Observation.ObservedAtUtc,
                "observer second tick observes at its own time");

            planningService.Release();
            TrajectoryObservationLoopTick? completedTick = null;
            bool completed = SpinWait.SpinUntil(() =>
            {
                var currentState = new VehicleMotionState(
                    new Pose2D(0.30d, 0d, 0d), 0.30d, null,
                    replanAt.AddSeconds(0.10d), 14L);
                completedTick = loop.Tick(replanAt.AddSeconds(0.10d), currentState,
                    CancellationToken.None);
                return !completedTick.PlanningInFlight;
            }, TimeSpan.FromSeconds(5d));

            Verification.True(completed, "observer delayed planning completes deterministically");
            Verification.True(completedTick != null, "observer delayed planning produces a completion tick");
            TrajectoryObservationLoopTick finalTick = completedTick!;
            Verification.True(finalTick.PlanningCompleted, "observer completion tick reports cycle completion");
            Verification.True(finalTick.LatestCycle != null && finalTick.LatestCycle.Published,
                "observer delayed planning result is consumed without a continuation");
            Verification.Equal(14L, finalTick.Observation.VehicleState.SequenceId,
                "observer post-plan observation does not reuse pre-plan state");
            Verification.Equal(replanAt.AddSeconds(0.10d), finalTick.Observation.ObservedAtUtc,
                "observer post-plan observation does not reuse pre-plan time");
        }
        finally
        {
            planningService.Release();
        }
    }

    private static void VerifiesFullDirectionSegmentPlansOncePerActiveSegment()
    {
        DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 7, 0, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "one-shot bootstrap succeeds");

        EmTrajectory published = CreatePublishedTrajectory(startedAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "one-shot");
        var loop = new TrajectoryObservationLoop(controller);
        controller.StartCycle(startedAt,
            new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 1L),
            CancellationToken.None).GetAwaiter().GetResult();

        TrajectoryObservationLoopTick? lastTick = null;
        for (int index = 0; index < 20; index++)
        {
            DateTimeOffset now = startedAt.AddSeconds((index + 1) * settings.ObserverPeriodSeconds);
            lastTick = loop.Tick(now,
                new VehicleMotionState(new Pose2D(0.10d + index * 0.01d, 0d, 0d), 0.10d, null,
                    now, 2L + index), CancellationToken.None);
        }

        Verification.Equal(1, planningService.Requests.Count,
            "full direction segment is planned only once per active segment");
        Verification.True(lastTick != null && !lastTick.PlanningStarted,
            "no coordinator-cadence replan after the full segment plan");
        Verification.True(lastTick != null && lastTick.Observation != null &&
                ReferenceEquals(published, lastTick.Observation.PublishedTrajectory),
            "observation continues to publish the frozen full-segment trajectory");
    }

    private static void VerifiesFullDirectionRequestCarriesValidatedScope()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 7, 1, 0, 0, TimeSpan.Zero);
        var source = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.FullDirectionSegment,
        };
        TrajectoryObservationSettings snapshot = source.CreateValidatedSnapshot();
        source.PlanningScope = EmPlanningScope.RollingHorizon;

        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "scope propagation bootstrap succeeds");

        EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, snapshot, planningService, "scope-check");
        snapshot.PlanningScope = EmPlanningScope.RollingHorizon;
        var state = new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, effectiveAt, 1L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();

        Verification.Equal(EmPlanningScope.FullDirectionSegment, planningService.Requests[0].PlanningScope,
            "controller propagates the validated full-direction scope into EM requests");
        Verification.True(!controller.ShouldStartCycle(effectiveAt.AddSeconds(1d)),
            "full scope blocks coordinator-cadence restart after the first plan");
    }

    private static void VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 2, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            DirectionConfirmationSamples = 1,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(baseBootstrap.Succeeded, "one-shot transition bootstrap succeeds");
        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments());

        EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "one-shot-forward", 0,
            TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d);
        EmTrajectory reverseTrajectory = CreateSegmentTrajectory(t0.AddSeconds(1d), "one-shot-reverse", 1,
            TravelDirection.Reverse, EmTerminalType.Goal, EmBoundaryType.Goal, 0d);
        var planningService = new SequenceTrajectoryPlanningService(forwardGearTrajectory, reverseTrajectory,
            reverseTrajectory);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "one-shot-advance");
        var stoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L);
        controller.StartCycle(t0, stoppedState, CancellationToken.None).GetAwaiter().GetResult();

        controller.TryAdvanceSegment(t0, stoppedState);
        DateTimeOffset stoppedAt = t0.AddSeconds(0.21d);
        var heldStoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, stoppedAt, 2L);
        controller.TryAdvanceSegment(stoppedAt, heldStoppedState);
        Verification.True(controller.ShouldStartCycle(stoppedAt),
            "the next full direction segment is planned after the real stop hold, before direction evidence");

        PlanningCycleResult pendingCycle = controller.StartCycle(stoppedAt, heldStoppedState, CancellationToken.None)
            .GetAwaiter().GetResult();
        Verification.True(pendingCycle.Published, "the stopped-state pending segment plan is independently publishable");
        Verification.Equal(2, planningService.Requests.Count,
            "the next segment is planned exactly once from the stopped state");
        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
            "the stopped-state pending request targets segment N+1");
        Verification.Equal(heldStoppedState.SequenceId, planningService.Requests[1].VehicleState.SequenceId,
            "the pending segment request uses the real stopped vehicle state");
        Verification.True(planningService.Requests[1].PreviousTrajectory == null,
            "a pending direction segment never reuses the old direction trajectory as an EM seed");
        Verification.Equal(0, controller.ActiveSegment.SegmentIndex,
            "planning N+1 does not replace the old segment before direction confirmation");
        Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PublishedTrajectory),
            "the old segment trajectory remains active while N+1 waits for confirmation");

        var reverseState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null,
            t0.AddSeconds(0.22d), 3L);
        Verification.True(controller.TryAdvanceSegment(t0.AddSeconds(0.22d), reverseState),
            "confirmed N to N+1 transition succeeds");
        Verification.Equal(1, controller.ActiveSegment.SegmentIndex,
            "the pending plan becomes active only after direction confirmation");
        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(0.23d)),
            "confirmed activation does not trigger a duplicate full-segment plan");
        Verification.Equal(2, planningService.Requests.Count,
            "the pending plan is reused at activation instead of planning N+1 twice");
        Verification.Equal(reverseState.CapturedAtUtc, controller.PublishedTrajectory.Metadata.EffectiveAtUtc,
            "pending trajectory effective time is rebased to its real activation time");
    }

    private static void VerifiesDeterministicSingleForwardObservationSimulation()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 8, 0, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.FullDirectionSegment,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(baseBootstrap.Succeeded, "deterministic single-forward simulation bootstrap succeeds");
        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, new[]
            {
                CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false,
                    EmBoundaryType.None, EmBoundaryType.Goal),
            });
        EmTrajectory trajectory = CreateSingleForwardSimulationTrajectory(t0);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedTrajectoryPlanningService(trajectory), "single-forward-simulation");
        var loop = new TrajectoryObservationLoop(controller);

        double x = 0d;
        double heading = 0d;
        var initialState = new VehicleMotionState(new Pose2D(x, 0d, heading), 0d, null, t0, 1L);
        controller.StartCycle(t0, initialState, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryControlCommand command = controller.Observe(t0, initialState).Command;
        TrajectoryObservationLoopTick? finalTick = null;
        const double deltaSeconds = 0.10d;
        for (int step = 1; step <= 10; step++)
        {
            x += command.SignedLongitudinalVelocity * Math.Cos(heading) * deltaSeconds;
            heading += command.YawRate * deltaSeconds;
            DateTimeOffset now = t0.AddSeconds(step * deltaSeconds);
            var state = new VehicleMotionState(new Pose2D(x, 0d, heading), command.SignedLongitudinalVelocity,
                null, now, step + 1L);
            finalTick = loop.Tick(now, state, CancellationToken.None);
            command = finalTick.Observation.Command;
        }

        Verification.NearlyEqual(1d, x, "simulated forward vehicle reaches the real Goal pose from executor commands");
        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, finalTick!.SegmentState.Phase,
            "simulated forward Goal completes the observation lifecycle");
        Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity,
            "simulated forward Goal receives the terminal zero-speed command");
    }

    private static void VerifiesFailedFullPlanDoesNotAutoRetry()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 3, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "failed-plan bootstrap succeeds");

        var planningService = new FailingTrajectoryPlanningService();
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "failed-one-shot");
        var loop = new TrajectoryObservationLoop(controller);
        var state = new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, t0, 1L);
        loop.Tick(t0, state, CancellationToken.None);

        TrajectoryObservationLoopTick? failureTick = null;
        bool consumed = SpinWait.SpinUntil(() =>
        {
            DateTimeOffset now = t0.AddSeconds(0.05d);
            var current = new VehicleMotionState(new Pose2D(0.11d, 0d, 0d), 0.10d, null, now, 2L);
            failureTick = loop.Tick(now, current, CancellationToken.None);
            return failureTick.LatestCycle != null;
        }, TimeSpan.FromSeconds(5d));
        Verification.True(consumed, "failed full plan is consumed and visible");
        Verification.Equal(1, planningService.Requests.Count,
            "failed full plan makes exactly one attempt");
        Verification.True(failureTick != null && failureTick.LatestCycle != null &&
                !failureTick.LatestCycle.Published,
            "failed full plan remains visible as unpublished");

        TrajectoryObservationLoopTick? lastTick = null;
        for (int index = 0; index < 20; index++)
        {
            DateTimeOffset now = t0.AddSeconds(0.10d + (index + 1) * settings.ObserverPeriodSeconds);
            lastTick = loop.Tick(now,
                new VehicleMotionState(new Pose2D(0.10d + index * 0.01d, 0d, 0d), 0.10d, null,
                    now, 3L + index), CancellationToken.None);
        }
        Verification.Equal(1, planningService.Requests.Count,
            "failed full plan does not automatically roll into coordinator-cadence retries");
        Verification.True(lastTick != null && lastTick.Observation != null && !lastTick.PlanningStarted,
            "observation continues after a failed full plan without starting retries");
    }

    private static void VerifiesStaticSnapshotExportsPlanningScope()
    {
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "static scope snapshot bootstrap succeeds");

        PlanningVisualizationStaticSnapshot snapshot = new TrajectoryObservationStaticSnapshotBuilder().Build(
            bootstrap, EmPlannerConfiguration.CreateDefault(), settings.CreateValidatedSnapshot(), 0L);
        Verification.True(snapshot.ConfigurationGroups.SelectMany(x => x.Entries).Any(x =>
                x.RawName == "PlanningScope" && x.Value == "FullDirectionSegment"),
            "static snapshot exports the validated full-direction scope");
    }

    private static void VerifiesSessionLayerCleanupDecisions()
    {
        Verification.True(!TrajectoryObservationSessionLifecycle.ShouldClearLayers(
                TrajectoryObservationSessionEndReason.BootstrapFailure),
            "observer bootstrap failure preserves the diagnostic world view");
        Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
                TrajectoryObservationSessionEndReason.RuntimeFault),
            "observer runtime fault clears all painter layers");
        Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
                TrajectoryObservationSessionEndReason.Cancellation),
            "observer cancellation clears all painter layers");
    }

    private static void VerifiesGearSwitchWaitStateForWorldPresentation()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 4, 0, 0, TimeSpan.Zero);
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt, EmTerminalType.GearSwitch);

        TrajectoryObservationRuntimeState beforeFinal = TrajectoryObservationRuntimeState.Create(
            effectiveAt.AddSeconds(0.99d), trajectory);
        Verification.True(!beforeFinal.WaitingAtGearSwitch,
            "observer does not paint gear-switch wait state before final time");
        Verification.Equal(string.Empty, beforeFinal.WorldNotice,
            "observer has no gear-switch world notice before final time");

        TrajectoryObservationRuntimeState atFinal = TrajectoryObservationRuntimeState.Create(
            effectiveAt.AddSeconds(1d), trajectory);
        Verification.True(atFinal.WaitingAtGearSwitch,
            "observer enters gear-switch wait state at final time");
        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段", atFinal.WorldNotice,
            "observer exposes the exact gear-switch state to the world painter");
        Verification.Equal(0, trajectory.Metadata.SegmentIndex,
            "observer gear-switch wait state remains on segment zero");

        var tracker = new TrajectoryObservationSegmentTracker(CreateDirectionalSegments(),
            new TrajectoryObservationSettings(), 0.01d);
        TrajectoryObservationRuntimeState planningState = TrajectoryObservationRuntimeState.Create(tracker.State);
        Verification.True(!planningState.WaitingAtGearSwitch,
            "tracker-planning state does not show a gear-switch wait notice merely because planned time elapsed");
        tracker.Update(effectiveAt.AddSeconds(1d), new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
                effectiveAt.AddSeconds(1d), 1L),
            CreateSegmentTrajectory(effectiveAt, "runtime-forward", 0, TravelDirection.Forward,
                EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d));
        TrajectoryObservationRuntimeState stoppedState = TrajectoryObservationRuntimeState.Create(tracker.State);
        Verification.True(stoppedState.WaitingAtGearSwitch,
            "actual tracker stop state shows the gear-switch wait notice");

        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
        string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
        Verification.True(presentationSource.Contains(
                "worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice"),
            "observer world painter draws the exact runtime wait state");
    }

    private static void VerifiesLsAndStUsePublishedTrajectoryData()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
        DirectionSegmentView segment = CreateStraightSegment();
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);

        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
        Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
        Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
        Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
        Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
        Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
        Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
        Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
            "observer first signed speed");
        Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
            "observer second signed speed");
        Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
        Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
        Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
        Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");

        var settings = new TrajectoryObservationSettings
        {
            MaximumOsqpIterations = 9000,
            TimeHorizonSeconds = 4d,
            OutputTimeStepSeconds = 0.20d,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper(
            new CoarsePathPlanningService(), new PathSmoothingService())
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");

        var planningService = new FixedTrajectoryPlanningService(trajectory);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
        PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
            .GetAwaiter().GetResult();
        Verification.True(firstCycle.Published, "observer first cycle publishes");
        Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
        Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
            "observer first trajectory identity");
        Verification.NearlyEqual(settings.ReplanPeriodSeconds,
            planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
            "observer configured replan period");
        Verification.NearlyEqual(settings.SolverTimeoutSeconds,
            planningService.Requests[0].Configuration.Scheduling.SolverTimeoutSeconds,
            "observer configured solver timeout");
        Verification.Equal(settings.MaximumOsqpIterations,
            planningService.Requests[0].Configuration.Solver.MaximumOsqpIterations,
            "observer configured OSQP iteration limit");
        Verification.NearlyEqual(settings.TimeHorizonSeconds,
            planningService.Requests[0].Configuration.Scheduling.TimeHorizonSeconds,
            "observer configured ST time horizon");
        Verification.NearlyEqual(settings.OutputTimeStepSeconds,
            planningService.Requests[0].Configuration.Scheduling.OutputTimeStepSeconds,
            "observer configured ST timestamp spacing");

        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
        Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
            "observer executor interpolates from published effective time");
    }

    private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer rolling snapshot bootstrap succeeds");

        EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "snapshot-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 2L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
            .GetAwaiter().GetResult();

        EmPlanningRequest rollingRequest = planningService.Requests[1];
        Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory),
            "observer rolling request uses the controlled published trajectory object");
        Verification.Equal(rollingRequest.PreviousTrajectory.Metadata.TrajectoryId,
            rollingRequest.PreviousTrajectoryId,
            "observer rolling request trajectory object and ID use one publication snapshot");
    }

    private static void VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 4, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
            DirectionConfirmationSamples = 1,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(baseBootstrap.Succeeded, "active-segment controller bootstrap succeeds");

        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments());
        EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "active-forward", 0,
            TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d);
        EmTrajectory reverseTrajectory = CreateSegmentTrajectory(t0.AddSeconds(1d), "active-reverse", 1,
            TravelDirection.Reverse, EmTerminalType.Goal, EmBoundaryType.Goal, 0d);
        var planningService = new SequenceTrajectoryPlanningService(forwardGearTrajectory, reverseTrajectory,
            reverseTrajectory);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "active-segment");

        VehicleMotionState stopped = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L);
        controller.StartCycle(t0, stopped, CancellationToken.None).GetAwaiter().GetResult();
        Verification.Equal(0, planningService.Requests[0].SegmentIndex,
            "first active-segment request uses segment zero");
        Verification.Equal(0, controller.ActiveSegment.SegmentIndex, "controller exposes initial active segment");

        controller.TryAdvanceSegment(t0, stopped);
        controller.TryAdvanceSegment(t0.AddSeconds(0.21d), new VehicleMotionState(
            new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L));
        bool advanced = controller.TryAdvanceSegment(t0.AddSeconds(0.22d), new VehicleMotionState(
            new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 3L));

        Verification.True(advanced, "confirmed reverse direction advances the controller");
        Verification.Equal(1, controller.ActiveSegment.SegmentIndex, "controller uses tracker-confirmed segment");
        Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PreviousTrajectoryForVisualization),
            "old direction trajectory is retained only for visualization");

        DateTimeOffset reverseAt = t0.AddSeconds(1d);
        controller.StartCycle(reverseAt, new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null,
            reverseAt, 4L), CancellationToken.None).GetAwaiter().GetResult();
        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
            "next active-segment request uses segment one");
        Verification.True(planningService.Requests[1].PreviousTrajectory == null,
            "new direction does not reuse the old trajectory as an EM seed");
        Verification.Equal("active-segment-trajectory-2", planningService.Requests[1].OutputTrajectoryId,
            "session cycle IDs remain monotonic after coordinator replacement");

        controller.StartCycle(reverseAt.AddSeconds(settings.ReplanPeriodSeconds), new VehicleMotionState(
            new Pose2D(0.9d, 0d, 0d), -0.03d, null, reverseAt.AddSeconds(settings.ReplanPeriodSeconds), 5L),
            CancellationToken.None).GetAwaiter().GetResult();
        Verification.True(ReferenceEquals(reverseTrajectory, planningService.Requests[2].PreviousTrajectory),
            "same direction rolling request retains its published trajectory as the seed");
    }

    private static void VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 5, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
            DirectionConfirmationSamples = 1,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments());
        EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "loop-forward", 0,
            TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d);
        var planningService = new DelayedTrajectoryPlanningService(forwardGearTrajectory, 2);
        try
        {
            var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "loop-segment");
            controller.StartCycle(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L),
                CancellationToken.None).GetAwaiter().GetResult();
            var loop = new TrajectoryObservationLoop(controller);

            loop.Tick(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 2L),
                CancellationToken.None);
            TrajectoryObservationLoopTick planningTick = loop.Tick(t0.AddSeconds(0.21d),
                new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 3L),
                CancellationToken.None);
            Verification.True(planningTick.PlanningInFlight, "new cycle is in flight before direction evidence");
            Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)),
                "delayed cycle entered planning service");

            TrajectoryObservationLoopTick blockedTick = loop.Tick(t0.AddSeconds(0.22d),
                new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 4L),
                CancellationToken.None);
            Verification.True(!blockedTick.SegmentAdvanced, "in-flight planning prevents a segment transition");
            Verification.Equal(0, blockedTick.SegmentState.ActiveSegmentIndex,
                "in-flight planning retains the immutable old segment state");

            planningService.Release();
            TrajectoryObservationLoopTick? advancedTick = null;
            int completedSequence = 5;
            bool advanced = SpinWait.SpinUntil(() =>
            {
                DateTimeOffset now = t0.AddSeconds(0.23d + (completedSequence - 5) * 0.01d);
                advancedTick = loop.Tick(now, new VehicleMotionState(
                    new Pose2D(1d, 0d, 0d), -0.03d, null, now, completedSequence++), CancellationToken.None);
                return advancedTick.SegmentAdvanced;
            }, TimeSpan.FromSeconds(5d));
            Verification.True(advanced,
                "loop advances only after it has consumed the completed planning task");
            Verification.Equal(1, advancedTick!.SegmentState.ActiveSegmentIndex,
                "loop tick exposes the advanced immutable segment state");
        }
        finally
        {
            planningService.Release();
        }
    }

    private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
            observation.PublishedTrajectory, CreateStraightSegment(), 0.5d);

        string text = TrajectoryObservationPresentationText.Create(observation, charts);

        Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."),
            "observer presentation observe-only notice");
        Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"),
            "observer presentation selected point time and path-S");
        Verification.True(text.Contains("signed speed=0.30 m/s"),
            "observer presentation signed speed");
        Verification.True(text.Contains("yaw rate=0.00 rad/s"),
            "observer presentation yaw rate");
        Verification.True(text.Contains("LS projection failures=0"),
            "observer presentation LS projection failures");
    }

    private static void VerifiesPlanningDiagnosticsKeepRawFailureReason()
    {
        var failed = new PlanningCycleResult(
            4L,
            new PlanningCycleIdentity(3L, "diagnostic-reference", 7L, string.Empty, 0),
            new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null,
                "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor"),
            false,
            "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor");

        TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
            failed, TimeSpan.FromMilliseconds(18d), false, null);

        Verification.True(diagnostic.Text.Contains("cycle=4"), "diagnostic has cycle version");
        Verification.True(diagnostic.Text.Contains("status=CorridorInfeasible"), "diagnostic preserves raw status");
        Verification.True(diagnostic.Text.Contains("published=False"), "diagnostic preserves publish state");
        Verification.True(diagnostic.Text.Contains("elapsed=18ms"), "diagnostic preserves elapsed time");
        Verification.True(diagnostic.Text.Contains("reason=map=3;reference=diagnostic-reference"),
            "diagnostic preserves planner failure reason");
        foreach (string expected in new[]
        {
            "longitudinalMode=", "remainingToBoundary=", "minimumStoppingDistance=",
            "minimumStoppingDuration=", "maximumStoppedReachableDistance=",
        })
        {
            Verification.True(diagnostic.Text.Contains(expected), "failure diagnostic includes " + expected);
        }
        Verification.True(!diagnostic.Text.Contains("trajectory summary:"),
            "failed diagnostic has no stale trajectory summary");

        TrajectoryObservationDiagnostic pending = TrajectoryObservationDiagnostics.Create(
            null, TimeSpan.Zero, true, null);
        Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion");
    }

    private static void VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
        var succeeded = new PlanningCycleResult(9L,
            new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0),
            new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty);

        string text = TrajectoryObservationDiagnostics.Create(
            succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text;
        foreach (string expected in new[]
        {
            "trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s",
            "pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3",
            "longitudinalMode=RollingContinuation", "terminalSpeed=0.400m/s", "terminalAcceleration=0.000m/s2",
        })
        {
            Verification.True(text.Contains(expected), "published diagnostic includes " + expected);
        }
    }

    private static void VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
        DirectionSegmentView segment = CreateStraightSegment();
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);

        string text = TrajectoryObservationDiagnostics.CreateCurveReport(charts, trajectory).Text;

        foreach (string section in new[]
        {
            "LS_CURVE", "ST_CURVE", "SPEED_CURVE", "ACCELERATION_CURVE", "JERK_CURVE",
            "referenceS(m)", "time(s)", "signedSpeed(m/s)", "acceleration(m/s2)", "jerk(m/s3)",
        })
        {
            Verification.True(text.Contains(section), "curve report includes " + section);
        }
    }

    private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
    {
        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));

        Verification.True(source.Contains("DrawLs(TrajectoryObservationCharts charts, string diagnosticText)"),
            "LS painter accepts planning diagnostic input");
        Verification.True(source.Contains("DrawSt(TrajectoryObservationCharts charts, string diagnosticText)"),
            "ST painter accepts planning diagnostic input");
        Verification.True(source.Contains("No published trajectory available for L-S chart.\\n"),
            "LS empty state includes diagnostic after chart label");
        Verification.True(source.Contains("No published trajectory available for T-S/T-V charts.\\n"),
            "ST empty state includes diagnostic after chart label");
    }

    private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis()
    {
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
            CreatePublishedTrajectory(new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero)),
            CreateStraightSegment(), 0.5d);

        TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts);

        Verification.Equal("ReferenceS (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label");
        Verification.Equal("l (m)", model.VerticalAxisLabel, "observer LS vertical axis label");
        Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS,
            "observer LS shared ReferenceS is horizontal");
        Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset,
            "observer LS lateral offset is vertical");
    }

    private static void VerifiesPainterPoseGeometryReplacesLargeArrow()
    {
        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));

        Verification.True(!source.Contains("endArrow: true"),
            "observer painter removes the large terminal arrow");
        Verification.True(source.Contains("endArrow: false"),
            "observer painter uses a thin heading ray without an arrow");
        foreach (string corner in new[]
        {
            "frontLeftX", "frontLeftY", "frontRightX", "frontRightY",
            "rearRightX", "rearRightY", "rearLeftX", "rearLeftY",
        })
        {
            Verification.True(source.Contains(corner), "observer pose outline includes " + corner);
        }
        Verification.True(source.Contains("VehicleParameters vehicle"),
            "observer pose outline is scaled from vehicle parameters");
        Verification.True(source.Contains("MillimetersPerMeter = 1000f") &&
            source.Contains("ToMillimeters(path[index - 1].X)") &&
            source.Contains("ToMillimeters(path[index - 1].Y)") &&
            source.Contains("ToMillimeters(path[index].X)") &&
            source.Contains("ToMillimeters(path[index].Y)"),
            "observer world paths use one millimetres-per-metre factor for x and y");
    }

    private static void VerifiesPainterStAxesUsePathSAndTimeUnits()
    {
        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));

        Verification.True(source.Contains("\"t (s)\""), "observer ST horizontal axis uses seconds");
        Verification.True(source.Contains("\"PathS (m)\""), "observer ST vertical axis uses true PathS");
    }

    private static void FreezesBootstrapVehicleForRollingRequests()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings
        {
            PlanningScope = EmPlanningScope.RollingHorizon,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        VehicleParameters originalVehicle = job.Vehicle;
        double expectedLength = originalVehicle.LengthMeters;
        double expectedWidth = originalVehicle.WidthMeters;
        double expectedMargin = originalVehicle.SafetyMarginMeters;
        double? expectedMaximumCurvature = originalVehicle.MaximumCurvaturePerMeter;
        double? expectedMinimumRadius = originalVehicle.MinimumTurningRadiusMeters;
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer vehicle snapshot bootstrap succeeds");

        originalVehicle.LengthMeters = 91d;
        originalVehicle.WidthMeters = 92d;
        originalVehicle.SafetyMarginMeters = 93d;
        originalVehicle.MaximumCurvaturePerMeter = 94d;
        originalVehicle.MinimumTurningRadiusMeters = 95d;

        EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "vehicle-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 3L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        AssertVehicleSnapshot(planningService.Requests[0].Vehicle, expectedLength, expectedWidth, expectedMargin,
            expectedMaximumCurvature, expectedMinimumRadius, "mutated bootstrap job vehicle");
        Verification.True(!ReferenceEquals(originalVehicle, planningService.Requests[0].Vehicle),
            "observer request does not retain mutable bootstrap vehicle object");

        job.Vehicle = new VehicleParameters
        {
            LengthMeters = 101d,
            WidthMeters = 102d,
            SafetyMarginMeters = 103d,
            MaximumCurvaturePerMeter = 104d,
            MinimumTurningRadiusMeters = 105d,
        };
        controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
            .GetAwaiter().GetResult();
        AssertVehicleSnapshot(planningService.Requests[1].Vehicle, expectedLength, expectedWidth, expectedMargin,
            expectedMaximumCurvature, expectedMinimumRadius, "replaced bootstrap job vehicle");
    }

    private static void VerifiesEmptyMapBootstrapProducesPublishableReference()
    {
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0.5d, 0.5d, 0d), new Pose2D(3.5d, 0.5d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);

        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);

        Verification.True(bootstrap.Succeeded, "observer empty-map bootstrap succeeds");
        Verification.True(bootstrap.CoarseResult.MapResult.Succeeded,
            "observer empty-map bootstrap builds a successful map");
        Verification.True(bootstrap.Map != null, "observer empty-map bootstrap returns a map");
        Verification.Equal(PlanningStatus.Success, bootstrap.CoarseResult.PlanningResult.Status,
            "observer empty-map coarse planning succeeds");
        PathSmoothingResult smoothedPath = bootstrap.SmoothedPath ?? throw new InvalidOperationException(
            "observer empty-map bootstrap has no smoothing result");
        Verification.True(IsPublishableSmoothingStatus(smoothedPath.Status),
            "observer empty-map bootstrap returns a publishable smoothing result");
        for (int index = 0; index < smoothedPath.Path.Count; index++)
        {
            Verification.True(!double.IsNaN(smoothedPath.Path[index].BodyClearance) &&
                    !double.IsInfinity(smoothedPath.Path[index].BodyClearance),
                "observer empty-map smoothing path has finite body clearance at " + index);
        }
        Verification.True(bootstrap.Segments.Count > 0,
            "observer empty-map bootstrap returns at least one direction segment");
    }

    private static bool IsPublishableSmoothingStatus(PathSmoothingStatus status)
    {
        return status == PathSmoothingStatus.Complete ||
            status == PathSmoothingStatus.PartialImprovement ||
            status == PathSmoothingStatus.NotNeeded ||
            status == PathSmoothingStatus.Unchanged;
    }

    private static void AssertVehicleSnapshot(VehicleParameters actual, double expectedLength, double expectedWidth,
        double expectedMargin, double? expectedMaximumCurvature, double? expectedMinimumRadius, string name)
    {
        Verification.NearlyEqual(expectedLength, actual.LengthMeters, name + " length");
        Verification.NearlyEqual(expectedWidth, actual.WidthMeters, name + " width");
        Verification.NearlyEqual(expectedMargin, actual.SafetyMarginMeters, name + " safety margin");
        Verification.Equal(expectedMaximumCurvature, actual.MaximumCurvaturePerMeter,
            name + " maximum curvature");
        Verification.Equal(expectedMinimumRadius, actual.MinimumTurningRadiusMeters,
            name + " minimum turning radius");
    }

    private static DirectionSegmentView CreateStraightSegment()
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
    }

    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt,
        EmTerminalType terminalType = EmTerminalType.Goal)
    {
        var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType,
            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
        });
    }

    private static IReadOnlyList<DirectionSegmentView> CreateDirectionalSegments()
    {
        return new[]
        {
            CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false,
                EmBoundaryType.None, EmBoundaryType.GearSwitchApproach),
            CreateDirectionalSegment(1, TravelDirection.Reverse, 1d, 0d, true,
                EmBoundaryType.GearSwitchDeparture, EmBoundaryType.Goal),
        };
    }

    private static DirectionSegmentView CreateDirectionalSegment(int index, TravelDirection direction, double startX,
        double endX, bool startIsGearSwitch, EmBoundaryType startBoundary, EmBoundaryType endBoundary)
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(startX, 0d, 0d, 0d, 0d, direction, 0d, 0d, 1d,
                startIsGearSwitch, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(endX, 0d, 0d, 0d, 1d, direction, 0d, 0d, 1d,
                false, SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(index, direction, points,
            new ReferenceBoundary(index, 0d, startBoundary, index),
            new ReferenceBoundary(index, 1d, endBoundary, index + 1d), index);
    }

    private static EmTrajectory CreateSegmentTrajectory(DateTimeOffset effectiveAt, string trajectoryId, int segmentIndex,
        TravelDirection direction, EmTerminalType terminalType, EmBoundaryType boundaryType, double x)
    {
        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 1L,
            "active-segment-reference", 1L, string.Empty, segmentIndex, direction, terminalType,
            terminalType == EmTerminalType.GearSwitch
                ? EmLongitudinalMode.ExactStopAtBoundary
                : EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(x, 0d, 0d, 0d, 0d, 0d, segmentIndex, 1d, 1d,
                direction, boundaryType, 0d, 0d),
        });
    }

    private static EmTrajectory CreateSingleForwardSimulationTrajectory(DateTimeOffset effectiveAt)
    {
        var metadata = new EmTrajectoryMetadata("single-forward-simulation", effectiveAt, effectiveAt, 1L,
            "single-forward-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal,
            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, 1d, 0d, 0d, 0, 0d, 0d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(1d, 0d, 0d, 0d, 1d, 0d, 0, 1d, 1d,
                TravelDirection.Forward, EmBoundaryType.Goal, 0d, 0d),
        });
    }

    private sealed class FixedTrajectoryPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;

        public FixedTrajectoryPlanningService(EmTrajectory trajectory)
        {
            this.trajectory = trajectory;
        }

        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            Requests.Add(request);
            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
        }
    }

    private sealed class SequenceTrajectoryPlanningService : IEmPlanningService
    {
        private readonly IReadOnlyList<EmTrajectory> trajectories;
        private int nextTrajectoryIndex;

        public SequenceTrajectoryPlanningService(params EmTrajectory[] trajectories)
        {
            this.trajectories = trajectories ?? throw new ArgumentNullException(nameof(trajectories));
        }

        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            Requests.Add(request);
            EmTrajectory trajectory = trajectories[Math.Min(nextTrajectoryIndex, trajectories.Count - 1)];
            nextTrajectoryIndex++;
            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
        }
    }

    private sealed class DelayedTrajectoryPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;
        private readonly int delayedCall;
        private readonly ManualResetEventSlim entered = new ManualResetEventSlim(false);
        private readonly ManualResetEventSlim release = new ManualResetEventSlim(false);
        private int callCount;

        public DelayedTrajectoryPlanningService(EmTrajectory trajectory, int delayedCall)
        {
            this.trajectory = trajectory;
            this.delayedCall = delayedCall;
        }

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            if (Interlocked.Increment(ref callCount) == delayedCall)
            {
                entered.Set();
                release.Wait(cancellationToken);
            }
            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
        }

        public bool WaitUntilEntered(TimeSpan timeout)
        {
            return entered.Wait(timeout);
        }

        public void Release()
        {
            release.Set();
        }
    }

    private sealed class FailingTrajectoryPlanningService : IEmPlanningService
    {
        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            Requests.Add(request);
            return new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null,
                "one-shot failure fixture");
        }
    }

    private static void VerifiesMovementTestKeepsNativePainterLazyAndConditional()
    {
        string movementTestPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestPath));
        Verification.True(!source.Contains("static readonly TrajectoryObservationPresentation"),
            "MovementTest has no static eager Painter presentation");
        Verification.True(source.Contains("settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null"),
            "MovementTest creates Painter only under the native visualization flag");
    }

    private static bool Throws(Action action)
    {
        try { action(); return false; }
        catch (ArgumentOutOfRangeException) { return true; }
    }
}
