using System;
using System.Collections.Generic;
using System.Reflection;
using System.Threading;
using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class EmPlanningServiceChecks
{
    public static void Run()
    {
        VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
        VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
        VerifiesServicePublishesRollingApproachAndExactStopModes();
        VerifiesRequestAndStateFailuresPublishNoTrajectory();
        VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
        VerifiesTimeoutFallbackAndCancellationSemantics();
        VerifiesPublicationFailureAndDebugIsolation();
    }

    private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic()
    {
        EmPlanningRequest forwardRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
        var forwardService = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success));
        EmPlanningResult firstForward = forwardService.Plan(forwardRequest, CancellationToken.None);
        EmPlanningResult secondForward = forwardService.Plan(forwardRequest, CancellationToken.None);
        VerifySuccess(firstForward, forwardRequest, EmTerminalType.Goal, "forward");
        VerifySuccess(secondForward, forwardRequest, EmTerminalType.Goal, "forward repeat");
        VerifySameTrajectory(firstForward, secondForward, "forward deterministic result");
        Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged");

        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, -0.01d, false, false);
        EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
            CancellationToken.None);
        VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
        Verification.True(reverse.Trajectory.Points[1].SignedLongitudinalVelocity < 0d,
            "reverse service publishes negative signed velocity");

        EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0d, true, false);
        EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(gearRequest,
            CancellationToken.None);
        VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear switch");

        EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0d, false, true);
        EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(rollingRequest,
            CancellationToken.None);
        VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
    }

    private static void VerifiesServicePublishesRollingApproachAndExactStopModes()
    {
        EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
            CreateReferencePath(TravelDirection.Forward, false, 10d), CreateMap(false, 12d));
        ConfigureFiveMeterWindowAndTwoSecondHorizon(rollingRequest.Configuration);
        EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
            rollingRequest, CancellationToken.None);
        VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "cycle 1 rolling");
        Verification.Equal(EmLongitudinalMode.RollingContinuation,
            rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
        Verification.Equal(21, rolling.Trajectory.Points.Count, "rolling publishes the two-second ST knot count");
        EmTrajectoryPoint rollingTerminal = rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1];
        Verification.True(rollingTerminal.PathS < 5d, "two-second ST output remains inside the five-metre LS window");
        Verification.True(rollingTerminal.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");

        EmPlanningRequest approachRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
            CreateReferencePath(TravelDirection.Forward, false, 4d), CreateMap(false, 5d));
        EmPlanningResult approach = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
            approachRequest, CancellationToken.None);
        VerifySuccess(approach, approachRequest, EmTerminalType.Goal, "cycle 2 approach");
        Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
            approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");
        Verification.True(approach.Trajectory.Points[approach.Trajectory.Points.Count - 1].SignedLongitudinalVelocity != 0d,
            "approach has no synthetic stop tail");

        EmPlanningRequest exactRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
            CreateReferencePath(TravelDirection.Forward, false, 0.0075d));
        ConfigureExactStopServiceScenario(exactRequest.Configuration);
        EmPlanningResult exact = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
            exactRequest, CancellationToken.None);
        VerifySuccess(exact, exactRequest, EmTerminalType.Goal, "cycle 3 exact goal stop");
        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
            exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
        AssertExactStopStabilization(exact.Trajectory, EmBoundaryType.Goal, "goal");

        EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
            EmFixtureFactory.CreateGearPairReferencePath(0.0075d));
        ConfigureExactStopServiceScenario(gearRequest.Configuration);
        EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
            gearRequest, CancellationToken.None);
        VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear-switch exact stop");
        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
            gear.Trajectory.Metadata.LongitudinalMode, "gear switch stops exactly");
        AssertExactStopStabilization(gear.Trajectory, EmBoundaryType.GearSwitchApproach, "gear switch");
        for (int index = 0; index < gear.Trajectory.Points.Count; index++)
        {
            Verification.Equal(TravelDirection.Forward, gear.Trajectory.Points[index].Direction,
                "gear-switch publication excludes the next direction point " + index);
        }
    }

    private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
    {
        configuration.Scheduling.DistanceHorizonMeters = 5d;
        configuration.Scheduling.TimeHorizonSeconds = 2d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
    }

    private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
    {
        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        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;
    }

    private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
    {
        int exactAnchor = -1;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            if (trajectory.Points[index].BoundaryType == boundaryType)
            {
                exactAnchor = index;
                break;
            }
        }
        Verification.True(exactAnchor >= 0, name + " has a real boundary anchor");
        Verification.NearlyEqual(0d, trajectory.Points[exactAnchor].SignedLongitudinalVelocity,
            name + " speed is zero");
        Verification.True(trajectory.Points.Count > exactAnchor + 1,
            name + " anchor is followed by a QP stabilization point");
        Verification.NearlyEqual(trajectory.Points[exactAnchor].PathS, trajectory.Points[exactAnchor + 1].PathS,
            name + " stabilization keeps the stop position");
        Verification.NearlyEqual(0d, trajectory.Points[exactAnchor + 1].SignedLongitudinalVelocity,
            name + " stabilization speed is zero");
    }

    private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
    {
        EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
            PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics()));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(invalidSmoothing,
            CancellationToken.None), EmPlanningStatus.InvalidReferencePath, "invalid smoothing");

        EmPlanningRequest stale = CreateRequest(TravelDirection.Forward, 0d, false, false);
        stale = ReplaceState(stale, new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null,
            stale.RequestedAtUtc.AddSeconds(-1d), stale.VehicleState.SequenceId));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stale,
            CancellationToken.None), EmPlanningStatus.StaleVehicleState, "stale state");

        EmPlanningRequest directionMismatch = CreateRequest(TravelDirection.Reverse, 0.02d, false, false);
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(directionMismatch,
            CancellationToken.None), EmPlanningStatus.StateDirectionMismatch, "state direction mismatch");
    }

    private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory()
    {
        EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(3d, 0d, 0d), 0d, null,
            projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
            CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");

        EmPlanningRequest corridorFailure = CreateRequest(TravelDirection.Forward, 0d, false, false,
            referencePath: null, map: CreateMap(true));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
            CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");

        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false,
            referencePath: CreateReferencePath(TravelDirection.Forward, false, 0.0055d));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure,
            CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient");

        EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false);
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular,
            CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible");
        EmPlanningResult longitudinalFallback = new EmPlanningService(
            new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, longitudinalFallback.Status,
            "longitudinal infeasible uses the validated fallback seed");
        Verification.True(longitudinalFallback.Trajectory != null,
            "longitudinal fallback still publishes a complete trajectory");
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
            CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
    }

    private static void VerifiesTimeoutFallbackAndCancellationSemantics()
    {
        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithoutFallback)).Plan(request,
            CancellationToken.None), EmPlanningStatus.SolverTimedOut, "timeout without fallback");

        EmPlanningResult fallback = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithFallback)).Plan(
            request, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, fallback.Status, "timeout uses only strict fallback");
        Verification.True(fallback.Trajectory != null && fallback.Trajectory.Points.Count > 0,
            "timeout fallback publishes a complete trajectory");

        using (var cancellation = new CancellationTokenSource())
        {
            cancellation.Cancel();
            VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(request,
                cancellation.Token), EmPlanningStatus.Cancelled, "cancellation");
        }
    }

    private static void VerifiesPublicationFailureAndDebugIsolation()
    {
        EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.PublicationValidationFailure,
            validationFailure.Map)).Plan(validationFailure, CancellationToken.None), EmPlanningStatus.ValidationFailed,
            "publication validation failure");

        EmPlanningRequest debugRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
        debugRequest.Configuration.Solver.NativeVerbose = true;
        EmPlanningResult debugIsolated = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success),
            new ThrowingDebugSink()).Plan(debugRequest, CancellationToken.None);
        VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
    }

    private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference()
    {
        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);

        var withoutPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
        EmPlanningResult withoutPrevious = new EmPlanningService(withoutPreviousSolver).Plan(request, CancellationToken.None);
        VerifySuccess(withoutPrevious, request, EmTerminalType.Goal, "no previous longitudinal seed");

        EmTrajectory validPrevious = CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Forward,
            request.SegmentIndex);
        EmPlanningRequest withPreviousRequest = ReplacePreviousTrajectory(request, validPrevious);
        var withPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
        EmPlanningResult withPrevious = new EmPlanningService(withPreviousSolver).Plan(withPreviousRequest,
            CancellationToken.None);
        VerifySuccess(withPrevious, withPreviousRequest, EmTerminalType.Goal, "valid previous longitudinal seed");

        QuadraticProgram withoutPreviousProblem = withoutPreviousSolver.LastLongitudinalProblem
            ?? throw new InvalidOperationException("The no-seed longitudinal QP was not captured.");
        QuadraticProgram withPreviousProblem = withPreviousSolver.LastLongitudinalProblem
            ?? throw new InvalidOperationException("The seeded longitudinal QP was not captured.");
        var layout = new LongitudinalVariableLayout((withPreviousProblem.VariableCount + 1) / 4);
        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)) >
            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
            "valid previous seed adds a nonzero previous-S soft-reference Hessian term");
        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)) >
            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
            "valid previous seed adds a nonzero previous-U soft-reference Hessian term");
        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.S(1)] -
            withoutPreviousProblem.LinearCost[layout.S(1)]) > 1e-12d,
            "valid previous seed adds a nonzero previous-S soft-reference linear term");
        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.U(1)] -
            withoutPreviousProblem.LinearCost[layout.U(1)]) > 1e-12d,
            "valid previous seed adds a nonzero previous-U soft-reference linear term");

        EmPlanningRequest incompatibleRequest = ReplacePreviousTrajectory(request,
            CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Reverse, request.SegmentIndex));
        var incompatibleSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
        EmPlanningResult incompatible = new EmPlanningService(incompatibleSolver).Plan(incompatibleRequest,
            CancellationToken.None);
        VerifySuccess(incompatible, incompatibleRequest, EmTerminalType.Goal, "incompatible previous seed");
        QuadraticProgram incompatibleProblem = incompatibleSolver.LastLongitudinalProblem
            ?? throw new InvalidOperationException("The incompatible-seed longitudinal QP was not captured.");
        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
            MatrixValue(incompatibleProblem.UpperTriangularP, layout.S(1), layout.S(1)),
            "incompatible previous seed omits previous-S soft-reference term");
        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
            MatrixValue(incompatibleProblem.UpperTriangularP, layout.U(1), layout.U(1)),
            "incompatible previous seed omits previous-U soft-reference term");
    }

    private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
        string name)
    {
        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            name + " successful status: " + result.FailureReason);
        Verification.True(result.Trajectory != null && result.Trajectory.Metadata.TerminalType == terminalType,
            name + " terminal type");
        string identifiers = "map=" + request.Map.SnapshotId + ";reference=" + request.ReferencePathId + ";state=" +
            request.VehicleState.SequenceId + ";previous=" + request.PreviousTrajectoryId + ";segment=" + request.SegmentIndex;
        Verification.True(result.FailureReason.IndexOf(identifiers, StringComparison.Ordinal) >= 0,
            name + " preserves request identifiers in deterministic diagnostics");
    }

    private static void VerifyFailure(EmPlanningResult result, EmPlanningStatus expected, string name)
    {
        Verification.Equal(expected, result.Status, name + " status: " + result.FailureReason);
        Verification.True(result.Trajectory == null, name + " publishes no partial trajectory");
    }

    private static void VerifySameTrajectory(EmPlanningResult left, EmPlanningResult right, string name)
    {
        Verification.Equal(left.Status, right.Status, name + " status");
        Verification.Equal(left.Trajectory.Points.Count, right.Trajectory.Points.Count, name + " point count");
        for (int index = 0; index < left.Trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint first = left.Trajectory.Points[index];
            EmTrajectoryPoint second = right.Trajectory.Points[index];
            Verification.NearlyEqual(first.X, second.X, name + " X " + index);
            Verification.NearlyEqual(first.Y, second.Y, name + " Y " + index);
            Verification.NearlyEqual(first.TimeFromStart, second.TimeFromStart, name + " time " + index);
            Verification.NearlyEqual(first.SignedLongitudinalVelocity, second.SignedLongitudinalVelocity,
                name + " signed speed " + index);
        }
    }

    private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
        bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null)
    {
        DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Solver.MaximumOuterIterations = 2;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        if (rolling)
            configuration.Scheduling.DistanceHorizonMeters = 0.30d;
        return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
            new VehicleParameters
            {
                LengthMeters = 0.10d,
                WidthMeters = 0.10d,
                SafetyMarginMeters = 0d,
                MaximumCurvaturePerMeter = 1d,
            },
            new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
            null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
            EmMotionModel.NonholonomicForwardReverse);
    }

    private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
    {
        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
            source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
    }

    private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
    {
        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState,
            source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
    }

    private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
        TravelDirection direction, int segmentIndex)
    {
        var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
            EmLongitudinalMode.RollingContinuation);
        double sign = direction == TravelDirection.Forward ? 1d : -1d;
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 0d, 0d, segmentIndex, 0.001d, 0.001d,
                direction, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 6d, 0d, segmentIndex, 0.001d, 0.001d,
                direction, EmBoundaryType.None, 0d, 0d),
        });
    }

    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
    {
        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
        {
            if (matrix.RowIndices[index] == row)
                return matrix.Values[index];
        }
        return 0d;
    }

    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch,
        double lengthMeters = 2d)
    {
        double endX = direction == TravelDirection.Forward ? lengthMeters : -lengthMeters;
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false,
                SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(endX, 0d, 0d, 0d, lengthMeters, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
                endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
        };
        var segments = new List<SmoothedPathSegment>
        {
            new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch),
        };
        var metrics = new PathQualityMetrics(true, lengthMeters, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
    }

    private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d)
    {
        IMapObstacleSource[] sources = blockStart
            ? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
                new IMapObstacle[] { new AxisAlignedRectangleObstacle(-20f, 20f, -20f, 20f) }) }
            : Array.Empty<IMapObstacleSource>();
        PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
        {
            Bounds = new MapBoundsMm((float)(-1000d * halfExtentMeters), (float)(1000d * halfExtentMeters), -1000f, 1000f),
            ResolutionMm = 20f,
            ObstacleSources = sources,
            AllowExplicitEmptyMap = !blockStart,
        });
        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
            "service map builds: " + result.FailureReason);
        return result.Map!;
    }

    private enum PipelineSolverMode
    {
        Success,
        LateralInfeasible,
        LongitudinalInfeasible,
        SolverUnavailable,
        TimeoutWithoutFallback,
        TimeoutWithFallback,
        PublicationValidationFailure,
    }

    private sealed class ScriptedPipelineSolver : IQpSolver
    {
        private readonly PipelineSolverMode mode;
        private readonly PlanningGridMap? mapToCorrupt;
        private int longitudinalCallCount;

        public QuadraticProgram? LastLongitudinalProblem { get; private set; }

        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
        {
            this.mode = mode;
            this.mapToCorrupt = mapToCorrupt;
        }

        public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
            CancellationToken cancellationToken)
        {
            bool longitudinal = problem.VariableCount > 100;
            if (mode == PipelineSolverMode.SolverUnavailable)
                return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
            if (!longitudinal)
            {
                if (mode == PipelineSolverMode.LateralInfeasible)
                    return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
                if (mode == PipelineSolverMode.TimeoutWithoutFallback)
                    return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
                return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
            }
            LastLongitudinalProblem = problem;
            if (mode == PipelineSolverMode.LongitudinalInfeasible)
                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
                CorruptMapAtOrigin(mapToCorrupt);
            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
            longitudinalCallCount++;
            return Result(QpSolveStatus.Solved, warmStart);
        }

        private static void CorruptMapAtOrigin(PlanningGridMap? map)
        {
            if (map == null || !map.TryWorldToGrid(0d, 0d, out int row, out int column))
                throw new InvalidOperationException("Unable to corrupt the publication test map.");
            FieldInfo occupiedField = typeof(PlanningGridMap).GetField("_occupied", BindingFlags.Instance | BindingFlags.NonPublic)!
                ?? throw new InvalidOperationException("Planning map occupancy storage was unavailable.");
            FieldInfo distanceField = typeof(PlanningGridMap).GetField("_conservativeDistances",
                BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Planning map distance storage was unavailable.");
            byte[] occupied = (byte[])occupiedField.GetValue(map)!;
            double[] distances = (double[])distanceField.GetValue(map)!;
            int index = row * map.Cols + column;
            occupied[index] = 1;
            distances[index] = 0d;
        }

        private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
        {
            int variableCount = problem.VariableCount;
            int knotCount = (variableCount + 1) / 4;
            var layout = new LongitudinalVariableLayout(knotCount);
            var jerk = new double[knotCount - 1];
            const int rampIntervals = 5;
            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
            var times = new double[knotCount];
            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
            var primal = new double[layout.VariableCount];
            for (int index = 0; index < 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 < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
            for (int index = 4 * rampIntervals; index < knotCount; index++)
            {
                primal[layout.S(index)] = terminalPathS;
                primal[layout.U(index)] = 0d;
                primal[layout.A(index)] = 0d;
            }
            return primal;
        }

        private static double ReadFixedVariable(QuadraticProgram problem, int variable)
        {
            for (int row = 0; row < problem.ConstraintCount; row++)
            {
                int entryCount = 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)
                            continue;
                        entryCount++;
                        if (column == variable)
                            coefficient = problem.ConstraintMatrix.Values[index];
                    }
                }
                if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
                    Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
                {
                    return problem.LowerBounds[row] / coefficient;
                }
            }
            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
        }

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

    private sealed class ThrowingDebugSink : IEmPlannerDebugSink
    {
        public void Write(string message)
        {
            throw new InvalidOperationException("debug sink failure");
        }
    }
}