using System;
namespace LinearAlgebra {
    class QR {
        // QR Decomposition of a matrix A
        public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
            int nRows = A.nRows;
            int nCols = A.nCols;

            float[,] Q = new float[nRows, nCols];
            float[,] R = new float[nCols, nCols];

            // Perform Gram-Schmidt orthogonalization
            for (uint colIx = 0; colIx < nCols; colIx++) {

                // Step 1: v = column(ix) of A
                float[] v = new float[nRows];
                for (int rowIx = 0; rowIx < nRows; rowIx++)
                    v[rowIx] = A.data[rowIx, colIx];

                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
                for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
                    float dotProd = 0;
                    for (int i = 0; i < nRows; i++)
                        dotProd += Q[i, colIx2] * v[i];
                    for (int i = 0; i < nRows; i++)
                        v[i] -= dotProd * Q[i, colIx2];
                }

                // Step 3: Normalize v to get column(ix) of Q
                float norm = 0;
                for (int rowIx = 0; rowIx < nRows; rowIx++)
                    norm += v[rowIx] * v[rowIx];
                norm = (float)Math.Sqrt(norm);

                for (int rowIx = 0; rowIx < nRows; rowIx++)
                    Q[rowIx, colIx] = v[rowIx] / norm;

                // Store the coefficients of R
                for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
                    R[colIx2, colIx] = 0;
                    for (int k = 0; k < nRows; k++)
                        R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
                }
            }
            return (new Matrix2(Q), new Matrix2(R));
        }

        // Reduced QR Decomposition of a matrix A
        public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
            int nRows = A.nRows;
            int nCols = A.nCols;

            float[,] Q = new float[nRows, nCols];
            float[,] R = new float[nCols, nCols];

            // Perform Gram-Schmidt orthogonalization
            for (int colIx = 0; colIx < nCols; colIx++) {

                // Step 1: v = column(colIx) of A
                float[] columnIx = new float[nRows];
                bool isZeroColumn = true;
                for (int rowIx = 0; rowIx < nRows; rowIx++) {
                    columnIx[rowIx] = A.data[rowIx, colIx];
                    if (columnIx[rowIx] != 0)
                        isZeroColumn = false;
                }
                if (isZeroColumn) {
                    for (int rowIx = 0; rowIx < nRows; rowIx++)
                        Q[rowIx, colIx] = 0;
                    // Set corresponding R element to 0
                    R[colIx, colIx] = 0;

                    Console.WriteLine($"zero column {colIx}");

                    continue;
                }

                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
                for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
                    // Compute the dot product of v and column(colIx2) of Q
                    float dotProduct = 0;
                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
                        dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
                    // Subtract the projection from v
                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
                        columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
                }

                // Step 3: Normalize v to get column(colIx) of Q
                float norm = 0;
                for (int rowIx = 0; rowIx < nRows; rowIx++)
                    norm += columnIx[rowIx] * columnIx[rowIx];
                if (norm == 0)
                    throw new Exception("invalid value");

                norm = (float)Math.Sqrt(norm);

                for (int rowIx = 0; rowIx < nRows; rowIx++)
                    Q[rowIx, colIx] = columnIx[rowIx] / norm;

                // Here is where it deviates from the Full QR Decomposition !

                // Step 4: Compute the row(colIx) of R
                for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
                    float dotProduct = 0;
                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
                        dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
                    R[colIx, colIx2] = dotProduct;
                }
            }
            if (!float.IsFinite(R[0, 0]))
                throw new Exception("invalid value");

            return (new Matrix2(Q), new Matrix2(R));
        }
    }

    class SVD {
        // According to ChatGPT, Mathnet uses  Golub-Reinsch SVD algorithm
        // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
        // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
        // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
        //
        // 2. Diagonalization: Once the matrix is in bidiagonal form,
        // the singular values are computed using an iterative process
        // (typically involving QR factorization or Jacobi rotations) until convergence.
        // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
        //
        // 3. Computing UU and VTVT: After obtaining the singular values,
        // the left singular vectors UU and right singular vectors VTVT are computed
        // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.

        // Bidiagnolizations through Householder transformations
        public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
            int m = A.nRows; // Rows of A
            int n = A.nCols; // Columns of A
            float[,] U1 = new float[m, m]; // Left orthogonal matrix
            float[,] V1 = new float[n, n]; // Right orthogonal matrix
            float[,] B = A.Clone().data; // Copy A to B for transformation

            // Initialize U1 and V1 as identity matrices
            for (int i = 0; i < m; i++)
                U1[i, i] = 1;
            for (int i = 0; i < n; i++)
                V1[i, i] = 1;

            // Perform Householder reflections to create a bidiagonal matrix B
            for (int j = 0; j < n; j++) {
                // Step 1: Construct the Householder vector y
                float[] y = new float[m - j];
                for (int i = j; i < m; i++)
                    y[i - j] = B[i, j];

                // Step 2: Compute the norm and scalar alpha
                float norm = 0;
                for (int i = 0; i < y.Length; i++)
                    norm += y[i] * y[i];
                norm = (float)Math.Sqrt(norm);

                if (B[j, j] > 0)
                    norm = -norm;

                float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));

                // Step 3: Apply the reflection to zero out below diagonal
                for (int k = j; k < n; k++) {
                    float dot = 0;
                    for (int i = j; i < m; i++)
                        dot += y[i - j] * B[i, k];
                    dot /= r;

                    for (int i = j; i < m; i++)
                        B[i, k] -= 2 * dot * y[i - j];
                }

                // Step 4: Update U1 with the Householder reflection (U1 * Householder)
                for (int i = j; i < m; i++)
                    U1[i, j] = y[i - j] / alpha;

                // Step 5: Update V1 (storing the Householder vector y)
                // Correct indexing: we only need to store part of y in V1 from index j to n
                for (int i = j; i < n; i++)
                    V1[j, i] = B[j, i];

                // Repeat steps for further columns if necessary
            }
            return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
        }

        public static Matrix2 Bidiagonalize(Matrix2 A) {
            int m = A.nRows; // Rows of A
            int n = A.nCols; // Columns of A
            float[,] B = A.Clone().data; // Copy A to B for transformation

            // Perform Householder reflections to create a bidiagonal matrix B
            for (int j = 0; j < n; j++) {
                // Step 1: Construct the Householder vector y
                float[] y = new float[m - j];
                for (int i = j; i < m; i++)
                    y[i - j] = B[i, j];

                // Step 2: Compute the norm and scalar alpha
                float norm = 0;
                for (int i = 0; i < y.Length; i++)
                    norm += y[i] * y[i];
                norm = (float)Math.Sqrt(norm);

                if (B[j, j] > 0)
                    norm = -norm;

                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));

                // Step 3: Apply the reflection to zero out below diagonal
                for (int k = j; k < n; k++) {
                    float dot = 0;
                    for (int i = j; i < m; i++)
                        dot += y[i - j] * B[i, k];
                    dot /= r;

                    for (int i = j; i < m; i++)
                        B[i, k] -= 2 * dot * y[i - j];
                }

                // Repeat steps for further columns if necessary
            }
            return new Matrix2(B);
        }

        // QR Iteration for diagonalization of a bidiagonal matrix B
        public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
            int m = B.nRows;
            int n = B.nCols;

            Matrix2 U = new(m, m); // Left singular vectors (U)
            Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
            float[] singularValues = new float[Math.Min(m, n)]; // Singular values

            // Initialize U and Vt as identity matrices
            for (int i = 0; i < m; i++)
                U.data[i, i] = 1;
            for (int i = 0; i < n; i++)
                Vt.data[i, i] = 1;

            // Perform QR iterations
            float tolerance = 1e-7f; //1e-12f; for double
            bool converged = false;
            while (!converged) {
                // Perform QR decomposition on the matrix B
                (Matrix2 Q, Matrix2 R) = QR.Decomposition(B);

                // Update B to be the product Q * R //R * Q
                B = R * Q;

                // Accumulate the transformations in U and Vt
                U *= Q;
                Vt *= R;

                // Check convergence by looking at the off-diagonal elements of B
                converged = true;
                for (int i = 0; i < m - 1; i++) {
                    for (int j = i + 1; j < n; j++) {
                        if (Math.Abs(B.data[i, j]) > tolerance) {
                            converged = false;
                            break;
                        }
                    }
                }
            }

            // Extract singular values (diagonal elements of B)
            for (int i = 0; i < Math.Min(m, n); i++)
                singularValues[i] = B.data[i, i];

            return (new Matrix1(singularValues), U, Vt);
        }

        public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
            if (A.nRows != A.nCols)
                throw new ArgumentException("SVD: matrix A has to be square.");

            Matrix2 B = Bidiagonalize(A);
            (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
            return (U, S, Vt);
        }
    }
}