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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class SparseCscMatrix
{
    public SparseCscMatrix(
        int rowCount,
        int columnCount,
        IReadOnlyList<double> values,
        IReadOnlyList<int> rowIndices,
        IReadOnlyList<int> columnPointers)
    {
        if (rowCount < 0)
            throw new ArgumentOutOfRangeException(nameof(rowCount));
        if (columnCount < 0)
            throw new ArgumentOutOfRangeException(nameof(columnCount));
        if (values == null)
            throw new ArgumentNullException(nameof(values));
        if (rowIndices == null)
            throw new ArgumentNullException(nameof(rowIndices));
        if (columnPointers == null)
            throw new ArgumentNullException(nameof(columnPointers));
        if (values.Count != rowIndices.Count)
            throw new ArgumentException("CSC values and row indices must have the same length.", nameof(rowIndices));
        if (columnPointers.Count != columnCount + 1)
            throw new ArgumentException("CSC column-pointer count must equal column count plus one.", nameof(columnPointers));
        if (columnPointers[0] != 0)
            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The first CSC column pointer must be zero.");
        if (columnPointers[columnCount] != values.Count)
            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The final CSC column pointer must equal the nonzero count.");

        for (int column = 0; column < columnCount; column++)
        {
            if (columnPointers[column] > columnPointers[column + 1])
                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must be monotonic.");
            if (columnPointers[column] < 0 || columnPointers[column + 1] > values.Count)
                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must stay within the nonzero count.");
        }

        for (int index = 0; index < values.Count; index++)
        {
            if (!NumericGuard.IsFinite(values[index]))
                throw new ArgumentOutOfRangeException(nameof(values), "CSC values must be finite.");
            if (rowIndices[index] < 0 || rowIndices[index] >= rowCount)
                throw new ArgumentOutOfRangeException(nameof(rowIndices), "CSC row indices must be within matrix bounds.");
        }

        for (int column = 0; column < columnCount; column++)
        {
            int previousRow = -1;
            for (int index = columnPointers[column]; index < columnPointers[column + 1]; index++)
            {
                if (rowIndices[index] <= previousRow)
                    throw new ArgumentException("CSC row indices must be strictly ascending in each column.", nameof(rowIndices));
                previousRow = rowIndices[index];
            }
        }

        RowCount = rowCount;
        ColumnCount = columnCount;
        Values = Copy(values);
        RowIndices = Copy(rowIndices);
        ColumnPointers = Copy(columnPointers);
    }

    public int RowCount { get; }

    public int ColumnCount { get; }

    public IReadOnlyList<double> Values { get; }

    public IReadOnlyList<int> RowIndices { get; }

    public IReadOnlyList<int> ColumnPointers { get; }

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