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 MaximumEnvelopeIterations = 5;
    private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
    private const double OrdinaryTerminalProbeFraction = 0.5d;
    private readonly IQpSolver _qpSolver;
    private readonly PathSpeedLimitBuilder _speedLimitBuilder;
    private readonly LongitudinalConstraintBuilder _constraintBuilder;
    private readonly LongitudinalSolutionValidator _solutionValidator;

    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();
        LongitudinalCandidate iterate;
        LongitudinalCandidate lastStrictCandidate = null;
        int remainingObjectiveIterations = iterationLimit;
        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))
            {
                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.");
            }
        }
        else
        {
            iterate = CreateInitialIterate(input, speedLimit);
            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
                lastStrictCandidate = null;
        }
        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++)
        {
            if (cancellationToken.IsCancellationRequested)
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
            if (remainingBudget <= TimeSpan.Zero)
            {
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
                    "Longitudinal optimization exhausted its solve budget.");
            }
            if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
            {
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
                    "Longitudinal constraints are infeasible: " + buildFailure);
            }

            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 (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);
            }
            if (solved.Status == QpSolveStatus.Cancelled)
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
                    "The longitudinal QP solver was cancelled: " + solved.Diagnostic);
            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
            {
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
                    "The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
            }
            if (solved.Status == QpSolveStatus.SolverUnavailable)
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
                    "The longitudinal QP solver is unavailable: " + solved.Diagnostic);
            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
            {
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
                    "The longitudinal QP solver failed: " + solved.Diagnostic);
            }
            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
            {
                lastCandidateRejection = "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))
            {
                lastCandidateRejection = "The solver primal does not match the ST variable layout.";
                continue;
            }
            if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate 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))
                {
                    iterate = nextIterate;
                    warmStart = ToPrimal(candidate);
                }
                continue;
            }

            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);
        }

        return lastStrictCandidate == null
            ? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
                lastCandidateRejection)
            : new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
    }

    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(MaximumEnvelopeIterations, 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, 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;
        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, 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);
            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);
            projectionSolveCount++;
            if (solved == null)
            {
                failureStatus = EmPlanningStatus.Failed;
                failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
                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 +
                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
                    solved.DualResidual + "): " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.Cancelled)
            {
                failureStatus = EmPlanningStatus.Cancelled;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.SolverUnavailable)
            {
                failureStatus = EmPlanningStatus.SolverUnavailable;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
            {
                failureStatus = EmPlanningStatus.Failed;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
                return false;
            }
            if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
                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))
                {
                    candidate = strict;
                    return true;
                }
                if (validationStatus == EmPlanningStatus.NoProgress)
                {
                    failureStatus = validationStatus;
                    failureReason = WithStaticSeedDiagnostic(validationFailure);
                    return false;
                }
                lastRejection = validationFailure;
            }

            if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
                    out LongitudinalCandidate nextLinearization))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
                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.";
        }
        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,
        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 (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
            return true;
        failureReason = "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 (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 = "exactJerkSeed=failed; sampledSeeds=failed";
        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)
    {
        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++)
            {
                double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
                if (direction == null)
                    continue;

                double[] bestJerk = jerk;
                double bestViolation = currentViolation;
                for (int sample = -256; sample <= 256; sample++)
                {
                    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 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 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)
    {
        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 static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
        LongitudinalCandidate candidate, int iteration)
    {
        int worstIndex = -1;
        double worstExcess = double.NegativeInfinity;
        for (int index = 0; index < candidate.S.Count; index++)
        {
            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;
            }
        }
        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;
    }

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

    private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
        EmPlanningStatus failureStatus, string failureReason)
    {
        return candidate == null
            ? Failed(failureStatus, failureReason)
            : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
    }

    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;
    }
}