import { m2_multiply } from "../../../geom/mat2/m2_multiply.js";
import { m2_polar_decomp } from "./m2_polar_decomp.js";

/**
 * Performs Singular Value Decomposition (SVD) on a 2x2 matrix, optimized for MLS-MPM.
 *
 * This function computes the SVD of a 2x2 matrix `m` such that m = U * Σ * V^T, where:
 *
 * - U is a 2x2 orthogonal matrix (rotation).
 * - Σ (sig) is a 2x2 diagonal matrix containing the singular values.
 * - V is a 2x2 orthogonal matrix (rotation).
 *
 * The function modifies U, sig, and V in-place. The input matrix `m` and the intermediate matrix `S` are overwritten during the computation.
 * The algorithm uses a closed-form solution and optimizations specific to 2x2 matrices, making it very fast. It also handles the case where the off-diagonal elements of the intermediate matrix S are close to zero, simplifying calculations.  The results are transposed, consistent with the Taichi programming language.
 * @param {number[]} U Output: 2x2 orthogonal matrix (rotation), stored as a flattened array [c, s, -s, c]. Modified in-place.
 * @param {number[]} S Output: Intermediate 2x2 matrix, overwritten during calculation. Modified in-place.
 * @param {number[]} V Output: 2x2 orthogonal matrix (rotation), stored as a flattened array. Modified in-place.
 * @param {number[]} sig Output: 2x2 diagonal matrix containing singular values, stored as a flattened array [σ1, 0, 0, σ2]. Modified in-place.
 * @param {number[]} m Input: 2x2 matrix, stored as a flattened array [a, b, c, d], representing [[a, c], [b, d]].  Will be overwritten.
 */
export function m2_svd(
    U,
    S,
    V,
    sig,
    m
) {
    // transposed as in taichi

    m2_polar_decomp(U, S, m);

    let c, s;

    if (Math.abs(S[1]) < 1e-6) {
        // If the off-diagonal element of S is close to zero,
        // S is already diagonal. No rotation needed for V.

        sig[0] = S[0];
        sig[1] = 0; // Ensure off-diagonal is exactly zero
        sig[2] = 0; // Ensure off-diagonal is exactly zero
        sig[3] = S[3];

        c = 1;
        s = 0;

    } else {
        const tao = 0.5 * (S[0] - S[3]);

        const w = Math.sqrt(tao * tao + S[1] * S[1]);

        // Numerical Methods for Engineers, Chapra, Canale, 6th Ed., p. 292
        const t = tao > 0 ? S[1] / (tao + w) : S[1] / (tao - w);

        c = 1.0 / Math.sqrt(t * t + 1);

        s = -t * c;

        const s2 = s * s;
        const c2 = c * c;

        const cs_2 = 2 * c * s;

        sig[0] = c2 * S[0] - cs_2 * S[1] + s2 * S[3];
        sig[1] = 0;
        sig[2] = 0;
        sig[3] = s2 * S[0] + cs_2 * S[1] + c2 * S[3];
    }

    if (sig[0] < sig[3]) {

        // Swap singular values if they are not in descending order.
        const tmp = sig[0];

        sig[0] = sig[3];
        sig[3] = tmp;

        // Correspondingly swap columns of V (remember, V is transposed here).

        V[0] = -s;
        V[1] = c;
        V[2] = -c;
        V[3] = -s;

    } else {

        // If already in descending order

        V[0] = c;
        V[1] = s;
        V[2] = -s;
        V[3] = c;

    }

    m2_multiply(U, U, V);
}