using System;
using System.Collections.Generic;
using System.Threading;
using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class LongitudinalModelChecks
{
    public static void Run()
    {
        VerifiesJerkLimitedStoppingProfileEndsAtRest();
        VerifiesStoppedReachabilityUsesTheSameJerkModel();
        VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
        VerifiesFinitePathSIndexedSpeedEnvelope();
        VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
        VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
        VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints();
        VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
        VerifiesModeSpecificSolutionValidation();
        VerifiesFullDirectionNoProgressValidation();
        VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
    }

    private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
    {
        Verification.True(JerkLimitedStoppingMath.TryCalculate(
            0.20d, 0d, 0.30d, 0.50d,
            out JerkLimitedStoppingProfile profile, out string failure),
            "jerk-limited stop builds: " + failure);
        Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
        Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
        Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
            "stop ends at zero speed");
        Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
            "stop releases acceleration to zero");

        Verification.True(JerkLimitedStoppingMath.TryCalculate(
            0.20d, 0.20d, 0.30d, 0.50d,
            out JerkLimitedStoppingProfile accelerating, out failure),
            "positive-acceleration stop builds: " + failure);
        Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
            "positive initial acceleration needs more stopping distance");
        Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
            "positive-acceleration stop also releases acceleration");
    }

    private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
    {
        double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
            0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
        Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
            "two-second stopped reach is finite and below unconstrained cruise distance");

        double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
            maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
        Verification.True(cap >= 0d && cap <= 0.20d,
            "distance inversion stays inside the direction speed range");
    }

    private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        LateralPath path = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(1d, 1d, 0d, 0d),
        });
        var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
            0d, 0d, EmTerminalType.RollingSafetyStop,
            EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
            rolling, out PathSpeedLimit envelope, out string failure);

        Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
        Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
            "rolling window end keeps a nonzero speed allowance");
        Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
        Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
            "rolling envelope reports its PathS upper bound");

        var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
            0d, 0d, EmTerminalType.Goal,
            EmLongitudinalMode.ApproachStopBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
        Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
        Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
        Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
            "approach stop boundary has an exact zero stopping limit");
    }

    private static void VerifiesFinitePathSIndexedSpeedEnvelope()
    {
        LateralPath directionPath = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(1d, 1d, 0d, 0d),
        });
        var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(),
            Array.Empty<double>(), Array.Empty<double>());
        EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
            out PathSpeedLimit directionEnvelope, out string directionFailureReason);
        Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
            directionFailureReason);
        Verification.NearlyEqual(1d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
            "default direction speed limit");

        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        LateralPath path = CreatePath(new[]
        {
            new PathFixture(10d, 0d, 0d, 0d),
            new PathFixture(20d, 2d, 2d, 4d),
            new PathFixture(20.5d, 4d, 20d, 0d),
            new PathFixture(21d, 5d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
        Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
        Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
            "curvature lateral-acceleration limit");
        Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
        Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
            double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
            "stopping speed limit uses the complete jerk-limited model");
        Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
            "combined limit chooses the finite minimum");
        double interpolationQueryPathS = 1.013d;
        int upperStation = 1;
        while (envelope.PathS[upperStation] < interpolationQueryPathS)
            upperStation++;
        double lowerPathS = envelope.PathS[upperStation - 1];
        double upperPathS = envelope.PathS[upperStation];
        double fraction = (interpolationQueryPathS - lowerPathS) / (upperPathS - lowerPathS);
        double expectedInterpolatedSpeed = envelope.MaximumSpeedMetersPerSecond[upperStation - 1] +
            (envelope.MaximumSpeedMetersPerSecond[upperStation] - envelope.MaximumSpeedMetersPerSecond[upperStation - 1]) *
            fraction;
        Verification.NearlyEqual(expectedInterpolatedSpeed, envelope.MaximumSpeedAt(interpolationQueryPathS),
            "speed envelope interpolates by PathS rather than ReferenceS");
        Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
    }

    private static void VerifiesStoppingEnvelopeIsRefinedOnActualPathS()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        LateralPath path = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(100d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
        Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
            "refined stopping envelope uses the complete jerk-limited stopping cap");
    }

    private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        LateralPath path = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(100d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
        double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
        Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d &&
            envelope.StoppingLimitAt(nearBoundaryPathS) <=
                MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS) + 1e-12d,
            "near-boundary speed cap conservatively interpolates jerk-limited distance inversion");
        Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
            "real stop boundary keeps an exact zero stopping cap");
    }

    private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        LateralPath shortPath = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(100d, 0.01d, 0d, 0d),
        });
        var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
            out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status,
            "rolling windows do not require a stop inside their local PathS extent: " + failureReason);
        Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");

        var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
        Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
            "real stop-boundary jerk/deceleration stopping precheck status");
        Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
        Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
    }

    private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.DistanceHorizonMeters = 1d;
        configuration.Scheduling.TimeHorizonSeconds = 2d;

        DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
        EmPlanningStatus status = new PlanningHorizonSelector().Select(
            longSegment, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
            out PlanningHorizonSelection rolling, out string failure);
        Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
        Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
            "distance horizon defines the LS window");
        Verification.Equal(EmLongitudinalMode.RollingContinuation,
            rolling.LongitudinalMode, "far boundary rolls");

        DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
        status = new PlanningHorizonSelector().Select(
            visibleButFar, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
            out PlanningHorizonSelection approach, out failure);
        Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
        Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
            approach.LongitudinalMode, "visible unreachable boundary approaches");

        DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
        status = new PlanningHorizonSelector().Select(
            reachableGoal, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
            out PlanningHorizonSelection exact, out failure);
        Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
            exact.LongitudinalMode, "reachable goal stops exactly");
        Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
            "exact stop preserves Goal identity");

        IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
        Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
        Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
            "short final interval moves the stop anchor earlier");
    }

    private static void VerifiesFullDirectionScopeSelectsActualSegmentBoundary()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.DistanceHorizonMeters = 1d;
        DirectionSegmentView tenMeterGoal = CreateSegment(10d, EmBoundaryType.Goal);
        var selector = new PlanningHorizonSelector();
        EmPlanningStatus status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
            EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection full, out string failure);
        Verification.Equal(EmPlanningStatus.Success, status, "full selection: " + failure);
        Verification.NearlyEqual(10d, full.WindowEndReferenceS, "full selection reaches actual segment end");
        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, full.LongitudinalMode,
            "full selection stops at its real boundary");
        Verification.True(full.HasStopBoundary, "full selection retains the real stop boundary");

        status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
            EmPlanningScope.RollingHorizon, configuration, out PlanningHorizonSelection rolling, out failure);
        Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
        Verification.NearlyEqual(4d, rolling.WindowEndReferenceS, "rolling selection retains the one-metre window");

        DirectionSegmentView gearSwitch = CreateSegment(10d, EmBoundaryType.GearSwitchApproach);
        status = selector.Select(gearSwitch, 3d, 0d, 0d,
            EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection gear, out failure);
        Verification.Equal(EmPlanningStatus.Success, status, "gear full selection: " + failure);
        Verification.Equal(EmTerminalType.GearSwitch, gear.TerminalType, "gear full selection preserves switch terminal");
        Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
    }

    private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 10d;
        configuration.Scheduling.DistanceHorizonMeters = 0.25d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;

        LateralPath shortPath = CreateStraightPath(0.50d);
        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule shortSchedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason);
        Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end");

        LateralPath longPath = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(1.50d, 1.50d, 2d, 0d),
            new PathFixture(3d, 3d, 0d, 0d),
        });
        status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule longSchedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason);
        Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds,
            "duration grows from s_end and limits");
        Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount,
            "adaptive schedule respects knot cap");
        Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule");
        Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count,
            "curvature and stopping envelopes add schedule breakpoints");
        Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS,
            longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end");
        Verification.NearlyEqual(0d,
            longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1],
            "schedule stops at s_end");

        EmPlannerConfiguration constrained = configuration.Copy();
        constrained.Scheduling.MaximumOptimizationKnotCount = 4;
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
            constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained,
            out LongitudinalKnotSchedule rejected, out failureReason);
        Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status,
            "undersized full-segment knot cap rejects rather than truncates");
        Verification.True(rejected == null, "resource rejection produces no partial schedule");
        Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 &&
            failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0,
            "resource rejection reports required and configured knots");
    }

    private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
        LateralPath path = CreateStraightPath(0.0075d);
        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule schedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason);

        Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null,
            "adaptive schedule is only a knot/reference/hold contract");
        Verification.True(schedule.TerminalHoldStartIndex > 0 &&
            schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count,
            "adaptive reference explicitly identifies its terminal hold boundary");
        Verification.True(schedule.TerminalHoldStartIndex >= 3,
            "adaptive exact-stop schedule reserves three independent motion jerk intervals");
        LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate();
        var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes,
            new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3);
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty<double>(), Array.Empty<double>());
        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection,
            out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason);

        var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder());
        Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit,
            out QuadraticProgram projectionProblem, out failureReason),
            "full exact-stop feasibility projection builds: " + failureReason);
        var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count);
        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d,
            "feasibility projection tracks scheduled PathS");
        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d,
            "feasibility projection tracks scheduled speed");
        Verification.Equal(9 * layout.KnotCount - 3 +
            3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount,
            "feasibility projection carries a PathS-linearized speed-envelope row for each motion knot");

        var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d,
            0d, 0, TimeSpan.Zero, "time limit", string.Empty));
        LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status,
            "initial feasibility timeout cannot publish a fallback");
        Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate");

        var solver = new FakeQpSolver(new[]
        {
            new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1,
                TimeSpan.Zero, "solved", string.Empty),
            new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d, 0d, 0,
                TimeSpan.Zero, "time limit", string.Empty),
        });
        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason);
        Verification.Equal(2, solver.SolveCallCount,
            "full scope consumes strict feasibility projection before the objective timeout");
        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit,
            result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _,
            out failureReason), "adaptive fallback is strict-feasible: " + failureReason);

        EmPlannerConfiguration denserPublication = configuration.Copy();
        denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d;
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
            denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication,
            out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason);
        Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count,
            "publication cadence does not change adaptive knot count");
        Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex,
            "publication cadence does not change the terminal hold boundary");
    }

    private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate()
    {
        double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d };
        double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d };
        var influence = new double[3, 3];
        for (int interval = 0; interval < 3; interval++)
        {
            var basis = new double[3];
            basis[interval] = 1d;
            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
            int last = response.S.Count - 1;
            influence[0, interval] = response.A[last];
            influence[1, interval] = response.U[last];
            influence[2, interval] = response.S[last];
        }
        double[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d });
        var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d };
        LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk);
        var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d };
        var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d };
        var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d };
        return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
    }

    private static double[] ToPrimal(LongitudinalCandidate candidate)
    {
        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
        var primal = new double[layout.VariableCount];
        for (int index = 0; index < layout.KnotCount; index++)
        {
            primal[layout.S(index)] = candidate.S[index];
            primal[layout.U(index)] = candidate.U[index];
            primal[layout.A(index)] = candidate.A[index];
        }
        for (int index = 0; index < layout.KnotCount - 1; index++)
            primal[layout.J(index)] = candidate.J[index];
        return primal;
    }

    private static double[] SolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide)
    {
        var augmented = new double[3, 4];
        for (int row = 0; row < 3; row++)
        {
            for (int column = 0; column < 3; column++)
                augmented[row, column] = matrix[row, column];
            augmented[row, 3] = rightHandSide[row];
        }
        for (int pivot = 0; pivot < 3; pivot++)
        {
            int bestRow = pivot;
            for (int row = pivot + 1; row < 3; row++)
            {
                if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot]))
                    bestRow = row;
            }
            for (int column = pivot; column < 4; column++)
            {
                double temporary = augmented[pivot, column];
                augmented[pivot, column] = augmented[bestRow, column];
                augmented[bestRow, column] = temporary;
            }
            double divisor = augmented[pivot, pivot];
            for (int column = pivot; column < 4; column++)
                augmented[pivot, column] /= divisor;
            for (int row = 0; row < 3; row++)
            {
                if (row == pivot)
                    continue;
                double factor = augmented[row, pivot];
                for (int column = pivot; column < 4; column++)
                    augmented[row, column] -= factor * augmented[pivot, column];
            }
        }
        return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
    }

    private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
    {
        var layout = new LongitudinalVariableLayout(5);
        Verification.Equal(19, layout.VariableCount, "ST variable count");
        for (int index = 0; index < 5; index++)
        {
            Verification.Equal(index, layout.S(index), "s index " + index);
            Verification.Equal(5 + index, layout.U(index), "u index " + index);
            Verification.Equal(10 + index, layout.A(index), "a index " + index);
        }
        for (int index = 0; index < 4; index++)
            Verification.Equal(15 + index, layout.J(index), "j index " + index);

        double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d };
        double[] jerk = { 0.30d, -0.10d, 0.20d, -0.20d };
        LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0.02d, jerk);
        for (int index = 0; index < jerk.Length; index++)
        {
            double dt = times[index + 1] - times[index];
            Verification.NearlyEqual(integrated.A[index] + dt * integrated.J[index], integrated.A[index + 1],
                "exact ST acceleration dynamics " + index);
            Verification.NearlyEqual(integrated.U[index] + dt * integrated.A[index] + 0.5d * dt * dt * integrated.J[index],
                integrated.U[index + 1], "exact ST speed dynamics " + index);
            Verification.NearlyEqual(integrated.S[index] + dt * integrated.U[index] +
                0.5d * dt * dt * integrated.A[index] + dt * dt * dt * integrated.J[index] / 6d,
                integrated.S[index + 1], "exact ST progress dynamics " + index);
        }
        Verification.True(integrated.SatisfiesExactDiscreteDynamics(1e-12d), "integrated ST candidate validates dynamics");

        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
        LateralPath path = CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(1d, 1d, 0d, 0d),
            new PathFixture(2d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
            new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "unit-scale speed envelope: " + speedFailure);

        Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
            integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
        var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
            new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
        EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
            out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
        Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
            "unit-scale rolling speed envelope: " + rollingSpeedFailure);
        Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
            rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
            "rolling ST QP builds: " + rollingFailure);
        Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
            "normalized speed and previous-U P coefficient");
        Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
            "normalized acceleration P coefficient");
        Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.J(0), layout.J(0)),
            "normalized jerk P coefficient");
        Verification.NearlyEqual(2.5d, MatrixValue(problem.UpperTriangularP, layout.S(0), layout.S(0)),
            "normalized previous-S P coefficient");
        Verification.NearlyEqual(0d, MatrixValue(problem.UpperTriangularP, layout.S(4), layout.S(4)),
            "fixed terminal S has no progress-reward coefficient");

        FindSingleVariableBounds(problem, layout.S(0), out double sLower, out double sUpper);
        Verification.NearlyEqual(0d, sLower, "S lower bound");
        Verification.NearlyEqual(2d, sUpper, "S upper bound");
        FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper);
        Verification.NearlyEqual(0d, uLower, "U nonnegative bound");
        Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
            "U retains its direction hard bound alongside the PathS envelope");
        int envelopeSegment = 0;
        while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1])
            envelopeSegment++;
        double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] -
            envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) /
            (envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
        double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
            envelopeSlope * envelope.PathS[envelopeSegment];
        var envelopeRow = new Dictionary<int, double> { { layout.U(1), 1d } };
        if (Math.Abs(envelopeSlope) > 1e-12d)
            envelopeRow.Add(layout.S(1), -envelopeSlope);
        Verification.Equal(1, CountBoundedRow(problem, envelopeRow,
            -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
            "U upper bound linearly re-evaluates the actual PathS envelope");
        FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
        Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
        Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");
        FindSingleVariableBounds(problem, layout.J(1), out double jLower, out double jUpper);
        Verification.NearlyEqual(-1d, jLower, "jerk lower bound");
        Verification.NearlyEqual(1d, jUpper, "jerk upper bound");

        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(0), 1d } }, 0d),
            "exact initial S");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(0), 1d } }, 0.10d),
            "exact initial U");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
            "exact initial A");
        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
            new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
            "rolling has no exact terminal S");
        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
            new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
            "rolling has no exact terminal U");
        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
            new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
            "rolling has no exact terminal A");
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
        for (int index = stabilizationStart; index < layout.KnotCount; index++)
        {
            Verification.Equal(1, CountExactEqualityRows(problem,
                new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
                "stop tail exact S " + index);
            Verification.Equal(1, CountExactEqualityRows(problem,
                new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
                "stop tail exact U " + index);
            Verification.Equal(1, CountExactEqualityRows(problem,
                new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
                "stop tail exact A " + index);
        }
        Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
        {
            { layout.S(1), 1d }, { layout.S(0), -1d },
        }, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
        {
            { layout.A(1), 1d }, { layout.A(0), -1d }, { layout.J(0), -0.05d },
        }, 0d), "exact ST acceleration equation");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
        {
            { layout.U(1), 1d }, { layout.U(0), -1d }, { layout.A(0), -0.05d }, { layout.J(0), -0.00125d },
        }, 0d), "exact ST speed equation");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
        {
            { layout.S(1), 1d }, { layout.S(0), -1d }, { layout.U(0), -0.05d }, { layout.A(0), -0.00125d },
            { layout.J(0), -0.000020833333333333333d },
        }, 0d), "exact ST progress equation");
    }

    private static void VerifiesModeSpecificSolutionValidation()
    {
        EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
        IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
        LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
            exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
        LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
        var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
            out PathSpeedLimit rollingEnvelope, out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);

        var validator = new LongitudinalSolutionValidator();
        Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
            out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
        Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
            "exact-stop failure identifies the stabilized tail: " + exactFailure);

        LateralPath rollingPath = CreateStraightPath(0.10d);
        var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
        LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
            exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
        Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
            out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);

        EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
        approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
        approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
        IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
        LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
            approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
        LateralPath approachPath = CreateStraightPath(0.035d);
        var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
            Array.Empty<double>(), Array.Empty<double>());
        var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
            Array.Empty<double>(), Array.Empty<double>());
        speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
            out PathSpeedLimit approachEnvelope, out speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
        Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
            out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
        Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
            "approach failure identifies the jerk-limited stoppable set");
    }

    private static void VerifiesFullDirectionNoProgressValidation()
    {
        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
        double[] times = { 0d, 0.10d, 0.20d, 0.30d, 0.50d };
        var fullSchedule = new LongitudinalKnotSchedule(times,
            new[] { 0d, 0.04d, 0.08d, 0.10d, 0.10d }, new[] { 0d, 0.20d, 0.10d, 0d, 0d }, true, 3);
        LateralPath path = CreateStraightPath(0.10d);
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, fullSchedule, Array.Empty<double>(), Array.Empty<double>());
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit speedLimit,
            out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "no-progress envelope: " + speedFailure);
        LongitudinalCandidate stationary = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d,
            new[] { 0d, 0d, 0d, 0d });

        var validator = new LongitudinalSolutionValidator();
        Verification.True(!validator.TryValidate(input, speedLimit, stationary, out _, out EmPlanningStatus failureStatus,
            out string failureReason), "nonterminal stationary full segment is rejected");
        Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, "stationary full segment has exact status");
        Verification.True(failureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
            "stationary full segment has exact diagnostic");
        Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input,
            speedLimit, stationary, out QuadraticProgram fullProblem, out string buildFailure),
            "full no-progress objective builds: " + buildFailure);
        var fullLayout = new LongitudinalVariableLayout(times.Length);
        double expectedReferenceLinearCost = -2d * configuration.Longitudinal.Weights.ReferenceSpeed *
            fullSchedule.ReferenceSpeedMetersPerSecond[1] /
            (input.DirectionMaximumSpeedMetersPerSecond * input.DirectionMaximumSpeedMetersPerSecond);
        Verification.NearlyEqual(expectedReferenceLinearCost, fullProblem.LinearCost[fullLayout.U(1)],
            "full direction objective tracks adaptive speed reference");

        LateralPath nearTerminalPath = CreateStraightPath(0.02d);
        var nearTerminalSchedule = new LongitudinalKnotSchedule(times,
            new[] { 0d, 0.01d, 0.02d, 0.02d, 0.02d }, new[] { 0d, 0d, 0d, 0d, 0d }, true, 3);
        var nearTerminalInput = new LongitudinalPlanningInput(nearTerminalPath, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.GearSwitch, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, nearTerminalSchedule, Array.Empty<double>(), Array.Empty<double>());
        speedStatus = new PathSpeedLimitBuilder().Build(nearTerminalInput, out PathSpeedLimit nearTerminalLimit,
            out speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
        Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
            out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract");
        Verification.True(failureStatus != EmPlanningStatus.NoProgress,
            "near-terminal gear-switch stop is not classified as no progress");
    }

    private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
    {
        LateralPath path = CreateStraightPath(1d);
        DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
        EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
        var builder = new LongitudinalPreviousTrajectorySeedBuilder();
        LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);

        Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
        Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
        Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
        Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
            "previous seed progress is monotone");
        Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
            "previous seed uses absolute progress speed");

        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
            "different direction returns an empty seed");
        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
            new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
            "different segment returns an empty seed");
        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
            new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
            "out-of-range absolute sampling returns an empty seed");
    }

    private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
    {
        var points = new List<LateralPathPoint>(fixtures.Count);
        for (int index = 0; index < fixtures.Count; index++)
        {
            PathFixture fixture = fixtures[index];
            points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
                0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
        }
        return new LateralPath(points, true);
    }

    private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
        int segmentIndex)
    {
        var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        double sign = direction == TravelDirection.Forward ? 1d : -1d;
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
                direction, EmBoundaryType.None, 0d, 0d),
        });
    }

    private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
    {
        var points = new List<SmoothedPathPoint>
        {
            Point(0d, 0d),
            Point(length, length),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
            new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
    }

    private static SmoothedPathPoint Point(double x, double s)
    {
        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
            SmoothedPathPointSource.Anchor);
    }

    private static EmPlannerConfiguration CreateUnitScaleConfiguration()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 0.20d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.05d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
        return configuration;
    }

    private static EmPlannerConfiguration CreateTaskFourConfiguration()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 0.30d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
        return configuration;
    }

    private static LateralPath CreateStraightPath(double pathUpperBoundS)
    {
        return CreatePath(new[]
        {
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
        });
    }

    private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
    {
        LongitudinalConfiguration limits = input.Configuration.Longitudinal;
        return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
            Math.Max(0d, input.StopBoundaryPathS - pathS),
            Math.Max(0d, input.InitialAccelerationMetersPerSecondSquared),
            limits.MaximumDecelerationMetersPerSecondSquared,
            limits.MaximumJerkMetersPerSecondCubed,
            input.DirectionMaximumSpeedMetersPerSecond);
    }

    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
    {
        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
        {
            if (matrix.RowIndices[index] == row)
                return matrix.Values[index];
        }
        return 0d;
    }

    private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
    {
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }))
            {
                lower = problem.LowerBounds[row];
                upper = problem.UpperBounds[row];
                return;
            }
        }
        throw new InvalidOperationException("No single-variable bounds were found for variable " + variable + ".");
    }

    private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
        double bound)
    {
        return CountBoundedRow(problem, expected, bound, bound);
    }

    private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
        double lower, double upper)
    {
        int count = 0;
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            if (Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
                Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d && RowMatches(problem.ConstraintMatrix, row, expected))
            {
                count++;
            }
        }
        return count;
    }

    private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
    {
        var actual = new Dictionary<int, double>();
        for (int column = 0; column < matrix.ColumnCount; column++)
        {
            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
            {
                if (matrix.RowIndices[index] == targetRow)
                    actual[column] = matrix.Values[index];
            }
        }
        if (actual.Count != expected.Count)
            return false;
        foreach (KeyValuePair<int, double> pair in expected)
        {
            if (!actual.TryGetValue(pair.Key, out double actualValue) || Math.Abs(actualValue - pair.Value) > 1e-12d)
                return false;
        }
        return true;
    }

    private sealed class PathFixture
    {
        public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
        {
            ReferenceS = referenceS;
            PathS = pathS;
            Curvature = curvature;
            CurvatureDerivative = curvatureDerivative;
        }

        public double ReferenceS { get; }
        public double PathS { get; }
        public double Curvature { get; }
        public double CurvatureDerivative { get; }
    }
}