/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2024-2024. All rights reserved.
 */
package matrix4cj

import std.math.*

/** Singular Value Decomposition.
 * <P>
 * For an m-by-n matrix A with m >= n, the singular value decomposition is
 * an m-by-n orthogonal matrix U, an n-by-n diagonal matrix S, and
 * an n-by-n orthogonal matrix V so that A = U*S*V'.
 * <P>
 * The singular values, sigma[k] = S[k][k], are ordered so that
 * sigma[0] >= sigma[1] >= ... >= sigma[n-1].
 * <P>
 * The singular value decompostion always exists, so the constructor will
 * never fail.  The matrix condition number and the effective numerical
 * rank can be computed from this decomposition.
 */
public class SingularValueDecomposition {
    /** Arrays for internal storage of U */
    private var U: Array<Array<Float64>>

    /** Arrays for internal storage of V */
    private var V: Array<Array<Float64>>

    /** Array for internal storage of singular values*/
    private var s: Array<Float64>

    /**row dimension*/
    private var m: Int64 = 0

    /**column dimension*/
    private var n: Int64 = 0

    /** 
     * Construct the singular value decomposition
     *  Structure to access U, S and V.
     * @param Arg    Rectangular matrix
     */
    public init(Arg: Matrix) {
        let A: Matrix = Arg.clone()
        m = Arg.rowNum
        n = Arg.colNum
        let nu: Int64 = min(m, n)
        s = Array<Float64>(min(m + 1, n)) {_ => 0.0}
        let array_dim_generated_nu = nu
        let array_dim_generated_n = n
        let array_dim_generated_m = m
        U = Array<Array<Float64>>(array_dim_generated_m) {_ => Array<Float64>(array_dim_generated_nu) {_ => 0.0}}
        V = Array<Array<Float64>>(array_dim_generated_n) {_ => Array<Float64>(array_dim_generated_n) {_ => 0.0}}
        let e: Array<Float64> = Array<Float64>(n) {_ => 0.0}
        let work: Array<Float64> = Array<Float64>(m) {_ => 0.0}
        let nct: Int64 = min(m - 1, n)
        let nrt: Int64 = max(0, min(n - 2, m))

        for (k in 0..max(nct, nrt)) {
            if (k < nct) {
                s[k] = 0.0
                for (i in k..m) {
                    s[k] = Maths.hypot(s[k], A[i, k])
                }
                if (s[k] != 0.0) {
                    if (A[k, k] < 0.0) {
                        s[k] = -s[k]
                    }
                    for (i in k..m) {
                        A[i, k] /= s[k]
                    }
                    A[k, k] += 1.0
                }
                s[k] = -s[k]
            }
            for (j in (k + 1)..n) {
                if ((k < nct) && (s[k] != 0.0)) {
                    // Apply the transformation.
                    var t: Float64 = 0.0
                    for (i in k..m) {
                        t += A[i, k] * A[i, j]
                    }
                    t = -t / A[k, k]
                    for (i in k..m) {
                        A[i, j] += t * A[i, k]
                    }
                }
                // Place the k-th row of A into e for the
                // subsequent calculation of the row transformation.
                e[j] = A[k, j]
            }
            if (k < nct) {
                // Place the transformation in U for subsequent back
                // multiplication.
                for (i in k..m) {
                    U[i][k] = A[i, k]
                }
            }
            if (k < nrt) {
                // Compute the k-th row transformation and place the
                // k-th super-diagonal in e[k].
                // Compute 2-norm without under/overflow.
                e[k] = 0.0
                for (i in (k + 1)..n) {
                    e[k] = Maths.hypot(e[k], e[i])
                }
                if (e[k] != 0.0) {
                    if (e[k + 1] < 0.0) {
                        e[k] = -e[k]
                    }
                    for (i in (k + 1)..n) {
                        e[i] /= e[k]
                    }
                    e[k + 1] += 1.0
                }
                e[k] = -e[k]
                if ((k + 1 < m) && (e[k] != 0.0)) {
                    // Apply the transformation.
                    for (i in (k + 1)..m) {
                        work[i] = 0.0
                    }
                    for (j in (k + 1)..n) {
                        for (i in (k + 1)..m) {
                            work[i] += e[j] * A[i, j]
                        }
                    }
                    for (j in (k + 1)..n) {
                        let t = -e[j] / e[k + 1]
                        for (i in (k + 1)..m) {
                            A[i, j] += t * work[i]
                        }
                    }
                }
                // Place the transformation in V for subsequent
                // back multiplication.
                for (i in k + 1..n) {
                    V[i][k] = e[i]
                }
            }
        }

        // Set up the final bidiagonal matrix or order p.
        var p = min(n, m + 1)
        if (nct < n) {
            s[nct] = A[nct, nct]
        }
        if (m < p) {
            s[p - 1] = 0.0
        }
        if (nrt + 1 < p) {
            e[nrt] = A[nrt, p - 1]
        }
        e[p - 1] = 0.0

        // If required, generate U.
        for (j in nct..nu) {
            for (i in 0..m) {
                U[i][j] = 0.0
            }
            U[j][j] = 1.0
        }
        for (k in (nct - 1..=0 : -1)) {
            if (s[k] != 0.0) {
                for (j in k + 1..nu) {
                    var t: Float64 = 0.0
                    for (i in k..m) {
                        t += U[i][k] * U[i][j]
                    }
                    t = -t / U[k][k]
                    for (i in k..m) {
                        U[i][j] += t * U[i][k]
                    }
                }
                for (i in k..m) {
                    U[i][k] = -U[i][k]
                }
                U[k][k] = 1.0 + U[k][k]
                for (i in 0..(k - 1)) {
                    U[i][k] = 0.0
                }
            } else {
                for (i in 0..m) {
                    U[i][k] = 0.0
                }
                U[k][k] = 1.0
            }
        }

        // If required, generate V.
        for (k in (n - 1)..=0 : -1) {
            if ((k < nrt) && (e[k] != 0.0)) {
                for (j in (k + 1)..nu) {
                    var t: Float64 = 0.0
                    for (i in (k + 1)..n) {
                        t += V[i][k] * V[i][j]
                    }
                    t = -t / V[k + 1][k]
                    for (i in (k + 1)..n) {
                        V[i][j] += t * V[i][k]
                    }
                }
            }
            for (i in 0..n) {
                V[i][k] = 0.0
            }
            V[k][k] = 1.0
        }

        // Main iteration loop for the singular values.
        let pp: Int64 = p - 1
        var iter: Int64 = 0
        let eps: Float64 = pow(2.0, -52.0)
        let tiny: Float64 = pow(2.0, -966.0)
        while (p > 0) {
            var k: Int64 = 0
            let kase: Int64
            // Here is where a test for too many iterations would go.

            // This section of the program inspects for
            // negligible elements in the s and e arrays.  On
            // completion the variables kase and k are set as follows.

            // kase = 1     if s(p) and e[k-1] are negligible and k<p
            // kase = 2     if s(k) is negligible and k<p
            // kase = 3     if e[k-1] is negligible, k<p, and
            //              s(k), ..., s(p) are not negligible (qr step).
            // kase = 4     if e(p-1) is negligible (convergence).

            for (k_t in (p - 2)..=-1 : -1) {
                k = k_t
                if (k == -1) {
                    break
                }
                if (abs(e[k]) <= tiny + eps * (abs(s[k]) + abs(s[k + 1]))) {
                    e[k] = 0.0
                    break
                }
            }
            if (k == p - 2) {
                kase = 4
            } else {
                var ks: Int64 = 0;
                for (ks_t in (p - 1)..=k : -1) {
                    ks = ks_t
                    if (ks == k) {
                        break
                    }
                    let t = (if (ks != p) {
                        abs(e[ks])
                    } else {
                        0.0
                    }) + (if (ks != k + 1) {
                        abs(e[ks - 1])
                    } else {
                        0.0
                    })
                    if (abs(s[ks]) <= tiny + eps * t) {
                        s[ks] = 0.0
                        break
                    }
                }
                if (ks == k) {
                    kase = 3
                } else if (ks == p - 1) {
                    kase = 1
                } else {
                    kase = 2
                    k = ks
                }
            }
            k++

            // Perform the task indicated by kase.
            match (kase) {
                // Deflate negligible s(p).
                case 1 =>
                    var f = e[p - 2]
                    e[p - 2] = 0.0
                    for (j in (p - 2)..=k : -1) {
                        var t = Maths.hypot(s[j], f)
                        let cs = s[j] / t
                        let sn = f / t
                        s[j] = t
                        if (j != k) {
                            f = -sn * e[j - 1]
                            e[j - 1] = cs * e[j - 1]
                        }
                        for (i in 0..n) {
                            t = cs * V[i][j] + sn * V[i][p - 1]
                            V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1]
                            V[i][j] = t
                        }
                    }
                // Split at negligible s(k).
                case 2 =>
                    var f = e[k - 1]
                    e[k - 1] = 0.0
                    for (j in k..p) {
                        var t = Maths.hypot(s[j], f)
                        let cs = s[j] / t
                        let sn = f / t
                        s[j] = t
                        f = -sn * e[j]
                        e[j] = cs * e[j]
                        for (i in 0..m) {
                            t = cs * U[i][j] + sn * U[i][k - 1]
                            U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1]
                            U[i][j] = t
                        }
                    }
                // Perform one qr step.
                case 3 =>
                    // Calculate the shift.
                    let scale: Float64 = max(max(max(max(abs(s[p - 1]), abs(s[p - 2])), abs(e[p - 2])), abs(s[k])),
                        abs(e[k]))
                    let sp: Float64 = s[p - 1] / scale
                    let spm1: Float64 = s[p - 2] / scale
                    let epm1: Float64 = e[p - 2] / scale
                    let sk: Float64 = s[k] / scale
                    let ek: Float64 = e[k] / scale
                    let b: Float64 = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2.0
                    let c: Float64 = (sp * epm1) * (sp * epm1)
                    var shift: Float64 = 0.0
                    if ((b != 0.0) || (c != 0.0)) {
                        shift = sqrt(b * b + c)
                        if (b < 0.0) {
                            shift = -shift
                        }
                        shift = c / (b + shift)
                    }
                    var f = (sk + sp) * (sk - sp) + shift
                    var g = sk * ek
                    for (j in k..(p - 1)) {
                        var t: Float64 = Maths.hypot(f, g)
                        var cs: Float64 = f / t
                        var sn: Float64 = g / t
                        if (j != k) {
                            e[j - 1] = t
                        }
                        f = cs * s[j] + sn * e[j]
                        e[j] = cs * e[j] - sn * s[j]
                        g = sn * s[j + 1]
                        s[j + 1] = cs * s[j + 1]
                        for (i in 0..n) {
                            t = cs * V[i][j] + sn * V[i][j + 1]
                            V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1]
                            V[i][j] = t
                        }
                        t = Maths.hypot(f, g)
                        cs = f / t
                        sn = g / t
                        s[j] = t
                        f = cs * e[j] + sn * s[j + 1]
                        s[j + 1] = -sn * e[j] + cs * s[j + 1]
                        g = sn * e[j + 1]
                        e[j + 1] = cs * e[j + 1]
                        if (j < m - 1) {
                            for (i in 0..m) {
                                t = cs * U[i][j] + sn * U[i][j + 1]
                                U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1]
                                U[i][j] = t
                            }
                        }
                    }
                    e[p - 2] = f
                    iter = iter + 1
                // Convergence.
                case 4 =>
                    // Make the singular values positive.
                    if (s[k] <= 0.0) {
                        s[k] = (if (s[k] < 0.0) {
                            -s[k]
                        } else {
                            0.0
                        })
                        for (i in 0..pp + 1) {
                            V[i][k] = -V[i][k]
                        }
                    }
                    // Order the singular values.
                    while (k < pp) {
                        if (s[k] >= s[k + 1]) {
                            break
                        }
                        var t: Float64 = s[k]
                        s[k] = s[k + 1]
                        s[k + 1] = t
                        if (k < n - 1) {
                            for (i in 0..n) {
                                t = V[i][k + 1]
                                V[i][k + 1] = V[i][k]
                                V[i][k] = t
                            }
                        }
                        if (k < m - 1) {
                            for (i in 0..m) {
                                t = U[i][k + 1]
                                U[i][k + 1] = U[i][k]
                                U[i][k] = t
                            }
                        }
                        k++
                    }
                    iter = 0
                    p--
                case _ => ()
            }
        }
    }

    /** 
     * Return the left singular vectors
     * @return     U
     */
    public func getU(): Matrix {
        return Matrix(U)
    }

    /** 
     * Return the right singular vectors
     * @return     V
     */
    public func getV(): Matrix {
        return Matrix(V)
    }

    /** 
     * Return the one-dimensional array of singular values
     * @return     diagonal of S.
     */    
    public func getSingularValues(): Array<Float64> {
        return s
    }

    /** 
     * Return the diagonal matrix of singular values
     * @return     S
     */
    public func getDiagonalValues(): Matrix {
        let X = Matrix(n, n)
        for (i in 0..n) {
            for (j in 0..n) {
                X[i, j] = 0.0
            }
            X[i, i] = this.s[i]
        }
        return X
    }

    /** 
     * Two norm
     * @return     max(S)
     */
    public func norm2(): Float64 {
        return s[0]
    }

    /** 
     * Two norm condition number
     * @return     max(S)/min(S)
     */
    public func cond(): Float64 {
        return s[0] / s[min(m, n) - 1]
    }

    /** 
     * Effective numerical matrix rank
     * @return     Number of nonnegligible singular values.
     */
    public func rank(): Int64 {
        let eps = pow(2.0, -52.0)
        let tol = Float64(max(m, n)) * s[0] * eps
        var r: Int64 = 0
        for (i in 0..s.size) {
            if (s[i] > tol) {
                r++
            }
        }
        return r
    }
}