using System;
using Algorithms.Numeric.Decomposition;
using Utilities.Extensions;

namespace Algorithms.Numeric.Pseudoinverse;

/// <summary>
///     The Moore–Penrose pseudo-inverse A+ of a matrix A,
///     is a general way to find the solution to the following system of linear equations:
///     ~b = A ~y. ~b e R^m; ~y e R^n; A e Rm×n.
///     There are varios methods for construction the pseudo-inverse.
///     This one is based on Singular Value Decomposition (SVD).
/// </summary>
public static class PseudoInverse
{
    /// <summary>
    ///     Return the pseudoinverse of a matrix based on the Moore-Penrose Algorithm.
    ///     using Singular Value Decomposition (SVD).
    /// </summary>
    /// <param name="inMat">Input matrix to find its inverse to.</param>
    /// <returns>The inverse matrix approximation of the input matrix.</returns>
    public static double[,] PInv(double[,] inMat)
    {
        // To compute the SVD of the matrix to find Sigma.
        var (u, s, v) = ThinSvd.Decompose(inMat);

        // To take the reciprocal of each non-zero element on the diagonal.
        var len = s.Length;

        var sigma = new double[len];
        for (var i = 0; i < len; i++)
        {
            sigma[i] = Math.Abs(s[i]) < 0.0001 ? 0 : 1 / s[i];
        }

        // To construct a diagonal matrix based on the vector result.
        var diag = sigma.ToDiagonalMatrix();

        // To construct the pseudo-inverse using the computed information above.
        var matinv = u.Multiply(diag).Multiply(v.Transpose());

        // To Transpose the result matrix.
        return matinv.Transpose();
    }
}