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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class LateralModelChecks
{
    public static void Run()
    {
        VerifiesDeterministicVariableLayout();
        VerifiesExactDiscreteDynamicsForUnequalStations();
        VerifiesPlanningInputBoundariesAndDefensiveCopies();
        VerifiesLateralResultPublicationContract();
        VerifiesNormalizedObjectiveAndHardConstraints();
        VerifiesAllNamedCostScales();
        VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
        VerifiesFakeSolverCapturesTheNeutralQpBoundary();
    }

    private static void VerifiesDeterministicVariableLayout()
    {
        LateralVariableLayout layout = CreateLayout(4);

        Verification.Equal(15, layout.VariableCount, "layout variable count");
        for (int index = 0; index < 4; index++)
        {
            Verification.Equal(index, layout.L(index), "l index " + index);
            Verification.Equal(4 + index, layout.DL(index), "dl index " + index);
            Verification.Equal(8 + index, layout.DDL(index), "ddl index " + index);
        }
        for (int index = 0; index < 3; index++)
            Verification.Equal(12 + index, layout.DDDL(index), "dddl index " + index);

        ExpectArgumentException(() => CreateLayout(1), "layout rejects fewer than two stations");
        ExpectArgumentException(() => layout.L(4), "l index bounds check");
        ExpectArgumentException(() => layout.DL(-1), "dl index bounds check");
        ExpectArgumentException(() => layout.DDL(4), "ddl index bounds check");
        ExpectArgumentException(() => layout.DDDL(3), "dddl index bounds check");
    }

    private static void VerifiesExactDiscreteDynamicsForUnequalStations()
    {
        double[] stations = { 0d, 0.4d, 1.25d, 2.5d };
        double[] jerks = { 0.5d, -0.3d, 0.2d };
        LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, -0.2d, 0.3d, jerks);

        for (int index = 0; index < jerks.Length; index++)
        {
            double ds = stations[index + 1] - stations[index];
            Verification.NearlyEqual(candidate.DDL[index] + ds * candidate.DDDL[index], candidate.DDL[index + 1],
                "exact ddl integration " + index);
            Verification.NearlyEqual(candidate.DL[index] + ds * candidate.DDL[index] + 0.5d * ds * ds * candidate.DDDL[index],
                candidate.DL[index + 1], "exact dl integration " + index);
            Verification.NearlyEqual(candidate.L[index] + ds * candidate.DL[index] + 0.5d * ds * ds * candidate.DDL[index] +
                ds * ds * ds * candidate.DDDL[index] / 6d, candidate.L[index + 1], "exact l integration " + index);
        }
        Verification.True(candidate.SatisfiesExactDiscreteDynamics(1e-12d), "integrated candidate validates exact dynamics");

        LateralCandidate inconsistent = new LateralCandidate(new[] { 0d, 1d }, new[] { 0d, 1d },
            new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d });
        Verification.True(!inconsistent.SatisfiesExactDiscreteDynamics(1e-12d), "candidate detects inconsistent dynamics");
        ExpectArgumentException(() => new LateralCandidate(new[] { 0d, 0d }, new[] { 0d, 0d },
            new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d }), "candidate rejects non-increasing stations");
    }

    private static void VerifiesPlanningInputBoundariesAndDefensiveCopies()
    {
        DirectionSegmentView segment = CreateStraightSegment();
        var corridorStations = new[]
        {
            new LateralInterval(0d, -0.3d, 0.3d, 0d),
            new LateralInterval(1d, -0.3d, 0.3d, 0d),
            new LateralInterval(2d, -0.3d, 0.3d, 0d),
        };
        var seeds = new[]
        {
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
        };
        LateralPlanningInput input = new LateralPlanningInput(segment, new StaticCorridor(corridorStations), seeds[0],
            EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), seeds);
        corridorStations[1] = new LateralInterval(1d, -0.1d, 0.1d, 0d);
        seeds[0] = new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d);

        Verification.NearlyEqual(-0.3d, input.Corridor.Stations[1].MinimumL, "input copies corridor stations");
        Verification.NearlyEqual(0d, input.PreviousTrajectorySeed[0].LateralOffset, "input copies seed list");
        ExpectArgumentException(() => new LateralPlanningInput(segment,
            new StaticCorridor(new[] { new LateralInterval(0d, -0.3d, 0.3d, 0d) }),
            input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
            "input rejects fewer than two stations");
        ExpectArgumentException(() => new LateralPlanningInput(segment,
            new StaticCorridor(new[]
            {
                new LateralInterval(0d, -0.3d, 0.3d, 0d),
                new LateralInterval(0d, -0.3d, 0.3d, 0d),
            }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
            "input rejects non-increasing corridor stations");
        ExpectArgumentException(() => new LateralPlanningInput(segment,
            new StaticCorridor(new[]
            {
                new LateralInterval(0.1d, -0.3d, 0.3d, 0d),
                new LateralInterval(2d, -0.3d, 0.3d, 0d),
            }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
            "input rejects start-corridor station mismatch");
        ExpectArgumentException(() => new LateralPlanningInput(segment,
            new StaticCorridor(new[]
            {
                new LateralInterval(0d, -0.1d, 0.1d, 0d),
                new LateralInterval(2d, -0.1d, 0.1d, 0d),
            }), new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d),
            EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
            "input rejects start projection outside first hard interval");
    }

    private static void VerifiesLateralResultPublicationContract()
    {
        LateralPath unvalidated = new LateralPath(new[] { CreatePathPoint(0d) }, false);
        LateralPath validated = new LateralPath(new[] { CreatePathPoint(0d), CreatePathPoint(1d) }, true);

        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.Success, unvalidated, string.Empty),
            "success requires an independently validated path");
        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback,
            new LateralPath(Array.Empty<LateralPathPoint>(), true), string.Empty),
            "fallback requires a non-empty path");
        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.LateralInfeasible, validated, string.Empty),
            "failed result has no candidate");
        LateralPlanningResult result = new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, validated, "fallback");
        Verification.Equal(validated, result.Path, "fallback path is preserved");
    }

    private static void VerifiesNormalizedObjectiveAndHardConstraints()
    {
        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
        LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration,
            new[] { 0.2d, -0.1d, 0.3d });
        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
            new[] { 0d, 0d });
        LateralConstraintBuilder builder = CreateConstraintBuilder();

        Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
            "unit-scale QP builds: " + failureReason);
        var layout = new LateralVariableLayout(3);
        Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
            "reference plus previous P coefficient");
        Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
            "jerk P coefficient");
        Verification.NearlyEqual(-2d, problem.LinearCost[layout.L(0)], "previous-seed q coefficient");

        for (int interval = 0; interval < 2; interval++)
        {
            double ds = input.ReferenceStations[interval + 1] - input.ReferenceStations[interval];
            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
            {
                { layout.DDL(interval), -1d },
                { layout.DDL(interval + 1), 1d },
                { layout.DDDL(interval), -ds },
            }), "ddl dynamics equality " + interval);
            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
            {
                { layout.DL(interval), -1d },
                { layout.DL(interval + 1), 1d },
                { layout.DDL(interval), -ds },
                { layout.DDDL(interval), -0.5d * ds * ds },
            }), "dl dynamics equality " + interval);
            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
            {
                { layout.L(interval), -1d },
                { layout.L(interval + 1), 1d },
                { layout.DL(interval), -ds },
                { layout.DDL(interval), -0.5d * ds * ds },
                { layout.DDDL(interval), -ds * ds * ds / 6d },
            }), "l dynamics equality " + interval);
        }

        for (int station = 0; station < layout.StationCount; station++)
        {
            Verification.True(HasFiniteNonEqualityBound(problem, layout.L(station)), "finite lateral hard bound " + station);
            Verification.True(HasFiniteNonEqualityBound(problem, layout.DL(station)), "finite slope hard bound " + station);
            Verification.True(HasFiniteNonEqualityBound(problem, layout.DDL(station)), "finite second-derivative hard bound " + station);
        }
        for (int interval = 0; interval < layout.StationCount - 1; interval++)
            Verification.True(HasFiniteNonEqualityBound(problem, layout.DDDL(interval)), "finite jerk hard bound " + interval);

        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.L(2), 1d } }),
            "goal terminal l equality");
        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
            "goal terminal dl equality");

        LateralPlanningInput rolling = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
            new[] { 0.2d, -0.1d, 0.3d });
        Verification.True(builder.TryBuild(rolling, linearization, out QuadraticProgram rollingProblem, out string rollingReason),
            "rolling QP builds: " + rollingReason);
        Verification.NearlyEqual(50d, MatrixValue(rollingProblem.UpperTriangularP, layout.L(2), layout.L(2)),
            "rolling terminal adds normalized objective cost");
        Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.L(2), 1d } }),
            "rolling terminal has no l equality");
        Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
            "rolling terminal has no dl equality");
    }

    private static void VerifiesAllNamedCostScales()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Corridor.MaximumLateralOffsetMeters = 2d;
        configuration.Lateral.MaximumLateralSlope = 4d;
        configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
        configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 6d;
        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
            new[] { 0.2d, 0.2d, 0.2d }, referenceCurvatureDerivative: 4d, maximumVehicleCurvature: 7d);
        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
            new[] { 0d, 0d });
        LateralConstraintBuilder builder = CreateConstraintBuilder();

        Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
            "non-unit-scale QP builds: " + failureReason);
        var layout = new LateralVariableLayout(3);
        Verification.NearlyEqual(7.5d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
            "reference and previous costs divide by lateral scale squared");
        Verification.NearlyEqual(-0.5d, problem.LinearCost[layout.L(0)],
            "previous target coefficient divides by lateral scale squared");
        Verification.NearlyEqual(0.125d, MatrixValue(problem.UpperTriangularP, layout.DL(0), layout.DL(0)),
            "heading cost divides by slope scale squared");
        Verification.NearlyEqual(5d / 9d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
            "jerk cost divides by third-derivative scale squared");
        Verification.NearlyEqual(0.4d + 10d / 49d + 3.125d,
            MatrixValue(problem.UpperTriangularP, layout.DDL(0), layout.DDL(0)),
            "second derivative, curvature, and curvature variation use their named scales");
        Verification.NearlyEqual(12.5d, MatrixValue(problem.UpperTriangularP, layout.L(2), layout.L(2)),
            "rolling terminal cost divides by lateral scale squared");

        EmPlannerConfiguration denominatorConfiguration = CreateUnitScaleConfiguration();
        denominatorConfiguration.Lateral.MaximumLateralStepPerIterationMeters = 0.5d;
        LateralPlanningInput denominatorInput = CreateModelInput(EmTerminalType.RollingSafetyStop, denominatorConfiguration,
            Array.Empty<double>(), 0d, 2d);
        Verification.True(builder.TryBuild(denominatorInput, linearization, out QuadraticProgram denominatorProblem,
            out string denominatorReason), "denominator QP builds: " + denominatorReason);
        Verification.True(HasBoundWithUpper(denominatorProblem, layout.L(0), 0.4d),
            "Frenet denominator is intersected as a finite hard lateral bound");
    }

    private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
    {
        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
            Array.Empty<double>(), 0d, 0d, 0.9d, 0.9d, 1d);
        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
            new[] { 0d, 0d });

        Verification.True(!CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
            out string failureReason), "empty corridor/trust intersection is infeasible before solve");
        Verification.True(problem == null && failureReason.Length > 0, "infeasible build returns no QP and a reason");
    }

    private static void VerifiesFakeSolverCapturesTheNeutralQpBoundary()
    {
        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
        LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration, Array.Empty<double>());
        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
            new[] { 0d, 0d });
        Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
            out string reason), "fake solver problem builds: " + reason);
        var expected = new QpSolveResult(QpSolveStatus.Solved, new double[problem.VariableCount], 0d, 0d, 0d, 1,
            TimeSpan.Zero, "fake", string.Empty);
        var solver = new FakeQpSolver(expected);
        var settings = new QpSolverSettings(10, 1e-5d, 1e-5d, TimeSpan.FromSeconds(1d), true, false, false);
        QpSolveResult actual = solver.Solve(problem, settings, new[] { 1d, 2d }, CancellationToken.None);

        Verification.Equal(expected, actual, "fake solver returns configured result");
        Verification.Equal(problem, solver.LastProblem, "fake solver records QP");
        Verification.Equal(settings, solver.LastSettings, "fake solver records settings");
        Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
    }

    private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
        double referenceCurvature = 0d)
    {
        var points = new List<SmoothedPathPoint>
        {
            Point(0d, 0d, referenceCurvatureDerivative, referenceCurvature),
            Point(1d, 1d, referenceCurvatureDerivative, referenceCurvature),
            Point(2d, 2d, referenceCurvatureDerivative, referenceCurvature),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
    }

    private static SmoothedPathPoint Point(double x, double s, double curvatureDerivative = 0d, double curvature = 0d)
    {
        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, curvature, curvature,
            curvatureDerivative, 1d,
            false, SmoothedPathPointSource.Anchor);
    }

    private static EmPlannerConfiguration CreateUnitScaleConfiguration()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Corridor.MaximumLateralOffsetMeters = 1d;
        configuration.Lateral.MaximumLateralSlope = 1d;
        configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 1d;
        configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 1d;
        return configuration;
    }

    private static LateralPlanningInput CreateModelInput(EmTerminalType terminalType, EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousL, double referenceCurvatureDerivative = 0d, double referenceCurvature = 0d,
        double startL = 0d, double corridorMinimum = -1d, double corridorMaximum = 1d,
        double maximumVehicleCurvature = 1d)
    {
        DirectionSegmentView segment = CreateStraightSegment(referenceCurvatureDerivative, referenceCurvature);
        double corridorSeed = Math.Max(corridorMinimum, Math.Min(corridorMaximum, 0d));
        var stations = new[]
        {
            new LateralInterval(0d, corridorMinimum, corridorMaximum, startL),
            new LateralInterval(1d, corridorMinimum, corridorMaximum, corridorSeed),
            new LateralInterval(2d, corridorMinimum, corridorMaximum, corridorSeed),
        };
        var seed = new List<FrenetProjection>();
        for (int index = 0; index < previousL.Count; index++)
            seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, index), previousL[index], 0d, 0d));
        return new LateralPlanningInput(segment, new StaticCorridor(stations),
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), startL, 0d, 0d), terminalType,
            CreateVehicle(maximumVehicleCurvature), configuration, seed);
    }

    private static LateralObjectiveBuilder CreateObjectiveBuilder()
    {
        return new LateralObjectiveBuilder();
    }

    private static LateralConstraintBuilder CreateConstraintBuilder()
    {
        return new LateralConstraintBuilder(CreateObjectiveBuilder());
    }

    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 int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected)
    {
        int count = 0;
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d &&
                RowMatches(problem.ConstraintMatrix, row, expected))
            {
                count++;
            }
        }
        return count;
    }

    private static bool HasFiniteNonEqualityBound(QuadraticProgram problem, int variable)
    {
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d &&
                RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
                !double.IsInfinity(problem.LowerBounds[row]) && !double.IsInfinity(problem.UpperBounds[row]))
            {
                return true;
            }
        }
        return false;
    }

    private static bool HasBoundWithUpper(QuadraticProgram problem, int variable, double upper)
    {
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
                Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
            {
                return true;
            }
        }
        return false;
    }

    private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
    {
        var actual = new Dictionary<int, double>();
        for (int column = 0; column < matrix.ColumnCount; column++)
        {
            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
            {
                if (matrix.RowIndices[index] == targetRow)
                    actual[column] = matrix.Values[index];
            }
        }
        if (actual.Count != expected.Count)
            return false;
        foreach (KeyValuePair<int, double> expectedEntry in expected)
        {
            if (!actual.TryGetValue(expectedEntry.Key, out double value) || Math.Abs(value - expectedEntry.Value) > 1e-12d)
                return false;
        }
        return true;
    }

    private static VehicleParameters CreateVehicle(double maximumCurvature = 1d)
    {
        return new VehicleParameters
        {
            LengthMeters = 0.1d,
            WidthMeters = 0.1d,
            SafetyMarginMeters = 0d,
            MaximumCurvaturePerMeter = maximumCurvature,
        };
    }

    private static LateralPathPoint CreatePathPoint(double referenceS)
    {
        return new LateralPathPoint(referenceS, referenceS, 0d, 0d, 0d, 0d, referenceS, 0d, 0d, 0d, 0d, 0d);
    }

    private static LateralVariableLayout CreateLayout(int stationCount)
    {
        return new LateralVariableLayout(stationCount);
    }

    private static void ExpectArgumentException(Action action, string name)
    {
        try
        {
            action();
        }
        catch (ArgumentException)
        {
            return;
        }
        throw new InvalidOperationException(name + " did not throw ArgumentException.");
    }
}