using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Threading;
using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class LongitudinalIntegrationChecks
{
    public static void Run()
    {
        VerifiesLastStrictCandidateSurvivesLaterTimeout();
        VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
        VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
        VerifiesRejectedTerminalPathSStillUpdatesEnvelope();
        VerifiesValidatedEndpointsAreCanonical();
        VerifiesWarmStartAndFiveIterationLimit();
        VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
        VerifiesCancellationInfeasibilityAndPlannerDelegation();
        RunRealOsqpInCleanPluginBundle();
    }

    public static void RunRealOsqp()
    {
        foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
        {
            LongitudinalPlanningResult first = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
                CancellationToken.None);
            LongitudinalPlanningResult second = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
                CancellationToken.None);
            VerifyRealScenario(scenario, first);
            VerifyRealScenario(scenario, second);
            VerifyDeterministicResult(scenario.Name, first, second);
        }
    }

    private static void RunRealOsqpInCleanPluginBundle()
    {
        string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-longitudinal-real-" + Guid.NewGuid().ToString("N"));
        try
        {
            Directory.CreateDirectory(pluginDirectory);
            foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
                File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
            string nativeSource = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory, "..", "..", "..", "..", "..",
                "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
            Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real longitudinal bundle");
            File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), true);

            var startInfo = new ProcessStartInfo
            {
                FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
                Arguments = "longitudinal-real-osqp-probe",
                WorkingDirectory = pluginDirectory,
                UseShellExecute = false,
                CreateNoWindow = true,
                RedirectStandardOutput = true,
                RedirectStandardError = true,
            };
            using (var process = new Process { StartInfo = startInfo })
            {
                process.Start();
                string standardOutput = process.StandardOutput.ReadToEnd();
                string standardError = process.StandardError.ReadToEnd();
                process.WaitForExit();
                if (process.ExitCode != 0 || standardOutput.IndexOf("PASS longitudinal-real-osqp", StringComparison.Ordinal) < 0)
                {
                    throw new InvalidOperationException("Real longitudinal OSQP clean-plugin probe exited " + process.ExitCode + ": " +
                        standardError + standardOutput);
                }
            }
        }
        finally
        {
            if (Directory.Exists(pluginDirectory))
                Directory.Delete(pluginDirectory, true);
        }
    }

    private static void VerifiesLastStrictCandidateSurvivesLaterTimeout()
    {
        LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(valid), 10d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
        });

        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "timeout after strict candidate returns fallback success");
        LongitudinalCandidate fallback = result.Candidate ?? throw new InvalidOperationException("Fallback candidate was missing.");
        Verification.NearlyEqual(valid.S[1], fallback.S[1], "last strict candidate remains the fallback");
    }

    private static void VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks()
    {
        LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
        double[] invalid = ToPrimal(valid);
        var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
        invalid[layout.U(1)] = 10d;
        var invalidSolver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, invalid, 1d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
        });
        LongitudinalPlanningResult invalidResult = new SequentialLongitudinalOptimizer(invalidSolver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, invalidResult.Status,
            "invalid solver vector cannot become fallback");
        Verification.True(invalidResult.Candidate == null, "invalid solver vector publishes no candidate");

        var inaccurateSolver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.SolvedInaccurate, ToPrimal(valid), 1d, 2e-5d, 0d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
        });
        LongitudinalPlanningResult inaccurateResult = new SequentialLongitudinalOptimizer(inaccurateSolver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, inaccurateResult.Status,
            "inaccurate residual candidate cannot become fallback");
        Verification.True(inaccurateResult.Candidate == null, "inaccurate residual publishes no candidate");
        double[] inaccuratePrimal = ToPrimal(valid);
        for (int index = 0; index < inaccuratePrimal.Length; index++)
        {
            Verification.NearlyEqual(inaccuratePrimal[index], inaccurateSolver.WarmStarts[1][index],
                "inaccurate finite primal only warms the next ST QP " + index);
        }
    }

    private static void VerifiesWarmStartAndFiveIterationLimit()
    {
        LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
        var firstSolveOnly = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
        new SequentialLongitudinalOptimizer(firstSolveOnly).Optimize(input, CancellationToken.None);
        Verification.Equal(true, firstSolveOnly.LastSettings != null && firstSolveOnly.LastSettings.EnableWarmStart,
            "first ST solve enables a dynamics-consistent native warm start");
        Verification.True(FromPrimal(valid.KnotTimes, firstSolveOnly.WarmStarts[0]).SatisfiesExactDiscreteDynamics(1e-12d),
            "first ST warm start satisfies exact constant-jerk dynamics");

        var results = new List<QpSolveResult>();
        for (int index = 0; index < 5; index++)
            results.Add(Result(QpSolveStatus.Solved, ToPrimal(valid), 100d - 10d * index));
        var solver = new FakeQpSolver(results);

        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.Success, result.Status, "five solved iterations publish success");
        Verification.Equal(5, solver.SolveCallCount, "ST has a hard five-envelope-iteration maximum");
        Verification.Equal(new LongitudinalVariableLayout(valid.KnotTimes.Count).VariableCount, solver.WarmStarts[0].Count,
            "first ST linearization seed remains a complete primal vector");
        Verification.Equal(true, solver.LastSettings != null && solver.LastSettings.EnableWarmStart,
            "later ST solves enable native warm start");
        for (int index = 0; index < solver.WarmStarts[1].Count; index++)
            Verification.NearlyEqual(ToPrimal(valid)[index], solver.WarmStarts[1][index], "strict candidate warms the next QP " + index);
    }

    private static void VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection()
    {
        LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate candidate);
        var curvedPath = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, candidate.S[1], 10000d),
            Point(2d, baseline.TerminalPathS, 0d),
        }, true);
        var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
            baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
        Verification.True(candidate.U[1] > envelope.MaximumSpeedAt(candidate.S[1]),
            "scripted candidate violates its own curvature speed envelope");
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(candidate), 2d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
        });

        new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
        Verification.Equal(2, solver.SolveCallCount, "rejected candidate reaches the next envelope iteration");
        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
        FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
        double initialProgress = solver.WarmStarts[0][layout.S(1)];
        double timeStep = candidate.KnotTimes[2] - candidate.KnotTimes[1];
        double expectedAdvance = Math.Max(Math.Max(0d, candidate.S[1] - initialProgress), candidate.U[1] * timeStep);
        double expectedProgress = candidate.S[1] + Math.Min(expectedAdvance,
            0.5d * (input.TerminalPathS - candidate.S[1]));
        Verification.True(expectedProgress > candidate.S[1], "scripted rejection advances the ST PathS envelope probe");
        Verification.NearlyEqual(envelope.MaximumSpeedAt(expectedProgress), secondUpper,
            "next ST QP samples the bounded forward-extrapolated PathS envelope");
    }

    private static void VerifiesRejectedTerminalPathSStillUpdatesEnvelope()
    {
        LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate valid);
        double[] perturbedProgress = new double[valid.S.Count];
        for (int index = 0; index < perturbedProgress.Length; index++)
            perturbedProgress[index] = valid.S[index];
        perturbedProgress[perturbedProgress.Length - 1] += 0.05d;
        var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, perturbedProgress, valid.U, valid.A, valid.J);
        var curvedPath = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, valid.S[1], 10000d),
            Point(2d, baseline.TerminalPathS, 0d),
        }, true);
        var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
            baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
        });

        new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
        Verification.Equal(2, solver.SolveCallCount, "rejected terminal PathS still reaches a new envelope iteration");
        var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
        FindSingleVariableBounds(solver.Problems[0], layout.U(1), out _, out double firstUpper);
        FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
        Verification.True(Math.Abs(firstUpper - secondUpper) > 1e-12d,
            "rejected terminal PathS is projected before the next envelope sample");
    }

    private static void VerifiesCancellationInfeasibilityAndPlannerDelegation()
    {
        LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
        using (var cancellation = new CancellationTokenSource())
        {
            cancellation.Cancel();
            var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d));
            LongitudinalPlanningResult cancelled = new SequentialLongitudinalOptimizer(cancellationSolver).Optimize(input,
                cancellation.Token);
            Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before QP solve");
            Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled ST does not call the QP solver");
        }

        var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
        LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.LongitudinalInfeasible, infeasible.Status, "QP infeasibility is longitudinal");

        var plannerSolver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(valid), 2d),
            Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
            Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
        });
        LongitudinalPlanningResult delegated = new LongitudinalPlanner(plannerSolver).Plan(input, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.Success, delegated.Status, "LongitudinalPlanner delegates to the ST optimizer");
    }

    private static void VerifiesNonzeroSpeedSeedIsStrictlyFeasible()
    {
        LongitudinalPlanningInput input = CreateRealScenario("seed", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d).Input;
        var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
            CancellationToken.None);
        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
            input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
        LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "nonzero-speed seed envelope: " + speedFailure);
        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
            out string validationFailure), "nonzero-speed seed is strictly feasible: " + validationFailure);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "strictly validated initial seed survives an immediate solver timeout");
        Verification.True(result.Candidate != null,
            "strictly validated initial seed is retained as the timeout fallback");
    }

    private static void VerifiesValidatedEndpointsAreCanonical()
    {
        LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
        double toleranceOffset = 0.5d * input.Configuration.Validation.KinematicTolerance;
        double[] progress = new double[valid.S.Count];
        double[] speed = new double[valid.U.Count];
        for (int index = 0; index < progress.Length; index++)
        {
            progress[index] = valid.S[index];
            speed[index] = valid.U[index];
        }
        progress[progress.Length - 1] += toleranceOffset;
        speed[speed.Length - 1] += toleranceOffset;
        var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, progress, speed, valid.A, valid.J);
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
        });

        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
            CancellationToken.None);

        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "canonical strict candidate remains the timeout fallback");
        LongitudinalCandidate canonical = result.Candidate ??
            throw new InvalidOperationException("Canonical fallback candidate was missing.");
        Verification.Equal(input.TerminalPathS, canonical.S[canonical.S.Count - 1],
            "validated terminal PathS is canonicalized exactly");
        Verification.Equal(0d, canonical.U[canonical.U.Count - 1],
            "validated terminal speed is canonicalized exactly");
    }

    private static IReadOnlyList<LongitudinalScenario> CreateRealOsqpScenarios()
    {
        return new[]
        {
            CreateRealScenario("forward", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
            CreateRealScenario("reverse", TravelDirection.Reverse, 0.50d, 0d, 0d, 0d),
            CreateRealScenario("curvature-limited", TravelDirection.Forward, 0.35d, 20d, 0d, 0d),
            CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d),
            CreateRealScenario("short-segment", TravelDirection.Forward, 0.05d, 0d, 0d, 0d),
            CreateRealScenario("zero-start-speed", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
        };
    }

    private static LongitudinalScenario CreateRealScenario(string name, TravelDirection direction, double terminalPathS,
        double middleCurvature, double initialSpeed, double initialAcceleration)
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Validation.KinematicTolerance = 1e-5d;
        var points = new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, terminalPathS * 0.5d, middleCurvature),
            Point(2d, terminalPathS, 0d),
        };
        return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
            initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
            configuration, Array.Empty<double>(), Array.Empty<double>()));
    }

    private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
    {
        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            scenario.Name + " returns a strict profile: " + result.FailureReason);
        LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException(scenario.Name + " candidate missing.");
        Verification.NearlyEqual(scenario.Input.TerminalPathS, candidate.S[candidate.S.Count - 1],
            scenario.Name + " exact terminal PathS");
        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], scenario.Name + " exact terminal speed");
        PathSpeedLimitBuilder builder = new PathSpeedLimitBuilder();
        EmPlanningStatus speedStatus = builder.Build(scenario.Input, out PathSpeedLimit envelope, out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, scenario.Name + " envelope: " + speedFailure);
        Verification.True(new LongitudinalSolutionValidator().TryValidate(scenario.Input, envelope, candidate,
            out _, out string validationFailure), scenario.Name + " strict physical validation: " + validationFailure);
    }

    private static void VerifyDeterministicResult(string name, LongitudinalPlanningResult first, LongitudinalPlanningResult second)
    {
        Verification.Equal(first.Status, second.Status, name + " deterministic status");
        LongitudinalCandidate left = first.Candidate ?? throw new InvalidOperationException(name + " first candidate missing.");
        LongitudinalCandidate right = second.Candidate ?? throw new InvalidOperationException(name + " second candidate missing.");
        for (int index = 0; index < left.S.Count; index++)
        {
            Verification.NearlyEqual(left.S[index], right.S[index], name + " deterministic S " + index);
            Verification.NearlyEqual(left.U[index], right.U[index], name + " deterministic U " + index);
            Verification.NearlyEqual(left.A[index], right.A[index], name + " deterministic A " + index);
        }
        for (int index = 0; index < left.J.Count; index++)
            Verification.NearlyEqual(left.J[index], right.J[index], name + " deterministic J " + index);
    }

    private static LongitudinalPlanningInput CreateFakeInput(out LongitudinalCandidate valid)
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 1d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.25d;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(1d, 0.25d);
        valid = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0d,
            new[] { -0.45714285714285714d, 0d, 0d, 0d });
        double terminalPathS = valid.S[valid.S.Count - 1];
        var path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, terminalPathS * 0.5d, 0d),
            Point(2d, terminalPathS, 0d),
        }, true);
        return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
            EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
    }

    private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
    {
        return new LateralPathPoint(referenceS, pathS, 0d, 0d, 0d, 0d, pathS, 0d, 0d, curvature, curvature, 0d);
    }

    private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
        double primalResidual = 0d, double dualResidual = 0d)
    {
        return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
            status.ToString(), string.Empty);
    }

    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 LongitudinalCandidate FromPrimal(IReadOnlyList<double> times, IReadOnlyList<double> primal)
    {
        var layout = new LongitudinalVariableLayout(times.Count);
        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 < j.Length; index++)
            j[index] = primal[layout.J(index)];
        return new LongitudinalCandidate(times, s, u, a, j);
    }

    private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
    {
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            int found = 0;
            double coefficient = 0d;
            for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
            {
                for (int index = problem.ConstraintMatrix.ColumnPointers[column];
                    index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
                {
                    if (problem.ConstraintMatrix.RowIndices[index] == row)
                    {
                        found++;
                        if (column == variable)
                            coefficient = problem.ConstraintMatrix.Values[index];
                    }
                }
            }
            if (found == 1 && Math.Abs(coefficient - 1d) <= 1e-12d)
            {
                lower = problem.LowerBounds[row];
                upper = problem.UpperBounds[row];
                return;
            }
        }
        throw new InvalidOperationException("No single-variable bounds were found for ST variable " + variable + ".");
    }

    private sealed class LongitudinalScenario
    {
        public LongitudinalScenario(string name, LongitudinalPlanningInput input)
        {
            Name = name;
            Input = input;
        }

        public string Name { get; }

        public LongitudinalPlanningInput Input { get; }
    }
}