using System;
using System.Diagnostics;
using Passer.LinearAlgebra;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
#endif

public readonly struct Slice {
    public int start { get; }
    public int stop { get; }
    public Slice(int start, int stop) {
        this.start = start;
        this.stop = stop;
    }
}

public class Matrix2 {
    public float[,] data { get; }

    public uint nRows => (uint)data.GetLength(0);
    public uint nCols => (uint)data.GetLength(1);

    public Matrix2(uint nRows, uint nCols) {
        this.data = new float[nRows, nCols];
    }
    public Matrix2(float[,] data) {
        this.data = data;
    }

    public static Matrix2 Zero(uint nRows, uint nCols) {
        return new Matrix2(nRows, nCols);
    }

    public static Matrix2 Identity(uint size) {
        return Diagonal(1, size);
        // float[,] resultData = new float[size, size];
        // for (int i = 0; i < size; i++)
        //     resultData[i, i] = 1.0f;
        // return new Matrix2(resultData);
    }

    public static Matrix2 Diagonal(Matrix1 v) {
        float[,] resultData = new float[v.magnitude, v.magnitude];
        for (int ix = 0; ix < v.magnitude; ix++)
            resultData[ix, ix] = v.data[ix];
        return new Matrix2(resultData);
    }
    public static Matrix2 Diagonal(float f, uint size) {
        float[,] resultData = new float[size, size];
        for (int ix = 0; ix < size; ix++)
            resultData[ix, ix] = f;
        return new Matrix2(resultData);
    }

    public static Matrix2 SkewMatrix(Vector3Float v) {
        float[,] result = new float[3, 3] {
            {0, -v.z, v.y},
            {v.z, 0, -v.x},
            {-v.y, v.x, 0}
        };
        return new Matrix2(result);
    }

    public Matrix2 Transpose() {
        float[,] resultData = new float[this.nCols, this.nRows];
        for (uint rowIx = 0; rowIx < this.nRows; rowIx++) {
            for (uint colIx = 0; colIx < this.nCols; colIx++)
                resultData[colIx, rowIx] = this.data[rowIx, colIx];
        }
        return new Matrix2(resultData);
        // double checked code
    }

    public static Matrix2 operator -(Matrix2 m) {
        float[,] result = new float[m.nRows, m.nCols];

        for (int i = 0; i < m.nRows; i++) {
            for (int j = 0; j < m.nCols; j++)
                result[i, j] = -m.data[i, j];
        }
        return new Matrix2(result);
    }

    public static Matrix2 operator -(Matrix2 A, Matrix2 B) {
        if (A.nRows != B.nRows || A.nCols != B.nCols)
            throw new System.ArgumentException("Size of A must match size of B.");

        float[,] result = new float[A.nRows, B.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++)
                result[i, j] = A.data[i, j] - B.data[i, j];
        }
        return new Matrix2(result);
    }

    public static Matrix2 operator +(Matrix2 A, Matrix2 B) {
        if (A.nRows != B.nRows || A.nCols != B.nCols)
            throw new System.ArgumentException("Size of A must match size of B.");

        float[,] result = new float[A.nRows, B.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++)
                result[i, j] = A.data[i, j] + B.data[i, j];
        }
        return new Matrix2(result);
    }

    public static Matrix2 operator *(Matrix2 A, Matrix2 B) {
        if (A.nCols != B.nRows)
            throw new System.ArgumentException("Number of columns in A must match number of rows in B.");

        float[,] result = new float[A.nRows, B.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < B.nCols; j++) {
                float sum = 0.0f;
                for (int k = 0; k < A.nCols; k++)
                    sum += A.data[i, k] * B.data[k, j];

                result[i, j] = sum;
            }
        }

        return new Matrix2(result);
        // double checked code
    }

    public static Matrix1 operator *(Matrix2 A, Matrix1 v) {
        float[] result = new float[A.nRows];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++) {
                result[i] += A.data[i, j] * v.data[j];
            }
        }

        return new Matrix1(result);
    }

    public static Vector3Float operator *(Matrix2 A, Vector3Float v) {
        return new Vector3Float(
            A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
            A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
            A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
        );
    }


    public static Matrix2 operator *(Matrix2 A, float s) {
        float[,] result = new float[A.nRows, A.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++)
                result[i, j] += A.data[i, j] * s;
        }

        return new Matrix2(result);
    }
    public static Matrix2 operator *(float scalar, Matrix2 A) {
        return A * scalar;
    }

    public Matrix2 Slice(Slice slice) {
        return Slice(slice.start, slice.stop);
    }
    public Matrix2 Slice(int from, int to) {
        if (from < 0 || to >= this.nRows)
            throw new System.ArgumentException("Slice index out of range.");

        float[,] result = new float[to - from, this.nCols];
        int resultRowIx = 0;
        for (int rowIx = from; rowIx < to; rowIx++) {
            for (int colIx = 0; colIx < this.nCols; colIx++) {
                result[resultRowIx, colIx] = this.data[rowIx, colIx];
            }
            resultRowIx++;
        }

        return new Matrix2(result);
    }
    public void UpdateSlice(Slice slice, Matrix2 m) {
        int mRowIx = 0;
        for (int rowIx = slice.start; rowIx < slice.stop; rowIx++) {
            for (int colIx = 0; colIx < this.nCols; colIx++)
                this.data[rowIx, colIx] = m.data[mRowIx, colIx];
        }
    }
    public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) {
        for (int i = rowRange.start; i < rowRange.stop; i++) {
            for (int j = colRange.start; j < colRange.stop; j++)
                this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop];
        }
    }

    public Matrix2 Inverse() {
        Matrix2 A = this;
        // unchecked
        uint n = A.nRows;

        // Create an identity matrix of the same size as the original matrix
        float[,] augmentedMatrix = new float[n, 2 * n];
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                augmentedMatrix[i, j] = A.data[i, j];
                augmentedMatrix[i, j + n] = (i == j) ? 1 : 0;  // Identity matrix
            }
        }

        // Perform Gaussian elimination
        for (int i = 0; i < n; i++) {
            // Find the pivot row
            float pivot = augmentedMatrix[i, i];
            if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
                throw new InvalidOperationException("Matrix is singular and cannot be inverted.");

            // Normalize the pivot row
            for (int j = 0; j < 2 * n; j++)
                augmentedMatrix[i, j] /= pivot;

            // Eliminate the column below the pivot
            for (int j = i + 1; j < n; j++) {
                float factor = augmentedMatrix[j, i];
                for (int k = 0; k < 2 * n; k++)
                    augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
            }
        }

        // Back substitution
        for (uint i = n - 1; i >= 0; i--) {
            // Eliminate the column above the pivot
            for (uint j = i - 1; j >= 0; j--) {
                float factor = augmentedMatrix[j, i];
                for (int k = 0; k < 2 * n; k++)
                    augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
            }
        }

        // Extract the inverse matrix from the augmented matrix
        float[,] inverse = new float[n, n];
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++)
                inverse[i, j] = augmentedMatrix[i, j + n];
        }

        return new Matrix2(inverse);
    }
}

public class Matrix1 {
    public float[] data { get; }

    public uint magnitude => (uint)data.GetLength(0);

    public Matrix1(uint magnitude) {
        this.data = new float[magnitude];
    }

    public Matrix1(float[] data) {
        this.data = data;
    }

    public static Matrix1 Zero(uint magnitude) {
        return new Matrix1(magnitude);
    }

    public static Matrix1 FromVector2(Vector2Float v) {
        float[] result = new float[2];
        result[0] = v.x;
        result[1] = v.y;
        return new Matrix1(result);
    }

    public static Matrix1 FromVector3(Vector3Float v) {
        float[] result = new float[3];
        result[0] = v.x;
        result[1] = v.y;
        result[2] = v.z;
        return new Matrix1(result);
    }

    public Vector2Float vector2 {
        get {
            if (this.magnitude != 2)
                throw new System.ArgumentException("Matrix1 must be of size 2");
            return new Vector2Float(this.data[0], this.data[1]);
        }
    }
    public Vector3Float vector3 {
        get {
            if (this.magnitude != 3)
                throw new System.ArgumentException("Matrix1 must be of size 3");
            return new Vector3Float(this.data[0], this.data[1], this.data[2]);
        }
    }

    public Matrix2 Transpose() {
        float[,] r = new float[1, this.magnitude];
        for (uint colIx = 0; colIx < this.magnitude; colIx++)
            r[1, colIx] = this.data[colIx];

        return new Matrix2(r);
    }

    public static float Dot(Matrix1 a, Matrix1 b) {
        if (a.magnitude != b.magnitude)
            throw new System.ArgumentException("Vectors must be of the same length.");

        float result = 0.0f;
        for (int i = 0; i < a.magnitude; i++) {
            result += a.data[i] * b.data[i];
        }
        return result;
    }

    public static Matrix1 operator *(Matrix1 A, float f) {
        float[] result = new float[A.magnitude];

        for (int i = 0; i < A.magnitude; i++)
            result[i] += A.data[i] * f;

        return new Matrix1(result);
    }
    public static Matrix1 operator *(float f, Matrix1 A) {
        return A * f;
    }

    public Matrix1 Slice(Slice range) {
        return Slice(range.start, range.stop);
    }
    public Matrix1 Slice(int from, int to) {
        if (from < 0 || to >= this.magnitude)
            throw new System.ArgumentException("Slice index out of range.");

        float[] result = new float[to - from];
        int resultIx = 0;
        for (int ix = from; ix < to; ix++)
            result[resultIx++] = this.data[ix];

        return new Matrix1(result);
    }
    public void UpdateSlice(Slice slice, Matrix1 v) {
        int vIx = 0;
        for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
            this.data[ix] = v.data[vIx];
    }
}

// public class Matrix {
//     private readonly uint rows = 0;
//     private readonly uint cols = 0;
//     private float[] data;

//     public Matrix(uint rows, uint cols) {
//         this.rows = rows;
//         this.cols = cols;
//     }

//     public static float[,] Diagonal(float[] v) {
//         float[,] r = new float[v.Length, v.Length];
//         for (int i = 0; i < v.Length; i++) {
//             r[i, i] = v[i];
//         }
//         return r;
//     }

//     public static float[,] Transpose(float[,] m) {
//         int rows = m.GetLength(0);
//         int cols = m.GetLength(1);
//         float[,] r = new float[cols, rows];
//         for (uint rowIx = 0; rowIx < rows; rowIx++) {
//             for (uint colIx = 0; colIx < cols; colIx++)
//                 r[colIx, rowIx] = m[rowIx, colIx];
//         }
//         return r;
//         // double checked code
//     }

//     public static void NegateColumn(float[,] m, uint colIx) {
//         for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) {
//             m[rowIx, colIx] = -m[rowIx, colIx];
//         }
//     }

//     public static float Determinant(float[,] matrix) {
//         int n = matrix.GetLength(0);
//         if (n != matrix.GetLength(1))
//             throw new System.ArgumentException("Matrix must be square.");

//         if (n == 1)
//             return matrix[0, 0]; // Base case for 1x1 matrix

//         if (n == 2) // Base case for 2x2 matrix
//             return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0];

//         float det = 0;
//         for (int col = 0; col < n; col++)
//             det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col));

//         return det;
//     }

//     // Helper function to compute the minor of a matrix
//     private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) {
//         int n = matrix.GetLength(0);
//         float[,] minor = new float[n - 1, n - 1];

//         int r = 0, c = 0;
//         for (int i = 0; i < n; i++) {
//             if (i == rowToRemove) continue;

//             c = 0;
//             for (int j = 0; j < n; j++) {
//                 if (j == colToRemove) continue;

//                 minor[r, c] = matrix[i, j];
//                 c++;
//             }
//             r++;
//         }

//         return minor;
//     }

//     public static float[,] MultiplyMatrices(float[,] A, float[,] B) {
//         int rowsA = A.GetLength(0);
//         int colsA = A.GetLength(1);
//         int rowsB = B.GetLength(0);
//         int colsB = B.GetLength(1);

//         if (colsA != rowsB)
//             throw new System.ArgumentException("Number of columns in A must match number of rows in B.");

//         float[,] result = new float[rowsA, colsB];

//         for (int i = 0; i < rowsA; i++) {
//             for (int j = 0; j < colsB; j++) {
//                 float sum = 0.0f;
//                 for (int k = 0; k < colsA; k++)
//                     sum += A[i, k] * B[k, j];

//                 result[i, j] = sum;
//             }
//         }

//         return result;
//         // double checked code
//     }

//     public static float[] MultiplyMatrixVector(float[,] A, float[] v) {
//         int rows = A.GetLength(0);
//         int cols = A.GetLength(1);
//         float[] result = new float[rows];

//         for (int i = 0; i < rows; i++) {
//             for (int j = 0; j < cols; j++) {
//                 result[i] += A[i, j] * v[j];
//             }
//         }

//         return result;
//     }

//     // Vector-matrix multiplication
//     public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) {
//         return new Vector3Float(
//             A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z,
//             A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z,
//             A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z
//         );
//     }

//     public static float[,] MultiplyMatrixScalar(float[,] A, float s) {
//         int rows = A.GetLength(0);
//         int cols = A.GetLength(1);
//         float[,] result = new float[rows, cols];

//         for (int i = 0; i < rows; i++) {
//             for (int j = 0; j < cols; j++) {
//                 result[i, j] += A[i, j] * s;
//             }
//         }

//         return result;
//     }

//     public static float[] GetColumn(float[,] M, int col) {
//         int rows = M.GetLength(0);
//         float[] column = new float[rows];
//         for (int i = 0; i < rows; i++) {
//             column[i] = M[i, col];
//         }
//         return column;
//     }

//     public static Vector3Float GetRow3(float[,] M, int rowIx) {
//         int cols = M.GetLength(1);
//         Vector3Float row = new(
//             M[rowIx, 0],
//             M[rowIx, 1],
//             M[rowIx, 2]
//         );
//         return row;
//     }

//     public static void SetRow3(float[,] M, int rowIx, Vector3Float v) {
//         M[rowIx, 0] = v.x;
//         M[rowIx, 1] = v.y;
//         M[rowIx, 2] = v.z;
//     }

//     public static float Dot(float[] a, float[] b) {
//         float sum = 0;
//         for (int i = 0; i < a.Length; i++) sum += a[i] * b[i];
//         return sum;
//     }

//     public static float[,] IdentityMatrix(int size) {
//         float[,] I = new float[size, size];
//         for (int i = 0; i < size; i++) I[i, i] = 1.0f;
//         return I;
//     }

// }