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()
    {
        VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
        VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
        VerifiesFullDirectionStaticStartMakesProgress();
        VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
        VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed();
        VerifiesFullDirectionNearZeroScheduleDurationIsReasonable();
        VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
        VerifiesExactStopIncludesAStabilizationTail();
        VerifiesLastStrictCandidateSurvivesLaterTimeout();
        VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
        VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
        VerifiesExactStopTailDoesNotDistortEnvelope();
        VerifiesValidatedEndpointsAreCanonical();
        VerifiesWarmStartAndFiveIterationLimit();
        VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
        VerifiesCancellationInfeasibilityAndPlannerDelegation();
        RunRealOsqpInCleanPluginBundle();
    }

    private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.DistanceHorizonMeters = 5d;
        configuration.Scheduling.TimeHorizonSeconds = 2d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 2.5d, 0d),
            Point(2d, 5d, 0d),
        }, true);
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));

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

        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "rolling seed remains a strict timeout fallback: " + result.FailureReason);
        LongitudinalCandidate candidate = result.Candidate ??
            throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
        Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots");
        Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
            "two-second ST does not consume the five-metre LS window");
        Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,
            "rolling ST keeps nonzero terminal speed");
    }

    private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 10d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 1d, 0d),
            Point(2d, 2d, 0d),
        }, true);

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule coarsePublication, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);

        EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
        densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
            densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
            out LongitudinalKnotSchedule densePublication, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
        Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
            "publication cadence does not determine full-segment optimization knot count");

        var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
            new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
            new[] { 0.80d, 0d });
        var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
        DateTimeOffset now = DateTimeOffset.UtcNow;
        var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
            string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
            EmPlanningScope.FullDirectionSegment);
        configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
        densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
        EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
        EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
            publicationResult, metadata);
        Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
            "halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
    }

    private static void VerifiesFullDirectionStaticStartMakesProgress()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(2.5d, 2.5d, 0d),
            Point(5d, 5d, 0d),
        }, true);

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "static-start envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule schedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "static-start schedule: " + failureReason);

        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
            CancellationToken.None);

        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            "static start succeeds: " + result.FailureReason);
        LongitudinalCandidate candidate = result.Candidate ??
            throw new InvalidOperationException("static start must expose a successful candidate.");
        Verification.True(HasValueGreaterThan(candidate.S, 0.05d), "static start makes measurable progress");
        Verification.True(HasValueGreaterThan(candidate.U, 0.05d), "static start accelerates");
        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
            "full segment stops at the terminal boundary");
    }

    private static void VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.SolverTimeoutSeconds = 5d;
        configuration.Solver.MaximumOsqpIterations = 100000;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(2.5d, 2.5d, 0d),
            Point(5d, 5d, 0d),
        }, true);

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.01d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "near-zero envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.01d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule schedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "near-zero schedule: " + failureReason);

        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.01d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
            CancellationToken.None);

        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            "near-zero full-direction start succeeds: " + result.FailureReason);
        LongitudinalCandidate candidate = result.Candidate ??
            throw new InvalidOperationException("Near-zero full-direction candidate was missing.");
        Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
            "near-zero start reaches the terminal boundary");
        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
            "near-zero start stops at the terminal boundary");
    }

    private static void VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.SolverTimeoutSeconds = 5d;
        configuration.Solver.MaximumOsqpIterations = 100000;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 2.25d, 0.1d),
            Point(2d, 4.5d, 0d),
        }, true);

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule schedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero schedule: " + failureReason);

        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.0001d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
            CancellationToken.None);

        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            "curved near-zero full-direction start succeeds: " + result.FailureReason);
        LongitudinalCandidate candidate = result.Candidate ??
            throw new InvalidOperationException("Curved near-zero full-direction candidate was missing.");
        Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
            "curved near-zero start reaches the terminal boundary");
        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
            "curved near-zero start stops at the terminal boundary");
    }

    private static void VerifiesFullDirectionNearZeroScheduleDurationIsReasonable()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 2.25d, 0.1d),
            Point(2d, 4.5d, 0d),
        }, true);

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "near-zero duration envelope: " + failureReason);
        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
            out LongitudinalKnotSchedule schedule, out failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "near-zero duration schedule: " + failureReason);

        double duration = schedule.KnotTimes[schedule.KnotTimes.Count - 1];
        Verification.True(duration < 120d,
            "near-zero schedule duration is normal: " + duration.ToString("R", System.Globalization.CultureInfo.InvariantCulture));
    }

    private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 0.0075d, 0d),
        }, true);
        var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
            new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
            new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
        var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
        DateTimeOffset now = DateTimeOffset.UtcNow;
        var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
            TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
            EmPlanningScope.FullDirectionSegment);

        EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);

        Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
            "full-scope publication reuses its candidate hold instead of appending a second hold");
        Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
            "full-scope publication emits the candidate's single nonzero terminal hold");
    }

    private static void VerifiesExactStopIncludesAStabilizationTail()
    {
        EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
        LateralPath path = new LateralPath(new[]
        {
            Point(0d, 0d, 0d),
            Point(1d, 0.00375d, 0d),
            Point(2d, 0.0075d, 0d),
        }, true);
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
            Array.Empty<double>(), Array.Empty<double>());
        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, "exact-stop seed envelope: " + speedFailure);
        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
            out string validationFailure), "exact-stop seed is strictly feasible: " + validationFailure);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "exact-stop seed remains a strict timeout fallback");
        LongitudinalCandidate candidate = result.Candidate ??
            throw new InvalidOperationException("Exact-stop timeout fallback candidate was missing.");
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
        for (int index = stabilizationStart; index < candidate.S.Count; index++)
        {
            Verification.NearlyEqual(input.StopBoundaryPathS, candidate.S[index], "stop-tail S " + index);
            Verification.NearlyEqual(0d, candidate.U[index], "stop-tail U " + index);
            Verification.NearlyEqual(0d, candidate.A[index], "stop-tail A " + index);
        }
    }

    private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
    {
        double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
        int count = 0;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
                Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
            {
                count++;
            }
        }
        return count;
    }

    private static bool HasValueGreaterThan(IReadOnlyList<double> values, double threshold)
    {
        for (int index = 0; index < values.Count; index++)
        {
            if (values[index] > threshold)
                return true;
        }
        return false;
    }

    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.SuccessWithFallback, invalidResult.Status,
            "invalid solver vector cannot replace the strict initial fallback");
        Verification.NearlyEqual(valid.U[1], invalidResult.Candidate?.U[1] ?? double.NaN,
            "invalid solver vector does not become the fallback 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.SuccessWithFallback, inaccurateResult.Status,
            "inaccurate residual candidate cannot replace the strict initial fallback");
        Verification.NearlyEqual(valid.U[1], inaccurateResult.Candidate?.U[1] ?? double.NaN,
            "inaccurate residual does not become the fallback 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.PathUpperBoundS, 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);
        Verification.True(Math.Abs(secondUpper - envelope.MaximumSpeedAt(candidate.S[1])) > 1e-12d,
            "scripted rejection advances the ST PathS envelope probe");
    }

    private static void VerifiesExactStopTailDoesNotDistortEnvelope()
    {
        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.PathUpperBoundS, 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.NearlyEqual(firstUpper, secondUpper,
            "exact stop-tail perturbations do not distort a moving-knot 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");
        }

        using (var cancellation = new CancellationTokenSource())
        {
            var solver = new CancellingQpSolver(
                new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d)), cancellation);
            LongitudinalPlanningResult cancelledAfterStrictCandidate = new SequentialLongitudinalOptimizer(solver).Optimize(
                input, cancellation.Token);
            Verification.Equal(EmPlanningStatus.Cancelled, cancelledAfterStrictCandidate.Status,
                "cancellation after a strict ST candidate is never fallback success");
            Verification.True(cancelledAfterStrictCandidate.Candidate == null,
                "cancelled ST result exposes no candidate");
        }

        var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
        LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, infeasible.Status,
            "QP infeasibility preserves the strict mode-specific seed");
        Verification.True(infeasible.Candidate != null, "QP infeasibility retains a safe fallback profile");

        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 = CreateFakeInput(out _);
        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);
        Verification.True(solver.WarmStarts.Count > 0,
            "nonzero-speed seed reaches the ST solver: " + result.Status + " " + result.FailureReason);
        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.StopBoundaryPathS, 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.50d, 0d, 0.05d, 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.ApproachStopBoundary,
            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.True(candidate.S[candidate.S.Count - 1] <= scenario.Input.PathUpperBoundS,
            scenario.Name + " remains inside the PathS window");
        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 = 1e-6d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 4d;
        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.8d, 0d, 0.8d, 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 EmPlannerConfiguration CreateExactStopSeedConfiguration()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
        return configuration;
    }

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