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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class SparseTripletBuilder
{
    private readonly int rowCount;
    private readonly int columnCount;
    private readonly bool upperTriangleOnly;
    private readonly List<Triplet> triplets = new List<Triplet>();

    public SparseTripletBuilder(int rowCount, int columnCount)
        : this(rowCount, columnCount, false)
    {
    }

    public SparseTripletBuilder(int rowCount, int columnCount, bool upperTriangleOnly)
    {
        if (rowCount < 0)
            throw new ArgumentOutOfRangeException(nameof(rowCount));
        if (columnCount < 0)
            throw new ArgumentOutOfRangeException(nameof(columnCount));

        this.rowCount = rowCount;
        this.columnCount = columnCount;
        this.upperTriangleOnly = upperTriangleOnly;
    }

    public void Add(int row, int column, double value)
    {
        if (row < 0 || row >= rowCount)
            throw new ArgumentOutOfRangeException(nameof(row));
        if (column < 0 || column >= columnCount)
            throw new ArgumentOutOfRangeException(nameof(column));
        if (!NumericGuard.IsFinite(value))
            throw new ArgumentOutOfRangeException(nameof(value), "Sparse triplet values must be finite.");
        if (upperTriangleOnly && row > column)
            throw new ArgumentOutOfRangeException(nameof(row), "Upper-triangular storage does not accept lower-triangular entries.");

        triplets.Add(new Triplet(row, column, value));
    }

    public SparseCscMatrix Build()
    {
        var ordered = new List<Triplet>(triplets);
        ordered.Sort(CompareTriplets);

        var values = new List<double>();
        var rows = new List<int>();
        var pointers = new List<int>(columnCount + 1) { 0 };
        int nextTriplet = 0;

        for (int column = 0; column < columnCount; column++)
        {
            while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column)
            {
                int row = ordered[nextTriplet].Row;
                double sum = 0d;
                do
                {
                    sum += ordered[nextTriplet].Value;
                    nextTriplet++;
                }
                while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column && ordered[nextTriplet].Row == row);

                if (!NumericGuard.IsFinite(sum))
                    throw new ArgumentOutOfRangeException(nameof(triplets), "Duplicate sparse triplets must sum to a finite value.");
                if (sum != 0d)
                {
                    rows.Add(row);
                    values.Add(sum);
                }
            }

            pointers.Add(values.Count);
        }

        return new SparseCscMatrix(rowCount, columnCount, values, rows, pointers);
    }

    private static int CompareTriplets(Triplet left, Triplet right)
    {
        int columnComparison = left.Column.CompareTo(right.Column);
        return columnComparison != 0 ? columnComparison : left.Row.CompareTo(right.Row);
    }

    private sealed class Triplet
    {
        public Triplet(int row, int column, double value)
        {
            Row = row;
            Column = column;
            Value = value;
        }

        public int Row { get; }

        public int Column { get; }

        public double Value { get; }
    }
}