using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
public sealed class SequentialLongitudinalOptimizer
{
    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()),
            new LongitudinalSolutionValidator())
    {
    }

    internal SequentialLongitudinalOptimizer(IQpSolver qpSolver, PathSpeedLimitBuilder speedLimitBuilder,
        LongitudinalConstraintBuilder constraintBuilder, LongitudinalSolutionValidator solutionValidator)
    {
        _qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
        _speedLimitBuilder = speedLimitBuilder ?? throw new ArgumentNullException(nameof(speedLimitBuilder));
        _constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
        _solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
    }

    public LongitudinalPlanningResult Optimize(LongitudinalPlanningInput input, CancellationToken cancellationToken)
    {
        if (input == null)
            return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
        if (cancellationToken.IsCancellationRequested)
            return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
        {
            double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
                input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
            if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
                Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
                    input.Configuration.Validation.KinematicTolerance)
            {
                input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d,
                    input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode,
                    input.Configuration, input.PlanningScope, input.KnotSchedule,
                    input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond);
            }
        }
        if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
                out int iterationLimit, out string configurationFailure))
        {
            return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
        }

        EmPlanningStatus speedStatus = _speedLimitBuilder.Build(input, out PathSpeedLimit speedLimit, out string speedFailure);
        if (speedStatus != EmPlanningStatus.Success)
            return Failed(speedStatus, speedFailure);

        var stopwatch = Stopwatch.StartNew();
        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, solveTrace, cancellationToken, out initialCandidate,
                    out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
                    out string projectionFailure))
            {
                string projectionTrace = solveTrace.Format();
                return Failed(projectionStatus, projectionFailure +
                    (string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
            }
            projectionSolveCount = usedProjectionSolveCount;
        }
        else
        {
            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);
            }
        }

        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 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 new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
                    "Longitudinal optimization exhausted its shared solve budget.");
            }
            bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
                out TimeSpan remainingAfterReserve);
            if (optionalImprovement && !hasOptionalSolveBudget)
            {
                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);
            }

            if (cancellationToken.IsCancellationRequested)
            {
                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)
            {
                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)
            {
                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)
            {
                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)
            {
                string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
                    solved.Diagnostic;
                AddSolveTrace(rejection);
                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
                    rejection);
            }

            if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
            {
                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 (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
                    stabilizationStart, out lastFailure))
            {
                accepted = false;
            }
            LongitudinalCandidate validated = null;
            if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
                    out string validationFailure))
            {
                accepted = false;
                lastFailure = validationFailure;
            }
            if (accepted)
            {
                candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
                if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
                {
                    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) + ").";
                }
            }
            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);
            }

            AddSolveTrace(lastFailure);
            warmStart = ToPrimal(candidate);
        }

        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,
        out TimeSpan totalBudget, out double convergenceTolerance, out int iterationLimit, out string failureReason)
    {
        settings = null;
        totalBudget = TimeSpan.Zero;
        convergenceTolerance = 0d;
        iterationLimit = 0;
        failureReason = string.Empty;
        if (input.Configuration == null || input.Configuration.Solver == null || input.Configuration.Scheduling == null)
        {
            failureReason = "Longitudinal solver configuration is required.";
            return false;
        }
        SolverConfiguration solver = input.Configuration.Solver;
        SchedulingConfiguration scheduling = input.Configuration.Scheduling;
        if (solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
            !IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
            !IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
        {
            failureReason = "Longitudinal solver configuration is invalid.";
            return false;
        }
        try
        {
            totalBudget = TimeSpan.FromSeconds(scheduling.SolverTimeoutSeconds);
            settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
                totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
            convergenceTolerance = solver.StrictResidualTolerance;
            iterationLimit = Math.Min(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
            return true;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return false;
        }
    }

    private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
        QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
        Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
        out LongitudinalCandidate candidate, out int projectionSolveCount,
        out EmPlanningStatus failureStatus, out string failureReason)
    {
        candidate = null;
        projectionSolveCount = 0;
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
        double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
            input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
        bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
            Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
                input.Configuration.Validation.KinematicTolerance;
        bool staticStartSeedUsed = false;
        string staticStartSeedFailure = string.Empty;
        TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
                out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
        {
            staticStartSeedUsed = true;
            candidate = staticStartSeed;
            return true;
        }
        string WithStaticSeedDiagnostic(string reason)
        {
            return staticStartEligible && !staticStartSeedUsed
                ? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason
                : reason;
        }

        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
        string lastRejection = string.Empty;
        for (int iteration = 0; iteration < iterationLimit; iteration++)
        {
            if (cancellationToken.IsCancellationRequested)
            {
                failureStatus = EmPlanningStatus.Cancelled;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled.");
                return false;
            }
            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
            if (remainingBudget <= TimeSpan.Zero)
            {
                failureStatus = EmPlanningStatus.SolverTimedOut;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the shared solve budget.");
                return false;
            }
            if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
                    out QuadraticProgram problem, out string buildFailure))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure);
                return false;
            }

            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(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(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
            {
                string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
                    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(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(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(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(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(rejection);
                return false;
            }
            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
            {
                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;
                }
                lastRejection = validationFailure;
            }

            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(rejection);
                return false;
            }
            linearizationIterate = nextLinearization;
            if (solved.Status == QpSolveStatus.SolvedInaccurate)
                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. " +
            lastRejection);
        return false;
    }

    private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
    {
        int knotCount = input.KnotSchedule.KnotTimes.Count;
        return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
            input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
    }

    private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit)
    {
        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        switch (input.Mode)
        {
            case EmLongitudinalMode.RollingContinuation:
                return CreateRollingSeed(input, times, speedLimit);
            case EmLongitudinalMode.ApproachStopBoundary:
                return CreateApproachSeed(input, times, speedLimit);
            case EmLongitudinalMode.ExactStopAtBoundary:
                return CreateExactStopSeed(input, times, speedLimit);
            default:
                throw new ArgumentOutOfRangeException(nameof(input.Mode));
        }
    }

    private static LongitudinalCandidate CreateRollingSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
        return CreateEnvelopeSeed(input, times, speedLimit);
    }

    private static LongitudinalCandidate CreateApproachSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
        return CreateEnvelopeSeed(input, times, speedLimit);
    }

    private static LongitudinalCandidate CreateEnvelopeSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        var jerk = new double[times.Count - 1];
        double s = 0d;
        double u = input.InitialProgressSpeedMetersPerSecond;
        double a = input.InitialAccelerationMetersPerSecondSquared;
        for (int index = 0; index < jerk.Length; index++)
        {
            double dt = times[index + 1] - times[index];
            double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
            double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS);
            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
            {
                double desiredSpeed = input.Direction == TravelDirection.Forward
                    ? configuration.DesiredForwardSpeedMetersPerSecond
                    : configuration.DesiredReverseSpeedMetersPerSecond;
                double scheduleSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index];
                targetSpeed = Math.Min(desiredSpeed, Math.Min(scheduleSpeed, speedLimitAtS));
            }
            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
                (-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt);
            lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt));
            double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
                (configuration.MaximumAccelerationMetersPerSecondSquared - a) / dt);
            double requestedJerk = 2d * (targetSpeed - u - a * dt) / (dt * dt);
            double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
            IntegrateStep(s, u, a, selectedJerk, dt, out double nextS, out double nextU, out double nextA);
            if (nextU > speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, nextS))) + 1e-12d)
            {
                double lower = lowerJerk;
                double upper = selectedJerk;
                for (int iteration = 0; iteration < 48; iteration++)
                {
                    double midpoint = 0.5d * (lower + upper);
                    IntegrateStep(s, u, a, midpoint, dt, out double probeS, out double probeU, out _);
                    if (probeU <= speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, probeS))))
                        lower = midpoint;
                    else
                        upper = midpoint;
                }
                selectedJerk = lower;
                IntegrateStep(s, u, a, selectedJerk, dt, out nextS, out nextU, out nextA);
            }
            jerk[index] = selectedJerk;
            s = nextS;
            u = nextU;
            a = nextA;
        }
        return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
            input.InitialAccelerationMetersPerSecondSquared, jerk);
    }

    private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
        {
            throw new InvalidOperationException("Full-direction exact-stop planning requires the initial feasibility projection.");
        }
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
            input.Configuration.Scheduling.OutputTimeStepSeconds);
        var motionTimes = new double[stabilizationStart + 1];
        for (int index = 0; index < motionTimes.Length; index++)
            motionTimes[index] = times[index];

        if (TryCreateCruiseThenBrakeSeed(input, motionTimes, input.StopBoundaryPathS,
                out LongitudinalCandidate cruiseThenBrake))
        {
            LongitudinalCandidate candidate = AppendExactStopTail(times, stabilizationStart,
                input.StopBoundaryPathS, cruiseThenBrake);
            if (_solutionValidator.TryValidate(input, speedLimit, candidate,
                    out LongitudinalCandidate validated, out _))
            {
                return validated;
            }
        }

        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit,
                out LongitudinalCandidate exactSeed))
            return exactSeed;

        return CreateApproachSeed(input, times, speedLimit);
    }

    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
    {
        candidate = null;
        failureReason = "unknown";
        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
        if (stabilizationStart < 5)
        {
            failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture);
            return false;
        }

        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
            return true;
        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;
        double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
            Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
                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;
            var jerk = new double[stabilizationStart];
            jerk[0] = firstJerk;
            jerk[1] = -firstJerk * firstDuration / secondDuration;
            if (!TryCloseExactStopEndpoint(input, times, stabilizationStart, jerk,
                    out LongitudinalCandidate probe))
            {
                continue;
            }
            if (_solutionValidator.TryValidate(input, speedLimit, probe,
                    out LongitudinalCandidate strict, out _))
            {
                candidate = strict;
                failureReason = string.Empty;
                return true;
            }
        }
        failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
        return false;
    }

    private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
    {
        candidate = null;
        int intervalCount = stabilizationStart;
        int maximumRamp = Math.Max(1, intervalCount / 6);
        var motionTimes = new double[intervalCount + 1];
        for (int index = 0; index <= intervalCount; index++)
            motionTimes[index] = times[index];

        for (int ramp = 1; ramp <= maximumRamp; ramp++)
        {
            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];
                var basisOffset = new double[intervalCount];
                int cursor = 0;
                for (int index = 0; index < ramp; index++)
                    basisAccel[cursor++] = 1d;
                for (int index = 0; index < plateau; index++)
                    cursor++;
                for (int index = 0; index < ramp; index++)
                    basisAccel[cursor++] = -1d;
                for (int index = 0; index < cruise; index++)
                    cursor++;
                for (int index = 0; index < ramp; index++)
                    basisBrake[cursor++] = -1d;
                for (int index = 0; index < plateau; index++)
                    cursor++;
                for (int index = 0; index < ramp; index++)
                    basisBrake[cursor++] = 1d;
                if (cursor != intervalCount)
                    continue;
                for (int index = 0; index < intervalCount; index++)
                    basisOffset[index] = 1d;

                LongitudinalCandidate accelResponse = LongitudinalCandidate.Integrate(
                    motionTimes, 0d, 0d, 0d, basisAccel);
                LongitudinalCandidate brakeResponse = LongitudinalCandidate.Integrate(
                    motionTimes, 0d, 0d, 0d, basisBrake);
                LongitudinalCandidate offsetResponse = LongitudinalCandidate.Integrate(
                    motionTimes, 0d, 0d, 0d, basisOffset);
                var influence = new double[3, 3];
                influence[0, 0] = accelResponse.A[intervalCount];
                influence[1, 0] = accelResponse.U[intervalCount];
                influence[2, 0] = accelResponse.S[intervalCount];
                influence[0, 1] = brakeResponse.A[intervalCount];
                influence[1, 1] = brakeResponse.U[intervalCount];
                influence[2, 1] = brakeResponse.S[intervalCount];
                influence[0, 2] = offsetResponse.A[intervalCount];
                influence[1, 2] = offsetResponse.U[intervalCount];
                influence[2, 2] = offsetResponse.S[intervalCount];

                double[] target = { 0d, 0d, input.StopBoundaryPathS };
                if (!TrySolveThreeByThree(influence, target, out double[] multipliers))
                    continue;

                var jerk = new double[intervalCount];
                for (int index = 0; index < intervalCount; index++)
                {
                    jerk[index] = multipliers[0] * basisAccel[index] +
                        multipliers[1] * basisBrake[index] +
                        multipliers[2] * basisOffset[index];
                }
                if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
                    return true;
            }
        }
        return false;
    }

    private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
        double stopBoundaryPathS, out LongitudinalCandidate candidate)
    {
        candidate = null;
        double initialSpeed = input.InitialProgressSpeedMetersPerSecond;
        double initialAcceleration = input.InitialAccelerationMetersPerSecondSquared;
        if (initialSpeed <= 0d || Math.Abs(initialAcceleration) > 1e-12d)
            return false;
        double timeStep = times[1] - times[0];
        for (int index = 1; index < times.Count - 1; index++)
        {
            if (Math.Abs((times[index + 1] - times[index]) - timeStep) > 1e-12d)
                return false;
        }
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        int intervalCount = times.Count - 1;
        int maximumRampIntervals = Math.Min(intervalCount / 2, checked((int)Math.Floor(
            configuration.MaximumDecelerationMetersPerSecondSquared /
            (configuration.MaximumJerkMetersPerSecondCubed * timeStep))));
        for (int rampIntervals = maximumRampIntervals; rampIntervals >= 1; rampIntervals--)
        {
            for (int plateauIntervals = 0; 2 * rampIntervals + plateauIntervals <= intervalCount; plateauIntervals++)
            {
                double jerkMagnitude = initialSpeed / (rampIntervals * (rampIntervals + plateauIntervals) *
                    timeStep * timeStep);
                double peakDeceleration = jerkMagnitude * rampIntervals * timeStep;
                if (jerkMagnitude > configuration.MaximumJerkMetersPerSecondCubed + 1e-12d ||
                    peakDeceleration > configuration.MaximumDecelerationMetersPerSecondSquared + 1e-12d)
                {
                    continue;
                }
                int brakingIntervals = 2 * rampIntervals + plateauIntervals;
                double brakingDistance = 0.5d * initialSpeed * brakingIntervals * timeStep;
                if (brakingDistance > stopBoundaryPathS + 1e-12d)
                    continue;
                int maximumCruiseIntervals = intervalCount - brakingIntervals;
                int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
                    (stopBoundaryPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
                var jerk = new double[intervalCount];
                int cursor = cruiseIntervals;
                for (int index = 0; index < rampIntervals; index++)
                    jerk[cursor++] = -jerkMagnitude;
                cursor += plateauIntervals;
                for (int index = 0; index < rampIntervals; index++)
                    jerk[cursor++] = jerkMagnitude;
                LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, initialSpeed, 0d, jerk);
                int lastIndex = integrated.S.Count - 1;
                if (Math.Abs(integrated.S[lastIndex] - stopBoundaryPathS) <= 1e-10d &&
                    Math.Abs(integrated.U[lastIndex]) <= 1e-10d && Math.Abs(integrated.A[lastIndex]) <= 1e-10d)
                {
                    candidate = integrated;
                    return true;
                }
            }
        }
        return false;
    }

    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;
        if (intervalCount < 3)
            return false;

        var motionTimes = new double[intervalCount + 1];
        for (int index = 0; index < motionTimes.Length; index++)
            motionTimes[index] = times[index];

        LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit);
        var influence = new double[3, intervalCount];
        for (int interval = 0; interval < intervalCount; interval++)
        {
            var basis = new double[intervalCount];
            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[] target =
        {
            -baseline.A[baseline.A.Count - 1],
            -baseline.U[baseline.U.Count - 1],
            input.StopBoundaryPathS - baseline.S[baseline.S.Count - 1],
        };
        var gram = new double[3, 3];
        for (int row = 0; row < 3; row++)
        {
            for (int column = 0; column < 3; column++)
            {
                for (int interval = 0; interval < intervalCount; interval++)
                    gram[row, column] += influence[row, interval] * influence[column, interval];
            }
        }
        if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
            return false;

        var jerk = new double[intervalCount];
        for (int interval = 0; interval < intervalCount; interval++)
        {
            jerk[interval] = baseline.J[interval];
            for (int row = 0; row < 3; row++)
                jerk[interval] += influence[row, interval] * multipliers[row];
        }

        if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
            return true;

        double currentViolation = CalculateExactSeedViolation(input, speedLimit,
            CreateExactCandidate(input, times, stabilizationStart, jerk));
        double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
        for (int pass = 0; pass < 4; pass++)
        {
            for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
            {
                if (HasReachedDeadline(stopwatch, deadline))
                    return false;
                double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
                if (direction == null)
                    continue;

                double[] bestJerk = jerk;
                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++)
                        probeJerk[interval] = jerk[interval] + scale * direction[interval];
                    LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, probeJerk);
                    double violation = CalculateExactSeedViolation(input, speedLimit, probe);
                    if (violation < bestViolation)
                    {
                        bestViolation = violation;
                        bestJerk = probeJerk;
                    }
                }
                jerk = bestJerk;
                currentViolation = bestViolation;
                if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
                    return true;
            }
        }
        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)
    {
        LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, jerk);
        return _solutionValidator.TryValidate(input, speedLimit, probe, out candidate, out _);
    }

    private static LongitudinalCandidate CreateExactCandidate(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, int stabilizationStart, IReadOnlyList<double> jerk)
    {
        var motionTimes = new double[stabilizationStart + 1];
        for (int index = 0; index < motionTimes.Length; index++)
            motionTimes[index] = times[index];
        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
            input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
        return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
    }

    private static bool TryCloseExactStopEndpoint(LongitudinalPlanningInput input, IReadOnlyList<double> times,
        int stabilizationStart, double[] jerk, out LongitudinalCandidate candidate)
    {
        candidate = null;
        var motionTimes = new double[stabilizationStart + 1];
        for (int index = 0; index < motionTimes.Length; index++)
            motionTimes[index] = times[index];
        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
            input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
        int terminalIndex = motion.S.Count - 1;
        double[] correction =
        {
            -motion.A[terminalIndex],
            -motion.U[terminalIndex],
            input.StopBoundaryPathS - motion.S[terminalIndex],
        };
        var influence = new double[3, 3];
        for (int basisIndex = 0; basisIndex < 3; basisIndex++)
        {
            var basis = new double[stabilizationStart];
            basis[stabilizationStart - 3 + basisIndex] = 1d;
            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
            influence[0, basisIndex] = response.A[terminalIndex];
            influence[1, basisIndex] = response.U[terminalIndex];
            influence[2, basisIndex] = response.S[terminalIndex];
        }
        if (!TrySolveThreeByThree(influence, correction, out double[] adjustment))
            return false;
        for (int index = 0; index < adjustment.Length; index++)
            jerk[stabilizationStart - 3 + index] += adjustment[index];
        candidate = CreateExactCandidate(input, times, stabilizationStart, jerk);
        return true;
    }

    private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        var jerk = new double[times.Count - 1];
        double speed = input.InitialProgressSpeedMetersPerSecond;
        double acceleration = input.InitialAccelerationMetersPerSecondSquared;
        for (int index = 0; index < jerk.Length; index++)
        {
            double dt = times[index + 1] - times[index];
            double targetSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index + 1];
            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
                (-configuration.MaximumDecelerationMetersPerSecondSquared - acceleration) / dt);
            double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
                (configuration.MaximumAccelerationMetersPerSecondSquared - acceleration) / dt);
            double requestedJerk = 2d * (targetSpeed - speed - acceleration * dt) / (dt * dt);
            double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
            jerk[index] = selectedJerk;
            IntegrateStep(0d, speed, acceleration, selectedJerk, dt, out _, out speed, out acceleration);
        }
        return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
            input.InitialAccelerationMetersPerSecondSquared, jerk);
    }

    private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex)
    {
        int intervalCount = influence.GetLength(1);
        double[] rightHandSide = { influence[0, basisIndex], influence[1, basisIndex], influence[2, basisIndex] };
        if (!TrySolveThreeByThree(gram, rightHandSide, out double[] multipliers))
            return null;
        var direction = new double[intervalCount];
        double magnitude = 0d;
        for (int interval = 0; interval < intervalCount; interval++)
        {
            direction[interval] = interval == basisIndex ? 1d : 0d;
            for (int row = 0; row < 3; row++)
                direction[interval] -= influence[row, interval] * multipliers[row];
            magnitude = Math.Max(magnitude, Math.Abs(direction[interval]));
        }
        if (magnitude <= 1e-12d)
            return null;
        for (int interval = 0; interval < intervalCount; interval++)
            direction[interval] /= magnitude;
        return direction;
    }

    private static double CalculateExactSeedViolation(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
        LongitudinalCandidate candidate)
    {
        double tolerance = input.Configuration.Validation.KinematicTolerance;
        double maximumAcceleration = input.Configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared;
        double maximumDeceleration = input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
        double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
        double violation = 0d;
        double previousS = double.NegativeInfinity;
        for (int index = 0; index < candidate.S.Count; index++)
        {
            double s = candidate.S[index];
            double u = candidate.U[index];
            double a = candidate.A[index];
            if (!IsFinite(s) || !IsFinite(u) || !IsFinite(a))
                return double.PositiveInfinity;
            violation += SquaredExcess(-s, tolerance);
            violation += SquaredExcess(s - input.PathUpperBoundS, tolerance);
            violation += SquaredExcess(previousS - s, tolerance);
            violation += SquaredExcess(-u, tolerance);
            violation += SquaredExcess(a - maximumAcceleration, tolerance);
            violation += SquaredExcess(-maximumDeceleration - a, tolerance);
            double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
            violation += SquaredExcess(u - speedLimitAtS, tolerance);
            if (!JerkLimitedStoppingMath.TryCalculate(u, a, maximumDeceleration, maximumJerk,
                    out JerkLimitedStoppingProfile stop, out _))
            {
                return double.PositiveInfinity;
            }
            violation += SquaredExcess(s + stop.DistanceMeters - input.StopBoundaryPathS, tolerance);
            previousS = s;
        }
        for (int index = 0; index < candidate.J.Count; index++)
        {
            if (!IsFinite(candidate.J[index]))
                return double.PositiveInfinity;
            violation += SquaredExcess(Math.Abs(candidate.J[index]) - maximumJerk, tolerance);
        }
        return violation;
    }

    private static double SquaredExcess(double actual, double tolerance)
    {
        double excess = Math.Max(0d, actual - tolerance);
        return excess * excess;
    }

    private static LongitudinalCandidate AppendExactStopTail(IReadOnlyList<double> times, int stabilizationStart,
        double stopBoundaryPathS, LongitudinalCandidate motion)
    {
        var s = new double[times.Count];
        var u = new double[times.Count];
        var a = new double[times.Count];
        var jerk = new double[times.Count - 1];
        int motionCount = Math.Min(stabilizationStart + 1, motion.S.Count);
        for (int index = 0; index < motionCount; index++)
        {
            s[index] = motion.S[index];
            u[index] = motion.U[index];
            a[index] = motion.A[index];
        }
        for (int index = 0; index < Math.Min(stabilizationStart, motion.J.Count); index++)
            jerk[index] = motion.J[index];
        for (int index = stabilizationStart; index < times.Count; index++)
        {
            s[index] = stopBoundaryPathS;
            u[index] = 0d;
            a[index] = 0d;
        }
        return new LongitudinalCandidate(times, s, u, a, jerk);
    }

    private static bool SatisfiesLongitudinalBounds(LongitudinalPlanningInput input, LongitudinalCandidate candidate)
    {
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        double previousS = double.NegativeInfinity;
        for (int index = 0; index < candidate.S.Count; index++)
        {
            if (candidate.S[index] < -1e-10d || candidate.S[index] > input.StopBoundaryPathS + 1e-10d ||
                candidate.S[index] < previousS - 1e-10d || candidate.U[index] < -1e-10d ||
                candidate.U[index] > input.DirectionMaximumSpeedMetersPerSecond + 1e-10d ||
                candidate.A[index] < -configuration.MaximumDecelerationMetersPerSecondSquared - 1e-10d ||
                candidate.A[index] > configuration.MaximumAccelerationMetersPerSecondSquared + 1e-10d)
            {
                return false;
            }
            previousS = candidate.S[index];
        }
        for (int index = 0; index < candidate.J.Count; index++)
        {
            if (Math.Abs(candidate.J[index]) > configuration.MaximumJerkMetersPerSecondCubed + 1e-10d)
                return false;
        }
        return true;
    }

    private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
        out double[] solution)
    {
        solution = new double[3];
        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;
            }
            if (Math.Abs(augmented[bestRow, pivot]) <= 1e-14d)
                return false;
            if (bestRow != pivot)
            {
                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];
            }
        }
        for (int row = 0; row < 3; row++)
            solution[row] = augmented[row, 3];
        return true;
    }

    private static bool TryCreateCandidate(IReadOnlyList<double> times, IReadOnlyList<double> primal,
        out LongitudinalCandidate candidate)
    {
        candidate = null;
        if (primal == null)
            return false;
        try
        {
            var layout = new LongitudinalVariableLayout(times.Count);
            if (primal.Count != layout.VariableCount)
                return false;
            var s = new double[layout.KnotCount];
            var u = new double[layout.KnotCount];
            var a = new double[layout.KnotCount];
            var j = new double[layout.KnotCount - 1];
            for (int index = 0; index < layout.KnotCount; index++)
            {
                s[index] = primal[layout.S(index)];
                u[index] = primal[layout.U(index)];
                a[index] = primal[layout.A(index)];
            }
            for (int index = 0; index < layout.KnotCount - 1; index++)
                j[index] = primal[layout.J(index)];
            candidate = new LongitudinalCandidate(times, s, u, a, j);
            return true;
        }
        catch (ArgumentException)
        {
            return false;
        }
    }

    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 bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
    {
        nextIterate = null;
        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
        var pathS = new double[candidate.S.Count];
        double previousPathS = double.NegativeInfinity;
        double tolerance = input.Configuration.Validation.KinematicTolerance;
        for (int index = 0; index < pathS.Length; index++)
        {
            double value = candidate.S[index];
            if (!IsFinite(value) || value < -tolerance || value > input.PathUpperBoundS + tolerance ||
                value < previousPathS - tolerance)
            {
                return false;
            }
            value = Math.Max(0d, Math.Min(input.PathUpperBoundS, value));
            pathS[index] = index >= stabilizationStart ? input.StopBoundaryPathS : Math.Max(previousPathS, value);
            previousPathS = pathS[index];
        }
        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, pathS, candidate.U, candidate.A, candidate.J);
        return true;
    }

    private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
    {
        return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&
            result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
    }

    private readonly struct TrustedSolveAttempt
    {
        internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
            int solveCount, bool accepted, string failureReason)
        {
            Status = status;
            Candidate = candidate;
            SolveCount = solveCount;
            Accepted = accepted;
            FailureReason = failureReason ?? string.Empty;
        }

        internal EmPlanningStatus Status { get; }

        internal LongitudinalCandidate Candidate { get; }

        internal int SolveCount { get; }

        internal bool Accepted { get; }

        internal string FailureReason { get; }
    }

    private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)
    {
        return new LongitudinalPlanningResult(status, null, reason);
    }

    private static LongitudinalCandidate CopyCandidate(LongitudinalCandidate source)
    {
        return new LongitudinalCandidate(source.KnotTimes, source.S, source.U, source.A, source.J);
    }

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    private static double Clamp(double value, double minimum, double maximum)
    {
        return Math.Max(minimum, Math.Min(maximum, value));
    }

    private static void IntegrateStep(double s, double u, double a, double jerk, double duration,
        out double nextS, out double nextU, out double nextA)
    {
        nextS = s + u * duration + 0.5d * a * duration * duration +
            jerk * duration * duration * duration / 6d;
        nextU = u + a * duration + 0.5d * jerk * duration * duration;
        nextA = a + jerk * duration;
    }
}