namespace RobotNet10.RobotApp.SLAM.Cartographer.Geometry;

/// <summary>
/// 3x3 matrix for representing covariance matrices
/// </summary>
public readonly struct Matrix3x3
{
    private readonly double[,] _data;

    /// <summary>
    /// Creates a new 3x3 matrix with all zeros
    /// </summary>
    public Matrix3x3()
    {
        _data = new double[3, 3];
    }

    /// <summary>
    /// Creates a new 3x3 matrix from a 2D array
    /// </summary>
    /// <param name="data">3x3 array of values</param>
    /// <exception cref="ArgumentException">Thrown if array is not 3x3</exception>
    public Matrix3x3(double[,] data)
    {
        if (data.GetLength(0) != 3 || data.GetLength(1) != 3)
        {
            throw new ArgumentException("Matrix must be 3x3", nameof(data));
        }
        _data = new double[3, 3];
        Array.Copy(data, _data, 9);
    }

    /// <summary>
    /// Gets or sets the value at the specified row and column
    /// </summary>
    public double this[int row, int col]
    {
        get => _data[row, col];
        set => _data[row, col] = value;
    }

    /// <summary>
    /// Gets the determinant of the matrix
    /// </summary>
    public double Determinant
    {
        get
        {
            return _data[0, 0] * (_data[1, 1] * _data[2, 2] - _data[1, 2] * _data[2, 1])
                 - _data[0, 1] * (_data[1, 0] * _data[2, 2] - _data[1, 2] * _data[2, 0])
                 + _data[0, 2] * (_data[1, 0] * _data[2, 1] - _data[1, 1] * _data[2, 0]);
        }
    }

    /// <summary>
    /// Creates an identity matrix
    /// </summary>
    public static Matrix3x3 Identity()
    {
        var matrix = new Matrix3x3();
        matrix[0, 0] = 1.0;
        matrix[1, 1] = 1.0;
        matrix[2, 2] = 1.0;
        return matrix;
    }

    /// <summary>
    /// Creates a covariance matrix for 2D pose (x, y, theta)
    /// </summary>
    /// <param name="xx">Variance in x</param>
    /// <param name="yy">Variance in y</param>
    /// <param name="tt">Variance in theta</param>
    /// <param name="xy">Covariance between x and y</param>
    /// <param name="xt">Covariance between x and theta</param>
    /// <param name="yt">Covariance between y and theta</param>
    public static Matrix3x3 Covariance(double xx, double yy, double tt, double xy = 0.0, double xt = 0.0, double yt = 0.0)
    {
        var matrix = new Matrix3x3();
        matrix[0, 0] = xx; // Var(x)
        matrix[1, 1] = yy; // Var(y)
        matrix[2, 2] = tt; // Var(theta)
        matrix[0, 1] = matrix[1, 0] = xy; // Cov(x,y)
        matrix[0, 2] = matrix[2, 0] = xt; // Cov(x,theta)
        matrix[1, 2] = matrix[2, 1] = yt; // Cov(y,theta)
        return matrix;
    }

    /// <summary>
    /// Gets all values as a 2D array
    /// </summary>
    public double[,] ToArray()
    {
        var result = new double[3, 3];
        Array.Copy(_data, result, 9);
        return result;
    }

    /// <summary>
    /// Gets the matrix as a flattened array (row-major order)
    /// </summary>
    public double[] ToFlatArray()
    {
        return
        [
            _data[0, 0], _data[0, 1], _data[0, 2],
            _data[1, 0], _data[1, 1], _data[1, 2],
            _data[2, 0], _data[2, 1], _data[2, 2]
        ];
    }

    public override string ToString()
    {
        return $"[{_data[0, 0]:F3}, {_data[0, 1]:F3}, {_data[0, 2]:F3}]\n" +
               $"[{_data[1, 0]:F3}, {_data[1, 1]:F3}, {_data[1, 2]:F3}]\n" +
               $"[{_data[2, 0]:F3}, {_data[2, 1]:F3}, {_data[2, 2]:F3}]";
    }
}