feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -10,13 +10,21 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
 public sealed class SequentialLongitudinalOptimizer
 {
-    private const int MaximumEnvelopeIterations = 5;
-    private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
-    private const double OrdinaryTerminalProbeFraction = 0.5d;
+    private const int MaximumAcceptedAnchorUpdates = 5;
+    private const int MaximumQpSolveCalls = 12;
+    private const double MinimumTrustRegionWidthMeters = 0.001d;
+    private const double ObjectiveAcceptanceRelativeTolerance = 1e-9d;
+    private const double HighPrecisionRetryTolerance = 1e-7d;
+    private const double StaticStartSeedBudgetFraction = 0.10d;
+    private static readonly TimeSpan MaximumStaticStartSeedBudget = TimeSpan.FromMilliseconds(250d);
+    private static readonly TimeSpan PublicationReserve = TimeSpan.FromMilliseconds(250d);
+    private static readonly double[] TrustRegionScales = { 1d, 0.5d, 0.25d, 0.125d };
    private readonly IQpSolver _qpSolver;
    private readonly PathSpeedLimitBuilder _speedLimitBuilder;
    private readonly LongitudinalConstraintBuilder _constraintBuilder;
    private readonly LongitudinalSolutionValidator _solutionValidator;
+    private readonly LongitudinalEnvelopeTrustRegionBuilder _trustRegionBuilder =
+        new LongitudinalEnvelopeTrustRegionBuilder();

    public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
        : this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
@@ -65,133 +73,418 @@ public sealed class SequentialLongitudinalOptimizer
            return Failed(speedStatus, speedFailure);

        var stopwatch = Stopwatch.StartNew();
-        LongitudinalCandidate iterate;
-        LongitudinalCandidate lastStrictCandidate = null;
-        int remainingObjectiveIterations = iterationLimit;
+        var solveTrace = new LongitudinalSolveTrace();
+        LongitudinalCandidate initialCandidate;
+        int projectionSolveCount = 0;
        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
        {
            if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
-                    iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
-                    out EmPlanningStatus projectionStatus, out string projectionFailure))
+                    iterationLimit, stopwatch, solveTrace, cancellationToken, out initialCandidate,
+                    out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
+                    out string projectionFailure))
            {
-                return Failed(projectionStatus, projectionFailure);
-            }
-            lastStrictCandidate = CopyCandidate(iterate);
-            remainingObjectiveIterations -= projectionSolveCount;
-            if (remainingObjectiveIterations <= 0)
-            {
-                return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
-                    "The strict initial feasibility projection consumed the configured outer-iteration budget.");
+                string projectionTrace = solveTrace.Format();
+                return Failed(projectionStatus, projectionFailure +
+                    (string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
            }
+            projectionSolveCount = usedProjectionSolveCount;
        }
        else
        {
-            iterate = CreateInitialIterate(input, speedLimit);
-            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
-                lastStrictCandidate = null;
+            LongitudinalCandidate seed = CreateInitialIterate(input, speedLimit);
+            if (!_solutionValidator.TryValidate(input, speedLimit, seed, out initialCandidate,
+                    out EmPlanningStatus initializationStatus, out string initializationFailure))
+            {
+                return Failed(initializationStatus, "No strictly validated longitudinal candidate was found. " +
+                    initializationFailure);
+            }
        }
-        double[] warmStart = ToPrimal(iterate);
-        bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
-        string lastCandidateRejection = string.Empty;
-        bool hasPreviousObjective = false;
-        double previousObjective = 0d;

-        for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
+        LongitudinalCandidate anchor = CopyCandidate(initialCandidate);
+        int stabilizationStart = GetStabilizationStart(input);
+        int qpSolveCount = projectionSolveCount;
+        int trustShrinkCount = 0;
+        int acceptedAnchorCount = 0;
+        int acceptedUpdateLimit = Math.Min(MaximumAcceptedAnchorUpdates, iterationLimit);
+        double finalTrustScale = 1d;
+        string lastRejection = string.Empty;
+
+        while (acceptedAnchorCount < acceptedUpdateLimit && qpSolveCount < MaximumQpSolveCalls)
        {
+            bool promoted = false;
+            for (int scaleIndex = 0; scaleIndex < TrustRegionScales.Length; scaleIndex++)
+            {
+                double scale = TrustRegionScales[scaleIndex];
+                finalTrustScale = scale;
+                if (cancellationToken.IsCancellationRequested || totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+                }
+                if (!_trustRegionBuilder.TryBuild(speedLimit, anchor, input.KnotSchedule.ReferencePathS,
+                        stabilizationStart, scale, MinimumTrustRegionWidthMeters,
+                        out LongitudinalEnvelopeTrustRegion region, out string regionFailure))
+                {
+                    lastRejection = regionFailure;
+                    break;
+                }
+                if (!_constraintBuilder.TryBuildTrusted(input, speedLimit, anchor, region,
+                        convergenceTolerance, out QuadraticProgram problem, out string buildFailure))
+                {
+                    lastRejection = buildFailure;
+                    break;
+                }
+                TrustedSolveAttempt attempt = SolveTrustedProblem(problem, input, speedLimit, anchor, settings,
+                    totalBudget, stopwatch, convergenceTolerance, stabilizationStart,
+                    MaximumQpSolveCalls - qpSolveCount, solveTrace, qpSolveCount, acceptedAnchorCount,
+                    scale, acceptedAnchorCount > 0, cancellationToken);
+                qpSolveCount += attempt.SolveCount;
+                lastRejection = attempt.FailureReason;
+                if (attempt.Status == EmPlanningStatus.Cancelled)
+                {
+                    return Failed(EmPlanningStatus.Cancelled, CreateRunDiagnostic(qpSolveCount,
+                        trustShrinkCount, acceptedAnchorCount, finalTrustScale, lastRejection, solveTrace));
+                }
+                if (attempt.Accepted)
+                {
+                    anchor = CopyCandidate(attempt.Candidate);
+                    acceptedAnchorCount++;
+                    promoted = true;
+                    break;
+                }
+                if (attempt.Status != EmPlanningStatus.SuccessWithFallback)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, false, lastRejection, solveTrace);
+                }
+                if (scaleIndex + 1 < TrustRegionScales.Length)
+                {
+                    double nextScale = TrustRegionScales[scaleIndex + 1];
+                    if (!region.CanShrinkTo(nextScale, MinimumTrustRegionWidthMeters, out string shrinkFailure))
+                    {
+                        lastRejection = shrinkFailure;
+                        break;
+                    }
+                    trustShrinkCount++;
+                }
+            }
+            if (!promoted)
+                break;
+        }
+
+        return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+            finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+    }
+
+    private TrustedSolveAttempt SolveTrustedProblem(QuadraticProgram problem,
+        LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        QpSolverSettings settings, TimeSpan totalBudget, Stopwatch stopwatch, double strictTolerance,
+        int stabilizationStart, int remainingCallCount, LongitudinalSolveTrace solveTrace,
+        int solveOrdinalOffset, int anchorUpdateIndex, double trustScale,
+        bool optionalImprovement, CancellationToken cancellationToken)
+    {
+        int solveCount = 0;
+        double anchorObjective = EvaluateObjective(problem, ToPrimal(anchor));
+        if (!IsFinite(anchorObjective))
+        {
+            return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                "The strict anchor objective is non-finite for the trusted QP.");
+        }
+
+        IReadOnlyList<double> warmStart = ToPrimal(anchor);
+        string lastFailure = string.Empty;
+        for (int attemptIndex = 0; attemptIndex < 2; attemptIndex++)
+        {
+            if (solveCount >= remainingCallCount)
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    string.IsNullOrWhiteSpace(lastFailure) ? "The longitudinal QP solve-call cap was reached." : lastFailure);
+            }
            if (cancellationToken.IsCancellationRequested)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    "Longitudinal optimization was cancelled before the trusted QP solve.");
+            }
            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
            if (remainingBudget <= TimeSpan.Zero)
            {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "Longitudinal optimization exhausted its solve budget.");
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    "Longitudinal optimization exhausted its shared solve budget.");
            }
-            if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
+            bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
+                out TimeSpan remainingAfterReserve);
+            if (optionalImprovement && !hasOptionalSolveBudget)
            {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "Longitudinal constraints are infeasible: " + buildFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    CreatePublicationReserveSkipDiagnostic(remainingAfterReserve));
+            }
+            TimeSpan solveBudget = optionalImprovement ? remainingAfterReserve : remainingBudget;
+
+            bool highPrecision = attemptIndex == 1;
+            double absoluteTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.AbsoluteTolerance;
+            double relativeTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.RelativeTolerance;
+            var solveStopwatch = Stopwatch.StartNew();
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, absoluteTolerance, relativeTolerance,
+                    solveBudget, settings.EnableWarmStart, settings.EnablePolishing,
+                    settings.EnableNativeVerboseOutput), warmStart, cancellationToken);
+            solveStopwatch.Stop();
+            solveCount++;
+            double candidateObjective = double.NaN;
+            void AddSolveTrace(string rejection)
+            {
+                solveTrace.Add(attemptIndex == 0 ? "normal" : "retry",
+                    solveOrdinalOffset + solveCount, anchorUpdateIndex, trustScale,
+                    solveBudget, remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
            }

-            QpSolveResult solved = _qpSolver.Solve(problem,
-                new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
-                    remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
-                    settings.EnablePolishing,
-                    settings.EnableNativeVerboseOutput),
-                warmStart, cancellationToken);
            if (cancellationToken.IsCancellationRequested)
-                return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
-            if (solved == null)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
-            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
            {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
-                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
-                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
+                const string rejection = "Longitudinal optimization was cancelled after the trusted QP solve.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved == null)
+            {
+                const string rejection = "The longitudinal QP solver returned no result.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
            }
            if (solved.Status == QpSolveStatus.Cancelled)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
-                    "The longitudinal QP solver was cancelled: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver was cancelled: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                string rejection = "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
+                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
+                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
            {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
+                string rejection = "The preflight-feasible longitudinal QP solver reported infeasibility;" +
+                    "solverNumericalAnomaly=true;status=" + solved.NativeStatus + ": " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                    rejection);
            }
            if (solved.Status == QpSolveStatus.SolverUnavailable)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
-                    "The longitudinal QP solver is unavailable: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver is unavailable: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverUnavailable, null, solveCount, false,
+                    rejection);
+            }
            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
            {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
-                    "The longitudinal QP solver failed: " + solved.Diagnostic);
+                string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
+                    solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
            }
-            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
+
+            if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
            {
-                lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
+                const string rejection =
+                    "The solver primal does not match the ST variable layout or contains non-finite values.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    rejection);
+            }
+
+            bool accepted = true;
+            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, strictTolerance))
+            {
+                accepted = false;
+                lastFailure = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
                    " (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
-                if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
-                    warmStart = ToPrimal(inaccurateCandidate);
-                continue;
            }
-            if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
+            if (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
+                    stabilizationStart, out lastFailure))
            {
-                lastCandidateRejection = "The solver primal does not match the ST variable layout.";
-                continue;
+                accepted = false;
            }
-            if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
+            LongitudinalCandidate validated = null;
+            if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
                    out string validationFailure))
            {
-                string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
-                    iteration + 1);
-                lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
-                    ? rejection
-                    : lastCandidateRejection + " | " + rejection;
-                if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
+                accepted = false;
+                lastFailure = validationFailure;
+            }
+            if (accepted)
+            {
+                candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
+                if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
                {
-                    iterate = nextIterate;
-                    warmStart = ToPrimal(candidate);
+                    accepted = false;
+                    lastFailure = "The strictly valid candidate worsens the current trusted-QP objective" +
+                        " (anchor=" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+                        ", candidate=" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) + ").";
                }
-                continue;
+            }
+            if (accepted && cancellationToken.IsCancellationRequested)
+            {
+                const string rejection = "Longitudinal optimization was cancelled before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted && totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+            {
+                const string rejection =
+                    "Longitudinal optimization exhausted its shared solve budget before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted)
+            {
+                AddSolveTrace(string.Empty);
+                return new TrustedSolveAttempt(EmPlanningStatus.Success, validated, solveCount, true, string.Empty);
+            }
+            if (highPrecision)
+            {
+                AddSolveTrace(lastFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    lastFailure);
            }

-            double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
-            double relativeObjectiveImprovement = hasPreviousObjective
-                ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
-                : double.PositiveInfinity;
-            lastStrictCandidate = CopyCandidate(validated);
-            iterate = validated;
-            warmStart = ToPrimal(validated);
-            previousObjective = solved.Objective;
-            hasPreviousObjective = true;
-            if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
-                return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+            AddSolveTrace(lastFailure);
+            warmStart = ToPrimal(candidate);
        }

-        return lastStrictCandidate == null
-            ? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
-                lastCandidateRejection)
-            : new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+        return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+            lastFailure);
+    }
+
+    private static double EvaluateObjective(QuadraticProgram problem, IReadOnlyList<double> primal)
+    {
+        if (problem == null || primal == null || primal.Count != problem.VariableCount)
+            return double.NaN;
+        double objective = 0d;
+        SparseCscMatrix hessian = problem.UpperTriangularP;
+        for (int column = 0; column < hessian.ColumnCount; column++)
+        {
+            double columnValue = primal[column];
+            if (!IsFinite(columnValue))
+                return double.NaN;
+            for (int entry = hessian.ColumnPointers[column]; entry < hessian.ColumnPointers[column + 1]; entry++)
+            {
+                int row = hessian.RowIndices[entry];
+                double term = hessian.Values[entry] * primal[row] * columnValue;
+                objective += row == column ? 0.5d * term : term;
+                if (!IsFinite(objective))
+                    return double.NaN;
+            }
+            objective += problem.LinearCost[column] * columnValue;
+            if (!IsFinite(objective))
+                return double.NaN;
+        }
+        return objective;
+    }
+
+    private static bool IsObjectiveAccepted(double anchorObjective, double candidateObjective)
+    {
+        if (!IsFinite(anchorObjective) || !IsFinite(candidateObjective))
+            return false;
+        double tolerance = ObjectiveAcceptanceRelativeTolerance * Math.Max(1d, Math.Abs(anchorObjective));
+        return candidateObjective <= anchorObjective + tolerance;
+    }
+
+    private static int GetStabilizationStart(LongitudinalPlanningInput input)
+    {
+        if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return input.KnotSchedule.KnotTimes.Count;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            return input.KnotSchedule.TerminalHoldStartIndex;
+        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(input.KnotSchedule.KnotTimes,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
+    private static bool TryFastValidateCandidate(QuadraticProgram problem, LongitudinalCandidate candidate,
+        double strictTolerance, int stabilizationStart, out string failureReason)
+    {
+        failureReason = string.Empty;
+        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+        LongitudinalQpAuditResult audit;
+        try
+        {
+            audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, candidate, strictTolerance,
+                layout, stabilizationStart);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = "The candidate cannot be audited against the current trusted QP: " + exception.Message;
+            return false;
+        }
+        if (audit.IsFeasible)
+            return true;
+        failureReason = "The candidate violates the current trusted QP" +
+            ";row=" + audit.WorstRow + ";category=" + audit.Category +
+            ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) + audit.Unit +
+            ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+        return false;
+    }
+
+    private static string CreateRunDiagnostic(int qpSolveCount, int trustShrinkCount,
+        int acceptedAnchorCount, double finalTrustScale, string lastRejection,
+        LongitudinalSolveTrace solveTrace)
+    {
+        return "qpSolves=" + qpSolveCount +
+            ",trustShrinks=" + trustShrinkCount +
+            ",acceptedAnchors=" + acceptedAnchorCount +
+            ",trustScale=" + finalTrustScale.ToString("R", CultureInfo.InvariantCulture) +
+            (string.IsNullOrWhiteSpace(lastRejection) ? string.Empty : ";lastRejection=" + lastRejection) +
+            (string.IsNullOrEmpty(solveTrace.Format()) ? string.Empty : ";solveTrace=" + solveTrace.Format());
+    }
+
+    internal static bool TryGetOptionalSolveBudget(TimeSpan remaining, out TimeSpan solveBudget)
+    {
+        solveBudget = remaining - PublicationReserve;
+        if (solveBudget <= TimeSpan.Zero)
+        {
+            solveBudget = TimeSpan.Zero;
+            return false;
+        }
+        return true;
+    }
+
+    private static string CreatePublicationReserveSkipDiagnostic(TimeSpan remainingAfterReserve)
+    {
+        return "remainingAfterReserveMs=" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ";publicationReserveMs=" + PublicationReserve.TotalMilliseconds.ToString(
+                "F0", CultureInfo.InvariantCulture) +
+            ";optionalImprovement=skipped";
+    }
+
+    private static LongitudinalPlanningResult FinishFromAnchor(LongitudinalCandidate anchor,
+        int acceptedAnchorCount, int qpSolveCount, int trustShrinkCount, double finalTrustScale,
+        bool cancelled, string lastRejection, LongitudinalSolveTrace solveTrace)
+    {
+        string diagnostic = CreateRunDiagnostic(qpSolveCount, trustShrinkCount, acceptedAnchorCount,
+            finalTrustScale, lastRejection, solveTrace);
+        if (cancelled)
+            return Failed(EmPlanningStatus.Cancelled, diagnostic);
+        EmPlanningStatus status = acceptedAnchorCount > 0
+            ? EmPlanningStatus.Success
+            : EmPlanningStatus.SuccessWithFallback;
+        return new LongitudinalPlanningResult(status, CopyCandidate(anchor), diagnostic);
    }

    private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
@@ -222,7 +515,7 @@ public sealed class SequentialLongitudinalOptimizer
            settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
                totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
            convergenceTolerance = solver.StrictResidualTolerance;
-            iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
+            iterationLimit = Math.Min(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
            return true;
        }
        catch (ArgumentException exception)
@@ -234,8 +527,9 @@ public sealed class SequentialLongitudinalOptimizer

    private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
        QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
-        Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
-        out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
+        Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
+        out LongitudinalCandidate candidate, out int projectionSolveCount,
+        out EmPlanningStatus failureStatus, out string failureReason)
    {
        candidate = null;
        projectionSolveCount = 0;
@@ -248,8 +542,9 @@ public sealed class SequentialLongitudinalOptimizer
                input.Configuration.Validation.KinematicTolerance;
        bool staticStartSeedUsed = false;
        string staticStartSeedFailure = string.Empty;
-        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
-                out staticStartSeedFailure))
+        TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
+        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
+                out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
        {
            staticStartSeedUsed = true;
            candidate = staticStartSeed;
@@ -289,60 +584,93 @@ public sealed class SequentialLongitudinalOptimizer

            double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
                input.Configuration.Validation.KinematicTolerance * 0.1d);
+            double anchorObjective = double.NaN;
+            var solveStopwatch = Stopwatch.StartNew();
            QpSolveResult solved = _qpSolver.Solve(problem,
                new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
                    remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
                    settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                ToPrimal(linearizationIterate), cancellationToken);
+            solveStopwatch.Stop();
            projectionSolveCount++;
+            int projectionCallOrdinal = projectionSolveCount;
+            double candidateObjective = double.NaN;
+            void AddProjectionTrace(string rejection)
+            {
+                TryGetOptionalSolveBudget(remainingBudget, out TimeSpan remainingAfterReserve);
+                solveTrace.Add("projection", projectionCallOrdinal, 0, 1d, remainingBudget,
+                    remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
+            }
            if (cancellationToken.IsCancellationRequested)
            {
+                const string rejection =
+                    "Initial full-direction feasibility projection was cancelled after the QP solve.";
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved == null)
            {
+                const string rejection = "The initial full-direction feasibility solver returned no result.";
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
            {
-                failureStatus = EmPlanningStatus.SolverTimedOut;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
-                    solved.DualResidual + "): " + solved.Diagnostic);
+                    solved.DualResidual + "): " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.Cancelled)
            {
+                string rejection =
+                    "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
            {
+                string rejection =
+                    "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.SolverUnavailable)
            {
+                string rejection =
+                    "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.SolverUnavailable;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
            {
+                string rejection = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
            {
+                const string rejection =
+                    "Initial full-direction feasibility solver primal does not match the ST layout.";
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -350,11 +678,13 @@ public sealed class SequentialLongitudinalOptimizer
                if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
                        out EmPlanningStatus validationStatus, out string validationFailure))
                {
+                    AddProjectionTrace(string.Empty);
                    candidate = strict;
                    return true;
                }
                if (validationStatus == EmPlanningStatus.NoProgress)
                {
+                    AddProjectionTrace(validationFailure);
                    failureStatus = validationStatus;
                    failureReason = WithStaticSeedDiagnostic(validationFailure);
                    return false;
@@ -365,8 +695,11 @@ public sealed class SequentialLongitudinalOptimizer
            if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
                    out LongitudinalCandidate nextLinearization))
            {
+                const string rejection =
+                    "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                AddProjectionTrace(rejection);
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                return false;
            }
            linearizationIterate = nextLinearization;
@@ -374,6 +707,7 @@ public sealed class SequentialLongitudinalOptimizer
                lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
            else if (string.IsNullOrEmpty(lastRejection))
                lastRejection = "Initial feasibility projection violated the strict physical validator.";
+            AddProjectionTrace(lastRejection);
        }
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
@@ -503,7 +837,7 @@ public sealed class SequentialLongitudinalOptimizer
    }

    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate, out string failureReason)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
    {
        candidate = null;
        failureReason = "unknown";
@@ -515,9 +849,11 @@ public sealed class SequentialLongitudinalOptimizer
        }

        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
-        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
+        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
            return true;
-        failureReason = "exactJerkSeed=failed";
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
+            return true;
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed";
        double firstDuration = times[1] - times[0];
        double secondDuration = times[2] - times[1];
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
@@ -526,6 +862,8 @@ public sealed class SequentialLongitudinalOptimizer
                configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
        for (int sample = -256; sample <= 256; sample++)
        {
+            if (HasReachedDeadline(stopwatch, deadline))
+                return false;
            if (sample == 0)
                continue;
            double firstJerk = maximumFirstJerk * sample / 256d;
@@ -545,18 +883,13 @@ public sealed class SequentialLongitudinalOptimizer
                return true;
            }
        }
-        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
-        {
-            failureReason = string.Empty;
-            return true;
-        }
-        failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
        return false;
    }

    private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
    {
        candidate = null;
        int intervalCount = stabilizationStart;
@@ -569,6 +902,8 @@ public sealed class SequentialLongitudinalOptimizer
        {
            for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
            {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                int cruise = intervalCount - 4 * ramp - 2 * plateau;
                var basisAccel = new double[intervalCount];
                var basisBrake = new double[intervalCount];
@@ -688,6 +1023,14 @@ public sealed class SequentialLongitudinalOptimizer

    private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
        int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
+    {
+        return TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, Stopwatch.StartNew(),
+            TimeSpan.MaxValue, out candidate);
+    }
+
+    private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, PathSpeedLimit speedLimit, Stopwatch stopwatch, TimeSpan deadline,
+        out LongitudinalCandidate candidate)
    {
        candidate = null;
        int intervalCount = stabilizationStart;
@@ -747,6 +1090,8 @@ public sealed class SequentialLongitudinalOptimizer
        {
            for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
            {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
                if (direction == null)
                    continue;
@@ -755,6 +1100,8 @@ public sealed class SequentialLongitudinalOptimizer
                double bestViolation = currentViolation;
                for (int sample = -256; sample <= 256; sample++)
                {
+                    if (HasReachedDeadline(stopwatch, deadline))
+                        return false;
                    double scale = maximumJerk * sample / 256d;
                    var probeJerk = new double[intervalCount];
                    for (int interval = 0; interval < intervalCount; interval++)
@@ -776,6 +1123,18 @@ public sealed class SequentialLongitudinalOptimizer
        return false;
    }

+    private static TimeSpan GetStaticStartSeedBudget(TimeSpan totalBudget)
+    {
+        double milliseconds = Math.Min(MaximumStaticStartSeedBudget.TotalMilliseconds,
+            Math.Max(1d, totalBudget.TotalMilliseconds * StaticStartSeedBudgetFraction));
+        return TimeSpan.FromMilliseconds(milliseconds);
+    }
+
+    private static bool HasReachedDeadline(Stopwatch stopwatch, TimeSpan deadline)
+    {
+        return stopwatch.Elapsed >= deadline;
+    }
+
    private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
        int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
        out LongitudinalCandidate candidate)
@@ -1065,72 +1424,6 @@ public sealed class SequentialLongitudinalOptimizer
        return primal;
    }

-    private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
-        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
-    {
-        nextIterate = null;
-        if (candidate.S.Count != previous.S.Count)
-            return false;
-        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
-            ? candidate.S.Count
-            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
-                ? input.KnotSchedule.TerminalHoldStartIndex
-                : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-                    input.Configuration.Scheduling.OutputTimeStepSeconds);
-        var candidateProgressSamples = new double[candidate.S.Count];
-        double priorProgress = double.NegativeInfinity;
-        double priorPreviousProgress = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
-        {
-            double candidateProgress = candidate.S[index];
-            double previousProgress = previous.S[index];
-            if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.PathUpperBoundS ||
-                previousProgress < priorPreviousProgress)
-            {
-                return false;
-            }
-            if (!IsFinite(candidateProgress))
-            {
-                candidateProgress = previousProgress;
-            }
-            candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
-            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
-                candidateProgress = input.StopBoundaryPathS;
-            candidateProgress = Math.Max(priorProgress, candidateProgress);
-            candidateProgressSamples[index] = candidateProgress;
-            priorProgress = candidateProgress;
-            priorPreviousProgress = previousProgress;
-        }
-        var progress = new double[candidate.S.Count];
-        double previousNextProgress = 0d;
-        for (int index = 0; index < progress.Length; index++)
-        {
-            double candidateProgress = candidateProgressSamples[index];
-            if (index == 0 || index == progress.Length - 1 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
-                index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
-            {
-                progress[index] = candidateProgress;
-            }
-            else
-            {
-                double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
-                double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
-                double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
-                double lookaheadAdvance = IsFinite(candidate.U[index])
-                    ? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
-                    : 0d;
-                double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
-                    (input.PathUpperBoundS - candidateProgress);
-                double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
-                progress[index] = candidateProgress + advance;
-            }
-            progress[index] = Math.Max(previousNextProgress, progress[index]);
-            previousNextProgress = progress[index];
-        }
-        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
-        return true;
-    }
-
    private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
    {
@@ -1161,52 +1454,27 @@ public sealed class SequentialLongitudinalOptimizer
            result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
    }

-    private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        LongitudinalCandidate candidate, int iteration)
+    private readonly struct TrustedSolveAttempt
    {
-        int worstIndex = -1;
-        double worstExcess = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
+        internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
+            int solveCount, bool accepted, string failureReason)
        {
-            if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
-                continue;
-            double candidateProgress = Math.Max(0d, Math.Min(speedLimit.PathUpperBoundS, candidate.S[index]));
-            double limit = speedLimit.MaximumSpeedAt(candidateProgress);
-            double excess = candidate.U[index] - limit;
-            if (excess > worstExcess)
-            {
-                worstExcess = excess;
-                worstIndex = index;
-            }
+            Status = status;
+            Candidate = candidate;
+            SolveCount = solveCount;
+            Accepted = accepted;
+            FailureReason = failureReason ?? string.Empty;
        }
-        if (worstIndex < 0)
-            return "";
-        return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
-            " and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
-    }

-    private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
-    {
-        double maximum = 0d;
-        for (int index = 0; index < previous.S.Count; index++)
-        {
-            maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
-            maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
-        }
-        return maximum;
-    }
+        internal EmPlanningStatus Status { get; }

-    private static double RelativeObjectiveImprovement(double previous, double current)
-    {
-        return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
-    }
+        internal LongitudinalCandidate Candidate { get; }

-    private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
-        EmPlanningStatus failureStatus, string failureReason)
-    {
-        return failureStatus == EmPlanningStatus.Cancelled || candidate == null
-            ? Failed(failureStatus, failureReason)
-            : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
+        internal int SolveCount { get; }
+
+        internal bool Accepted { get; }
+
+        internal string FailureReason { get; }
    }

    private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)