using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class OptimizationChecks
{
    public static void Run()
    {
        VerifyCanonicalCscAssembly();
        VerifyInvalidTripletsAreRejected();
        VerifyUpperTriangularHessianStorage();
        VerifyQuadraticProgramValidation();
        VerifyQuadraticProgramDefensivelyCopiesInputs();
    }

    private static void VerifyCanonicalCscAssembly()
    {
        var builder = new SparseTripletBuilder(3, 2);
        builder.Add(2, 1, 1.5d);
        builder.Add(0, 0, 2d);
        builder.Add(1, 1, 3d);
        builder.Add(2, 1, 0.5d);
        builder.Add(1, 0, -2d);
        builder.Add(0, 1, 7d);
        builder.Add(0, 1, -7d);

        SparseCscMatrix matrix = builder.Build();

        EMPlannerVerificationHost.Verification.Equal(3, matrix.ColumnPointers.Count, "CSC column-pointer length");
        EMPlannerVerificationHost.Verification.Equal(0, matrix.ColumnPointers[0], "CSC first pointer");
        EMPlannerVerificationHost.Verification.Equal(2, matrix.ColumnPointers[1], "CSC second pointer");
        EMPlannerVerificationHost.Verification.Equal(4, matrix.ColumnPointers[2], "CSC final pointer");
        EMPlannerVerificationHost.Verification.Equal(4, matrix.Values.Count, "CSC nonzero count");
        EMPlannerVerificationHost.Verification.Equal(0, matrix.RowIndices[0], "CSC first row");
        EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[1], "CSC second row");
        EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[2], "CSC third row");
        EMPlannerVerificationHost.Verification.Equal(2, matrix.RowIndices[3], "CSC fourth row");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[0], "CSC first value");
        EMPlannerVerificationHost.Verification.NearlyEqual(-2d, matrix.Values[1], "CSC second value");
        EMPlannerVerificationHost.Verification.NearlyEqual(3d, matrix.Values[2], "CSC third value");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[3], "CSC duplicate sum");
    }

    private static void VerifyInvalidTripletsAreRejected()
    {
        var builder = new SparseTripletBuilder(2, 2);

        AssertArgumentOutOfRange(() => builder.Add(-1, 0, 1d), "negative row");
        AssertArgumentOutOfRange(() => builder.Add(0, -1, 1d), "negative column");
        AssertArgumentOutOfRange(() => builder.Add(2, 0, 1d), "row outside matrix");
        AssertArgumentOutOfRange(() => builder.Add(0, 2, 1d), "column outside matrix");
        AssertArgumentOutOfRange(() => builder.Add(0, 0, double.NaN), "NaN triplet");
        AssertArgumentOutOfRange(() => builder.Add(0, 0, double.PositiveInfinity), "infinite triplet");
    }

    private static void VerifyUpperTriangularHessianStorage()
    {
        var upperBuilder = new SparseTripletBuilder(2, 2, true);
        upperBuilder.Add(0, 0, 1d);
        upperBuilder.Add(0, 1, 2d);
        AssertArgumentOutOfRange(() => upperBuilder.Add(1, 0, 3d), "lower-triangular Hessian entry");

        SparseCscMatrix upperHessian = upperBuilder.Build();
        var constraints = new SparseTripletBuilder(1, 2);
        constraints.Add(0, 0, 1d);
        constraints.Add(0, 1, 1d);

        var lowerTriangleBuilder = new SparseTripletBuilder(2, 2);
        lowerTriangleBuilder.Add(0, 0, 1d);
        lowerTriangleBuilder.Add(1, 0, 3d);
        AssertArgumentOutOfRange(
            () => new QuadraticProgram(lowerTriangleBuilder.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 0d }, new[] { 1d }),
            "lower-triangular quadratic-program Hessian");

        EMPlannerVerificationHost.Verification.Equal(2, upperHessian.ColumnCount, "upper Hessian column count");
    }

    private static void VerifyQuadraticProgramValidation()
    {
        var nonSquareHessian = new SparseCscMatrix(1, 2, new double[0], new int[0], new[] { 0, 0, 0 });
        var oneVariableHessian = new SparseTripletBuilder(1, 1, true);
        oneVariableHessian.Add(0, 0, 1d);
        var oneConstraint = new SparseTripletBuilder(1, 1);
        oneConstraint.Add(0, 0, 1d);

        AssertArgumentOutOfRange(
            () => new QuadraticProgram(nonSquareHessian, new[] { 0d, 0d }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
            "non-square Hessian");
        AssertArgumentOutOfRange(
            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { 2d }, new[] { 1d }),
            "inverted constraint bounds");
        AssertArgumentOutOfRange(
            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { double.NaN }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
            "non-finite linear cost");
        AssertArgumentOutOfRange(
            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { -1e30d }, new[] { 2e30d }),
            "out-of-range finite bound");
    }

    private static void VerifyQuadraticProgramDefensivelyCopiesInputs()
    {
        var hessianBuilder = new SparseTripletBuilder(1, 1, true);
        hessianBuilder.Add(0, 0, 1d);
        var constraintBuilder = new SparseTripletBuilder(1, 1);
        constraintBuilder.Add(0, 0, 1d);

        var linearCost = new List<double> { -2d };
        var lowerBounds = new List<double> { 0d };
        var upperBounds = new List<double> { 1d };
        QuadraticProgram problem = new QuadraticProgram(
            hessianBuilder.Build(),
            linearCost,
            constraintBuilder.Build(),
            lowerBounds,
            upperBounds);

        linearCost[0] = 100d;
        lowerBounds[0] = -100d;
        upperBounds[0] = 100d;

        EMPlannerVerificationHost.Verification.Equal(1, problem.VariableCount, "micro problem variable count");
        EMPlannerVerificationHost.Verification.Equal(1, problem.ConstraintCount, "micro problem constraint count");
        EMPlannerVerificationHost.Verification.NearlyEqual(-2d, problem.LinearCost[0], "copied linear cost");
        EMPlannerVerificationHost.Verification.NearlyEqual(0d, problem.LowerBounds[0], "copied lower bound");
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, problem.UpperBounds[0], "copied upper bound");
    }

    private static void AssertArgumentOutOfRange(Action action, string name)
    {
        try
        {
            action();
        }
        catch (ArgumentOutOfRangeException)
        {
            return;
        }

        throw new InvalidOperationException(name + " did not throw ArgumentOutOfRangeException.");
    }
}