BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/MatrixHelper.cs

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// Helper class for matrix operations used in Extended Kalman Filter
/// Implements basic operations for small matrices (up to 6x6)
/// </summary>
public static class MatrixHelper
{
    /// <summary>
    /// Create identity matrix of size n x n
    /// </summary>
    public static double[,] CreateIdentity(int n)
    {
        var result = new double[n, n];
        for (int i = 0; i < n; i++)
            result[i, i] = 1.0;
        return result;
    }

    /// <summary>
    /// Create zero matrix of size rows x cols
    /// </summary>
    public static double[,] CreateZeros(int rows, int cols)
    {
        return new double[rows, cols];
    }

    /// <summary>
    /// Create diagonal matrix from array of diagonal values
    /// </summary>
    public static double[,] CreateDiagonal(double[] diagonal)
    {
        int n = diagonal.Length;
        var result = new double[n, n];
        for (int i = 0; i < n; i++)
            result[i, i] = diagonal[i];
        return result;
    }

    /// <summary>
    /// Matrix multiplication: C = A * B
    /// </summary>
    public static double[,] Multiply(double[,] a, double[,] b)
    {
        int aRows = a.GetLength(0);
        int aCols = a.GetLength(1);
        int bRows = b.GetLength(0);
        int bCols = b.GetLength(1);

        if (aCols != bRows)
            throw new ArgumentException($"Matrix dimensions incompatible for multiplication: ({aRows}x{aCols}) * ({bRows}x{bCols})");

        var result = new double[aRows, bCols];
        for (int i = 0; i < aRows; i++)
        {
            for (int j = 0; j < bCols; j++)
            {
                double sum = 0.0;
                for (int k = 0; k < aCols; k++)
                    sum += a[i, k] * b[k, j];
                result[i, j] = sum;
            }
        }
        return result;
    }

    /// <summary>
    /// Matrix addition: C = A + B
    /// </summary>
    public static double[,] Add(double[,] a, double[,] b)
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);

        if (rows != b.GetLength(0) || cols != b.GetLength(1))
            throw new ArgumentException("Matrix dimensions must match for addition");

        var result = new double[rows, cols];
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                result[i, j] = a[i, j] + b[i, j];
        return result;
    }

    /// <summary>
    /// Matrix subtraction: C = A - B
    /// </summary>
    public static double[,] Subtract(double[,] a, double[,] b)
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);

        if (rows != b.GetLength(0) || cols != b.GetLength(1))
            throw new ArgumentException("Matrix dimensions must match for subtraction");

        var result = new double[rows, cols];
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                result[i, j] = a[i, j] - b[i, j];
        return result;
    }

    /// <summary>
    /// Matrix transpose: B = A^T
    /// </summary>
    public static double[,] Transpose(double[,] a)
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);
        var result = new double[cols, rows];

        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                result[j, i] = a[i, j];
        return result;
    }

    /// <summary>
    /// Matrix scalar multiplication: B = scalar * A
    /// </summary>
    public static double[,] ScalarMultiply(double scalar, double[,] a)
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);
        var result = new double[rows, cols];

        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                result[i, j] = scalar * a[i, j];
        return result;
    }

    /// <summary>
    /// Matrix inverse using Gauss-Jordan elimination (for small matrices)
    /// </summary>
    public static double[,] Inverse(double[,] a)
    {
        int n = a.GetLength(0);
        if (n != a.GetLength(1))
            throw new ArgumentException("Matrix must be square for inversion");

        // Create augmented matrix [A | I]
        var augmented = new double[n, 2 * n];
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < n; j++)
                augmented[i, j] = a[i, j];
            augmented[i, n + i] = 1.0;
        }

        // Gauss-Jordan elimination
        for (int i = 0; i < n; i++)
        {
            // Find pivot
            int maxRow = i;
            for (int k = i + 1; k < n; k++)
            {
                if (Math.Abs(augmented[k, i]) > Math.Abs(augmented[maxRow, i]))
                    maxRow = k;
            }

            // Swap rows
            if (maxRow != i)
            {
                for (int k = 0; k < 2 * n; k++)
                    (augmented[i, k], augmented[maxRow, k]) = (augmented[maxRow, k], augmented[i, k]);
            }

            // Check for singular matrix
            if (Math.Abs(augmented[i, i]) < 1e-10)
                throw new InvalidOperationException("Matrix is singular and cannot be inverted");

            // Scale pivot row
            double pivot = augmented[i, i];
            for (int j = 0; j < 2 * n; j++)
                augmented[i, j] /= pivot;

            // Eliminate column
            for (int k = 0; k < n; k++)
            {
                if (k != i)
                {
                    double factor = augmented[k, i];
                    for (int j = 0; j < 2 * n; j++)
                        augmented[k, j] -= factor * augmented[i, j];
                }
            }
        }

        // Extract inverse from augmented matrix
        var result = new double[n, n];
        for (int i = 0; i < n; i++)
            for (int j = 0; j < n; j++)
                result[i, j] = augmented[i, n + j];

        return result;
    }

    /// <summary>
    /// Copy matrix
    /// </summary>
    public static double[,] Copy(double[,] a)
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);
        var result = new double[rows, cols];

        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                result[i, j] = a[i, j];
        return result;
    }

    /// <summary>
    /// Print matrix (for debugging)
    /// </summary>
    public static string ToString(double[,] a, string format = "F4")
    {
        int rows = a.GetLength(0);
        int cols = a.GetLength(1);
        var sb = new System.Text.StringBuilder();

        for (int i = 0; i < rows; i++)
        {
            for (int j = 0; j < cols; j++)
            {
                sb.Append(a[i, j].ToString(format));
                if (j < cols - 1)
                    sb.Append("  ");
            }
            if (i < rows - 1)
                sb.AppendLine();
        }
        return sb.ToString();
    }
}