using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class QuadraticProgram
{
    public const double MaximumFiniteBound = 1e30d;

    public QuadraticProgram(
        SparseCscMatrix upperTriangularP,
        IReadOnlyList<double> q,
        SparseCscMatrix a,
        IReadOnlyList<double> lowerBounds,
        IReadOnlyList<double> upperBounds)
    {
        if (upperTriangularP == null)
            throw new ArgumentNullException(nameof(upperTriangularP));
        if (q == null)
            throw new ArgumentNullException(nameof(q));
        if (a == null)
            throw new ArgumentNullException(nameof(a));
        if (lowerBounds == null)
            throw new ArgumentNullException(nameof(lowerBounds));
        if (upperBounds == null)
            throw new ArgumentNullException(nameof(upperBounds));
        if (upperTriangularP.RowCount != upperTriangularP.ColumnCount)
            throw new ArgumentOutOfRangeException(nameof(upperTriangularP), "The quadratic Hessian must be square.");
        if (q.Count != upperTriangularP.ColumnCount)
            throw new ArgumentOutOfRangeException(nameof(q), "The linear cost length must match the variable count.");
        if (a.ColumnCount != upperTriangularP.ColumnCount)
            throw new ArgumentOutOfRangeException(nameof(a), "Constraint columns must match the variable count.");
        if (lowerBounds.Count != a.RowCount || upperBounds.Count != a.RowCount)
            throw new ArgumentOutOfRangeException(nameof(lowerBounds), "Constraint bounds must match the constraint count.");

        ValidateUpperTriangle(upperTriangularP);
        ValidateFinite(q, nameof(q));
        ValidateBounds(lowerBounds, upperBounds);

        UpperTriangularP = CopyMatrix(upperTriangularP);
        LinearCost = Copy(q);
        ConstraintMatrix = CopyMatrix(a);
        LowerBounds = Copy(lowerBounds);
        UpperBounds = Copy(upperBounds);
        VariableCount = UpperTriangularP.ColumnCount;
        ConstraintCount = ConstraintMatrix.RowCount;
    }

    public SparseCscMatrix UpperTriangularP { get; }

    public IReadOnlyList<double> LinearCost { get; }

    public SparseCscMatrix ConstraintMatrix { get; }

    public IReadOnlyList<double> LowerBounds { get; }

    public IReadOnlyList<double> UpperBounds { get; }

    public int VariableCount { get; }

    public int ConstraintCount { get; }

    private static void ValidateUpperTriangle(SparseCscMatrix matrix)
    {
        for (int column = 0; column < matrix.ColumnCount; column++)
        {
            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
            {
                if (matrix.RowIndices[index] > column)
                    throw new ArgumentOutOfRangeException(nameof(matrix), "The quadratic Hessian must store only its upper triangle.");
            }
        }
    }

    private static void ValidateFinite(IReadOnlyList<double> values, string parameterName)
    {
        for (int index = 0; index < values.Count; index++)
        {
            if (!NumericGuard.IsFinite(values[index]))
                throw new ArgumentOutOfRangeException(parameterName, "Values must be finite.");
        }
    }

    private static void ValidateBounds(IReadOnlyList<double> lowerBounds, IReadOnlyList<double> upperBounds)
    {
        for (int index = 0; index < lowerBounds.Count; index++)
        {
            double lower = lowerBounds[index];
            double upper = upperBounds[index];
            if (!NumericGuard.IsFinite(lower) || !NumericGuard.IsFinite(upper) ||
                lower < -MaximumFiniteBound || upper > MaximumFiniteBound || lower > upper)
            {
                throw new ArgumentOutOfRangeException(nameof(lowerBounds),
                    "Constraint bounds must be finite, within the supported range, and ordered.");
            }
        }
    }

    private static SparseCscMatrix CopyMatrix(SparseCscMatrix source)
    {
        return new SparseCscMatrix(source.RowCount, source.ColumnCount, source.Values, source.RowIndices, source.ColumnPointers);
    }

    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source.Count);
        for (int index = 0; index < source.Count; index++)
            copy.Add(source[index]);
        return new ReadOnlyCollection<T>(copy);
    }
}