// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later

// decomposition.hpp
//
// Matrix decomposition algorithms
//
// This file implements various matrix decomposition algorithms.
// It includes LU decomposition, Cholesky decomposition, QR decomposition, SVD, and more.
//
// Main features and complexity (n×n matrix):
// - LU decomposition       — with partial pivoting, O(n^3)
// - Cholesky decomposition — symmetric positive definite matrices only, O(n^3/3) ≈ half of LU
// - QR decomposition       — Householder transformation, O(2n^3/3), used for least-squares problems
// - SVD                    — Golub-Reinsch method, O(mn^2 + n^3) (when m≥n)
// - LDL decomposition      — symmetric matrices (need not be positive definite), a variant of Cholesky
//
// Related:
// - eigenvalues.hpp — eigenvalue decomposition (SVD-based or QR iteration)
// - solvers.hpp     — linear solvers using LU/Cholesky/QR
//
// References:
//   Golub, Van Loan "Matrix Computations" 4th ed. Ch.2 (LU), Ch.4 (Cholesky/QR), Ch.6 (SVD)
//   Anderson et al. "LAPACK Users' Guide" 3rd ed.

#ifndef SANGI_DECOMPOSITION_HPP
#define SANGI_DECOMPOSITION_HPP

#include <math/core/common.hpp>
#include <math/core/matrix.hpp>
#include <math/core/vector.hpp>
#include <math/core/Complex.hpp>
#include <math/core/traits.hpp>
#include <math/concepts/algebraic_concepts.hpp>
#include <math/linalg/static_dims.hpp>
#include <cassert>

#include <vector>
#include <limits>
#ifdef SANGI_SVD_PHASE_TIMING
#include <chrono>
#include <cstdio>
#endif
#include <cmath>
#include <utility>
#include <algorithm>
#include <queue>
#include <stack>
#include <functional>
#include <unordered_map>

namespace sangi {
    namespace algorithms {

        /**
         * @brief Check whether a matrix is symmetric
         * @tparam T element type
         * @param a matrix to check
         * @param tolerance tolerance
         * @return true if the matrix is symmetric, false otherwise
         */
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        bool is_symmetric(const M& a,
                          decltype(numeric_traits<typename M::value_type>::epsilon()) tolerance
                              = numeric_traits<typename M::value_type>::epsilon() * 10) {
            using T = typename M::value_type;
            if (a.rows() != a.cols()) {
                return false;
            }

            for (std::size_t i = 0; i < a.rows(); ++i) {
                for (std::size_t j = i + 1; j < a.cols(); ++j) {
                    if constexpr (numeric_traits<T>::is_complex) {
                        // Complex: Hermitian check a(i,j) == conj(a(j,i))
                        if (std::abs(a(i, j) - sangi::conj(a(j, i))) > tolerance) {
                            return false;
                        }
                    } else {
                        if (std::abs(a(i, j) - a(j, i)) > tolerance) {
                            return false;
                        }
                    }
                }
            }

            return true;
        }

        /**
         * @brief Check whether a matrix is positive definite
         * @tparam T element type
         * @param a matrix to check
         * @return true if the matrix is positive definite (Hermitian positive definite for Complex)
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        bool is_positive_definite(const MA& a) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            if (!is_symmetric(a)) {
                return false;
            }

            // Check that all diagonal elements are positive (necessary condition)
            for (typename Matrix<T>::size_type i = 0; i < a.rows(); ++i) {
                if constexpr (numeric_traits<T>::is_complex) {
                    // Diagonal elements of a Hermitian matrix are real and should be positive
                    if (a(i, i).real() <= 0) {
                        return false;
                    }
                } else {
                    if (a(i, i) <= numeric_traits<T>::zero()) {
                        return false;
                    }
                }
            }

            // Check whether Cholesky decomposition is possible (sufficient condition)
            try {
                cholesky_decomposition(a);
                return true;
            }
            catch (const MathError&) {
                return false;
            }
        }

        // Pivot selection strategy
        enum class PivotStrategy {
            None,       // No pivot selection (use diagonal elements as-is)
            Partial,    // Partial pivot selection (largest absolute value within column, smallest height for Rational)
            Full        // Full pivot selection (select from the entire remaining submatrix)
        };

        // ── Internal helper for pivot selection ──
        // Uses numeric_traits<T>::pivotBetter to select with the optimal criterion for the type
        // (floating point: largest abs, Rational: smallest height)

        template<typename T>
        struct PivotSelector {
            // Partial pivot: select the best from rows k..n-1 of column k
            static typename Matrix<T>::size_type
            selectPartial(const Matrix<T>& lu, typename Matrix<T>::size_type k,
                          typename Matrix<T>::size_type n) {
                typename Matrix<T>::size_type best = k;
                for (typename Matrix<T>::size_type i = k + 1; i < n; ++i) {
                    if (lu(i, k) == numeric_traits<T>::zero()) continue;
                    if (lu(best, k) == numeric_traits<T>::zero() ||
                        numeric_traits<T>::pivotBetter(lu(i, k), lu(best, k)))
                        best = i;
                }
                return best;
            }

            // Full pivot: select the best from submatrix (k..n-1, k..n-1)
            static std::pair<typename Matrix<T>::size_type, typename Matrix<T>::size_type>
            selectFull(const Matrix<T>& lu, typename Matrix<T>::size_type k,
                       typename Matrix<T>::size_type n) {
                typename Matrix<T>::size_type bestR = k, bestC = k;
                for (typename Matrix<T>::size_type i = k; i < n; ++i) {
                    for (typename Matrix<T>::size_type j = k; j < n; ++j) {
                        if (lu(i, j) == numeric_traits<T>::zero()) continue;
                        if (lu(bestR, bestC) == numeric_traits<T>::zero() ||
                            numeric_traits<T>::pivotBetter(lu(i, j), lu(bestR, bestC))) {
                            bestR = i; bestC = j;
                        }
                    }
                }
                return { bestR, bestC };
            }
        };

        /**
         * @brief Compute the LU decomposition
         * @tparam T element type
         * @param a matrix to decompose
         * @param pivot pivot strategy (default: Partial)
         * @return a pair of the LU matrix and pivot information
         *         The LU matrix stores L (lower triangular part) and U (upper triangular part) in a single matrix
         *         The pivot information is the record of row swaps
         *         For full pivoting, column swaps are also recorded (n rows + n columns)
         */
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        std::pair<Matrix<typename M::value_type>, std::vector<std::size_t>>
            lu_decomposition(const M& a,
                             PivotStrategy pivot = PivotStrategy::Partial) {
            using T = typename M::value_type;
            SANGI_STATIC_ASSERT_SQUARE(M);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: lu_decomposition: matrix must be square");
                throw DimensionError("lu_decomposition: matrix must be square");
            }

            const auto n = a.rows();

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                Matrix<T> lu(n, n);
                for (std::size_t i = 0; i < n; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        lu(i, j) = a(i, j);
                std::vector<lapack_int> ipiv(n);
                lapack_int info;
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sgetrf(LAPACK_ROW_MAJOR, (lapack_int)n, (lapack_int)n,
                                          lu.data(), (lapack_int)n, ipiv.data());
                else
                    info = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, (lapack_int)n, (lapack_int)n,
                                          lu.data(), (lapack_int)n, ipiv.data());
                if (info > 0)
                    throw MathError("lu_decomposition: singular matrix detected");
                // Convert LAPACKE ipiv from 1-based → 0-based
                std::vector<typename Matrix<T>::size_type> pivots(n);
                for (std::size_t i = 0; i < n; ++i)
                    pivots[i] = static_cast<typename Matrix<T>::size_type>(ipiv[i] - 1);
                return { lu, pivots };
            }
#endif

            Matrix<T> lu(n, n);
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    lu(i, j) = a(i, j);
            std::vector<std::size_t> pivots(n);

            // Block LU decomposition (LAPACK DGETRF style) — for float/double with n >= 64
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                constexpr std::size_t NB = 64;
                if (n >= NB) {
                    T* __restrict data = lu.data();
                    // L21 sign-flipped buffer (reused across the whole loop)
                    Matrix<T> L21_buf(n, NB);

                    for (std::size_t jb = 0; jb < n; jb += NB) {
                        const std::size_t jend = (std::min)(jb + NB, n);
                        const std::size_t nb = jend - jb;

                        // Phase 1: panel factorization (columns jb..jend-1)
                        for (std::size_t j = jb; j < jend; ++j) {
                            // Partial pivot selection
                            std::size_t pivot_row = j;
                            T pivot_val = std::abs(data[j * n + j]);
                            for (std::size_t i = j + 1; i < n; ++i) {
                                T av = std::abs(data[i * n + j]);
                                if (av > pivot_val) { pivot_val = av; pivot_row = i; }
                            }
                            pivots[j] = pivot_row;

                            // Row swap (all rows)
                            if (pivot_row != j) {
                                T* row_j = data + j * n;
                                T* row_p = data + pivot_row * n;
                                for (std::size_t c = 0; c < n; ++c)
                                    std::swap(row_j[c], row_p[c]);
                            }

                            // Singularity check
                            if (std::abs(data[j * n + j]) <= std::numeric_limits<T>::epsilon())
                                throw MathError("lu_decomposition: singular matrix detected");

                            // L column scaling
                            T inv_diag = T(1) / data[j * n + j];
                            for (std::size_t i = j + 1; i < n; ++i)
                                data[i * n + j] *= inv_diag;

                            // In-panel rank-1 update (columns j+1..jend-1 only, SIMD axpy)
                            {
                                const std::size_t panel_rem = jend - j - 1;
                                if (panel_rem > 0) {
                                    for (std::size_t i = j + 1; i < n; ++i) {
                                        T neg_lij = -data[i * n + j];
                                        computation::simd::axpy_simd<T>(
                                            &data[i * n + j + 1], neg_lij,
                                            &data[j * n + j + 1], panel_rem);
                                    }
                                }
                            }
                        }

                        if (jend >= n) break;

                        const std::size_t remaining = n - jend;

                        // Phase 2: TRSM — solve L11 * U12 = A12 by forward substitution
                        // L11 is the unit lower triangular part of lu(jb..jend-1, jb..jend-1)
                        // A12/U12 is lu(jb..jend-1, jend..n-1) — overwritten in-place
                        for (std::size_t r = 1; r < nb; ++r) {
                            for (std::size_t t = 0; t < r; ++t) {
                                T neg_lrt = -data[(jb + r) * n + (jb + t)];
                                computation::simd::axpy_simd<T>(
                                    &data[(jb + r) * n + jend],
                                    neg_lrt,
                                    &data[(jb + t) * n + jend],
                                    remaining);
                            }
                        }

                        // Phase 3: trailing matrix update A22 -= L21 * U12
                        // Sign-flip L21 and copy into the buffer; U12 is referenced directly
                        {
                            L21_buf.resize(remaining, nb);
                            for (std::size_t i = 0; i < remaining; ++i) {
                                const T* __restrict src = &data[(jend + i) * n + jb];
                                T* __restrict dst = &L21_buf(i, 0);
                                for (std::size_t j = 0; j < nb; ++j)
                                    dst[j] = -src[j];
                            }
                            // gemm: A22 += (-L21) * U12 — stride-based direct accumulation
                            DefaultComputePolicy<T>::gemm(
                                L21_buf.data(), &data[jb * n + jend],
                                &data[jend * n + jend],
                                remaining, remaining, nb, nb, n, n);
                        }
                    }
                    return { lu, pivots };
                }
            }

            // Scalar LU decomposition (small matrices or non-floating-point types)
            // For full pivoting, column swaps are also recorded: pivots[0..n-1] = rows, pivots[n..2n-1] = columns
            std::vector<typename Matrix<T>::size_type> col_pivots;
            if (pivot == PivotStrategy::Full) {
                col_pivots.resize(n);
                for (typename Matrix<T>::size_type i = 0; i < n; ++i) col_pivots[i] = i;
            }

            for (typename Matrix<T>::size_type k = 0; k < n; ++k) {
                // Pivot selection
                typename Matrix<T>::size_type pivot_row = k;
                typename Matrix<T>::size_type pivot_col = k;

                if (pivot == PivotStrategy::Partial) {
                    pivot_row = PivotSelector<T>::selectPartial(lu, k, n);
                } else if (pivot == PivotStrategy::Full) {
                    auto [pr, pc] = PivotSelector<T>::selectFull(lu, k, n);
                    pivot_row = pr;
                    pivot_col = pc;
                }
                // PivotStrategy::None: pivot_row = k, pivot_col = k (as-is)

                pivots[k] = pivot_row;

                // Row swap
                if (pivot_row != k) {
                    for (typename Matrix<T>::size_type j = 0; j < n; ++j)
                        std::swap(lu(k, j), lu(pivot_row, j));
                }

                // Column swap (full pivoting only)
                if (pivot == PivotStrategy::Full && pivot_col != k) {
                    for (typename Matrix<T>::size_type i = 0; i < n; ++i)
                        std::swap(lu(i, k), lu(i, pivot_col));
                    std::swap(col_pivots[k], col_pivots[pivot_col]);
                }

                if (numeric_traits<T>::abs(lu(k, k)) <= numeric_traits<T>::epsilon()) {
                    throw MathError("lu_decomposition: singular matrix detected");
                }

                for (typename Matrix<T>::size_type i = k + 1; i < n; ++i) {
                    const T factor = lu(i, k) / lu(k, k);
                    lu(i, k) = factor;

                    for (typename Matrix<T>::size_type j = k + 1; j < n; ++j) {
                        lu(i, j) -= factor * lu(k, j);
                    }
                }
            }

            // Full pivoting: append column swap information to the second half of pivots
            if (pivot == PivotStrategy::Full) {
                pivots.resize(2 * n);
                for (typename Matrix<T>::size_type i = 0; i < n; ++i)
                    pivots[n + i] = col_pivots[i];
            }

            return { lu, pivots };
        }

        /**
         * @brief Solve a system of linear equations using LU decomposition
         * @tparam T element type
         * @param a coefficient matrix
         * @param b right-hand-side vector
         * @param pivot pivot strategy (default: Partial)
         * @return solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> lu_solve(const MA& a, const VB& b,
                           PivotStrategy pivot = PivotStrategy::Partial) {
            using T = sangi::element_t<MA>;
            // Compile-time dimension checks fire only when both operands
            // carry static dimensions (StaticMatrix / StaticVector).
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: lu_solve: matrix must be square");
                throw DimensionError("lu_solve: matrix must be square");
            }

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: lu_solve: matrix and vector dimensions mismatch");
                throw DimensionError("lu_solve: matrix and vector dimensions mismatch");
            }

            const auto n = a.rows();

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                Matrix<T> Acopy = a;
                Vector<T> x = b;
                std::vector<lapack_int> ipiv(n);
                lapack_int info;
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sgesv(LAPACK_ROW_MAJOR, (lapack_int)n, 1,
                                         Acopy.data(), (lapack_int)n, ipiv.data(),
                                         x.data(), 1);
                else
                    info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, (lapack_int)n, 1,
                                         Acopy.data(), (lapack_int)n, ipiv.data(),
                                         x.data(), 1);
                if (info > 0)
                    throw MathError("lu_solve: singular matrix detected");
                return x;
            }
#endif

            // LU decomposition
            auto [lu, pivots] = lu_decomposition(a, pivot);

            const bool fullPivot = (pivot == PivotStrategy::Full);

            // Prepare the right-hand-side vector
            Vector<T> x = b;

            // Row pivot permutation
            for (std::size_t i = 0; i < n; ++i) {
                if (pivots[i] != i) {
                    std::swap(x[i], x[pivots[i]]);
                }
            }

            // float/double: forward/backward substitution with raw pointers
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                const T* __restrict ld = lu.data();
                T* __restrict xd = x.data();
                // Forward substitution (Ly = b)
                for (std::size_t i = 1; i < n; ++i) {
                    const T* __restrict row = ld + i * n;
                    T sum = xd[i];
                    for (std::size_t j = 0; j < i; ++j)
                        sum -= row[j] * xd[j];
                    xd[i] = sum;
                }
                // Backward substitution (Ux = y)
                for (std::size_t ii = 0; ii < n; ++ii) {
                    std::size_t i = n - 1 - ii;
                    const T* __restrict row = ld + i * n;
                    T sum = xd[i];
                    for (std::size_t j = i + 1; j < n; ++j)
                        sum -= row[j] * xd[j];
                    xd[i] = sum / row[i];
                }
                // Full pivoting: inverse permutation of column swaps
                if (fullPivot) {
                    Vector<T> tmp = x;
                    for (std::size_t i = 0; i < n; ++i)
                        x[pivots[n + i]] = tmp[i];
                }
                return x;
            }

            // Generic type: forward substitution (Ly = b)
            for (std::size_t i = 1; i < n; ++i) {
                for (std::size_t j = 0; j < i; ++j) {
                    x[i] -= lu(i, j) * x[j];
                }
            }

            // Backward substitution (Ux = y)
            for (std::size_t i = n; i-- > 0; ) {
                for (std::size_t j = i + 1; j < n; ++j) {
                    x[i] -= lu(i, j) * x[j];
                }
                x[i] /= lu(i, i);
            }

            // Full pivoting: inverse permutation of column swaps
            if (fullPivot) {
                Vector<T> tmp = x;
                for (std::size_t i = 0; i < n; ++i)
                    x[pivots[n + i]] = tmp[i];
            }

            return x;
        }

        /**
         * @brief Solve a system of linear equations by Gaussian elimination (augmented-matrix based)
         *
         * Without going through LU decomposition, directly forward-eliminate + back-substitute the augmented matrix [A|b].
         * For Rational, the smallest-height pivot suppresses coefficient growth.
         *
         * @tparam T element type (double, Float, Rational, etc.)
         * @param a coefficient matrix (n×n)
         * @param b right-hand-side vector (n)
         * @param pivot pivot strategy (default: Partial)
         * @return solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> gaussian_elimination(const MA& a, const VB& b,
                                       PivotStrategy pivot = PivotStrategy::Partial) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: gaussian_elimination: matrix must be square");
                throw DimensionError("gaussian_elimination: matrix must be square");
            }
            if (a.rows() != b.size()) {
                assert(false && "DimensionError: gaussian_elimination: matrix and vector dimensions mismatch");
                throw DimensionError("gaussian_elimination: matrix and vector dimensions mismatch");
            }

            const auto n = a.rows();

            // Build the augmented matrix [A|b] (n × n+1)
            Matrix<T> aug(n, n + 1);
            for (std::size_t i = 0; i < n; ++i) {
                for (std::size_t j = 0; j < n; ++j)
                    aug(i, j) = a(i, j);
                aug(i, n) = b[i];
            }

            // Track column order for full pivoting
            std::vector<std::size_t> colOrder(n);
            for (std::size_t i = 0; i < n; ++i) colOrder[i] = i;

            // Forward elimination
            for (std::size_t k = 0; k < n; ++k) {
                // Pivot selection
                std::size_t pivotRow = k, pivotCol = k;

                if (pivot == PivotStrategy::Partial) {
                    // Partial pivot: select from rows k..n-1 of column k
#ifdef SANGI_RATIONAL_HPP
                    if constexpr (std::is_same_v<T, Rational>) {
                        std::size_t best = n;
                        Int bestH;
                        for (std::size_t i = k; i < n; ++i) {
                            if (aug(i, k).isZero()) continue;
                            Int h = height(aug(i, k));
                            if (best == n || h < bestH) { bestH = h; best = i; }
                        }
                        if (best != n) pivotRow = best;
                    } else
#endif
                    {
                        T bestVal = numeric_traits<T>::abs(aug(k, k));
                        for (std::size_t i = k + 1; i < n; ++i) {
                            T v = numeric_traits<T>::abs(aug(i, k));
                            if (v > bestVal) { bestVal = v; pivotRow = i; }
                        }
                    }
                } else if (pivot == PivotStrategy::Full) {
                    // Full pivot: select from submatrix (k..n-1, k..n-1)
#ifdef SANGI_RATIONAL_HPP
                    if constexpr (std::is_same_v<T, Rational>) {
                        std::size_t bestR = n, bestC = k;
                        Int bestH;
                        for (std::size_t i = k; i < n; ++i) {
                            for (std::size_t j = k; j < n; ++j) {
                                if (aug(i, j).isZero()) continue;
                                Int h = height(aug(i, j));
                                if (bestR == n || h < bestH) { bestH = h; bestR = i; bestC = j; }
                            }
                        }
                        if (bestR != n) { pivotRow = bestR; pivotCol = bestC; }
                    } else
#endif
                    {
                        T bestVal = numeric_traits<T>::abs(aug(k, k));
                        for (std::size_t i = k; i < n; ++i) {
                            for (std::size_t j = k; j < n; ++j) {
                                T v = numeric_traits<T>::abs(aug(i, j));
                                if (v > bestVal) { bestVal = v; pivotRow = i; pivotCol = j; }
                            }
                        }
                    }
                }

                // Row swap
                if (pivotRow != k) {
                    for (std::size_t j = 0; j <= n; ++j)
                        std::swap(aug(k, j), aug(pivotRow, j));
                }

                // Column swap (full pivoting only; the b column is not swapped)
                if (pivot == PivotStrategy::Full && pivotCol != k) {
                    for (std::size_t i = 0; i < n; ++i)
                        std::swap(aug(i, k), aug(i, pivotCol));
                    std::swap(colOrder[k], colOrder[pivotCol]);
                }

                // Singularity check
                if (numeric_traits<T>::abs(aug(k, k)) <= numeric_traits<T>::epsilon())
                    throw MathError("gaussian_elimination: singular matrix detected");

                // Elimination
                for (std::size_t i = k + 1; i < n; ++i) {
                    T factor = aug(i, k) / aug(k, k);
                    for (std::size_t j = k + 1; j <= n; ++j)
                        aug(i, j) -= factor * aug(k, j);
                    aug(i, k) = T(0);
                }
            }

            // Backward substitution
            Vector<T> y(n);
            for (std::size_t i = n; i-- > 0; ) {
                y[i] = aug(i, n);
                for (std::size_t j = i + 1; j < n; ++j)
                    y[i] -= aug(i, j) * y[j];
                y[i] /= aug(i, i);
            }

            // Full pivoting: inverse permutation of column swaps
            if (pivot == PivotStrategy::Full) {
                Vector<T> x(n);
                for (std::size_t i = 0; i < n; ++i)
                    x[colOrder[i]] = y[i];
                return x;
            }
            return y;
        }

        /**
         * @brief Compute the inverse matrix using LU decomposition
         * @tparam T element type
         * @param a original matrix
         * @param pivot pivot strategy (default: Partial)
         * @return inverse matrix
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        Matrix<sangi::element_t<MA>> lu_inverse(const MA& a,
                             PivotStrategy pivot = PivotStrategy::Partial) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: lu_inverse: matrix must be square");
                throw DimensionError("lu_inverse: matrix must be square");
            }

            const auto n = a.rows();

            // Solve the system for each column vector of the identity matrix
            Matrix<T> inverse(n, n);
            Vector<T> e(n, T(0));
            Vector<T> x(n);

            for (typename Matrix<T>::size_type j = 0; j < n; ++j) {
                // Set the column vector of the identity matrix
                for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                    e[i] = (i == j) ? T(1) : T(0);
                }

                // Solve the system
                x = lu_solve(a, e, pivot);

                // Store the result into the inverse matrix
                for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                    inverse(i, j) = x[i];
                }
            }

            return inverse;
        }

        /**
         * @brief Compute the determinant using LU decomposition
         * @tparam T element type
         * @param a original matrix
         * @param pivot pivot strategy (default: Partial)
         * @return value of the determinant
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        sangi::element_t<MA> lu_determinant(const MA& a,
                         PivotStrategy pivot = PivotStrategy::Partial) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: lu_determinant: matrix must be square");
                throw DimensionError("lu_determinant: matrix must be square");
            }

            const auto n = a.rows();

            // LU decomposition
            auto [lu, pivots] = lu_decomposition(a, pivot);

            // Compute the determinant
            T det = T(1);
            int sign = 1;

            // Compute the sign change due to pivot swaps
            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                if (pivots[i] != i) {
                    sign = -sign;
                }
            }

            // Product of the diagonal elements
            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                det *= lu(i, i);
            }

            return det * T(sign);
        }

        /**
         * @brief Compute the determinant using the Bareiss algorithm (for integer types, division-free)
         *
         * Using Sylvester's identity, which guarantees that the divisions in the elimination always divide evenly,
         * computes the determinant of an integer matrix in O(n³) keeping intermediate results integral.
         * It can also be used for floating-point types, but lu_determinant is more efficient.
         *
         * @tparam T element type (Int, int, long long, etc.)
         * @param a square matrix
         * @return determinant det(A)
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        sangi::element_t<MA> bareiss_determinant(const MA& a) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: bareiss_determinant: matrix must be square");
                throw DimensionError("bareiss_determinant: matrix must be square");
            }

            const auto n = a.rows();
            if (n == 0) return T(1);
            if (n == 1) return a(0, 0);
            if (n == 2) return a(0, 0) * a(1, 1) - a(0, 1) * a(1, 0);

            Matrix<T> M = a;
            int sign = 1;
            T prev_pivot = T(1);

            for (typename Matrix<T>::size_type k = 0; k < n; ++k) {
                // Partial pivot selection
                typename Matrix<T>::size_type pivot_row = k;
                for (typename Matrix<T>::size_type i = k + 1; i < n; ++i) {
                    if (M(i, k) != T(0) &&
                        (M(pivot_row, k) == T(0) ||
                         numeric_traits<T>::pivotBetter(M(i, k), M(pivot_row, k))))
                        pivot_row = i;
                }

                if (pivot_row != k) {
                    for (typename Matrix<T>::size_type j = 0; j < n; ++j)
                        std::swap(M(k, j), M(pivot_row, j));
                    sign = -sign;
                }

                if (M(k, k) == T(0))
                    return T(0);

                for (typename Matrix<T>::size_type i = k + 1; i < n; ++i) {
                    for (typename Matrix<T>::size_type j = k + 1; j < n; ++j) {
                        M(i, j) = (M(i, j) * M(k, k) - M(i, k) * M(k, j)) / prev_pivot;
                    }
                }

                prev_pivot = M(k, k);
            }

            return M(n - 1, n - 1) * T(sign);
        }

        /**
         * @brief Compute the Cholesky decomposition
         * @tparam T element type
         * @param a matrix to decompose (symmetric positive definite matrix)
         * @return the L matrix of the Cholesky decomposition (lower triangular matrix)
         */
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        Matrix<typename M::value_type> cholesky_decomposition(const M& a) {
            using T = typename M::value_type;
            SANGI_STATIC_ASSERT_SQUARE(M);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: cholesky_decomposition: matrix must be square");
                throw DimensionError("cholesky_decomposition: matrix must be square");
            }

            const auto n = a.rows();

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                Matrix<T> L(n, n);
                for (std::size_t i = 0; i < n; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        L(i, j) = a(i, j);
                lapack_int info;
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_spotrf(LAPACK_ROW_MAJOR, 'L', (lapack_int)n,
                                          L.data(), (lapack_int)n);
                else
                    info = LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'L', (lapack_int)n,
                                          L.data(), (lapack_int)n);
                if (info > 0)
                    throw MathError("cholesky_decomposition: matrix is not positive definite");
                // Zero out the upper triangular part (LAPACKE writes only the lower triangle, leaving the upper as-is)
                for (std::size_t i = 0; i < n; ++i)
                    for (std::size_t j = i + 1; j < n; ++j)
                        L(i, j) = T(0);
                return L;
            }
#endif

            // float/double block Cholesky — no is_symmetric check needed
            // (non-PD is detected via sqrt; symmetry is the caller's responsibility)
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                const std::size_t NB = (n < 256) ? 32 : 64;
                if (n >= 32) {
                    // Copy the whole of A (the upper triangle is not read)
                    Matrix<T> L(n, n);
                    for (std::size_t i = 0; i < n; ++i)
                        for (std::size_t j = 0; j < n; ++j)
                            L(i, j) = a(i, j);

                    T* __restrict data = L.data();
                    // Pre-allocate buffers (reused across the whole loop)
                    // trsm_cols: column buffer for TRSM (columns are contiguous)
                    std::vector<T> trsm_cols(n * NB);
                    // L_CB_data: row-major buffer for SYRK (rows are contiguous, lda=nb)
                    std::vector<T> L_CB_data(n * NB);

                    for (std::size_t jb = 0; jb < n; jb += NB) {
                        const std::size_t jend = (std::min)(jb + NB, n);
                        const std::size_t nb = jend - jb;

                        // Phase 1: diagonal-block Cholesky (nb×nb)
                        using PT = PacketTraits<T>;
                        constexpr std::size_t PS = PT::size;
                        for (std::size_t j = jb; j < jend; ++j) {
                            // Diagonal element: L(j,j)² = A(j,j) - Σ_k L(j,k)²
                            T sum = data[j * n + j];
                            for (std::size_t k = jb; k < j; ++k)
                                sum -= data[j * n + k] * data[j * n + k];
                            if (sum <= T(0))
                                throw MathError("cholesky_decomposition: matrix is not positive definite");
                            data[j * n + j] = std::sqrt(sum);
                            T inv_diag = T(1) / data[j * n + j];

                            // Off-diagonal elements of column j: L(i,j) = (A(i,j) - Σ_k L(i,k)*L(j,k)) / L(j,j)
                            const std::size_t col_len = jend - j - 1;
                            if (col_len == 0) continue;

                            // Vectorize the dot product over the k loop with SIMD
                            // For each row i = j+1..jend-1, sum[i] -= L(i,k)*L(j,k)
                            // L(j,k) = data[j*n+k], L(i,k) = data[i*n+k]
                            for (std::size_t k = jb; k < j; ++k) {
                                T ljk = data[j * n + k];
                                for (std::size_t i = j + 1; i < jend; ++i)
                                    data[i * n + j] -= data[i * n + k] * ljk;
                            }
                            for (std::size_t i = j + 1; i < jend; ++i)
                                data[i * n + j] *= inv_diag;
                        }

                        if (jend >= n) break;

                        const std::size_t remaining = n - jend;
                        // Phase 2: TRSM — apply axpy_simd via the column buffer
                        {
                            T* tcols = trsm_cols.data();
                            for (std::size_t j = 0; j < nb; ++j)
                                for (std::size_t i = 0; i < remaining; ++i)
                                    tcols[j * remaining + i] = data[(jend + i) * n + (jb + j)];

                            for (std::size_t j = 0; j < nb; ++j) {
                                T* col_j = tcols + j * remaining;
                                for (std::size_t k = 0; k < j; ++k) {
                                    T neg_ljk = -data[(jb + j) * n + (jb + k)];
                                    computation::simd::axpy_simd<T>(
                                        col_j, neg_ljk, tcols + k * remaining, remaining);
                                }
                                T inv_diag = T(1) / data[(jb + j) * n + (jb + j)];
                                for (std::size_t i = 0; i < remaining; ++i)
                                    col_j[i] *= inv_diag;
                            }

                            // Write the TRSM result back to L while making a row-major copy for SYRK
                            for (std::size_t i = 0; i < remaining; ++i) {
                                for (std::size_t j = 0; j < nb; ++j) {
                                    T val = tcols[j * remaining + i];
                                    data[(jend + i) * n + (jb + j)] = val;
                                    L_CB_data[i * nb + j] = val;
                                }
                            }
                        }

                        // Phase 3: SYRK — data[jend..n, jend..n] -= L_CB * L_CB^T
                        // Compute only the lower triangle directly (no transpose buffer, halving FLOPs)
                        {
                            const std::size_t m = remaining;
                            const std::size_t kk = nb;
                            const T* __restrict A = L_CB_data.data();
                            const std::size_t TB = 64;  // The SYRK tile is always 64 (independent of NB)

                            for (std::size_t ib = 0; ib < m; ib += TB) {
                                const std::size_t i_end = (std::min)(ib + TB, m);

                                // Off-diagonal tile (jb2 < ib): 2×4 microkernel
                                for (std::size_t jb2 = 0; jb2 < ib; jb2 += TB) {
                                    const std::size_t j_end = (std::min)(jb2 + TB, m);
                                    // Process two rows at a time: share the load in the j direction
                                    std::size_t i = ib;
                                    for (; i + 2 <= i_end; i += 2) {
                                        const T* __restrict ai0 = A + (i+0) * kk;
                                        const T* __restrict ai1 = A + (i+1) * kk;
                                        T* __restrict ci0 = data + (jend + i+0) * n + jend;
                                        T* __restrict ci1 = data + (jend + i+1) * n + jend;
                                        std::size_t j = jb2;
                                        for (; j + 4 <= j_end; j += 4) {
                                            const T* __restrict aj0 = A + (j+0)*kk;
                                            const T* __restrict aj1 = A + (j+1)*kk;
                                            const T* __restrict aj2 = A + (j+2)*kk;
                                            const T* __restrict aj3 = A + (j+3)*kk;
                                            auto c00 = PT::mul(PT::load(ai0), PT::load(aj0));
                                            auto c01 = PT::mul(PT::load(ai0), PT::load(aj1));
                                            auto c02 = PT::mul(PT::load(ai0), PT::load(aj2));
                                            auto c03 = PT::mul(PT::load(ai0), PT::load(aj3));
                                            auto c10 = PT::mul(PT::load(ai1), PT::load(aj0));
                                            auto c11 = PT::mul(PT::load(ai1), PT::load(aj1));
                                            auto c12 = PT::mul(PT::load(ai1), PT::load(aj2));
                                            auto c13 = PT::mul(PT::load(ai1), PT::load(aj3));
                                            for (std::size_t p = PS; p < kk; p += PS) {
                                                auto va0 = PT::load(ai0 + p);
                                                auto va1 = PT::load(ai1 + p);
                                                auto vb0 = PT::load(aj0 + p);
                                                auto vb1 = PT::load(aj1 + p);
                                                auto vb2 = PT::load(aj2 + p);
                                                auto vb3 = PT::load(aj3 + p);
                                                c00 = PT::fmadd(va0, vb0, c00); c01 = PT::fmadd(va0, vb1, c01);
                                                c02 = PT::fmadd(va0, vb2, c02); c03 = PT::fmadd(va0, vb3, c03);
                                                c10 = PT::fmadd(va1, vb0, c10); c11 = PT::fmadd(va1, vb1, c11);
                                                c12 = PT::fmadd(va1, vb2, c12); c13 = PT::fmadd(va1, vb3, c13);
                                            }
                                            ci0[j+0] -= PT::reduce_add(c00); ci0[j+1] -= PT::reduce_add(c01);
                                            ci0[j+2] -= PT::reduce_add(c02); ci0[j+3] -= PT::reduce_add(c03);
                                            ci1[j+0] -= PT::reduce_add(c10); ci1[j+1] -= PT::reduce_add(c11);
                                            ci1[j+2] -= PT::reduce_add(c12); ci1[j+3] -= PT::reduce_add(c13);
                                        }
                                        for (; j < j_end; ++j) {
                                            auto s0 = PT::mul(PT::load(ai0), PT::load(A + j*kk));
                                            auto s1 = PT::mul(PT::load(ai1), PT::load(A + j*kk));
                                            for (std::size_t p = PS; p < kk; p += PS) {
                                                auto vb = PT::load(A + j*kk + p);
                                                s0 = PT::fmadd(PT::load(ai0 + p), vb, s0);
                                                s1 = PT::fmadd(PT::load(ai1 + p), vb, s1);
                                            }
                                            ci0[j] -= PT::reduce_add(s0);
                                            ci1[j] -= PT::reduce_add(s1);
                                        }
                                    }
                                    // Remainder rows
                                    for (; i < i_end; ++i) {
                                        const T* __restrict ai = A + i * kk;
                                        T* __restrict ci = data + (jend + i) * n + jend;
                                        std::size_t j = jb2;
                                        for (; j + 4 <= j_end; j += 4) {
                                            auto s0 = PT::mul(PT::load(ai), PT::load(A + (j+0)*kk));
                                            auto s1 = PT::mul(PT::load(ai), PT::load(A + (j+1)*kk));
                                            auto s2 = PT::mul(PT::load(ai), PT::load(A + (j+2)*kk));
                                            auto s3 = PT::mul(PT::load(ai), PT::load(A + (j+3)*kk));
                                            for (std::size_t p = PS; p < kk; p += PS) {
                                                auto va = PT::load(ai + p);
                                                s0 = PT::fmadd(va, PT::load(A + (j+0)*kk + p), s0);
                                                s1 = PT::fmadd(va, PT::load(A + (j+1)*kk + p), s1);
                                                s2 = PT::fmadd(va, PT::load(A + (j+2)*kk + p), s2);
                                                s3 = PT::fmadd(va, PT::load(A + (j+3)*kk + p), s3);
                                            }
                                            ci[j+0] -= PT::reduce_add(s0); ci[j+1] -= PT::reduce_add(s1);
                                            ci[j+2] -= PT::reduce_add(s2); ci[j+3] -= PT::reduce_add(s3);
                                        }
                                        for (; j < j_end; ++j) {
                                            auto s = PT::mul(PT::load(ai), PT::load(A + j*kk));
                                            for (std::size_t p = PS; p < kk; p += PS)
                                                s = PT::fmadd(PT::load(ai + p), PT::load(A + j*kk + p), s);
                                            ci[j] -= PT::reduce_add(s);
                                        }
                                    }
                                }

                                // Diagonal tile: lower triangle only
                                for (std::size_t i = ib; i < i_end; ++i) {
                                    const T* __restrict ai = A + i * kk;
                                    T* __restrict ci = data + (jend + i) * n + jend;
                                    std::size_t j = ib;
                                    for (; j + 4 <= i + 1; j += 4) {
                                        auto s0 = PT::mul(PT::load(ai), PT::load(A + (j+0)*kk));
                                        auto s1 = PT::mul(PT::load(ai), PT::load(A + (j+1)*kk));
                                        auto s2 = PT::mul(PT::load(ai), PT::load(A + (j+2)*kk));
                                        auto s3 = PT::mul(PT::load(ai), PT::load(A + (j+3)*kk));
                                        for (std::size_t p = PS; p < kk; p += PS) {
                                            auto va = PT::load(ai + p);
                                            s0 = PT::fmadd(va, PT::load(A + (j+0)*kk + p), s0);
                                            s1 = PT::fmadd(va, PT::load(A + (j+1)*kk + p), s1);
                                            s2 = PT::fmadd(va, PT::load(A + (j+2)*kk + p), s2);
                                            s3 = PT::fmadd(va, PT::load(A + (j+3)*kk + p), s3);
                                        }
                                        ci[j+0] -= PT::reduce_add(s0);
                                        ci[j+1] -= PT::reduce_add(s1);
                                        ci[j+2] -= PT::reduce_add(s2);
                                        ci[j+3] -= PT::reduce_add(s3);
                                    }
                                    for (; j <= i; ++j) {
                                        auto s = PT::mul(PT::load(ai), PT::load(A + j*kk));
                                        for (std::size_t p = PS; p < kk; p += PS)
                                            s = PT::fmadd(PT::load(ai + p), PT::load(A + j*kk + p), s);
                                        ci[j] -= PT::reduce_add(s);
                                    }
                                }
                            }
                        }
                    }

                    return L;
                }

                // float/double small matrix (n < 32): scalar Cholesky (in-place)
                {
                    Matrix<T> L(n, n);
                    for (std::size_t i = 0; i < n; ++i)
                        for (std::size_t j = 0; j < n; ++j)
                            L(i, j) = a(i, j);
                    T* __restrict ld = L.data();
                    for (std::size_t i = 0; i < n; ++i) {
                        T sum = ld[i * n + i];
                        for (std::size_t k = 0; k < i; ++k)
                            sum -= ld[i * n + k] * ld[i * n + k];
                        if (sum <= T(0))
                            throw MathError("cholesky_decomposition: matrix is not positive definite");
                        ld[i * n + i] = std::sqrt(sum);
                        T inv_diag = T(1) / ld[i * n + i];
                        for (std::size_t j = i + 1; j < n; ++j) {
                            T s = ld[j * n + i];
                            for (std::size_t k = 0; k < i; ++k)
                                s -= ld[j * n + k] * ld[i * n + k];
                            ld[j * n + i] = s * inv_diag;
                        }
                    }
                    return L;
                }
            }

            // Generic type: symmetry/Hermitian check + scalar Cholesky
            {
                const auto eps10 = numeric_traits<T>::epsilon() * 10;
                for (std::size_t i = 0; i < n; ++i) {
                    for (std::size_t j = i + 1; j < n; ++j) {
                        if constexpr (numeric_traits<T>::is_complex) {
                            // Hermitian: a(i,j) == conj(a(j,i))
                            if (std::abs(a(i, j) - sangi::conj(a(j, i))) > eps10)
                                throw MathError("cholesky_decomposition: matrix must be Hermitian");
                        } else {
                            if (std::abs(a(i, j) - a(j, i)) > eps10)
                                throw MathError("cholesky_decomposition: matrix must be symmetric");
                        }
                    }
                }
            }

            const T zero = numeric_traits<T>::zero();
            const T one  = numeric_traits<T>::one();

            Matrix<T> L(n, n, zero);
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j <= i; ++j)
                    L(i, j) = a(i, j);

            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                T sum = L(i, i);
                for (typename Matrix<T>::size_type k = 0; k < i; ++k) {
                    if constexpr (numeric_traits<T>::is_complex)
                        sum -= sangi::conj(L(i, k)) * L(i, k);
                    else
                        sum -= L(i, k) * L(i, k);
                }
                if constexpr (numeric_traits<T>::is_complex) {
                    // For Hermitian positive definite, the sum of the diagonal element is real positive
                    auto re = sum.real();
                    if (re <= 0)
                        throw MathError("cholesky_decomposition: matrix is not positive definite");
                    L(i, i) = T(std::sqrt(re));
                } else {
                    if (sum <= zero)
                        throw MathError("cholesky_decomposition: matrix is not positive definite");
                    L(i, i) = std::sqrt(sum);
                }
                T inv_diag = one / L(i, i);
                for (typename Matrix<T>::size_type j = i + 1; j < n; ++j) {
                    sum = L(j, i);
                    for (typename Matrix<T>::size_type k = 0; k < i; ++k) {
                        if constexpr (numeric_traits<T>::is_complex)
                            sum -= L(j, k) * sangi::conj(L(i, k));
                        else
                            sum -= L(j, k) * L(i, k);
                    }
                    L(j, i) = sum * inv_diag;
                }
            }
            return L;
        }

        /**
         * @brief Solve a system of linear equations using the Cholesky decomposition
         * @tparam T element type
         * @param a coefficient matrix (symmetric positive definite matrix)
         * @param b right-hand-side vector
         * @return solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> cholesky_solve(const MA& a, const VB& b_basevec) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            Vector<T> b(b_basevec);  // 1 copy via BaseVector ctor (contiguous buffer for the SIMD path)
            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: cholesky_solve: matrix must be square");
                throw DimensionError("cholesky_solve: matrix must be square");
            }

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: cholesky_solve: matrix and vector dimensions mismatch");
                throw DimensionError("cholesky_solve: matrix and vector dimensions mismatch");
            }

            const auto n = a.rows();

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                Matrix<T> Acopy = a;
                Vector<T> x = b;
                lapack_int info;
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sposv(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, 1,
                                         Acopy.data(), (lapack_int)n,
                                         x.data(), 1);
                else
                    info = LAPACKE_dposv(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, 1,
                                         Acopy.data(), (lapack_int)n,
                                         x.data(), 1);
                if (info > 0)
                    throw MathError("cholesky_solve: matrix is not positive definite");
                return x;
            }
#endif

            // Cholesky decomposition
            Matrix<T> L = cholesky_decomposition(a);

            // float/double: forward/backward substitution with raw pointers
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                const T* __restrict ld = L.data();
                const T* __restrict bd = b.data();

                // Forward substitution (Ly = b) — row-wise read (contiguous)
                Vector<T> y(n);
                T* __restrict yd = y.data();
                for (std::size_t i = 0; i < n; ++i) {
                    const T* __restrict row = ld + i * n;
                    T sum = bd[i];
                    std::size_t j = 0;
                    using PT = PacketTraits<T>;
                    constexpr std::size_t PS = PT::size;
                    if (i >= PS) {
                        auto vsum = PT::mul(PT::load(row), PT::load(yd));
                        for (j = PS; j + PS <= i; j += PS)
                            vsum = PT::fmadd(PT::load(row + j), PT::load(yd + j), vsum);
                        sum -= PT::reduce_add(vsum);
                    }
                    for (; j < i; ++j)
                        sum -= row[j] * yd[j];
                    yd[i] = sum / row[i];
                }

                // Backward substitution (L^T x = y) — column-wise axpy (contiguous via row reads)
                Vector<T> x = y;
                T* __restrict xd = x.data();
                for (std::size_t jj = 0; jj < n; ++jj) {
                    std::size_t j = n - 1 - jj;
                    xd[j] /= ld[j * n + j];
                    T neg_xj = -xd[j];
                    const T* __restrict row_j = ld + j * n;
                    computation::simd::axpy_simd<T>(xd, neg_xj, row_j, j);
                }
                return x;
            }

            // Generic type: forward substitution (Ly = b)
            Vector<T> y(n);
            for (std::size_t i = 0; i < n; ++i) {
                T sum = b[i];
                for (std::size_t j = 0; j < i; ++j)
                    sum -= L(i, j) * y[j];
                y[i] = sum / L(i, i);
            }

            // Backward substitution (L^H x = y)  *for real numbers L^H = L^T
            Vector<T> x(n);
            for (std::size_t i = n; i-- > 0; ) {
                T sum = y[i];
                for (std::size_t j = i + 1; j < n; ++j) {
                    if constexpr (numeric_traits<T>::is_complex)
                        sum -= sangi::conj(L(j, i)) * x[j];
                    else
                        sum -= L(j, i) * x[j];
                }
                // L(i,i) is always real positive due to Cholesky, so conj is unnecessary
                x[i] = sum / L(i, i);
            }

            return x;
        }

        // ====================================================================
        // Householder transformation standalone API
        // ====================================================================

        /**
         * @brief Compute the Householder vector and coefficient
         *
         * For input vector x, returns v, beta, alpha satisfying
         * (I - beta * v * v^T) * x = alpha * e1.
         *
         * @param x input vector (size >= 1)
         * @return {v, beta, alpha} — v: Householder vector, beta: coefficient, alpha: first component of the result
         */
        template<typename T>
        std::tuple<Vector<T>, T, T> householder_vector(const BaseVector<T>& x_basevec) {
            Vector<T> x(x_basevec);  // 1 copy via BaseVector ctor
            const auto n = x.size();
            if (n == 0) {
                assert(false && "DimensionError: householder_vector: empty vector");
                throw DimensionError("householder_vector: empty vector");
            }

            const T zero = numeric_traits<T>::zero();
            const T one  = numeric_traits<T>::one();
            const T two  = one + one;

            // Compute the norm of x (Complex: conj(x[i]) * x[i])
            T sigma = zero;
            for (std::size_t i = 1; i < n; ++i) {
                if constexpr (numeric_traits<T>::is_complex)
                    sigma += sangi::conj(x[i]) * x[i];
                else
                    sigma += x[i] * x[i];
            }

            Vector<T> v = x;

            if constexpr (numeric_traits<T>::is_complex) {
                auto sigma_abs = std::abs(sigma);
                if (sigma_abs == 0) {
                    // x is in the e1 direction
                    return {v, zero, x[0]};
                }
                // Compute ||x||
                auto x_norm_real = std::sqrt(std::abs(sangi::conj(x[0]) * x[0] + sigma));
                T x_norm = T(x_norm_real);

                // Sign: match the phase
                auto abs_x0 = std::abs(x[0]);
                T alpha = (abs_x0 > decltype(abs_x0)(0)) ?
                    T(-x_norm_real) * x[0] / T(abs_x0) : T(x_norm_real);

                v[0] = x[0] - alpha;

                // beta = 2 / (v^H v)
                T v_norm_sq = sangi::conj(v[0]) * v[0] + sigma;
                T beta = two / v_norm_sq;

                return {v, beta, alpha};
            } else {
                if (sigma == zero && x[0] >= zero) {
                    // x is already in the positive e1 direction → identity transform
                    return {v, zero, x[0]};
                }
                if (sigma == zero && x[0] < zero) {
                    // x = -alpha * e1 → sign flip
                    v[0] = zero;
                    return {v, two, -x[0]};
                }

                T x_norm = static_cast<T>(std::sqrt(
                    static_cast<double>(x[0] * x[0] + sigma)));

                // Choose the sign for numerical stability
                T alpha = (x[0] >= zero) ? -x_norm : x_norm;

                v[0] = x[0] - alpha;

                // beta = 2 / (v^T v) = 2 / (v[0]^2 + sigma)
                T v_norm_sq = v[0] * v[0] + sigma;
                T beta = two / v_norm_sq;

                return {v, beta, alpha};
            }
        }

        /**
         * @brief Apply a Householder reflection from the left: A ← (I - beta * v * v^H) * A
         *
         * Applied to the specified row range [row_start, row_start+v.size()) of matrix A.
         * @param v Householder vector
         * @param beta coefficient
         * @param A target matrix (updated in-place)
         * @param row_start starting row of application (default 0)
         * @param col_start starting column of application (default 0)
         */
        template<typename T>
        void apply_householder_left(
            const BaseVector<T>& v_basevec, T beta,
            Matrix<T>& A,
            std::size_t row_start = 0,
            std::size_t col_start = 0)
        {
            Vector<T> v(v_basevec);  // 1 copy via BaseVector ctor
            const auto vn = v.size();
            const auto cols = A.cols();
            const T zero = numeric_traits<T>::zero();

            // For each column j, w[j] = v^H * A(row_start:row_start+vn, j)
            for (std::size_t j = col_start; j < cols; ++j) {
                T dot = zero;
                for (std::size_t i = 0; i < vn; ++i) {
                    if constexpr (numeric_traits<T>::is_complex)
                        dot += sangi::conj(v[i]) * A(row_start + i, j);
                    else
                        dot += v[i] * A(row_start + i, j);
                }
                T factor = beta * dot;
                for (std::size_t i = 0; i < vn; ++i) {
                    A(row_start + i, j) -= factor * v[i];
                }
            }
        }

        /**
         * @brief Apply a Householder reflection from the right: A ← A * (I - beta * v * v^H)
         *
         * Applied to the specified column range [col_start, col_start+v.size()) of matrix A.
         */
        template<typename T>
        void apply_householder_right(
            Matrix<T>& A,
            const BaseVector<T>& v_basevec, T beta,
            std::size_t row_start = 0,
            std::size_t col_start = 0)
        {
            Vector<T> v(v_basevec);  // 1 copy via BaseVector ctor
            const auto vn = v.size();
            const auto rows = A.rows();
            const T zero = numeric_traits<T>::zero();

            // For each row i, w[i] = A(i, col_start:col_start+vn) * v
            for (std::size_t i = row_start; i < rows; ++i) {
                T dot = zero;
                for (std::size_t j = 0; j < vn; ++j) {
                    dot += A(i, col_start + j) * v[j];
                }
                T factor = beta * dot;
                for (std::size_t j = 0; j < vn; ++j) {
                    if constexpr (numeric_traits<T>::is_complex)
                        A(i, col_start + j) -= factor * sangi::conj(v[j]);
                    else
                        A(i, col_start + j) -= factor * v[j];
                }
            }
        }

        /**
         * @brief Direct solver via Householder QR decomposition: Ax = b
         *
         * For an m×n matrix A (m >= n), returns the least-squares solution.
         * For m == n it is the direct solution of a nonsingular system.
         *
         * @param A coefficient matrix (m×n, m >= n)
         * @param b right-hand-side vector (size m)
         * @return x solution vector (size n)
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> householder_solve(const MA& A, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            const auto m = A.rows();
            const auto n = A.cols();

            if (m < n) {
                assert(false && "DimensionError: householder_solve: requires m >= n (overdetermined or square)");
                throw DimensionError("householder_solve: requires m >= n (overdetermined or square)");
            }
            if (b.size() != m) {
                assert(false && "DimensionError: householder_solve: b size must equal A rows");
                throw DimensionError("householder_solve: b size must equal A rows");
            }

            // R = copy of A, d = copy of b (compute Q^T b directly)
            Matrix<T> R = A;
            Vector<T> d = b;

            auto min_mn = std::min(m, n);

            // Block Householder QR (WY representation) — for float/double with n >= 64
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                constexpr std::size_t NB = 16;
                if (min_mn >= NB * 2) {
                    T* __restrict rdata = R.data();
                    T* __restrict ddata = d.data();
                    // Pre-allocate Householder coefficients tau and buffers (reused across the whole loop)
                    std::vector<T> tau(min_mn);
                    std::vector<T> w_buf(n);
                    std::vector<T> T_fac(NB * NB);
                    std::vector<T> VT_rm(NB * m);
                    std::vector<T> V_rm(m * NB);
                    std::vector<T> W_buf(NB * n);

                    for (std::size_t jb = 0; jb < min_mn; jb += NB) {
                        const std::size_t jend = (std::min)(jb + NB, min_mn);
                        const std::size_t nb = jend - jb;

                        // Phase 1: panel factorization (columns jb..jend-1)
                        // Scalar Householder for each column j, SIMD-applied to the remaining columns in the panel
                        using PT = PacketTraits<T>;
                        constexpr std::size_t PS = PT::size;

                        for (std::size_t j = jb; j < jend; ++j) {
                            const std::size_t len = m - j;
                            // sigma = sum of squares below diagonal
                            T sigma = T(0);
                            for (std::size_t i = 1; i < len; ++i)
                                sigma += rdata[(j + i) * n + j] * rdata[(j + i) * n + j];

                            if (sigma == T(0) && rdata[j * n + j] >= T(0)) {
                                tau[j] = T(0);
                                continue;
                            }

                            T x0 = rdata[j * n + j];
                            T x_norm = std::sqrt(x0 * x0 + sigma);
                            T alpha = (x0 >= T(0)) ? -x_norm : x_norm;
                            T v0 = x0 - alpha;
                            T v_norm_sq = v0 * v0 + sigma;
                            T tau_j = T(2) * v0 * v0 / v_norm_sq;
                            tau[j] = tau_j;

                            // Normalize v (v[0] = 1, v[i] /= v0)
                            T inv_v0 = T(1) / v0;
                            for (std::size_t i = 1; i < len; ++i)
                                rdata[(j + i) * n + j] *= inv_v0;
                            rdata[j * n + j] = alpha;

                            // Apply Householder to the remaining columns in the panel (SIMD)
                            // v = [1, rdata[j+1..m-1, j]]
                            // Process dot + axpy for PS columns simultaneously
                            const std::size_t col_start = j + 1;
                            const std::size_t col_end = jend;
                            const std::size_t rem_cols = col_end - col_start;
                            const auto neg_tau = PT::set1(-tau_j);
                            std::size_t cc = 0;

                            for (; cc + PS <= rem_cols; cc += PS) {
                                const std::size_t c = col_start + cc;
                                // Pass 1: dot product (v^T * A[:,c:c+PS-1])
                                auto dot_v = PT::load(&rdata[j * n + c]); // v[0]=1
                                for (std::size_t i = 1; i < len; ++i) {
                                    auto vi = PT::set1(rdata[(j + i) * n + j]);
                                    dot_v = PT::fmadd(vi, PT::load(&rdata[(j + i) * n + c]), dot_v);
                                }
                                // factor = -tau * dot (negated to prepare for fmadd)
                                auto neg_factor = PT::mul(neg_tau, dot_v);
                                // Pass 2: A[:,c:c+PS-1] += neg_factor * v
                                PT::store(&rdata[j * n + c],
                                    PT::add(PT::load(&rdata[j * n + c]), neg_factor)); // v[0]=1
                                for (std::size_t i = 1; i < len; ++i) {
                                    auto vi = PT::set1(rdata[(j + i) * n + j]);
                                    T* ptr = &rdata[(j + i) * n + c];
                                    PT::store(ptr, PT::fmadd(neg_factor, vi, PT::load(ptr)));
                                }
                            }
                            // Scalar remainder
                            for (; cc < rem_cols; ++cc) {
                                const std::size_t c = col_start + cc;
                                T dot = rdata[j * n + c];
                                for (std::size_t i = 1; i < len; ++i)
                                    dot += rdata[(j + i) * n + j] * rdata[(j + i) * n + c];
                                T factor = tau_j * dot;
                                rdata[j * n + c] -= factor;
                                for (std::size_t i = 1; i < len; ++i)
                                    rdata[(j + i) * n + c] -= factor * rdata[(j + i) * n + j];
                            }

                            // Apply to d as well
                            {
                                T dot = ddata[j];
                                for (std::size_t i = 1; i < len; ++i)
                                    dot += rdata[(j + i) * n + j] * ddata[j + i];
                                T factor = tau_j * dot;
                                ddata[j] -= factor;
                                for (std::size_t i = 1; i < len; ++i)
                                    ddata[j + i] -= factor * rdata[(j + i) * n + j];
                            }
                        }

                        if (jend >= n) continue;

                        const std::size_t trailing = n - jend;
                        const std::size_t pr = m - jb; // panel rows

                        // Phase 2: batch-update the trailing columns with the WY block representation
                        // Q_block = I - V * T * V^T, T is nb×nb upper triangular
                        //
                        // (b) Expand V into a row-major buffer (done first since it is used in building the T factor)
                        // VT_rm = V^T in row-major (nb × pr, lda=pr) — for gemm step (c)
                        // V_rm = V in row-major (pr × nb, lda=nb) — for gemm step (e)
                        std::fill_n(VT_rm.data(), nb * pr, T(0));
                        std::fill_n(V_rm.data(), pr * nb, T(0));
                        for (std::size_t kk = 0; kk < nb; ++kk) {
                            const std::size_t j = jb + kk;
                            VT_rm[kk * pr + kk] = T(1);
                            V_rm[kk * nb + kk] = T(1);
                            const std::size_t len_j = m - j;
                            for (std::size_t i = 1; i < len_j; ++i) {
                                T val = rdata[(j + i) * n + j];
                                VT_rm[kk * pr + kk + i] = val;
                                V_rm[(kk + i) * nb + kk] = val;
                            }
                        }

                        // (a) Build the T factor (using the contiguous VT_rm buffer)
                        std::fill_n(T_fac.data(), nb * nb, T(0));
                        for (std::size_t jj = 0; jj < nb; ++jj) {
                            const std::size_t j = jb + jj;
                            T_fac[jj * nb + jj] = tau[j];
                            if (tau[j] == T(0)) continue;
                            // T[0:jj, jj] = -tau[j] * T[0:jj, 0:jj] * V[0:jj]^T * v_jj
                            // Use VT_rm: v_kk is VT_rm[kk*pr + kk:], v_jj is VT_rm[jj*pr + jj:]
                            const std::size_t len_j = m - j;
                            for (std::size_t kk = 0; kk < jj; ++kk) {
                                // Dot product over contiguous memory (VT_rm[kk, jj:jj+len_j] and VT_rm[jj, jj:jj+len_j])
                                const T* __restrict vk_ptr = &VT_rm[kk * pr + jj];
                                const T* __restrict vj_ptr = &VT_rm[jj * pr + jj];
                                T dot = T(0);
                                for (std::size_t i = 0; i < len_j; ++i)
                                    dot += vk_ptr[i] * vj_ptr[i];
                                T_fac[kk * nb + jj] = dot;
                            }
                            // T[0:jj, jj] = -tau[j] * T[0:jj, 0:jj] * z
                            for (std::size_t kk = 0; kk < jj; ++kk) {
                                T sum = T(0);
                                for (std::size_t ll = kk; ll < jj; ++ll)
                                    sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj];
                                T_fac[kk * nb + jj] = -tau[j] * sum;
                            }
                        }

                        // (c) W = V^T * A_trail  (nb × trailing) via gemm
                        // VT_rm (nb × pr, lda=pr) * A_trail (pr × trailing, ldb=n) → W (nb × trailing)
                        std::fill_n(W_buf.data(), nb * trailing, T(0));
                        DefaultComputePolicy<T>::gemm(
                            VT_rm.data(), &rdata[jb * n + jend], W_buf.data(),
                            nb, trailing, pr, pr, n, trailing);

                        // (d) W = T^T * W  (lower triangular nb×nb × nb×trailing)
                        // Since Q^T = I - V * T^T * V^T, apply T^T
                        // In-place lower-triangular trmv: compute from bottom to top
                        for (std::size_t kk = nb; kk-- > 0; ) {
                            T t_kk = T_fac[kk * nb + kk];
                            if (t_kk != T(1)) {
                                T* w_row = &W_buf[kk * trailing];
                                for (std::size_t c = 0; c < trailing; ++c)
                                    w_row[c] *= t_kk;
                            }
                            for (std::size_t ll = 0; ll < kk; ++ll) {
                                T t_lk = T_fac[ll * nb + kk];
                                if (t_lk == T(0)) continue;
                                computation::simd::axpy_simd<T>(
                                    &W_buf[kk * trailing], t_lk, &W_buf[ll * trailing], trailing);
                            }
                        }

                        // (e) A_trail -= V * W via gemm
                        // Sign-flip W so that A_trail += V * (-W)
                        for (std::size_t i = 0; i < nb * trailing; ++i)
                            W_buf[i] = -W_buf[i];
                        // V_rm (pr × nb, lda=nb) * (-W) (nb × trailing, ldb=trailing)
                        // → accumulate into A_trail (pr × trailing, ldc=n)
                        DefaultComputePolicy<T>::gemm(
                            V_rm.data(), W_buf.data(), &rdata[jb * n + jend],
                            pr, trailing, nb, nb, trailing, n);
                    }

                    // Backward substitution
                    double max_diag = 0.0;
                    for (std::size_t i = 0; i < n; ++i) {
                        double ad = std::abs(static_cast<double>(rdata[i * n + i]));
                        if (ad > max_diag) max_diag = ad;
                    }
                    double sing_tol = max_diag * static_cast<double>(n) *
                                      std::numeric_limits<double>::epsilon();
                    if (sing_tol == 0.0) sing_tol = std::numeric_limits<double>::epsilon();

                    Vector<T> x(n, T{0});
                    for (std::size_t ii = 0; ii < n; ++ii) {
                        std::size_t i = n - 1 - ii;
                        T sum = ddata[i];
                        for (std::size_t j = i + 1; j < n; ++j)
                            sum -= rdata[i * n + j] * x[j];
                        if (std::abs(static_cast<double>(rdata[i * n + i])) < sing_tol)
                            throw MathError("householder_solve: singular or near-singular matrix");
                        x[i] = sum / rdata[i * n + i];
                    }
                    return x;
                }
            }

            // Scalar Householder QR (small matrices or non-floating-point types)
            for (std::size_t k = 0; k < min_mn; ++k) {
                // Build the Householder vector from row k onward of column k
                std::size_t len = m - k;
                Vector<T> x(len);
                for (std::size_t i = 0; i < len; ++i) {
                    x[i] = R(k + i, k);
                }

                auto [v, beta, alpha] = householder_vector(x);

                if (beta == T{0}) continue;

                // Apply to R from the left
                apply_householder_left(v, beta, R, k, k);

                // Apply to d as well: d ← (I - beta * v * v^T) * d
                T dot_d = T{0};
                for (std::size_t i = 0; i < len; ++i) {
                    dot_d += v[i] * d[k + i];
                }
                T factor_d = beta * dot_d;
                for (std::size_t i = 0; i < len; ++i) {
                    d[k + i] -= factor_d * v[i];
                }
            }

            // For singularity detection: the largest absolute value of R's diagonal
            double max_diag = 0.0;
            for (std::size_t i = 0; i < n; ++i) {
                double ad = std::abs(static_cast<double>(R(i, i)));
                if (ad > max_diag) max_diag = ad;
            }
            double sing_tol = max_diag * static_cast<double>(n) *
                              std::numeric_limits<double>::epsilon();
            if (sing_tol == 0.0) sing_tol = std::numeric_limits<double>::epsilon();

            // Backward substitution: R(0:n, 0:n) x = d(0:n)
            Vector<T> x(n, T{0});
            for (std::size_t ii = 0; ii < n; ++ii) {
                std::size_t i = n - 1 - ii;
                T sum = d[i];
                for (std::size_t j = i + 1; j < n; ++j) {
                    sum -= R(i, j) * x[j];
                }
                T rii = R(i, i);
                if (std::abs(static_cast<double>(rii)) < sing_tol) {
                    throw MathError("householder_solve: singular or near-singular matrix");
                }
                x[i] = sum / rii;
            }

            return x;
        }

        // ====================================================================
        // Bidiagonalization standalone API
        // ====================================================================

        /**
         * @brief Structure storing the result of bidiagonalization
         *
         * A = U * B * V^T
         * B is an upper bidiagonal matrix (diagonal d, superdiagonal e)
         */
        template<typename T>
        struct BidiagonalResult {
            Matrix<T> U;      ///< left orthogonal matrix (m×m)
            Vector<T> d;      ///< diagonal elements (size min(m,n))
            Vector<T> e;      ///< superdiagonal elements (size min(m,n)-1, e[k] = B(k, k+1))
            Matrix<T> V;      ///< right orthogonal matrix (n×n)
        };

        /**
         * @brief Bidiagonalization by Householder transformation
         *
         * Transforms an m×n matrix A (m >= n) into an upper bidiagonal matrix B.
         * Returns U (m×m), B (upper bidiagonal), V (n×n) satisfying A = U * B * V^T.
         *
         * For m < n, bidiagonalize A^T and transpose back.
         *
         * @param A input matrix
         * @return BidiagonalResult<T> {U, d, e, V}
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        BidiagonalResult<sangi::element_t<MA>> bidiagonalize(const MA& A) {
            using T = sangi::element_t<MA>;
            // rectangular OK (for m < n, handle A^T)

            const size_t m = A.rows();
            const size_t n = A.cols();

            if (m == 0 || n == 0) {
                assert(false && "DimensionError: bidiagonalize: empty matrix");
                throw DimensionError("bidiagonalize: empty matrix");
            }

            if (m < n) {
                assert(false && "DimensionError: bidiagonalize: requires m >= n");
                throw DimensionError(
                    "bidiagonalize: requires m >= n (use A^T for wide matrices)");
            }

            // below, m >= n
            const size_t min_mn = n;

            // Working copy
            Matrix<T> W(A);

            // Initialize U (m×m), V (n×n) as identity matrices
            Matrix<T> U = Matrix<T>::identity(m);
            Matrix<T> V = Matrix<T>::identity(n);

            const T zero = numeric_traits<T>::zero();
            const T one  = numeric_traits<T>::one();

            // Diagonal and superdiagonal elements
            std::vector<T> diag_vals(n, zero);
            std::vector<T> super_vals(n, zero);  // super_vals[k+1] = B(k, k+1)

            for (size_t k = 0; k < n; ++k) {

                // --- Left Householder: zero out W(k:m, k) from row k+1 onward ---
                {
                    auto sigma_sq = decltype(std::abs(zero))(0);
                    for (size_t i = k; i < m; ++i)
                        sigma_sq += std::abs(W(i, k)) * std::abs(W(i, k));
                    auto alpha_r = std::sqrt(sigma_sq);

                    if (alpha_r > 0) {
                        T alpha;
                        if constexpr (numeric_traits<T>::is_complex) {
                            auto abs_wkk = std::abs(W(k, k));
                            alpha = (abs_wkk > 0) ?
                                T(-alpha_r) * W(k, k) / T(abs_wkk) : T(alpha_r);
                        } else {
                            alpha = (W(k, k) > zero) ? T(-alpha_r) : T(alpha_r);
                        }
                        W(k, k) -= alpha;
                        T beta = one / (alpha * W(k, k));  // = -2 / (v^H v)

                        // Apply to W(k:m, k+1:n)
                        for (size_t j = k + 1; j < n; ++j) {
                            T dot_val = zero;
                            for (size_t i = k; i < m; ++i) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += sangi::conj(W(i, k)) * W(i, j);
                                else
                                    dot_val += W(i, k) * W(i, j);
                            }
                            dot_val *= beta;
                            for (size_t i = k; i < m; ++i)
                                W(i, j) += W(i, k) * dot_val;
                        }

                        // Accumulate into U (U = U * H_k)
                        for (size_t i = 0; i < m; ++i) {
                            T dot_val = zero;
                            for (size_t j = k; j < m; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += U(i, j) * sangi::conj(W(j, k));
                                else
                                    dot_val += U(i, j) * W(j, k);
                            }
                            dot_val *= beta;
                            for (size_t j = k; j < m; ++j)
                                U(i, j) += dot_val * W(j, k);
                        }

                        diag_vals[k] = alpha;
                    } else {
                        diag_vals[k] = W(k, k);
                    }
                }

                // --- Right Householder: zero out W(k, k+2:n) ---
                if (k + 2 <= n - 1) {
                    auto sigma_sq = decltype(std::abs(zero))(0);
                    for (size_t j = k + 1; j < n; ++j)
                        sigma_sq += std::abs(W(k, j)) * std::abs(W(k, j));
                    auto alpha_r = std::sqrt(sigma_sq);

                    if (alpha_r > 0) {
                        T alpha;
                        if constexpr (numeric_traits<T>::is_complex) {
                            auto abs_wk1 = std::abs(W(k, k + 1));
                            alpha = (abs_wk1 > 0) ?
                                T(-alpha_r) * W(k, k + 1) / T(abs_wk1) : T(alpha_r);
                        } else {
                            alpha = (W(k, k + 1) > zero) ? T(-alpha_r) : T(alpha_r);
                        }
                        W(k, k + 1) -= alpha;
                        T beta = one / (alpha * W(k, k + 1));

                        // Apply to W(k+1:m, k+1:n)
                        for (size_t i = k + 1; i < m; ++i) {
                            T dot_val = zero;
                            for (size_t j = k + 1; j < n; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += sangi::conj(W(k, j)) * W(i, j);
                                else
                                    dot_val += W(k, j) * W(i, j);
                            }
                            dot_val *= beta;
                            for (size_t j = k + 1; j < n; ++j)
                                W(i, j) += W(k, j) * dot_val;
                        }

                        // Accumulate into V
                        for (size_t i = 0; i < n; ++i) {
                            T dot_val = zero;
                            for (size_t j = k + 1; j < n; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += sangi::conj(W(k, j)) * V(i, j);
                                else
                                    dot_val += W(k, j) * V(i, j);
                            }
                            dot_val *= beta;
                            for (size_t j = k + 1; j < n; ++j)
                                V(i, j) += W(k, j) * dot_val;
                        }

                        super_vals[k + 1] = alpha;
                    } else {
                        super_vals[k + 1] = W(k, k + 1);
                    }
                } else if (k + 1 < n) {
                    super_vals[k + 1] = W(k, k + 1);
                }
            }

            // Convert the result into Vectors
            Vector<T> d(min_mn);
            for (size_t i = 0; i < min_mn; ++i)
                d[i] = diag_vals[i];

            Vector<T> e(min_mn > 1 ? min_mn - 1 : 0);
            for (size_t i = 0; i < e.size(); ++i)
                e[i] = super_vals[i + 1];

            return { std::move(U), std::move(d), std::move(e), std::move(V) };
        }

        /**
         * @brief Utility to explicitly construct a bidiagonal matrix
         *
         * @param d diagonal elements (size p)
         * @param e superdiagonal elements (size p-1)
         * @return Matrix<T> upper bidiagonal matrix (p×p)
         */
        template<typename T>
        Matrix<T> build_bidiagonal_matrix(const BaseVector<T>& d_basevec, const BaseVector<T>& e_basevec) {
            Vector<T> d(d_basevec);  // 1 copy via BaseVector ctor
            Vector<T> e(e_basevec);  // 1 copy via BaseVector ctor
            const size_t p = d.size();
            if (p > 1 && e.size() != p - 1) {
                assert(false && "DimensionError: build_bidiagonal_matrix: e.size() must equal d.size()-1");
                throw DimensionError("build_bidiagonal_matrix: e.size() must equal d.size()-1");
            }
            Matrix<T> B(p, p, numeric_traits<T>::zero());
            for (size_t i = 0; i < p; ++i)
                B(i, i) = d[i];
            for (size_t i = 0; i + 1 < p; ++i)
                B(i, i + 1) = e[i];
            return B;
        }

        // ====================================================================
        // QR decomposition
        // ====================================================================

        /**
         * @brief Compute the QR decomposition (via Householder reflections)
         * @tparam T element type
         * @param a matrix to decompose
         * @return a pair of Q and R
         */
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        std::pair<Matrix<typename M::value_type>, Matrix<typename M::value_type>>
        qr_decomposition(const M& a) {
            using T = typename M::value_type;
            const auto m = a.rows();
            const auto n = a.cols();

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                const auto k = std::min(m, n);
                Matrix<T> R(m, n);
                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        R(i, j) = a(i, j);
                std::vector<T> tau(k);
                lapack_int info;
                // QR decomposition (Householder form)
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sgeqrf(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)n,
                                          R.data(), (lapack_int)n, tau.data());
                else
                    info = LAPACKE_dgeqrf(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)n,
                                          R.data(), (lapack_int)n, tau.data());
                if (info != 0)
                    throw MathError("qr_decomposition: LAPACKE_geqrf failed");
                // Generate Q explicitly: copy the Householder vectors into an m×m matrix
                Matrix<T> Q(m, m, T(0));
                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        Q(i, j) = R(i, j);
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sorgqr(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)m,
                                          (lapack_int)k, Q.data(), (lapack_int)m, tau.data());
                else
                    info = LAPACKE_dorgqr(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)m,
                                          (lapack_int)k, Q.data(), (lapack_int)m, tau.data());
                if (info != 0)
                    throw MathError("qr_decomposition: LAPACKE_orgqr failed");
                // Zero out the lower triangular part of R
                for (std::size_t i = 1; i < m; ++i)
                    for (std::size_t j = 0; j < std::min(i, n); ++j)
                        R(i, j) = T(0);
                return { Q, R };
            }
#endif

            Matrix<T> Q(m, m);
            Matrix<T> R(m, n);
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    R(i, j) = a(i, j);

            const T zero = numeric_traits<T>::zero();
            const T one  = numeric_traits<T>::one();
            const T two  = one + one;

            // Initialize Q as the identity matrix
            for (typename Matrix<T>::size_type i = 0; i < m; ++i) {
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    Q(i, j) = (i == j) ? one : zero;
                }
            }

            // Computation via Householder reflections
            for (typename Matrix<T>::size_type k = 0; k < std::min(m, n); ++k) {
                // Compute the Householder vector
                Vector<T> x(m - k);
                for (typename Matrix<T>::size_type i = 0; i < m - k; ++i) {
                    x[i] = R(k + i, k);
                }

                // Compute the norm (Complex-aware: dot returns conj(x)*x, so take the real part)
                auto dot_xx = dot(x, x);
                auto x_norm_real = std::sqrt(std::abs(dot_xx));
                T x_norm = T(x_norm_real);

                // Zero-vector check
                if (x_norm_real < numeric_traits<decltype(x_norm_real)>::epsilon()) {
                    continue;
                }

                // Signed norm (for Complex, match the phase)
                T alpha;
                if constexpr (numeric_traits<T>::is_complex) {
                    auto abs_x0 = std::abs(x[0]);
                    alpha = (abs_x0 > decltype(abs_x0)(0)) ?
                        T(-x_norm_real) * x[0] / T(abs_x0) : T(x_norm_real);
                } else {
                    alpha = (x[0] >= zero) ? -x_norm : x_norm;
                }

                // Modify only the first element (x[0] - alpha)
                x[0] -= alpha;

                // Squared norm of the Householder vector
                auto x_norm_squared = dot(x, x);

                // Check for numerical stability
                if (std::abs(x_norm_squared) < numeric_traits<decltype(x_norm_real)>::epsilon()) {
                    continue;
                }

                // Apply the Householder matrix
                for (typename Matrix<T>::size_type j = k; j < n; ++j) {
                    T dot_product = zero;
                    for (typename Matrix<T>::size_type i = 0; i < m - k; ++i) {
                        if constexpr (numeric_traits<T>::is_complex)
                            dot_product += sangi::conj(x[i]) * R(k + i, j);
                        else
                            dot_product += x[i] * R(k + i, j);
                    }

                    T factor = (two * dot_product) / x_norm_squared;

                    for (typename Matrix<T>::size_type i = 0; i < m - k; ++i) {
                        R(k + i, j) -= factor * x[i];
                    }
                }

                // Likewise apply the Householder matrix to Q
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    T dot_product = zero;
                    for (typename Matrix<T>::size_type i = 0; i < m - k; ++i) {
                        if constexpr (numeric_traits<T>::is_complex)
                            dot_product += sangi::conj(x[i]) * Q(k + i, j);
                        else
                            dot_product += x[i] * Q(k + i, j);
                    }

                    T factor = (two * dot_product) / x_norm_squared;

                    for (typename Matrix<T>::size_type i = 0; i < m - k; ++i) {
                        Q(k + i, j) -= factor * x[i];
                    }
                }
            }

            // Q is composed of column vectors, so a transpose is actually required
            Matrix<T> Q_transpose(m, m);
            for (typename Matrix<T>::size_type i = 0; i < m; ++i) {
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    Q_transpose(i, j) = Q(j, i);
                }
            }

            return { Q_transpose, R };
        }

        /**
         * @brief Solve a system of linear equations using QR decomposition
         * @tparam T element type
         * @param a coefficient matrix
         * @param b right-hand-side vector
         * @return solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> qr_solve(const MA& a, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: qr_solve: matrix and vector dimensions mismatch");
                throw DimensionError("qr_solve: matrix and vector dimensions mismatch");
            }

            // float/double: use householder_solve (does not form Q explicitly)
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                return householder_solve(a, b);
            }

            const auto m = a.rows();
            const auto n = a.cols();

            // QR decomposition
            auto [Q, R] = qr_decomposition(a);

            // Compute Q^T * b
            Vector<T> Q_transpose_b(m);
            for (typename Matrix<T>::size_type i = 0; i < m; ++i) {
                Q_transpose_b[i] = T(0);
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    Q_transpose_b[i] += Q(j, i) * b[j];  // Q^T = Q(j,i)
                }
            }

            // Backward substitution (Rx = Q^T * b)
            Vector<T> x(n, T(0));

            // Least-squares solution (for overdetermined systems)
            const auto min_rank = std::min(m, n);

            for (typename Matrix<T>::size_type i = min_rank; i-- > 0; ) {
                T sum = Q_transpose_b[i];
                for (typename Matrix<T>::size_type j = i + 1; j < min_rank; ++j) {
                    sum -= R(i, j) * x[j];
                }

                // Check for zero diagonal elements
                if (std::abs(R(i, i)) < std::numeric_limits<T>::epsilon()) {
                    // In the case of a singular matrix
                    // Consider switching to singular value decomposition (SVD)
                    throw MathError("qr_solve: matrix is singular or nearly singular");
                }

                x[i] = sum / R(i, i);
            }

            return x;
        }

        /**
         * @brief QR decomposition with column pivoting (via Householder reflections)
         *
         * Decomposes into the form AP = QR.
         * At each step, select the column with the largest residual norm and perform a column swap.
         * This makes R's diagonal elements |R(0,0)| >= |R(1,1)| >= ..., enabling
         * numerical rank determination.
         *
         * @tparam T element type
         * @param a the m×n matrix to decompose
         * @return a tuple of (Q, R, perm)
         *   - Q: m×m orthogonal matrix
         *   - R: m×n upper triangular matrix
         *   - perm: column permutation vector (perm[j] = original column index)
         *
         * Meaning of the permutation: the perm[j]-th column of A = the j-th column of Q*R
         * That is, A * P = Q * R (where P is a permutation matrix)
         */
        template<typename T>
        std::tuple<Matrix<T>, Matrix<T>, std::vector<typename Matrix<T>::size_type>>
            qr_col_pivoting(const BaseMatrix<T>& a) {
            using size_type = typename Matrix<T>::size_type;
            const auto m = a.rows();
            const auto n = a.cols();
            const auto k = std::min(m, n);

            Matrix<T> Q(m, m);
            Matrix<T> R(a);  // 1 copy: body modifies R in place

            // Initialize Q as the identity matrix
            for (size_type i = 0; i < m; ++i)
                for (size_type j = 0; j < m; ++j)
                    Q(i, j) = (i == j) ? T(1) : T(0);

            // Permutation vector (initialized to the identity permutation)
            std::vector<size_type> perm(n);
            for (size_type j = 0; j < n; ++j) perm[j] = j;

            // Precompute the squared residual norm of each column
            std::vector<T> col_norms_sq(n, T(0));
            for (size_type j = 0; j < n; ++j) {
                for (size_type i = 0; i < m; ++i) {
                    col_norms_sq[j] += R(i, j) * R(i, j);
                }
            }

            for (size_type step = 0; step < k; ++step) {
                // Search for the column with the largest norm (from columns step onward)
                size_type max_col = step;
                T max_norm = col_norms_sq[step];
                for (size_type j = step + 1; j < n; ++j) {
                    if (col_norms_sq[j] > max_norm) {
                        max_norm = col_norms_sq[j];
                        max_col = j;
                    }
                }

                // Column swap
                if (max_col != step) {
                    // Swap the columns of R
                    for (size_type i = 0; i < m; ++i) {
                        std::swap(R(i, step), R(i, max_col));
                    }
                    // Swap the norms
                    std::swap(col_norms_sq[step], col_norms_sq[max_col]);
                    // Record the permutation
                    std::swap(perm[step], perm[max_col]);
                }

                // Compute the Householder vector
                Vector<T> x(m - step);
                for (size_type i = 0; i < m - step; ++i) {
                    x[i] = R(step + i, step);
                }

                T x_norm = std::sqrt(dot(x, x));

                if (x_norm < std::numeric_limits<T>::epsilon()) {
                    continue;
                }

                T alpha = (x[0] >= T(0)) ? -x_norm : x_norm;
                x[0] -= alpha;

                T x_norm_sq = dot(x, x);
                if (x_norm_sq < std::numeric_limits<T>::epsilon()) {
                    continue;
                }

                // Apply the Householder transformation to R
                for (size_type j = step; j < n; ++j) {
                    T dp = T(0);
                    for (size_type i = 0; i < m - step; ++i) {
                        dp += x[i] * R(step + i, j);
                    }
                    T factor = (T(2) * dp) / x_norm_sq;
                    for (size_type i = 0; i < m - step; ++i) {
                        R(step + i, j) -= factor * x[i];
                    }
                }

                // Apply the Householder transformation to Q
                for (size_type j = 0; j < m; ++j) {
                    T dp = T(0);
                    for (size_type i = 0; i < m - step; ++i) {
                        dp += x[i] * Q(step + i, j);
                    }
                    T factor = (T(2) * dp) / x_norm_sq;
                    for (size_type i = 0; i < m - step; ++i) {
                        Q(step + i, j) -= factor * x[i];
                    }
                }

                // Update the residual norms (columns step+1 onward)
                for (size_type j = step + 1; j < n; ++j) {
                    col_norms_sq[j] -= R(step, j) * R(step, j);
                    if (col_norms_sq[j] < T(0)) col_norms_sq[j] = T(0);
                }
            }

            // Transpose Q
            Matrix<T> Qt(m, m);
            for (size_type i = 0; i < m; ++i)
                for (size_type j = 0; j < m; ++j)
                    Qt(i, j) = Q(j, i);

            return { Qt, R, perm };
        }

        /**
         * @brief Compute the numerical rank of a column-pivoted QR decomposition
         *
         * Determine the numerical rank from R's diagonal elements.
         * When |R(i,i)| > threshold, the i-th column is considered independent.
         *
         * @param R the upper triangular matrix obtained from qr_col_pivoting
         * @param threshold threshold (default: eps * max(m,n) * |R(0,0)|)
         * @return numerical rank
         */
        template<typename T>
        typename Matrix<T>::size_type qr_rank(
            const Matrix<T>& R,
            T threshold = T(-1))
        {
            using size_type = typename Matrix<T>::size_type;
            const auto k = std::min(R.rows(), R.cols());
            if (k == 0) return 0;

            if (threshold < T(0)) {
                // Default threshold: eps * max(m,n) * |R(0,0)|
                T max_dim = T(std::max(R.rows(), R.cols()));
                threshold = std::numeric_limits<T>::epsilon() * max_dim
                          * std::abs(R(0, 0));
            }

            size_type rank = 0;
            for (size_type i = 0; i < k; ++i) {
                if (std::abs(R(i, i)) > threshold) {
                    ++rank;
                }
            }
            return rank;
        }

        /**
         * @brief Solve a system of linear equations using QR decomposition with column pivoting
         *
         * Solve A*x = b via column-pivoted QR decomposition.
         * For rank-deficient cases, solve only for the rank-many variables and set the rest to 0.
         *
         * @param a coefficient matrix (m×n)
         * @param b right-hand-side vector (m)
         * @return solution vector (n)
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> qr_col_pivoting_solve(const MA& a, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: qr_col_pivoting_solve: dimensions mismatch");
                throw DimensionError(
                    "qr_col_pivoting_solve: matrix and vector dimensions mismatch");
            }

            const auto m = a.rows();
            const auto n = a.cols();

            auto [Q, R, perm] = qr_col_pivoting(a);

            // Compute Q^T * b
            Vector<T> Qtb(m, T(0));
            for (typename Matrix<T>::size_type i = 0; i < m; ++i) {
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    Qtb[i] += Q(j, i) * b[j];
                }
            }

            // Determine the numerical rank
            auto r = qr_rank(R);

            // Backward substitution (up to rank r)
            Vector<T> y(n, T(0));
            for (typename Matrix<T>::size_type i = r; i-- > 0; ) {
                T sum = Qtb[i];
                for (typename Matrix<T>::size_type j = i + 1; j < r; ++j) {
                    sum -= R(i, j) * y[j];
                }
                y[i] = sum / R(i, i);
            }

            // Apply the inverse permutation to restore the original column order
            Vector<T> x(n, T(0));
            for (typename Matrix<T>::size_type j = 0; j < n; ++j) {
                x[perm[j]] = y[j];
            }

            return x;
        }

        // ====================================================================
        // Tridiagonalization standalone API
        // ====================================================================

        /**
         * @brief Structure storing the result of tridiagonalization
         *
         * Symmetric matrix A = Q * T * Q^T
         * T is a tridiagonal matrix (diagonal d, subdiagonal e)
         */
        template<typename T>
        struct TridiagonalResult {
            Matrix<T> Q;      ///< orthogonal matrix (n×n)
            Vector<T> d;      ///< diagonal elements (size n)
            Vector<T> e;      ///< subdiagonal elements (size n-1, e[k] = T(k, k+1) = T(k+1, k))
        };

        /**
         * @brief Tridiagonalization of a symmetric matrix by Householder transformation
         *
         * Transforms an n×n symmetric matrix A into a tridiagonal matrix T.
         * Returns Q (orthogonal matrix) and T (tridiagonal) satisfying A = Q * T * Q^T.
         *
         * Reference: Golub & Van Loan "Matrix Computations" 4th ed. §8.3
         *
         * @param A symmetric matrix (n×n)
         * @return TridiagonalResult<T> {Q, d, e}
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        TridiagonalResult<sangi::element_t<MA>> tridiagonalize(const MA& A) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            const auto n = A.rows();

            if (n != A.cols()) {
                assert(false && "DimensionError: tridiagonalize: matrix must be square");
                throw DimensionError("tridiagonalize: matrix must be square");
            }
            if (n == 0) {
                assert(false && "DimensionError: tridiagonalize: empty matrix");
                throw DimensionError("tridiagonalize: empty matrix");
            }

            // Symmetry check (T-aware, Float-aware)
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = i + 1; j < n; ++j) {
                    T aij = A(i, j);
                    T aji = A(j, i);
                    T scale = std::max(std::abs(aij), T(1));
                    T diff = std::abs(aij - aji);
                    T tol = scale * T(static_cast<int>(n)) * std::numeric_limits<T>::epsilon()
                          + T(1) * std::numeric_limits<T>::epsilon();  // small constant term
                    if (diff > tol)
                        throw MathError("tridiagonalize: matrix must be symmetric");
                }

            // Working copy
            Matrix<T> W(A);

            // Save the Householder vectors, coefficients, and the corresponding k
            struct HouseholderInfo {
                std::size_t k;
                Vector<T> v;
                T beta;
            };
            std::vector<HouseholderInfo> hh_list;

            // Householder tridiagonalization
            for (std::size_t k = 0; k + 2 <= n; ++k) {
                const std::size_t len = n - k - 1;

                // sigma = Σ W(k+2:n, k)²
                T sigma = T(0);
                for (std::size_t i = 1; i < len; ++i)
                    sigma += W(k + 1 + i, k) * W(k + 1 + i, k);

                T x0 = W(k + 1, k);

                if (sigma == T(0) && x0 >= T(0)) {
                    continue;  // already in tridiagonal form
                }

                T alpha = std::sqrt(x0 * x0 + sigma);
                if (x0 > T(0)) alpha = -alpha;

                T v0 = x0 - alpha;
                T beta = T(2) / (v0 * v0 + sigma);

                // Save v
                Vector<T> v(len);
                v[0] = v0;
                for (std::size_t i = 1; i < len; ++i)
                    v[i] = W(k + 1 + i, k);

                hh_list.push_back({ k, v, beta });

                // Symmetric Householder update: W ← (I - β vvᵀ) W (I - β vvᵀ)
                // Golub & Van Loan's efficient formula:
                //   p = β * W(k+1:n, k+1:n) * v
                //   K = β/2 * vᵀp
                //   q = p - K * v
                //   W(k+1:n, k+1:n) -= v*qᵀ + q*vᵀ

                Vector<T> p(len, T(0));
                for (std::size_t i = 0; i < len; ++i) {
                    T dot = T(0);
                    for (std::size_t j = 0; j < len; ++j)
                        dot += W(k + 1 + i, k + 1 + j) * v[j];
                    p[i] = beta * dot;
                }

                T K = T(0);
                for (std::size_t i = 0; i < len; ++i)
                    K += v[i] * p[i];
                K *= beta / T(2);

                Vector<T> q(len);
                for (std::size_t i = 0; i < len; ++i)
                    q[i] = p[i] - K * v[i];

                for (std::size_t i = 0; i < len; ++i)
                    for (std::size_t j = 0; j < len; ++j)
                        W(k + 1 + i, k + 1 + j) -= v[i] * q[j] + q[i] * v[j];

                // Set the subdiagonal element
                W(k + 1, k) = alpha;
                W(k, k + 1) = alpha;
            }

            // Extract the diagonal and subdiagonal elements
            Vector<T> d(n, T(0));
            Vector<T> e(n > 1 ? n - 1 : 0, T(0));
            for (std::size_t i = 0; i < n; ++i)
                d[i] = W(i, i);
            for (std::size_t i = 0; i + 1 < n; ++i)
                e[i] = W(i, i + 1);

            // Backward accumulation of Q: Q = H_0 * H_1 * ... * H_{n-3}
            // Q = I, in reverse order Q ← (I - β v vᵀ) * Q
            // However, since each H_k acts on rows k+1:n:
            //   Q(k+1:n, :) -= β * v * (vᵀ * Q(k+1:n, :))
            Matrix<T> Q = Matrix<T>::identity(n);
            for (std::size_t ii = 0; ii < hh_list.size(); ++ii) {
                std::size_t idx = hh_list.size() - 1 - ii;
                auto& hh = hh_list[idx];
                std::size_t k = hh.k;
                const auto& v = hh.v;
                T beta = hh.beta;
                std::size_t len = v.size();

                // Q(k+1:k+1+len, :) -= β * v * (vᵀ * Q(k+1:k+1+len, :))
                for (std::size_t j = 0; j < n; ++j) {
                    T dot = T(0);
                    for (std::size_t i = 0; i < len; ++i)
                        dot += v[i] * Q(k + 1 + i, j);
                    T factor = beta * dot;
                    for (std::size_t i = 0; i < len; ++i)
                        Q(k + 1 + i, j) -= factor * v[i];
                }
            }

            return { std::move(Q), std::move(d), std::move(e) };
        }

        /**
         * @brief Compact result of tridiagonalization (keeps the Householder vectors, does not form Q)
         *
         * Instead of forming Q explicitly in the back-transformation phase of eigen_symmetric,
         * applying the Householder vectors directly to Z saves O(n³).
         */
        template<typename T>
        struct TridiagonalCompact {
            struct Reflector {
                std::size_t k;
                Vector<T> v;
                T beta;
            };
            std::vector<Reflector> refs;
            Vector<T> d;
            Vector<T> e;
        };

        /**
         * @brief Householder tridiagonalization (compact form)
         *
         * Does not form Q explicitly; returns the list of Householder vectors and coefficients.
         * Used to apply the Householder columns directly to Z in eigen_symmetric's back-transformation.
         * Applies the lower-triangular access optimization to SYMV.
         *
         * @param A symmetric matrix (n×n) — symmetry must be verified by the caller
         * @return TridiagonalCompact<T> {refs, d, e}
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        TridiagonalCompact<sangi::element_t<MA>> tridiagonalize_compact(const MA& A) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            const auto n = A.rows();
            if (n == 0) {
                assert(false && "DimensionError: tridiagonalize_compact: empty matrix");
                throw DimensionError("tridiagonalize_compact: empty matrix");
            }

            Matrix<T> W(A);
            TridiagonalCompact<T> result;
            result.d = Vector<T>(n, T(0));
            result.e = Vector<T>(n > 1 ? n - 1 : 0, T(0));

            for (std::size_t k = 0; k + 2 <= n; ++k) {
                const std::size_t len = n - k - 1;

                T sigma = T(0);
                for (std::size_t i = 1; i < len; ++i)
                    sigma += W(k + 1 + i, k) * W(k + 1 + i, k);

                T x0 = W(k + 1, k);

                if (sigma == T(0) && x0 >= T(0)) {
                    result.refs.push_back({k, Vector<T>(len, T(0)), T(0)});
                    continue;
                }

                T alpha = std::sqrt(x0 * x0 + sigma);
                if (x0 > T(0)) alpha = -alpha;

                T v0 = x0 - alpha;
                T beta = T(2) / (v0 * v0 + sigma);

                Vector<T> v(len);
                v[0] = v0;
                for (std::size_t i = 1; i < len; ++i)
                    v[i] = W(k + 1 + i, k);

                result.refs.push_back({k, v, beta});

                // p = β * W(k+1:n, k+1:n) * v
                Vector<T> p(len, T(0));
                {
                    T* __restrict wd = W.data();
                    const std::size_t wn = W.cols();
                    const T* __restrict vd = v.data();
                    T* __restrict pd = p.data();
                    for (std::size_t i = 0; i < len; ++i) {
                        const T* __restrict row = wd + (k + 1 + i) * wn + (k + 1);
                        T dot = T(0);
                        for (std::size_t j = 0; j < len; ++j)
                            dot += row[j] * vd[j];
                        pd[i] = beta * dot;
                    }
                }

                T K = T(0);
                for (std::size_t i = 0; i < len; ++i)
                    K += v[i] * p[i];
                K *= beta / T(2);

                Vector<T> q(len);
                for (std::size_t i = 0; i < len; ++i)
                    q[i] = p[i] - K * v[i];

                {
                    T* __restrict wd = W.data();
                    const std::size_t wn = W.cols();
                    const T* __restrict vd = v.data();
                    const T* __restrict qd = q.data();
                    for (std::size_t i = 0; i < len; ++i) {
                        T* __restrict row = wd + (k + 1 + i) * wn + (k + 1);
                        const T vi = vd[i], qi = qd[i];
                        for (std::size_t j = 0; j < len; ++j)
                            row[j] -= vi * qd[j] + qi * vd[j];
                    }
                }

                W(k + 1, k) = alpha;
                W(k, k + 1) = alpha;
            }

            for (std::size_t i = 0; i < n; ++i)
                result.d[i] = W(i, i);
            for (std::size_t i = 0; i + 1 < n; ++i)
                result.e[i] = W(i, i + 1);

            return result;
        }

        /**
         * @brief Apply a sequence of Householder reflections to matrix Z from the left
         *
         * Z ← Q * Z = H_0 * H_1 * ... * H_{m-1} * Z
         * H_k = I - β_k * v_k * v_kᵀ (acts on rows k+1:n)
         *
         * Apply from the inside: first H_{m-1}, then H_{m-2}, ..., last H_0
         * Transforms Z directly without forming Q explicitly.
         *
         * Column-major loop order: keeping each column in the L1 cache while
         * applying all reflectors (effective when Z exceeds L2).
         */
        template<typename T>
        void apply_householder_sequence(
            const std::vector<typename TridiagonalCompact<T>::Reflector>& refs,
            std::size_t n, T* __restrict z_col_major, std::size_t n_cols)
        {
            const std::size_t m = refs.size();
            if (m == 0) return;

            // Pre-extract the reflector data into a flat buffer
            // (avoid the indirection overhead of Vector<T>)
            struct FlatRef { std::size_t k; std::size_t len; T beta; const T* vd; };
            std::vector<FlatRef> flat(m);
            for (std::size_t ii = 0; ii < m; ++ii) {
                std::size_t idx = m - 1 - ii;
                const auto& hh = refs[idx];
                flat[ii] = {hh.k, hh.v.size(), hh.beta, hh.v.data()};
            }

            // Column blocking + 4-column unrolling:
            // Load v once and apply to 4 columns simultaneously (register reuse + ILP)
            constexpr std::size_t NB_Z = 64;
            for (std::size_t j_blk = 0; j_blk < n_cols; j_blk += NB_Z) {
                const std::size_t j_end = std::min(j_blk + NB_Z, n_cols);
                for (std::size_t ii = 0; ii < m; ++ii) {
                    const auto& fr = flat[ii];
                    if (fr.beta == T(0)) continue;
                    const T* __restrict vd = fr.vd;
                    const std::size_t k1 = fr.k + 1;
                    const std::size_t len = fr.len;
                    const T bt = fr.beta;
                    std::size_t j = j_blk;
                    for (; j + 4 <= j_end; j += 4) {
                        T* __restrict c0 = z_col_major + j * n;
                        T* __restrict c1 = c0 + n;
                        T* __restrict c2 = c1 + n;
                        T* __restrict c3 = c2 + n;
                        T d0 = T(0), d1 = T(0), d2 = T(0), d3 = T(0);
                        for (std::size_t i = 0; i < len; ++i) {
                            T vi = vd[i];
                            d0 += vi * c0[k1 + i];
                            d1 += vi * c1[k1 + i];
                            d2 += vi * c2[k1 + i];
                            d3 += vi * c3[k1 + i];
                        }
                        d0 *= bt; d1 *= bt; d2 *= bt; d3 *= bt;
                        for (std::size_t i = 0; i < len; ++i) {
                            T vi = vd[i];
                            c0[k1 + i] -= d0 * vi;
                            c1[k1 + i] -= d1 * vi;
                            c2[k1 + i] -= d2 * vi;
                            c3[k1 + i] -= d3 * vi;
                        }
                    }
                    for (; j < j_end; ++j) {
                        T* __restrict col = z_col_major + j * n;
                        T dot = T(0);
                        for (std::size_t i = 0; i < len; ++i)
                            dot += vd[i] * col[k1 + i];
                        const T factor = bt * dot;
                        for (std::size_t i = 0; i < len; ++i)
                            col[k1 + i] -= factor * vd[i];
                    }
                }
            }
        }

        /**
         * @brief Utility to explicitly construct a tridiagonal matrix
         *
         * @param d diagonal elements (size n)
         * @param e subdiagonal elements (size n-1)
         * @return Matrix<T> tridiagonal matrix (n×n)
         */
        template<typename T>
        Matrix<T> build_tridiagonal_matrix(const BaseVector<T>& d_basevec, const BaseVector<T>& e_basevec) {
            Vector<T> d(d_basevec);  // 1 copy via BaseVector ctor
            Vector<T> e(e_basevec);  // 1 copy via BaseVector ctor
            const size_t n = d.size();
            if (n > 1 && e.size() != n - 1) {
                assert(false && "DimensionError: build_tridiagonal_matrix: e.size() must equal d.size()-1");
                throw DimensionError("build_tridiagonal_matrix: e.size() must equal d.size()-1");
            }
            Matrix<T> T_mat(n, n, T(0));
            for (size_t i = 0; i < n; ++i)
                T_mat(i, i) = d[i];
            for (size_t i = 0; i + 1 < n; ++i) {
                T_mat(i, i + 1) = e[i];
                T_mat(i + 1, i) = e[i];
            }
            return T_mat;
        }

        // Function computing the eigenvalue decomposition of a symmetric matrix (cyclic Jacobi method)
        // Computes all eigenvalues and eigenvectors via orthogonal rotations
        // Input: A (n×n symmetric matrix)
        // Output: { eigenvalues (ascending), eigenvectors (column vectors) }
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        std::tuple<Vector<typename M::value_type>, Matrix<typename M::value_type>>
        symmetric_eigen(const M& A) {
            using T = typename M::value_type;
            const size_t n = A.rows();

            if (n == 0) return { Vector<T>(0), Matrix<T>(0, 0) };
            if (n == 1) return { Vector<T>{A(0, 0)}, Matrix<T>::identity(1) };

            const T eps = std::numeric_limits<T>::epsilon();
            const int max_sweep = 50;

            Matrix<T> V(n, n, T(0));
            for (size_t i = 0; i < n; ++i) V(i, i) = T(1);

            Matrix<T> S(n, n);
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    S(i, j) = A(i, j);

            for (int sweep = 0; sweep < max_sweep; ++sweep) {
                // Frobenius norm of the off-diagonal elements
                T off_norm = T(0);
                for (size_t i = 0; i < n; ++i)
                    for (size_t j = i + 1; j < n; ++j)
                        off_norm += S(i, j) * S(i, j);

                if (off_norm <= eps * eps) break;

                // Threshold (Rutishauser): skip small elements in the first 4 sweeps
                const T threshold = (sweep < 4)
                    ? T(0.2) * off_norm / T(n * n)
                    : T(0);

                // Cyclic Jacobi: scan all (p, q) pairs in row order
                for (size_t p = 0; p < n - 1; ++p) {
                    for (size_t q = p + 1; q < n; ++q) {
                        T apq = S(p, q);
                        T g = std::abs(apq);

                        // Skip if below the threshold
                        if (g * g <= threshold) continue;
                        // Also skip if already sufficiently small
                        if (g <= eps * (std::abs(S(p, p)) + std::abs(S(q, q)))) {
                            S(p, q) = S(q, p) = T(0);
                            continue;
                        }

                        // Stable rotation-parameter computation (avoids atan2/sin/cos)
                        T h = S(q, q) - S(p, p);
                        T t; // t = s/c = tan(θ)
                        if (std::abs(h) <= eps * g) {
                            t = (apq >= T(0)) ? T(1) : T(-1);
                        } else {
                            T tau = h / (T(2) * apq);
                            t = (tau >= T(0))
                                ? T(1) / (tau + std::sqrt(T(1) + tau * tau))
                                : T(-1) / (-tau + std::sqrt(T(1) + tau * tau));
                        }
                        T c = T(1) / std::sqrt(T(1) + t * t);
                        T s = t * c;
                        T tau_gs = s / (T(1) + c); // Givens parameter

                        // Update S: S = J^T S J (diagonal and matrix elements)
                        T dp = S(p, p) - t * apq;
                        T dq = S(q, q) + t * apq;
                        S(p, p) = dp;
                        S(q, q) = dq;
                        S(p, q) = S(q, p) = T(0);

                        for (size_t r = 0; r < n; ++r) {
                            if (r == p || r == q) continue;
                            T srp = S(r, p);
                            T srq = S(r, q);
                            S(r, p) = S(p, r) = srp - s * (srq + tau_gs * srp);
                            S(r, q) = S(q, r) = srq + s * (srp - tau_gs * srq);
                        }

                        // Update the eigenvectors
                        for (size_t r = 0; r < n; ++r) {
                            T vrp = V(r, p);
                            T vrq = V(r, q);
                            V(r, p) = vrp - s * (vrq + tau_gs * vrp);
                            V(r, q) = vrq + s * (vrp - tau_gs * vrq);
                        }
                    }
                }
            }

            // Extract the eigenvalues and sort in ascending order
            Vector<T> eigenvalues(n);
            for (size_t i = 0; i < n; ++i) eigenvalues[i] = S(i, i);

            // Selection sort (eigenvectors are reordered simultaneously)
            for (size_t i = 0; i < n - 1; ++i) {
                size_t mi = i;
                for (size_t j = i + 1; j < n; ++j)
                    if (eigenvalues[j] < eigenvalues[mi]) mi = j;
                if (mi != i) {
                    std::swap(eigenvalues[i], eigenvalues[mi]);
                    for (size_t r = 0; r < n; ++r)
                        std::swap(V(r, i), V(r, mi));
                }
            }

            return { eigenvalues, V };
        }

        /**
         * @brief Compute the singular value decomposition (SVD)
         * @tparam T        element type
         * @param a         matrix to decompose
         * @param transV    {true: return V^T, false: return V}
         * @return a tuple of {U, Σ, V^T}
         */

        // ---------------------------------------------------------------
        // Golub-Kahan SVD (bidiagonalization + implicit-shift QR iteration)
        //
        // The old implementation used Jacobi on A^T*A, squaring the condition number,
        // degrading singular-value accuracy to sqrt(eps) ≈ 1e-8 even for double.
        // This implementation bidiagonalizes A directly via Householder transformation and
        // obtains the singular values via Golub-Kahan QR iteration.
        // Reference: Golub & Van Loan "Matrix Computations" 4th ed. §8.6
        // ---------------------------------------------------------------

        namespace svd_detail {

            // Givens rotation: [c s; -s c]^T [a; b] = [r; 0]
            template<typename T>
            void givens(T a, T b, T& c, T& s) {
                const T z = numeric_traits<T>::zero();
                const T o = numeric_traits<T>::one();
                if (b == z) {
                    c = o; s = z;
                } else if (std::abs(b) > std::abs(a)) {
                    T tau = -a / b;
                    s = o / std::sqrt(o + tau * tau);
                    c = s * tau;
                } else {
                    T tau = -b / a;
                    c = o / std::sqrt(o + tau * tau);
                    s = c * tau;
                }
            }

            // ----- Symmetric tridiagonal QL iteration (base case of D&C) -----
            // d[0..n-1] diagonal, e[0..n-1] subdiagonal (e[n-1]=0 sentinel)
            // Q[n×n] row-major: initialized to the identity by the caller
            // Output: d = eigenvalues (ascending), columns of Q = eigenvectors
            template<typename T>
            void tridiag_ql_eig(T* __restrict d, T* __restrict e,
                                size_t n, T* __restrict Q) {
                const T eps = std::numeric_limits<T>::epsilon();
                const int max_iter = 30 * static_cast<int>(n);
                int total_iter = 0;

                for (size_t l = 0; l < n; ) {
                    size_t mi = l;
                    while (mi + 1 < n) {
                        if (std::abs(e[mi]) <= eps * (std::abs(d[mi]) + std::abs(d[mi + 1])))
                            break;
                        ++mi;
                    }
                    if (mi == l) { ++l; continue; }
                    if (++total_iter > max_iter) break;

                    T g = (d[l + 1] - d[l]) / (T(2) * e[l]);
                    T r = std::sqrt(g * g + T(1));
                    g = d[mi] - d[l] + e[l] / (g + ((g >= T(0)) ? r : -r));
                    T s = T(1), c = T(1), p = T(0);
                    bool lucky = false;

                    for (size_t ii = 0; ii < mi - l; ++ii) {
                        size_t i = mi - 1 - ii;
                        T f = s * e[i];
                        T b = c * e[i];
                        if (std::abs(f) >= std::abs(g)) {
                            c = g / f; r = std::sqrt(c * c + T(1));
                            e[i + 1] = f * r; s = T(1) / r; c *= s;
                        } else {
                            s = f / g; r = std::sqrt(s * s + T(1));
                            e[i + 1] = g * r; c = T(1) / r; s *= c;
                        }
                        if (e[i + 1] == T(0)) {
                            d[i + 1] -= p; e[mi] = T(0);
                            lucky = true; break;
                        }
                        g = d[i + 1] - p;
                        r = (d[i] - g) * s + T(2) * c * b;
                        p = s * r;
                        d[i + 1] = g + p;
                        g = c * r - b;

                        for (size_t k = 0; k < n; ++k) {
                            T qi = Q[k * n + i], qi1 = Q[k * n + i + 1];
                            Q[k * n + i + 1] = s * qi + c * qi1;
                            Q[k * n + i]     = c * qi - s * qi1;
                        }
                    }
                    if (!lucky) { d[l] -= p; e[l] = g; e[mi] = T(0); }
                }

                // Sort ascending + reorder Q's columns
                for (size_t i = 0; i + 1 < n; ++i) {
                    size_t mi = i;
                    for (size_t j = i + 1; j < n; ++j)
                        if (d[j] < d[mi]) mi = j;
                    if (mi != i) {
                        std::swap(d[i], d[mi]);
                        for (size_t k = 0; k < n; ++k)
                            std::swap(Q[k * n + i], Q[k * n + mi]);
                    }
                }
            }

            // ----- Secular equation solver -----
            // Solve f(λ) = 1 + ρ * Σ z2[i]/(d[i]-λ) = 0 within (lo, hi)
            // z2[i] = z[i]² (precomputed)
            template<typename T>
            T secular_solve(const T* d, const T* z2, T rho, size_t n,
                            T lo, T hi) {
                const T eps = std::numeric_limits<T>::epsilon();
                T lam = (lo + hi) / T(2);

                for (int iter = 0; iter < 100; ++iter) {
                    if (hi - lo <= T(4) * eps * (std::abs(hi) + std::abs(lo) + T(1)))
                        break;

                    T f = T(1), fp = T(0);
                    for (size_t i = 0; i < n; ++i) {
                        T diff = d[i] - lam;
                        if (std::abs(diff) < eps * eps) continue;
                        T t = z2[i] / diff;
                        f  += rho * t;
                        fp += rho * t / diff;
                    }

                    // Bracket update
                    if (f * rho < T(0)) lo = lam; else hi = lam;
                    if (std::abs(f) <= eps * T(n)) break;

                    // Newton step (with bracket guard)
                    if (std::abs(fp) > T(0)) {
                        T next = lam - f / fp;
                        lam = (next > lo && next < hi) ? next : (lo + hi) / T(2);
                    } else {
                        lam = (lo + hi) / T(2);
                    }
                }
                return lam;
            }

            // ----- Symmetric tridiagonal Divide-and-Conquer eigenvalue decomposition -----
            // d[0..n-1] diagonal (output: eigenvalues, ascending)
            // e[0..n-1] subdiagonal (e[n-1]=0 sentinel, destroyed)
            // Q[n×n] row-major: output is the eigenvector columns
            template<typename T>
            void tridiag_dc_eig(T* d, T* e, size_t n, T* Q) {
                constexpr size_t DC_BASE = 25;

                if (n <= DC_BASE) {
                    std::fill(Q, Q + n * n, T(0));
                    for (size_t i = 0; i < n; ++i) Q[i * n + i] = T(1);
                    tridiag_ql_eig(d, e, n, Q);
                    return;
                }

                const size_t k = n / 2, n2 = n - k;
                const T rho = e[k - 1];

                d[k - 1] -= rho;
                d[k]     -= rho;
                e[k - 1]  = T(0);

                // Recursion: partial eigenvector matrices
                std::vector<T> Q1(k * k), Q2(n2 * n2);
                tridiag_dc_eig(d, e, k, Q1.data());
                tridiag_dc_eig(d + k, e + k, n2, Q2.data());

                // z vector: last row of Q1 + first row of Q2
                std::vector<T> z(n);
                for (size_t j = 0; j < k;  ++j) z[j]     = Q1[(k - 1) * k + j];
                for (size_t j = 0; j < n2; ++j) z[k + j]  = Q2[j];

                // Merge-sort d[0..k-1] and d[k..n-1]
                std::vector<T> ds(n), zs(n);
                std::vector<size_t> perm(n);
                {
                    size_t a = 0, b = k, p = 0;
                    while (a < k && b < n) {
                        if (d[a] <= d[b]) { ds[p] = d[a]; zs[p] = z[a]; perm[p] = a; ++a; }
                        else              { ds[p] = d[b]; zs[p] = z[b]; perm[p] = b; ++b; }
                        ++p;
                    }
                    while (a < k) { ds[p] = d[a]; zs[p] = z[a]; perm[p] = a; ++a; ++p; }
                    while (b < n) { ds[p] = d[b]; zs[p] = z[b]; perm[p] = b; ++b; ++p; }
                }

                const T eps = std::numeric_limits<T>::epsilon();

                // Compute ||z|| and the maximum |d|
                T z_norm = T(0), d_max = T(0);
                for (size_t i = 0; i < n; ++i) {
                    z_norm += zs[i] * zs[i];
                    d_max = std::max(d_max, std::abs(ds[i]));
                }
                z_norm = std::sqrt(z_norm);

                std::vector<T> lam(n);
                std::vector<T> W(n * n, T(0));

                if (std::abs(rho) > eps && z_norm > eps) {
                    // === Deflation (equivalent to LAPACK dlaed2) ===
                    const T tol = T(8) * eps * std::max(d_max, std::abs(rho) * z_norm);

                    // deflated[i]: if true, the eigenvalue is ds[i] and the eigenvector is e_i
                    std::vector<bool> deflated(n, false);
                    size_t n_defl = 0;

                    // Type 1: |z[i]| is small → deflation
                    for (size_t i = 0; i < n; ++i) {
                        if (std::abs(zs[i]) <= tol) {
                            deflated[i] = true;
                            ++n_defl;
                        }
                    }

                    // Type 2: adjacent d[i] ≈ d[i+1] → eliminate z[i] via Givens
                    for (size_t i = 0; i + 1 < n; ++i) {
                        if (deflated[i]) continue;
                        if (std::abs(ds[i + 1] - ds[i]) <= tol) {
                            // Via Givens, z[i] → 0, z[i+1] → sqrt(z[i]²+z[i+1]²)
                            T r = std::sqrt(zs[i] * zs[i] + zs[i + 1] * zs[i + 1]);
                            if (r > T(0)) {
                                zs[i] = T(0);
                                zs[i + 1] = r;
                            }
                            deflated[i] = true;
                            ++n_defl;
                        }
                    }

                    // Indices of the non-deflated components
                    const size_t n_active = n - n_defl;
                    std::vector<size_t> active_idx(n_active);
                    {
                        size_t p = 0;
                        for (size_t i = 0; i < n; ++i)
                            if (!deflated[i]) active_idx[p++] = i;
                    }

                    // Set the deflated eigenvalues and eigenvectors
                    for (size_t i = 0; i < n; ++i) {
                        if (deflated[i]) {
                            lam[i] = ds[i];
                            W[i * n + i] = T(1);
                        }
                    }

                    if (n_active > 0) {
                        // Extract d, z of the non-deflated part
                        std::vector<T> da(n_active), za2(n_active), za(n_active);
                        for (size_t j = 0; j < n_active; ++j) {
                            da[j]  = ds[active_idx[j]];
                            za[j]  = zs[active_idx[j]];
                            za2[j] = za[j] * za[j];
                        }

                        // Solve the secular equation (n_active eigenvalues)
                        T za_norm2 = T(0);
                        for (size_t j = 0; j < n_active; ++j) za_norm2 += za2[j];

                        std::vector<T> la(n_active);
                        for (size_t jj = 0; jj < n_active; ++jj) {
                            T lo, hi;
                            if (rho > T(0)) {
                                lo = da[jj];
                                hi = (jj + 1 < n_active) ? da[jj + 1]
                                                          : da[n_active - 1] + rho * za_norm2;
                            } else {
                                hi = da[jj];
                                lo = (jj > 0) ? da[jj - 1]
                                              : da[0] + rho * za_norm2;
                            }
                            la[jj] = secular_solve(da.data(), za2.data(), rho,
                                                   n_active, lo, hi);
                            lam[active_idx[jj]] = la[jj];
                        }

                        // === Gu-Eisenstat z recomputation (equivalent to LAPACK dlaed3) ===
                        // z̃[i]² = Π_j (λ[j]-d[i]) / Π_{k≠i} (d[k]-d[i])
                        // Compute in log space to avoid overflow
                        std::vector<T> z_new(n_active);
                        for (size_t i = 0; i < n_active; ++i) {
                            T log_abs = T(0);
                            int sign_prod = 1;
                            for (size_t j = 0; j < n_active; ++j) {
                                T v = la[j] - da[i];
                                if (v < T(0)) { sign_prod = -sign_prod; v = -v; }
                                log_abs += std::log(std::max(v, std::numeric_limits<T>::min()));
                            }
                            for (size_t j = 0; j < n_active; ++j) {
                                if (j == i) continue;
                                T v = da[j] - da[i];
                                if (v < T(0)) { sign_prod = -sign_prod; v = -v; }
                                log_abs -= std::log(std::max(v, std::numeric_limits<T>::min()));
                            }
                            // ratio = sign_prod * exp(log_abs), should be positive
                            T abs_val = std::exp(log_abs / T(2));
                            z_new[i] = std::copysign(abs_val, za[i]);
                        }

                        // Eigenvectors: w[j][i] = z_new[i] / (d[i] - λ[j])
                        for (size_t jj = 0; jj < n_active; ++jj) {
                            const size_t out_idx = active_idx[jj];
                            T norm2 = T(0);
                            for (size_t ii = 0; ii < n_active; ++ii) {
                                T diff = da[ii] - la[jj];
                                T wi = (std::abs(diff) > eps * eps)
                                       ? z_new[ii] / diff : T(0);
                                W[active_idx[ii] * n + out_idx] = wi;
                                norm2 += wi * wi;
                            }
                            if (norm2 > T(0)) {
                                T inv = T(1) / std::sqrt(norm2);
                                for (size_t ii = 0; ii < n_active; ++ii)
                                    W[active_idx[ii] * n + out_idx] *= inv;
                            }
                        }
                    }

                    // === Sort ascending (lam[]) ===
                    // The eigenvalue order may be disturbed after the secular equation/deflation
                    // → sort and also swap W's columns
                    std::vector<size_t> sort_idx(n);
                    std::iota(sort_idx.begin(), sort_idx.end(), 0);
                    std::sort(sort_idx.begin(), sort_idx.end(),
                              [&](size_t a, size_t b) { return lam[a] < lam[b]; });

                    std::vector<T> lam_sorted(n);
                    std::vector<T> W_sorted(n * n, T(0));
                    for (size_t j = 0; j < n; ++j) {
                        lam_sorted[j] = lam[sort_idx[j]];
                        for (size_t i = 0; i < n; ++i)
                            W_sorted[i * n + j] = W[i * n + sort_idx[j]];
                    }
                    lam = std::move(lam_sorted);
                    W = std::move(W_sorted);

                } else {
                    // ρ ≈ 0: merge only
                    for (size_t i = 0; i < n; ++i) {
                        lam[i] = ds[i];
                        W[i * n + i] = T(1);
                    }
                }

                // Restore W's rows to the original order: WU[perm[i], :] = W[i, :]
                std::vector<T> WU(n * n, T(0));
                for (size_t i = 0; i < n; ++i) {
                    const size_t oi = perm[i];
                    for (size_t j = 0; j < n; ++j)
                        WU[oi * n + j] = W[i * n + j];
                }

                // Output the eigenvalues
                for (size_t i = 0; i < n; ++i) d[i] = lam[i];

                // Merge: Q = [Q1 0; 0 Q2] * WU (gemm)
                std::fill(Q, Q + n * n, T(0));
                DefaultComputePolicy<T>::gemm(
                    Q1.data(), WU.data(), Q,
                    k, n, k, k, n, n);
                DefaultComputePolicy<T>::gemm(
                    Q2.data(), WU.data() + k * n, Q + k * n,
                    n2, n, n2, n2, n, n);
            }

            // ----- Golub-Kahan QR iteration (bidiagonal SVD) -----
            // d[0..n-1] diagonal elements (in/out: singular values)
            // e[0..n-1] superdiagonal elements (e[0]=0, destroyed)
            // Uc[n*n], Vc[n*n] column-major: assumed initialized to the identity
            template<typename T>
            void bidiag_qr_svd(T* d, T* e, size_t n, T* Uc, T* Vc) {
                const T eps = std::numeric_limits<T>::epsilon();
                const int max_iter = 100 * static_cast<int>(n);

                // Correct negative diagonal elements to positive
                for (size_t i = 0; i < n; ++i) {
                    if (d[i] < T(0)) {
                        d[i] = -d[i];
                        if (i + 1 < n) e[i + 1] = -e[i + 1];
                        T* col = &Uc[i * n];
                        for (size_t j = 0; j < n; ++j) col[j] = -col[j];
                    }
                }

                // Column-major Givens helper (contiguous memory access)
                auto givens_cols = [&](T* __restrict mat, size_t dim, size_t c1, size_t c2, T c, T s) {
                    T* __restrict col1 = mat + c1 * dim;
                    T* __restrict col2 = mat + c2 * dim;
                    size_t i = 0;
#ifdef __AVX2__
                    if constexpr (std::is_same_v<T, double>) {
                        __m256d vc = _mm256_set1_pd(c);
                        __m256d vs = _mm256_set1_pd(s);
                        for (; i + 4 <= dim; i += 4) {
                            __m256d a = _mm256_loadu_pd(col1 + i);
                            __m256d b = _mm256_loadu_pd(col2 + i);
                            _mm256_storeu_pd(col1 + i, _mm256_add_pd(_mm256_mul_pd(vc, a), _mm256_mul_pd(vs, b)));
                            _mm256_storeu_pd(col2 + i, _mm256_sub_pd(_mm256_mul_pd(vc, b), _mm256_mul_pd(vs, a)));
                        }
                    }
#endif
                    for (; i < dim; ++i) {
                        T a = col1[i], b = col2[i];
                        col1[i] =  c * a + s * b;
                        col2[i] =  c * b - s * a;
                    }
                };

                for (int iter = 0; iter < max_iter; ++iter) {
                    for (size_t i = 1; i < n; ++i) {
                        if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i])))
                            e[i] = T(0);
                    }

                    size_t q = n;
                    while (q > 1 && e[q - 1] == T(0)) --q;
                    if (q <= 1) break;

                    size_t p = q - 1;
                    while (p > 0 && e[p] != T(0)) --p;

                    // Deflation of zero diagonal elements
                    bool handled_zero = false;
                    for (size_t i = p; i < q; ++i) {
                        T ref = T(0);
                        if (i + 1 < n && e[i + 1] != T(0)) ref = std::max(ref, std::abs(e[i + 1]));
                        if (i > 0 && e[i] != T(0)) ref = std::max(ref, std::abs(e[i]));
                        if (ref == T(0)) continue;
                        if (std::abs(d[i]) > eps * ref) continue;

                        d[i] = T(0);
                        handled_zero = true;

                        if (i + 1 < q && e[i + 1] != T(0)) {
                            T bulge = e[i + 1];
                            e[i + 1] = T(0);
                            for (size_t j = i + 1; j < q; ++j) {
                                T c, s;
                                givens(d[j], bulge, c, s);
                                d[j] = c * d[j] - s * bulge;
                                if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; }
                                givens_cols(Uc, n, j, i, c, -s);
                                if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break;
                            }
                        } else if (i > p && e[i] != T(0)) {
                            T bulge = e[i];
                            e[i] = T(0);
                            for (size_t jj = 0; jj < i - p; ++jj) {
                                size_t j = i - 1 - jj;
                                T c, s;
                                givens(d[j], bulge, c, s);
                                d[j] = c * d[j] - s * bulge;
                                if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; }
                                givens_cols(Vc, n, j, i, c, -s);
                                if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break;
                            }
                        }
                        break;
                    }
                    if (handled_zero) continue;

                    // Wilkinson shift
                    T d_qm1 = d[q - 1], d_qm2 = d[q - 2];
                    T e_qm1 = e[q - 1];
                    T e_qm2 = (q >= 3 && q - 2 > p) ? e[q - 2] : T(0);
                    T t11 = d_qm2 * d_qm2 + e_qm2 * e_qm2;
                    T t12 = d_qm2 * e_qm1;
                    T t22 = d_qm1 * d_qm1 + e_qm1 * e_qm1;
                    T delta = (t11 - t22) / T(2);
                    T mu = t22 - t12 * t12 / (delta + (delta >= T(0) ? T(1) : T(-1))
                        * std::sqrt(delta * delta + t12 * t12));

                    T x = d[p] * d[p] - mu;
                    T z = d[p] * e[p + 1];

                    for (size_t k = p; k < q - 1; ++k) {
                        T c, s;
                        givens(x, z, c, s);
                        if (k > p) e[k] = c * e[k] - s * z;
                        T dk = d[k], dk1 = d[k + 1], ek1 = e[k + 1];
                        d[k]     =  c * dk  - s * ek1;
                        e[k + 1] =  s * dk  + c * ek1;
                        T bulge  = -s * dk1;
                        d[k + 1] =  c * dk1;
                        givens_cols(Vc, n, k, k + 1, c, -s);

                        x = d[k]; z = bulge;
                        givens(x, z, c, s);
                        d[k]     = c * x - s * z;
                        T ek1_new = e[k + 1];
                        e[k + 1] =  c * ek1_new - s * d[k + 1];
                        d[k + 1] =  s * ek1_new + c * d[k + 1];
                        if (k + 2 < q) { z = -s * e[k + 2]; e[k + 2] = c * e[k + 2]; }
                        givens_cols(Uc, n, k, k + 1, c, -s);

                        x = e[k + 1];
                    }
                }
            }

        } // namespace svd_detail

        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        std::tuple<Matrix<typename M::value_type>, Vector<typename M::value_type>, Matrix<typename M::value_type>>
        svd_decomposition(const M& A, bool transV = true) {
            using T = typename M::value_type;
            // If called with something other than Matrix<T> (e.g. StaticMatrix), materialize and
            // delegate to the Matrix<T> path (avoiding dependencies such as .transpose()).
            if constexpr (!std::is_same_v<M, Matrix<T>>) {
                Matrix<T> A_dyn(A.rows(), A.cols());
                for (std::size_t i = 0; i < A.rows(); ++i)
                    for (std::size_t j = 0; j < A.cols(); ++j)
                        A_dyn(i, j) = A(i, j);
                return svd_decomposition(A_dyn, transV);
            } else {
            const size_t m = A.rows();
            const size_t n = A.cols();
            const size_t min_mn = std::min(m, n);

            if (min_mn == 0) {
                return { Matrix<T>(m, min_mn), Vector<T>(0), Matrix<T>(n, n) };
            }

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                // thin U + full V^T:
                //   U: m × min(m,n) (thin, expected shape of test BDCSVD.Tall350x250)
                //   Vt: n × n (full, needed for padeHermite's null-space extraction)
                // Even for wide (m < n), LAPACK can handle it directly, so transpose recursion is unnecessary.
                // The old m<n recursion path returned a thin U^T, dropping the null-space columns of V_A,
                // which broke padeHermite.
                Matrix<T> Acopy = A;
                Vector<T> s(min_mn);
                Matrix<T> U(m, min_mn, T(0));
                Matrix<T> Vt(n, n, T(0));
                lapack_int info;

                // For large matrices, divide-and-conquer SVD (gesdd) is faster than Golub-Reinsch (gesvd).
                // gesdd has no jobu/jobvt and is controlled by a single jobz, so to obtain the desired
                // "thin U + full Vt", split into cases as follows:
                //   m >= n: jobz='S' → U is m×min(=m×n, thin), Vt is min×n(=n×n, full)
                //   m <  n: jobz='A' → U is m×m(min=m so coincides with thin), Vt is n×n(full)
                // In both cases U=m×min_mn(ldu=min_mn), Vt=n×n(ldvt=n), returning the same shape
                // as the gesvd path.
                constexpr size_t GESDD_CUTOFF = 1000;
                const bool use_dc = (min_mn >= GESDD_CUTOFF);

                if (use_dc) {
                    const char jobz = (m >= n) ? 'S' : 'A';
                    if constexpr (std::is_same_v<T, float>)
                        info = LAPACKE_sgesdd(LAPACK_ROW_MAJOR, jobz,
                                              (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n,
                                              s.data(), U.data(), (lapack_int)min_mn,
                                              Vt.data(), (lapack_int)n);
                    else
                        info = LAPACKE_dgesdd(LAPACK_ROW_MAJOR, jobz,
                                              (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n,
                                              s.data(), U.data(), (lapack_int)min_mn,
                                              Vt.data(), (lapack_int)n);
                    if (info > 0)
                        throw MathError("svd_decomposition: LAPACKE_gesdd did not converge");
                } else {
                    // jobu='S', jobvt='A'
                    std::vector<T> superb(min_mn > 0 ? min_mn - 1 : 0);
                    if constexpr (std::is_same_v<T, float>)
                        info = LAPACKE_sgesvd(LAPACK_ROW_MAJOR, 'S', 'A',
                                              (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n,
                                              s.data(), U.data(), (lapack_int)min_mn,
                                              Vt.data(), (lapack_int)n, superb.data());
                    else
                        info = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, 'S', 'A',
                                              (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n,
                                              s.data(), U.data(), (lapack_int)min_mn,
                                              Vt.data(), (lapack_int)n, superb.data());
                    if (info > 0)
                        throw MathError("svd_decomposition: LAPACKE_gesvd did not converge");
                }

                if (transV)
                    return { U, s, Vt };
                else
                    return { U, s, Vt.transpose() };
            }
#endif

            // No MKL: old m < n recursion path (without LAPACK there is no direct way to compute
            // thin U + full V^T, so compute via transpose)
            if (m < n) {
                auto [V, s, Ut] = svd_decomposition(A.transpose(), true);
                if (transV)
                    return { Ut.transpose(), s, V.transpose() };
                else
                    return { Ut.transpose(), s, V };
            }

            // ===============================================================
            // float/double fast path: lazy accumulation + column-major QR + WY back-transformation
            // ===============================================================
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {

            Matrix<T> W(A);
            std::vector<T> d(n, T(0));
            std::vector<T> e(n, T(0));
            std::vector<T> tau_left(n, T(0));
            std::vector<T> tau_right(n, T(0));

            // Phase 1: Householder bidiagonalization
            // Left reflector: normalized vector in W(k+1:m, k), tau_left[k]
            // Right reflector: normalized vector in W(k, k+2:n), tau_right[k]
#ifdef SANGI_SVD_PHASE_TIMING
            auto _svd_t0 = std::chrono::high_resolution_clock::now();
#endif
            T* __restrict wdata = W.data();
            std::vector<T> _hh_work(n);
            for (size_t k = 0; k < n; ++k) {
                // --- Left Householder ---
                {
                    T sigma = T(0);
                    for (size_t i = k + 1; i < m; ++i)
                        sigma += wdata[i * n + k] * wdata[i * n + k];

                    if (sigma == T(0) && wdata[k * n + k] >= T(0)) {
                        tau_left[k] = T(0);
                        d[k] = wdata[k * n + k];
                    } else {
                        T x0 = wdata[k * n + k];
                        T x_norm = std::sqrt(x0 * x0 + sigma);
                        T alpha = (x0 >= T(0)) ? -x_norm : x_norm;
                        T v0 = x0 - alpha;
                        tau_left[k] = T(2) * v0 * v0 / (v0 * v0 + sigma);
                        T inv_v0 = T(1) / v0;
                        for (size_t i = k + 1; i < m; ++i)
                            wdata[i * n + k] *= inv_v0;

                        // Apply to W(k:m, k+1:n): H = I - tau * v * v^T
                        T tau_k = tau_left[k];
                        const size_t ncols = n - k - 1;
                        {
                            T* __restrict w = _hh_work.data();
                            for (size_t c = 0; c < ncols; ++c)
                                w[c] = wdata[k * n + (k + 1 + c)];
                            for (size_t i = k + 1; i < m; ++i) {
                                T vi = wdata[i * n + k];
                                computation::simd::axpy_simd<T>(w, vi, &wdata[i * n + k + 1], ncols);
                            }
                            for (size_t c = 0; c < ncols; ++c) w[c] *= tau_k;
                            for (size_t c = 0; c < ncols; ++c)
                                wdata[k * n + (k + 1 + c)] -= w[c];
                            for (size_t i = k + 1; i < m; ++i) {
                                T vi = wdata[i * n + k];
                                computation::simd::axpy_simd<T>(&wdata[i * n + k + 1], -vi, w, ncols);
                            }
                        }
                        d[k] = alpha;
                        wdata[k * n + k] = alpha;
                    }
                }
                // --- Right Householder (SIMD dot product) ---
                if (k + 2 <= n - 1) {
                    T sigma = T(0);
                    for (size_t jj = k + 2; jj < n; ++jj)
                        sigma += wdata[k * n + jj] * wdata[k * n + jj];

                    if (sigma == T(0) && wdata[k * n + k + 1] >= T(0)) {
                        tau_right[k] = T(0);
                        e[k + 1] = wdata[k * n + k + 1];
                    } else {
                        T x0 = wdata[k * n + k + 1];
                        T x_norm = std::sqrt(x0 * x0 + sigma);
                        T alpha = (x0 >= T(0)) ? -x_norm : x_norm;
                        T v0 = x0 - alpha;
                        tau_right[k] = T(2) * v0 * v0 / (v0 * v0 + sigma);
                        T inv_v0 = T(1) / v0;
                        for (size_t jj = k + 2; jj < n; ++jj)
                            wdata[k * n + jj] *= inv_v0;

                        // W(k+1:m, k+1:n) -= tau * (A*v) * v^T (SIMD dot + axpy)
                        T tau_k = tau_right[k];
                        const size_t rcols = n - k - 2;
                        const T* __restrict vrow = &wdata[k * n + k + 2];
                        using PT = PacketTraits<T>;
                        constexpr size_t PS = PT::size;
                        for (size_t i = k + 1; i < m; ++i) {
                            T* __restrict row_i = &wdata[i * n + k + 1];
                            // SIMD dot product: row_i[0] + sum(vrow[c]*row_i[1+c])
                            T dot = row_i[0]; // v[0]=1
                            if constexpr (PS > 1) {
                                auto acc = PT::set1(T(0));
                                size_t c = 0;
                                for (; c + PS <= rcols; c += PS)
                                    acc = PT::fmadd(PT::load(vrow + c), PT::load(row_i + 1 + c), acc);
                                dot += PT::reduce_add(acc);
                                for (; c < rcols; ++c)
                                    dot += vrow[c] * row_i[1 + c];
                            } else {
                                for (size_t c = 0; c < rcols; ++c)
                                    dot += vrow[c] * row_i[1 + c];
                            }
                            T factor = tau_k * dot;
                            row_i[0] -= factor;
                            if (rcols > 0)
                                computation::simd::axpy_simd<T>(row_i + 1, -factor, vrow, rcols);
                        }
                        e[k + 1] = alpha;
                    }
                } else if (k + 1 < n) {
                    e[k + 1] = W(k, k + 1);
                }
            }

#ifdef SANGI_SVD_PHASE_TIMING
            auto _svd_t1 = std::chrono::high_resolution_clock::now();
#endif
            // Phase 2: bidiagonal → singular values and singular vectors
            const T eps = std::numeric_limits<T>::epsilon();

            // Column-major n×n matrices (used in Phase 3)
            std::vector<T> Uc(n * n, T(0)); // U_bidiag column-major
            std::vector<T> Vc(n * n, T(0)); // V_bidiag column-major
            for (size_t i = 0; i < n; ++i) { Uc[i * n + i] = T(1); Vc[i * n + i] = T(1); }

            constexpr size_t DC_SVD_CUTOFF = 200;
            if (n > DC_SVD_CUTOFF) {
                // ===== BDCSVD: B^T*B → symmetric tridiagonal D&C =====
                // Save the original bidiagonal elements
                std::vector<T> bd(d.begin(), d.begin() + n);
                std::vector<T> be(e.begin(), e.begin() + n);

                // T = B^T*B: symmetric tridiagonal
                std::vector<T> td(n), te(n, T(0));
                for (size_t i = 0; i < n; ++i)
                    td[i] = bd[i] * bd[i] + be[i] * be[i]; // be[0]=0
                for (size_t i = 0; i + 1 < n; ++i)
                    te[i] = bd[i] * be[i + 1];

                // D&C eigenvalue decomposition: td → eigenvalues (ascending), Q_eig → eigenvectors
                std::vector<T> Q_eig(n * n);
                svd_detail::tridiag_dc_eig(td.data(), te.data(), n, Q_eig.data());

                // Singular values: σ = sqrt(λ)
                for (size_t i = 0; i < n; ++i)
                    d[i] = std::sqrt(std::max(td[i], T(0)));

                // V = Q_eig → Vc (column-major), U = B*V*Σ^{-1} → Uc (column-major)
                for (size_t j = 0; j < n; ++j) {
                    T sigma = d[j];

                    // V → Vc
                    for (size_t i = 0; i < n; ++i)
                        Vc[j * n + i] = Q_eig[i * n + j];

                    // U = B*V*Σ^{-1}: (B*v)_i = bd[i]*v_{i,j} + be[i+1]*v_{i+1,j}
                    if (sigma > eps) {
                        T inv_sigma = T(1) / sigma;
                        for (size_t i = 0; i < n; ++i) {
                            T bv = bd[i] * Q_eig[i * n + j];
                            if (i + 1 < n) bv += be[i + 1] * Q_eig[(i + 1) * n + j];
                            Uc[j * n + i] = bv * inv_sigma;
                        }
                    }
                    // For σ ≈ 0, column j of Uc remains 0 (initialized from identity)
                }
            } else {
            // ===== Golub-Kahan QR iteration =====
            const int max_iter = 100 * static_cast<int>(n);

            // Correct negative diagonal elements to positive
            for (size_t i = 0; i < n; ++i) {
                if (d[i] < T(0)) {
                    d[i] = -d[i];
                    if (i + 1 < n) e[i + 1] = -e[i + 1];
                    // Negate column i of Uc
                    T* col = &Uc[i * n];
                    for (size_t j = 0; j < n; ++j) col[j] = -col[j];
                }
            }

            // Column-major Givens helper (contiguous memory access)
            auto givens_cols = [&](T* __restrict mat, size_t dim, size_t c1, size_t c2, T c, T s) {
                T* __restrict col1 = mat + c1 * dim;
                T* __restrict col2 = mat + c2 * dim;
                size_t i = 0;
#ifdef __AVX2__
                if constexpr (std::is_same_v<T, double>) {
                    __m256d vc = _mm256_set1_pd(c);
                    __m256d vs = _mm256_set1_pd(s);
                    for (; i + 4 <= dim; i += 4) {
                        __m256d a = _mm256_loadu_pd(col1 + i);
                        __m256d b = _mm256_loadu_pd(col2 + i);
                        _mm256_storeu_pd(col1 + i, _mm256_add_pd(_mm256_mul_pd(vc, a), _mm256_mul_pd(vs, b)));
                        _mm256_storeu_pd(col2 + i, _mm256_sub_pd(_mm256_mul_pd(vc, b), _mm256_mul_pd(vs, a)));
                    }
                }
#endif
                for (; i < dim; ++i) {
                    T a = col1[i], b = col2[i];
                    col1[i] =  c * a + s * b;
                    col2[i] =  c * b - s * a;
                }
            };

            for (int iter = 0; iter < max_iter; ++iter) {
                // Convergence test
                for (size_t i = 1; i < n; ++i) {
                    if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i])))
                        e[i] = T(0);
                }

                size_t q = n;
                while (q > 1 && e[q - 1] == T(0)) --q;
                if (q <= 1) break;

                size_t p = q - 1;
                while (p > 0 && e[p] != T(0)) --p;

                // Deflation of zero diagonal elements
                bool handled_zero = false;
                for (size_t i = p; i < q; ++i) {
                    T ref = T(0);
                    if (i + 1 < n && e[i + 1] != T(0)) ref = std::max(ref, std::abs(e[i + 1]));
                    if (i > 0 && e[i] != T(0)) ref = std::max(ref, std::abs(e[i]));
                    if (ref == T(0)) continue;
                    if (std::abs(d[i]) > eps * ref) continue;

                    d[i] = T(0);
                    handled_zero = true;

                    if (i + 1 < q && e[i + 1] != T(0)) {
                        T bulge = e[i + 1];
                        e[i + 1] = T(0);
                        for (size_t j = i + 1; j < q; ++j) {
                            T c, s;
                            svd_detail::givens(d[j], bulge, c, s);
                            d[j] = c * d[j] - s * bulge;
                            if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; }
                            givens_cols(Uc.data(), n, j, i, c, -s);
                            if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break;
                        }
                    } else if (i > p && e[i] != T(0)) {
                        T bulge = e[i];
                        e[i] = T(0);
                        for (size_t jj = 0; jj < i - p; ++jj) {
                            size_t j = i - 1 - jj;
                            T c, s;
                            svd_detail::givens(d[j], bulge, c, s);
                            d[j] = c * d[j] - s * bulge;
                            if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; }
                            givens_cols(Vc.data(), n, j, i, c, -s);
                            if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break;
                        }
                    }
                    break;
                }
                if (handled_zero) continue;

                // Wilkinson shift
                T d_qm1 = d[q - 1], d_qm2 = d[q - 2];
                T e_qm1 = e[q - 1];
                T e_qm2 = (q >= 3 && q - 2 > p) ? e[q - 2] : T(0);
                T t11 = d_qm2 * d_qm2 + e_qm2 * e_qm2;
                T t12 = d_qm2 * e_qm1;
                T t22 = d_qm1 * d_qm1 + e_qm1 * e_qm1;
                T delta = (t11 - t22) / T(2);
                T mu = t22 - t12 * t12 / (delta + (delta >= T(0) ? T(1) : T(-1))
                    * std::sqrt(delta * delta + t12 * t12));

                T x = d[p] * d[p] - mu;
                T z = d[p] * e[p + 1];

                for (size_t k = p; k < q - 1; ++k) {
                    // Right Givens (columns k, k+1)
                    T c, s;
                    svd_detail::givens(x, z, c, s);
                    if (k > p) e[k] = c * e[k] - s * z;
                    T dk = d[k], dk1 = d[k + 1], ek1 = e[k + 1];
                    d[k]     =  c * dk  - s * ek1;
                    e[k + 1] =  s * dk  + c * ek1;
                    T bulge  = -s * dk1;
                    d[k + 1] =  c * dk1;
                    givens_cols(Vc.data(), n, k, k + 1, c, -s);

                    // Left Givens (rows k, k+1)
                    x = d[k]; z = bulge;
                    svd_detail::givens(x, z, c, s);
                    d[k]     = c * x - s * z;
                    T ek1_new = e[k + 1];
                    e[k + 1] =  c * ek1_new - s * d[k + 1];
                    d[k + 1] =  s * ek1_new + c * d[k + 1];
                    if (k + 2 < q) { z = -s * e[k + 2]; e[k + 2] = c * e[k + 2]; }
                    givens_cols(Uc.data(), n, k, k + 1, c, -s);

                    x = e[k + 1];
                }
            }
            } // end else (QR iteration)

#ifdef SANGI_SVD_PHASE_TIMING
            auto _svd_t2 = std::chrono::high_resolution_clock::now();
#endif
            // Phase 3: back-transformation
            // Negative singular values to positive
            for (size_t i = 0; i < n; ++i) {
                if (d[i] < T(0)) {
                    d[i] = -d[i];
                    T* col = &Uc[i * n];
                    for (size_t j = 0; j < n; ++j) col[j] = -col[j];
                }
            }

            // Sort descending (column-major, so swap columns)
            for (size_t i = 0; i < n - 1; ++i) {
                size_t max_idx = i;
                for (size_t j = i + 1; j < n; ++j)
                    if (d[j] > d[max_idx]) max_idx = j;
                if (max_idx != i) {
                    std::swap(d[i], d[max_idx]);
                    // Swap columns of Uc, Vc
                    T* u1 = &Uc[i * n], *u2 = &Uc[max_idx * n];
                    T* v1 = &Vc[i * n], *v2 = &Vc[max_idx * n];
                    for (size_t j = 0; j < n; ++j) { std::swap(u1[j], u2[j]); std::swap(v1[j], v2[j]); }
                }
            }

            // Convert Uc, Vc to row-major → n×n U_bidiag, V_bidiag
            Matrix<T> Ub(n, n);
            Matrix<T> Vb(n, n);
            for (size_t i = 0; i < n; ++i)
                for (size_t j = 0; j < n; ++j) {
                    Ub(i, j) = Uc[j * n + i]; // column-major→row-major
                    Vb(i, j) = Vc[j * n + i];
                }

            // U = Q_left * Ub: apply the left Householder reflectors to Ub in reverse order to expand to m×n
            // result (m×n): first n rows = Ub, remaining m-n rows = 0
            Matrix<T> U_out(m, n, T(0));
            for (size_t i = 0; i < n; ++i)
                for (size_t j = 0; j < n; ++j)
                    U_out(i, j) = Ub(i, j);

            // Apply the left reflectors via WY blocks in reverse order: H_{n-1}, ..., H_0
            // Q = H_0*H_1*...*H_{n-1}, applied from the right by block
            {
                T* __restrict udata = U_out.data();
                constexpr size_t NB_BT = 32;
                size_t k_end = n;
                while (k_end > 0) {
                    const size_t nb = std::min(NB_BT, k_end);
                    const size_t k_start = k_end - nb;
                    const size_t pr = m - k_start;
                    const size_t cols = n;

                    // T factor (nb × nb upper triangular)
                    std::vector<T> T_fac(nb * nb, T(0));
                    for (size_t jj = 0; jj < nb; ++jj) {
                        const size_t kj = k_start + jj;
                        T_fac[jj * nb + jj] = tau_left[kj];
                        if (tau_left[kj] == T(0)) continue;
                        for (size_t kk = 0; kk < jj; ++kk) {
                            const size_t kc = k_start + kk;
                            T dot = W(kj, kc);
                            for (size_t i = jj + 1; i < pr; ++i)
                                dot += W(k_start + i, kc) * W(k_start + i, kj);
                            T_fac[kk * nb + jj] = dot;
                        }
                        for (size_t kk = 0; kk < jj; ++kk) {
                            T sum = T(0);
                            for (size_t ll = kk; ll < jj; ++ll)
                                sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj];
                            T_fac[kk * nb + jj] = -tau_left[kj] * sum;
                        }
                    }

                    // V^T (nb × pr) and V (pr × nb), row-major
                    std::vector<T> VT_rm(nb * pr, T(0));
                    std::vector<T> V_rm(pr * nb, T(0));
                    for (size_t jj = 0; jj < nb; ++jj) {
                        VT_rm[jj * pr + jj] = T(1);
                        V_rm[jj * nb + jj] = T(1);
                        for (size_t i = jj + 1; i < pr; ++i) {
                            T val = W(k_start + i, k_start + jj);
                            VT_rm[jj * pr + i] = val;
                            V_rm[i * nb + jj] = val;
                        }
                    }

                    // (1) Wbuf = V^T * U_sub  (nb × cols)
                    std::vector<T> Wbuf(nb * cols, T(0));
                    DefaultComputePolicy<T>::gemm(
                        VT_rm.data(), udata + k_start * cols, Wbuf.data(),
                        nb, cols, pr, pr, cols, cols);

                    // (2) Wbuf = T * Wbuf (upper triangular, top to bottom)
                    for (size_t j = 0; j < nb; ++j) {
                        T t_jj = T_fac[j * nb + j];
                        T* w_row = &Wbuf[j * cols];
                        if (t_jj != T(1)) {
                            for (size_t c = 0; c < cols; ++c) w_row[c] *= t_jj;
                        }
                        for (size_t l = j + 1; l < nb; ++l) {
                            T t_jl = T_fac[j * nb + l];
                            if (t_jl == T(0)) continue;
                            computation::simd::axpy_simd<T>(
                                w_row, t_jl, &Wbuf[l * cols], cols);
                        }
                    }

                    // (3) U_sub -= V * Wbuf
                    for (size_t i = 0; i < nb * cols; ++i) Wbuf[i] = -Wbuf[i];
                    DefaultComputePolicy<T>::gemm(
                        V_rm.data(), Wbuf.data(), udata + k_start * cols,
                        pr, cols, nb, nb, cols, cols);

                    k_end = k_start;
                }
            }

            // Apply the right reflectors via WY blocks in reverse order: G_{n-3}, ..., G_0
            {
                T* __restrict vdata = Vb.data();
                constexpr size_t NB_BT = 32;
                if (n >= 3) {
                    const size_t num_ref = n - 2;
                    size_t k_end = num_ref;
                    while (k_end > 0) {
                        const size_t nb = std::min(NB_BT, k_end);
                        const size_t k_start = k_end - nb;
                        const size_t start_row = k_start + 1;
                        const size_t pr = n - start_row;
                        const size_t cols = n;

                        // T factor (nb × nb upper triangular)
                        std::vector<T> T_fac(nb * nb, T(0));
                        for (size_t jj = 0; jj < nb; ++jj) {
                            const size_t kj = k_start + jj;
                            T_fac[jj * nb + jj] = tau_right[kj];
                            if (tau_right[kj] == T(0)) continue;
                            for (size_t kk = 0; kk < jj; ++kk) {
                                const size_t kc = k_start + kk;
                                T dot = W(kc, start_row + jj);
                                for (size_t i = jj + 1; i < pr; ++i)
                                    dot += W(kc, start_row + i) * W(kj, start_row + i);
                                T_fac[kk * nb + jj] = dot;
                            }
                            for (size_t kk = 0; kk < jj; ++kk) {
                                T sum = T(0);
                                for (size_t ll = kk; ll < jj; ++ll)
                                    sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj];
                                T_fac[kk * nb + jj] = -tau_right[kj] * sum;
                            }
                        }

                        // V^T (nb × pr) and V (pr × nb)
                        std::vector<T> VT_rm(nb * pr, T(0));
                        std::vector<T> V_rm(pr * nb, T(0));
                        for (size_t jj = 0; jj < nb; ++jj) {
                            const size_t kj = k_start + jj;
                            VT_rm[jj * pr + jj] = T(1);
                            V_rm[jj * nb + jj] = T(1);
                            for (size_t i = jj + 1; i < pr; ++i) {
                                T val = W(kj, start_row + i);
                                VT_rm[jj * pr + i] = val;
                                V_rm[i * nb + jj] = val;
                            }
                        }

                        // (1) Wbuf = V^T * Vb_sub  (nb × cols)
                        std::vector<T> Wbuf(nb * cols, T(0));
                        DefaultComputePolicy<T>::gemm(
                            VT_rm.data(), vdata + start_row * cols, Wbuf.data(),
                            nb, cols, pr, pr, cols, cols);

                        // (2) Wbuf = T * Wbuf (upper triangular, top to bottom)
                        for (size_t j = 0; j < nb; ++j) {
                            T t_jj = T_fac[j * nb + j];
                            T* w_row = &Wbuf[j * cols];
                            if (t_jj != T(1)) {
                                for (size_t c = 0; c < cols; ++c) w_row[c] *= t_jj;
                            }
                            for (size_t l = j + 1; l < nb; ++l) {
                                T t_jl = T_fac[j * nb + l];
                                if (t_jl == T(0)) continue;
                                computation::simd::axpy_simd<T>(
                                    w_row, t_jl, &Wbuf[l * cols], cols);
                            }
                        }

                        // (3) Vb_sub -= V * Wbuf
                        for (size_t i = 0; i < nb * cols; ++i) Wbuf[i] = -Wbuf[i];
                        DefaultComputePolicy<T>::gemm(
                            V_rm.data(), Wbuf.data(), vdata + start_row * cols,
                            pr, cols, nb, nb, cols, cols);

                        k_end = k_start;
                    }
                }
            }

#ifdef SANGI_SVD_PHASE_TIMING
            auto _svd_t3 = std::chrono::high_resolution_clock::now();
            std::fprintf(stderr, "SVD n=%zu: Phase1=%.1fms Phase2=%.1fms Phase3=%.1fms\n",
                n,
                std::chrono::duration<double,std::milli>(_svd_t1 - _svd_t0).count(),
                std::chrono::duration<double,std::milli>(_svd_t2 - _svd_t1).count(),
                std::chrono::duration<double,std::milli>(_svd_t3 - _svd_t2).count());
#endif
            // Build the result
            Vector<T> sigma(min_mn);
            for (size_t i = 0; i < min_mn; ++i) sigma[i] = d[i];

            if (transV) {
                Matrix<T> Vt(min_mn, n);
                for (size_t i = 0; i < min_mn; ++i)
                    for (size_t j = 0; j < n; ++j)
                        Vt(i, j) = Vb(j, i);
                return { U_out, sigma, Vt };
            } else {
                Matrix<T> V_out(n, min_mn);
                for (size_t i = 0; i < n; ++i)
                    for (size_t j = 0; j < min_mn; ++j)
                        V_out(i, j) = Vb(i, j);
                return { U_out, sigma, V_out };
            }

            } // end float/double fast path

            // ===============================================================
            // Generic path (non float/double) — Complex<T>-aware
            // ===============================================================

            // Real type: the underlying real type for Complex, otherwise T
            using real_t = decltype(std::abs(T()));

            const T zero = numeric_traits<T>::zero();
            const real_t rzero = real_t(0);
            const real_t rone  = real_t(1);
            const real_t rtwo  = real_t(2);

            Matrix<T> W(A);

            Matrix<T> U = Matrix<T>::identity(m);
            Matrix<T> V = Matrix<T>::identity(n);

            // d[], e[] are the bidiagonal elements — type T during bidiagonalize, made real later
            std::vector<T> d_cx(n, zero);
            std::vector<T> e_cx(n, zero);

            for (size_t k = 0; k < n; ++k) {
                // --- Left Householder ---
                {
                    real_t sigma_sq = rzero;
                    for (size_t i = k; i < m; ++i)
                        sigma_sq += std::abs(W(i, k)) * std::abs(W(i, k));
                    real_t alpha_r = std::sqrt(sigma_sq);
                    if (alpha_r > rzero) {
                        // Sign selection (Complex: match the phase)
                        T alpha;
                        if constexpr (numeric_traits<T>::is_complex) {
                            auto abs_wkk = std::abs(W(k, k));
                            alpha = (abs_wkk > rzero) ?
                                T(-alpha_r) * W(k, k) / T(abs_wkk) : T(alpha_r);
                        } else {
                            alpha = (W(k, k) > zero) ? T(-alpha_r) : T(alpha_r);
                        }
                        W(k, k) -= alpha;
                        // tau = 2 / (v^H v), v = W(k:m, k)
                        real_t vhv = rzero;
                        for (size_t i = k; i < m; ++i)
                            vhv += std::abs(W(i, k)) * std::abs(W(i, k));
                        T tau = T(rtwo / vhv);
                        // W(:, j) -= tau * v * (v^H * W(:, j))
                        for (size_t j = k + 1; j < n; ++j) {
                            T dot_val = zero;
                            for (size_t i = k; i < m; ++i) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += sangi::conj(W(i, k)) * W(i, j);
                                else
                                    dot_val += W(i, k) * W(i, j);
                            }
                            dot_val = tau * dot_val;
                            for (size_t i = k; i < m; ++i) W(i, j) -= W(i, k) * dot_val;
                        }
                        // U(:, j) -= tau * U(:, j) * (v^H) → U = U * (I - tau v v^H)
                        for (size_t i = 0; i < m; ++i) {
                            T dot_val = zero;
                            for (size_t j = k; j < m; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += U(i, j) * W(j, k);
                                else
                                    dot_val += U(i, j) * W(j, k);
                            }
                            dot_val = tau * dot_val;
                            for (size_t j = k; j < m; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    U(i, j) -= dot_val * sangi::conj(W(j, k));
                                else
                                    U(i, j) -= dot_val * W(j, k);
                            }
                        }
                        d_cx[k] = alpha;
                    } else {
                        d_cx[k] = W(k, k);
                    }
                }
                // --- Right Householder ---
                if (k + 2 <= n - 1) {
                    real_t sigma_sq = rzero;
                    for (size_t j = k + 1; j < n; ++j)
                        sigma_sq += std::abs(W(k, j)) * std::abs(W(k, j));
                    real_t alpha_r = std::sqrt(sigma_sq);
                    if (alpha_r > rzero) {
                        T alpha;
                        if constexpr (numeric_traits<T>::is_complex) {
                            auto abs_wk1 = std::abs(W(k, k + 1));
                            alpha = (abs_wk1 > rzero) ?
                                T(-alpha_r) * W(k, k + 1) / T(abs_wk1) : T(alpha_r);
                        } else {
                            alpha = (W(k, k + 1) > zero) ? T(-alpha_r) : T(alpha_r);
                        }
                        W(k, k + 1) -= alpha;
                        // tau = 2 / (v^H v), v = W(k, k+1:n)
                        real_t vhv = rzero;
                        for (size_t j = k + 1; j < n; ++j)
                            vhv += std::abs(W(k, j)) * std::abs(W(k, j));
                        T tau = T(rtwo / vhv);
                        // W(i, :) -= tau * (W(i,:) * v) * v^H
                        for (size_t i = k + 1; i < m; ++i) {
                            T dot_val = zero;
                            for (size_t j = k + 1; j < n; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += W(i, j) * sangi::conj(W(k, j));
                                else
                                    dot_val += W(i, j) * W(k, j);
                            }
                            dot_val = tau * dot_val;
                            for (size_t j = k + 1; j < n; ++j) W(i, j) -= dot_val * W(k, j);
                        }
                        // V = V * (I - tau v v^H)
                        for (size_t i = 0; i < n; ++i) {
                            T dot_val = zero;
                            for (size_t j = k + 1; j < n; ++j) {
                                if constexpr (numeric_traits<T>::is_complex)
                                    dot_val += V(i, j) * sangi::conj(W(k, j));
                                else
                                    dot_val += V(i, j) * W(k, j);
                            }
                            dot_val = tau * dot_val;
                            for (size_t j = k + 1; j < n; ++j) V(i, j) -= dot_val * W(k, j);
                        }
                        e_cx[k + 1] = alpha;
                    } else {
                        e_cx[k + 1] = W(k, k + 1);
                    }
                } else if (k + 1 < n) {
                    e_cx[k + 1] = W(k, k + 1);
                }
            }

            // Make d_cx[], e_cx[] real and absorb the phases into U, V
            std::vector<real_t> d(n, rzero);
            std::vector<real_t> e(n, rzero);

            if constexpr (numeric_traits<T>::is_complex) {
                // Phase cleanup: U^H A V = B (complex bidiagonal)
                // Convert B to real positive bidiagonal: B = D_L * B_real * D_R^H
                // Alternately make d[k] and e[k+1] real positive, absorbing the phases into U, V
                for (size_t k = 0; k < n; ++k) {
                    // Make d_cx[k] real positive
                    real_t abs_dk = std::abs(d_cx[k]);
                    d[k] = abs_dk;
                    if (abs_dk > rzero) {
                        T phase = d_cx[k] / T(abs_dk);
                        T pconj = sangi::conj(phase);
                        // Absorb the phase into column k of U: U[:,k] *= phase
                        for (size_t i = 0; i < m; ++i) U(i, k) *= phase;
                        // Propagate to e_cx[k+1]: e_cx[k+1] *= pconj
                        if (k + 1 < n) e_cx[k + 1] = pconj * e_cx[k + 1];
                    }
                    // Make e_cx[k+1] real positive
                    // V[:,k+1] *= conj(phase) applies conj(phase) to column k+1 of B
                    if (k + 1 < n) {
                        real_t abs_ek1 = std::abs(e_cx[k + 1]);
                        e[k + 1] = abs_ek1;
                        if (abs_ek1 > rzero) {
                            T phase = e_cx[k + 1] / T(abs_ek1);
                            T pconj = sangi::conj(phase);
                            // V[:,k+1] *= pconj → applies pconj to column k+1 of B
                            // e[k+1] *= pconj = |e|*phase * conj(phase)/|phase|^2 → real positive
                            for (size_t i = 0; i < n; ++i) V(i, k + 1) *= pconj;
                            // pconj also propagates to d_cx[k+1]
                            d_cx[k + 1] = pconj * d_cx[k + 1];
                        }
                    }
                }
            } else {
                // Real type: d_cx, e_cx can be used directly as real_t
                for (size_t i = 0; i < n; ++i) {
                    d[i] = real_t(d_cx[i]);
                    e[i] = real_t(e_cx[i]);
                }
                // Correct negative diagonal elements to positive
                for (size_t i = 0; i < n; ++i) {
                    if (d[i] < rzero) {
                        d[i] = -d[i];
                        if (i + 1 < n) e[i + 1] = -e[i + 1];
                        for (size_t j = 0; j < m; ++j) U(j, i) = -U(j, i);
                    }
                }
            }

            const real_t eps = numeric_traits<real_t>::epsilon();
            const int max_iter = 100 * static_cast<int>(n);

            // Golub-Kahan QR iteration (d[], e[] are real, Givens rotations are also real)
            for (int iter = 0; iter < max_iter; ++iter) {
                for (size_t i = 1; i < n; ++i)
                    if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i]))) e[i] = rzero;
                size_t q = n;
                while (q > 1 && e[q - 1] == rzero) --q;
                if (q <= 1) break;
                size_t p = q - 1;
                while (p > 0 && e[p] != rzero) --p;
                bool handled_zero = false;
                for (size_t i = p; i < q; ++i) {
                    real_t ref = rzero;
                    if (i + 1 < n && e[i + 1] != rzero) ref = std::max(ref, std::abs(e[i + 1]));
                    if (i > 0 && e[i] != rzero) ref = std::max(ref, std::abs(e[i]));
                    if (ref == rzero) continue;
                    if (std::abs(d[i]) > eps * ref) continue;
                    d[i] = rzero; handled_zero = true;
                    if (i + 1 < q && e[i + 1] != rzero) {
                        real_t bulge = e[i + 1]; e[i + 1] = rzero;
                        for (size_t j = i + 1; j < q; ++j) {
                            real_t c, s; svd_detail::givens(d[j], bulge, c, s);
                            d[j] = c * d[j] - s * bulge;
                            if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; }
                            for (size_t l = 0; l < m; ++l) { T ui=U(l,i), uj=U(l,j); U(l,i)=T(c)*ui+T(s)*uj; U(l,j)=T(-s)*ui+T(c)*uj; }
                            if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break;
                        }
                    } else if (i > p && e[i] != rzero) {
                        real_t bulge = e[i]; e[i] = rzero;
                        for (size_t jj = 0; jj < i - p; ++jj) {
                            size_t j = i - 1 - jj; real_t c, s; svd_detail::givens(d[j], bulge, c, s);
                            d[j] = c * d[j] - s * bulge;
                            if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; }
                            for (size_t l = 0; l < n; ++l) { T vi=V(l,i), vj=V(l,j); V(l,i)=T(c)*vi+T(s)*vj; V(l,j)=T(-s)*vi+T(c)*vj; }
                            if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break;
                        }
                    }
                    break;
                }
                if (handled_zero) continue;
                real_t d_qm1=d[q-1], d_qm2=d[q-2], e_qm1=e[q-1];
                real_t e_qm2 = (q>=3 && q-2>p) ? e[q-2] : rzero;
                real_t t11=d_qm2*d_qm2+e_qm2*e_qm2, t12=d_qm2*e_qm1, t22=d_qm1*d_qm1+e_qm1*e_qm1;
                real_t delta=(t11-t22)/rtwo;
                real_t mu=t22-t12*t12/(delta+(delta>=rzero?rone:real_t(-1))*std::sqrt(delta*delta+t12*t12));
                real_t x=d[p]*d[p]-mu, z=d[p]*e[p+1];
                for (size_t k = p; k < q - 1; ++k) {
                    real_t c, s; svd_detail::givens(x, z, c, s);
                    if (k > p) e[k] = c*e[k]-s*z;
                    real_t dk=d[k], dk1=d[k+1], ek1=e[k+1];
                    d[k]=c*dk-s*ek1; e[k+1]=s*dk+c*ek1; real_t bulge=-s*dk1; d[k+1]=c*dk1;
                    for (size_t i=0;i<n;++i) { T vk=V(i,k),vk1=V(i,k+1); V(i,k)=T(c)*vk-T(s)*vk1; V(i,k+1)=T(s)*vk+T(c)*vk1; }
                    x=d[k]; z=bulge; svd_detail::givens(x, z, c, s);
                    d[k]=c*x-s*z; real_t ek1_new=e[k+1]; e[k+1]=c*ek1_new-s*d[k+1]; d[k+1]=s*ek1_new+c*d[k+1];
                    if (k+2<q) { z=-s*e[k+2]; e[k+2]=c*e[k+2]; }
                    for (size_t j=0;j<m;++j) { T uk=U(j,k),uk1=U(j,k+1); U(j,k)=T(c)*uk-T(s)*uk1; U(j,k+1)=T(s)*uk+T(c)*uk1; }
                    x = e[k+1];
                }
            }

            // Final sign correction (d[] should already be positive, just in case)
            for (size_t i = 0; i < n; ++i) {
                if (d[i] < rzero) { d[i] = -d[i]; for (size_t j = 0; j < m; ++j) U(j, i) = -U(j, i); }
            }
            // Sort descending
            for (size_t i = 0; i < n - 1; ++i) {
                size_t max_idx = i;
                for (size_t j = i + 1; j < n; ++j) if (d[j] > d[max_idx]) max_idx = j;
                if (max_idx != i) {
                    std::swap(d[i], d[max_idx]);
                    for (size_t j = 0; j < m; ++j) std::swap(U(j, i), U(j, max_idx));
                    for (size_t j = 0; j < n; ++j) std::swap(V(j, i), V(j, max_idx));
                }
            }
            Vector<T> sigma(min_mn);
            for (size_t i = 0; i < min_mn; ++i) sigma[i] = T(d[i]);
            Matrix<T> U_out(m, min_mn);
            for (size_t i = 0; i < m; ++i) for (size_t j = 0; j < min_mn; ++j) U_out(i, j) = U(i, j);
            if (transV) {
                Matrix<T> Vt(min_mn, n);
                for (size_t i = 0; i < min_mn; ++i)
                    for (size_t j = 0; j < n; ++j) {
                        if constexpr (numeric_traits<T>::is_complex)
                            Vt(i, j) = sangi::conj(V(j, i)); // V^H
                        else
                            Vt(i, j) = V(j, i); // V^T
                    }
                return { U_out, sigma, Vt };
            } else {
                Matrix<T> V_out(n, min_mn);
                for (size_t i = 0; i < n; ++i) for (size_t j = 0; j < min_mn; ++j) V_out(i, j) = V(i, j);
                return { U_out, sigma, V_out };
            }
            } // end of `if constexpr Matrix<T>` else branch
        }

        /**
         * @brief Reconstruct the original matrix from the SVD result
         * @tparam T element type
         * @param U left singular vector matrix (m x k)
         * @param sigma singular value vector (k)
         * @param V right singular vector matrix (n x k)
         * @return the reconstructed matrix A (m x n)
         */
        template<typename T>
        Matrix<T> reconstruct_matrix_from_svd(
            const BaseMatrix<T>& U,     // U
            const BaseVector<T>& s,     // diagonal elements of Σ
            const BaseMatrix<T>& Vt)    // V^T
        {
            const auto m = U.rows();
            const auto n = Vt.cols();  // number of columns of V^T
            const auto r = s.size();  // usually min(m, n)
            Matrix<T> result(m, n, numeric_traits<T>::zero());

            // Reconstruction A = U * Σ * Vt
            for (std::size_t i = 0; i < m; ++i) {
                for (std::size_t j = 0; j < n; ++j) {
                    for (std::size_t k = 0; k < r; ++k) {
                        result(i, j) += U(i, k) * s[k] * Vt(k, j);
                    }
                }
            }
            return result;
        }

        /**
         * @brief Solve a system of linear equations using the improved SVD
         * @tparam T    element type
         * @param a     coefficient matrix
         * @param b     right-hand-side vector
         * @param rcond threshold for the reciprocal condition number (default is 100 times machine epsilon)
         * @return      solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> svd_solve(const MA& a, const VB& b,
                            sangi::element_t<MA> rcond = sangi::element_t<MA>(numeric_traits<sangi::element_t<MA>>::epsilon() * 100)) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: svd_solve: matrix and vector dimensions mismatch");
                throw DimensionError("svd_solve: matrix and vector dimensions mismatch");
            }

            const T zero = numeric_traits<T>::zero();
            const auto eps = numeric_traits<T>::epsilon();
            const auto m = a.rows();
            const auto n = a.cols();
            const auto min_mn = std::min(m, n);

            // 1. SVD decomposition
            auto [U, sigma, Vt] = svd_decomposition(a);

            // 2. Find the largest singular value (sigma is always real-valued)
            auto max_sigma_abs = decltype(std::abs(zero))(0);
            for (typename Matrix<T>::size_type i = 0; i < min_mn; ++i) {
                auto si = std::abs(sigma[i]);
                if (si > max_sigma_abs) max_sigma_abs = si;
            }

            if (max_sigma_abs < eps) {
                return Vector<T>(n, zero);
            }

            // 3. Compute the threshold
            auto threshold_abs = std::abs(rcond) * max_sigma_abs;
            auto sigma_min_abs = max_sigma_abs;
            for (typename Matrix<T>::size_type i = 0; i < min_mn; ++i) {
                auto si = std::abs(sigma[i]);
                if (si > 0) {
                    if (si < sigma_min_abs) sigma_min_abs = si;
                }
            }
            threshold_abs = std::max(threshold_abs, eps * sigma_min_abs * min_mn);

            // 4. Compute U^H * b (Complex: conjugate transpose)
            Vector<T> Utb(min_mn, zero);
            for (typename Matrix<T>::size_type i = 0; i < min_mn; ++i) {
                for (typename Matrix<T>::size_type j = 0; j < m; ++j) {
                    if constexpr (numeric_traits<T>::is_complex)
                        Utb[i] += sangi::conj(U(j, i)) * b[j];
                    else
                        Utb[i] += U(j, i) * b[j];
                }
            }

            // 5. Compute the solution
            Vector<T> x(n, zero);

            for (typename Matrix<T>::size_type j = 0; j < min_mn; ++j) {
                auto sj = std::abs(sigma[j]);
                if (sj > threshold_abs) {
                    T factor = Utb[j] / sigma[j];
                    for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                        x[i] += Vt(j,i) * factor;
                    }
                }
                else if (sj > 0) {
                    // For singular values below the threshold, continuously reduce the weight
                    T weight = T(sj / threshold_abs);
                    T factor = Utb[j] * weight / sigma[j];
                    for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                        x[i] += Vt(j, i) * factor;
                    }
                }
            }

            return x;
        }

        /**
         * @brief Compute the LDL decomposition
         * @tparam T element type
         * @param a matrix to decompose (symmetric matrix)
         * @return a pair of L and D (returns L and D separately)
         */
        template<typename M>
            requires concepts::MatrixOf<M, typename M::value_type>
        std::pair<Matrix<typename M::value_type>, Vector<typename M::value_type>>
        ldl_decomposition(const M& a) {
            using T = typename M::value_type;
            SANGI_STATIC_ASSERT_SQUARE(M);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: ldl_decomposition: matrix must be square");
                throw DimensionError("ldl_decomposition: matrix must be square");
            }

            if (!is_symmetric(a)) {
                throw MathError("ldl_decomposition: matrix must be symmetric");
            }

            const auto n = a.rows();
            Matrix<T> L(n, n);
            Vector<T> D(n);

            // Initialize L as the identity matrix
            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                for (typename Matrix<T>::size_type j = 0; j < n; ++j) {
                    L(i, j) = (i == j) ? T(1) : T(0);
                }
            }

            // Compute the LDL decomposition
            for (typename Matrix<T>::size_type j = 0; j < n; ++j) {
                // Compute D[j]
                T sum = a(j, j);
                for (typename Matrix<T>::size_type k = 0; k < j; ++k) {
                    sum -= L(j, k) * L(j, k) * D[k];
                }
                D[j] = sum;

                // Compute the remaining elements of L
                for (typename Matrix<T>::size_type i = j + 1; i < n; ++i) {
                    sum = a(i, j);
                    for (typename Matrix<T>::size_type k = 0; k < j; ++k) {
                        sum -= L(i, k) * L(j, k) * D[k];
                    }

                    // Check for zero diagonal elements
                    if (std::abs(D[j]) < std::numeric_limits<T>::epsilon()) {
                        throw MathError("ldl_decomposition: division by zero");
                    }

                    L(i, j) = sum / D[j];
                }
            }

            return { L, D };
        }

        /**
         * @brief Solve a system of linear equations using the LDL decomposition
         * @tparam T element type
         * @param a coefficient matrix (symmetric matrix)
         * @param b right-hand-side vector
         * @return solution vector
         */
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> ldl_solve(const MA& a, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (a.rows() != a.cols()) {
                assert(false && "DimensionError: ldl_solve: matrix must be square");
                throw DimensionError("ldl_solve: matrix must be square");
            }

            if (a.rows() != b.size()) {
                assert(false && "DimensionError: ldl_solve: matrix and vector dimensions mismatch");
                throw DimensionError("ldl_solve: matrix and vector dimensions mismatch");
            }

            const auto n = a.rows();

            // LDL decomposition
            auto [L, D] = ldl_decomposition(a);

            // Forward substitution (Ly = b)
            Vector<T> y(n);
            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                T sum = b[i];
                for (typename Matrix<T>::size_type j = 0; j < i; ++j) {
                    sum -= L(i, j) * y[j];
                }
                y[i] = sum;
            }

            // Diagonal scaling (Dz = y)
            Vector<T> z(n);
            for (typename Matrix<T>::size_type i = 0; i < n; ++i) {
                if (std::abs(D[i]) < std::numeric_limits<T>::epsilon()) {
                    throw MathError("ldl_solve: division by zero");
                }
                z[i] = y[i] / D[i];
            }

            // Backward substitution (L^T x = z)
            Vector<T> x(n);
            for (typename Matrix<T>::size_type i = n; i-- > 0; ) {
                T sum = z[i];
                for (typename Matrix<T>::size_type j = i + 1; j < n; ++j) {
                    sum -= L(j, i) * x[j];
                }
                x[i] = sum;
            }

            return x;
        }

        /**
         * @brief Compute the inner product of vectors
         * @tparam T element type
         * @tparam V vector type
         * @param a first vector
         * @param b second vector
         * @return value of the inner product
         */
        template<typename T, typename V>
            requires concepts::VectorOf<V, T>
        T dot(const V& a, const V& b) {
            if (a.size() != b.size()) {
                assert(false && "DimensionError: dot: vector dimensions mismatch");
                throw DimensionError("dot: vector dimensions mismatch");
            }

            T result = T(0);
            for (typename V::size_type i = 0; i < a.size(); ++i) {
                result += a[i] * b[i];
            }

            return result;
        }

        /**
         * @brief Compute the norm of a vector
         * @tparam T element type
         * @tparam V vector type
         * @param v vector
         * @return value of the norm
         */
        template<typename T, typename V>
            requires concepts::VectorOf<V, T>
        T norm(const V& v) {
            return std::sqrt(dot(v, v));
        }

        /**
         * @brief Compute the rank of a matrix
         * Count the singular values of the SVD that are at or above the threshold.
         * @param a input matrix
         * @param tol threshold (auto-set if negative: max(m,n) * eps * sigma_max)
         * @return rank
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        size_t rank(const MA& a, sangi::element_t<MA> tol = sangi::element_t<MA>(-1)) {
            using T = sangi::element_t<MA>;
            // No square requirement: rank is defined for any m x n matrix.

            auto [U, sigma, Vt] = svd_decomposition(a);
            const size_t k = sigma.size();
            if (k == 0) return 0;

            // Auto-set the threshold
            T threshold = tol;
            if (threshold < T(0)) {
                T sigma_max = sigma[0];
                for (size_t i = 1; i < k; ++i) {
                    if (sigma[i] > sigma_max) sigma_max = sigma[i];
                }
                size_t max_dim = std::max(a.rows(), a.cols());
                threshold = sigma_max * std::numeric_limits<T>::epsilon() * T(max_dim);
            }

            size_t r = 0;
            for (size_t i = 0; i < k; ++i) {
                if (sigma[i] > threshold) ++r;
            }
            return r;
        }

        /**
         * @brief Compute the condition number of a matrix
         * 2-norm condition number: sigma_max / sigma_min
         * @param a input matrix (square matrix)
         * @return condition number (+infinity for singular matrices)
         */
        template<typename T>
        T conditionNumber(const BaseMatrix<T>& a) {
            auto [U, sigma, Vt] = svd_decomposition(a);
            const size_t k = sigma.size();
            if (k == 0) {
                return std::numeric_limits<T>::infinity();
            }

            T sigma_max = sigma[0];
            T sigma_min = sigma[0];
            for (size_t i = 1; i < k; ++i) {
                if (sigma[i] > sigma_max) sigma_max = sigma[i];
                if (sigma[i] < sigma_min) sigma_min = sigma[i];
            }

            if (sigma_min <= T(0)) {
                return std::numeric_limits<T>::infinity();
            }

            return sigma_max / sigma_min;
        }

        // ====================================================================
        // Condition Number Estimation
        // ====================================================================

        namespace condest_detail {

            // Solve Ax = b with an LU-decomposed matrix (forward/backward substitution)
            template<typename T>
            Vector<T> lu_solve_factored(
                const BaseMatrix<T>& lu,
                const std::vector<typename Matrix<T>::size_type>& pivots,
                const BaseVector<T>& b)
            {
                const auto n = lu.rows();
                Vector<T> x(b);  // 1 copy via BaseVector ctor (mutated below)

                // Pivot permutation + forward substitution (Ly = Pb)
                for (std::size_t k = 0; k < n; ++k) {
                    if (pivots[k] != k) std::swap(x[k], x[pivots[k]]);
                    for (std::size_t i = k + 1; i < n; ++i)
                        x[i] -= lu(i, k) * x[k];
                }

                // Backward substitution (Ux = y)
                for (std::size_t ii = 0; ii < n; ++ii) {
                    std::size_t i = n - 1 - ii;
                    for (std::size_t j = i + 1; j < n; ++j)
                        x[i] -= lu(i, j) * x[j];
                    x[i] /= lu(i, i);
                }
                return x;
            }

            // Solve A^T x = b with an LU-decomposed matrix
            template<typename T>
            Vector<T> lu_solve_transpose_factored(
                const BaseMatrix<T>& lu,
                const std::vector<typename Matrix<T>::size_type>& pivots,
                const BaseVector<T>& b)
            {
                const auto n = lu.rows();
                Vector<T> x(b);  // 1 copy via BaseVector ctor (mutated below)

                // Forward substitution (U^T y = b)
                for (std::size_t i = 0; i < n; ++i) {
                    x[i] /= lu(i, i);
                    for (std::size_t j = i + 1; j < n; ++j)
                        x[j] -= lu(i, j) * x[i];
                }

                // Backward substitution (L^T z = y)
                for (std::size_t ii = 0; ii < n; ++ii) {
                    std::size_t i = n - 1 - ii;
                    for (std::size_t j = i + 1; j < n; ++j)
                        x[i] -= lu(j, i) * x[j];
                    // L's diagonal is 1
                }

                // Inverse pivot permutation
                for (std::size_t ii = 0; ii < n; ++ii) {
                    std::size_t k = n - 1 - ii;
                    if (pivots[k] != k) std::swap(x[k], x[pivots[k]]);
                }
                return x;
            }

        } // namespace condest_detail

        /**
         * @brief Estimate ‖A⁻¹‖₁ via the Hager-Higham algorithm
         *
         * Takes an LU-decomposed matrix and, without explicitly building A⁻¹,
         * estimates ‖A⁻¹‖₁ in O(n²).
         * Reference: Higham (1988) "FORTRAN codes for estimating the one-norm of
         * a real or complex matrix, with applications to condition estimation"
         *
         * @param lu LU-decomposed matrix (output of lu_decomposition)
         * @param pivots pivot information
         * @return estimate of ‖A⁻¹‖₁ (a lower bound)
         */
        template<typename T>
        T estimate_inv_norm1(
            const BaseMatrix<T>& lu,
            const std::vector<typename Matrix<T>::size_type>& pivots)
        {
            const auto n = lu.rows();
            if (n == 0) return T(0);

            // Hager-Higham: at most 5 iterations
            const int max_iter = 5;

            // x = (1/n, ..., 1/n)
            Vector<T> x(n, T(1) / static_cast<T>(n));

            T gamma = T(0);

            for (int iter = 0; iter < max_iter; ++iter) {
                // w = A⁻¹ x
                auto w = condest_detail::lu_solve_factored(lu, pivots, x);

                // gamma_new = ‖w‖₁
                T gamma_new = T(0);
                for (std::size_t i = 0; i < n; ++i)
                    gamma_new += std::abs(static_cast<double>(w[i]));

                if (iter > 0 && gamma_new <= gamma) {
                    break;
                }
                gamma = gamma_new;

                // y = sign(w)
                Vector<T> y(n);
                for (std::size_t i = 0; i < n; ++i)
                    y[i] = (w[i] >= T(0)) ? T(1) : T(-1);

                // z = A^{-T} y
                auto z = condest_detail::lu_solve_transpose_factored(lu, pivots, y);

                // ‖z‖∞
                T z_inf = T(0);
                std::size_t j_max = 0;
                for (std::size_t i = 0; i < n; ++i) {
                    T az = static_cast<T>(std::abs(static_cast<double>(z[i])));
                    if (az > z_inf) { z_inf = az; j_max = i; }
                }

                // z^T w = Σ y_i * w_i (= ‖w‖₁ since y=sign(w))
                // Convergence test: ‖z‖∞ ≤ z^T * w ⇔ ‖z‖∞ ≤ gamma
                if (z_inf <= gamma) {
                    break;
                }

                // x = e_{j_max}
                for (std::size_t i = 0; i < n; ++i) x[i] = T(0);
                x[j_max] = T(1);
            }

            return static_cast<T>(gamma);
        }

        /**
         * @brief Estimate of the 1-norm condition number: κ₁(A) = ‖A‖₁ · ‖A⁻¹‖₁
         *
         * O(n³ + n²) ≈ O(n³) via LU decomposition + Hager-Higham estimation, but
         * much faster than the SVD-based conditionNumber().
         * For a singular matrix, the LU decomposition throws an exception.
         *
         * @param A square matrix
         * @return estimate of κ₁(A)
         */
        template<typename T>
        T condition_number_1norm(const BaseMatrix<T>& A) {
            if (A.rows() != A.cols()) {
                assert(false && "DimensionError: condition_number_1norm: matrix must be square");
                throw DimensionError("condition_number_1norm: matrix must be square");
            }
            const auto n = A.rows();
            if (n == 0) return T(0);

            // ‖A‖₁ = max column sum
            T norm1_A = T(0);
            for (std::size_t j = 0; j < n; ++j) {
                T col_sum = T(0);
                for (std::size_t i = 0; i < n; ++i)
                    col_sum += static_cast<T>(std::abs(static_cast<double>(A(i, j))));
                if (col_sum > norm1_A) norm1_A = col_sum;
            }

            auto [lu, pivots] = lu_decomposition(A);
            T norm1_inv = estimate_inv_norm1(lu, pivots);

            return norm1_A * norm1_inv;
        }

        /**
         * @brief Estimate of the ∞-norm condition number: κ∞(A) = ‖A‖∞ · ‖A⁻¹‖∞ = κ₁(A^T)
         *
         * @param A square matrix
         * @return estimate of κ∞(A)
         */
        template<typename T>
        T condition_number_inf(const BaseMatrix<T>& A) {
            return condition_number_1norm(A.transposed());  // 0-copy view
        }

        /**
         * @brief Estimate of the reciprocal condition number: rcond₁(A) = 1 / κ₁(A)
         *
         * Corresponds to LAPACK's dgecon. The closer to 0, the closer to singular.
         *
         * @param A square matrix
         * @return estimate of 1/κ₁(A) (LU throws for a singular matrix)
         */
        template<typename T>
        T rcond_1norm(const BaseMatrix<T>& A) {
            T cond = condition_number_1norm(A);
            if (cond == T(0)) return T(0);
            return T(1) / cond;
        }

        //=====================================================================
        // Hessenberg decomposition
        //=====================================================================

        /**
         * @brief Hessenberg decomposition: A = Q H Qᵀ
         *
         * Transforms a square matrix A into an upper Hessenberg matrix H via Householder reflections.
         * H(i,j) = 0 for i > j+1. Q is an orthogonal matrix.
         *
         * @tparam T element type (double, float, etc.)
         * @param A input matrix (n×n square matrix)
         * @return {H, Q} — H: upper Hessenberg matrix, Q: orthogonal matrix (A = Q*H*Qᵀ)
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        std::pair<Matrix<sangi::element_t<MA>>, Matrix<sangi::element_t<MA>>>
        hessenberg_decomposition(const MA& A) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            const auto n = A.rows();
            if (n != A.cols()) {
                assert(false && "DimensionError: hessenberg_decomposition: matrix must be square");
                throw DimensionError("hessenberg_decomposition: matrix must be square");
            }

            Matrix<T> H = A;
            Matrix<T> Q(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) Q(i, i) = T(1);

            if (n <= 2) {
                return {H, Q};
            }

            // Householder reflection for columns k = 0, 1, ..., n-3
            for (std::size_t k = 0; k < n - 2; ++k) {
                // Build the Householder vector v (targeting H(k+1:n, k))
                const std::size_t m = n - k - 1; // vector length
                std::vector<T> v(m);
                for (std::size_t i = 0; i < m; ++i) {
                    v[i] = H(k + 1 + i, k);
                }

                // ||v||₂
                T norm_v = T(0);
                for (std::size_t i = 0; i < m; ++i) {
                    norm_v += v[i] * v[i];
                }
                norm_v = std::sqrt(norm_v);

                if (norm_v <= std::numeric_limits<T>::epsilon() * std::abs(H(k, k)) * T(10)) {
                    continue; // already zero → skip
                }

                // Add sign(v[0]) * ||v|| to v[0]
                T alpha = (v[0] >= T(0)) ? -norm_v : norm_v;
                v[0] -= alpha;

                // Normalize v
                T norm_v2 = T(0);
                for (std::size_t i = 0; i < m; ++i) {
                    norm_v2 += v[i] * v[i];
                }
                if (norm_v2 <= T(0)) continue;
                T inv_norm = T(1) / std::sqrt(norm_v2);
                for (std::size_t i = 0; i < m; ++i) {
                    v[i] *= inv_norm;
                }

                // H ← (I - 2vvᵀ) H (from the left): H[k+1:n, :] -= 2v(vᵀ H[k+1:n, :])
                for (std::size_t j = 0; j < n; ++j) {
                    T dot = T(0);
                    for (std::size_t i = 0; i < m; ++i) {
                        dot += v[i] * H(k + 1 + i, j);
                    }
                    dot *= T(2);
                    for (std::size_t i = 0; i < m; ++i) {
                        H(k + 1 + i, j) -= v[i] * dot;
                    }
                }

                // H ← H (I - 2vvᵀ) (from the right): H[:, k+1:n] -= 2(H[:, k+1:n] v)vᵀ
                for (std::size_t i = 0; i < n; ++i) {
                    T dot = T(0);
                    for (std::size_t j = 0; j < m; ++j) {
                        dot += H(i, k + 1 + j) * v[j];
                    }
                    dot *= T(2);
                    for (std::size_t j = 0; j < m; ++j) {
                        H(i, k + 1 + j) -= dot * v[j];
                    }
                }

                // Q ← Q (I - 2vvᵀ) (accumulate from the right)
                for (std::size_t i = 0; i < n; ++i) {
                    T dot = T(0);
                    for (std::size_t j = 0; j < m; ++j) {
                        dot += Q(i, k + 1 + j) * v[j];
                    }
                    dot *= T(2);
                    for (std::size_t j = 0; j < m; ++j) {
                        Q(i, k + 1 + j) -= dot * v[j];
                    }
                }
            }

            // Clean up tiny values below the subdiagonal
            for (std::size_t j = 0; j < n; ++j) {
                for (std::size_t i = j + 2; i < n; ++i) {
                    H(i, j) = T(0);
                }
            }

            return {H, Q};
        }

        //=====================================================================
        // Schur decomposition (real Schur)
        //=====================================================================

        /**
         * @brief Real Schur decomposition: A = Q T Qᵀ
         *
         * T is a quasi-upper-triangular matrix (real Schur form):
         *   - real eigenvalues are 1×1 diagonal blocks
         *   - complex conjugate pairs are 2×2 diagonal blocks
         * Q is an orthogonal matrix.
         *
         * Algorithm: Hessenberg decomposition → Francis double-shift QR iteration
         *
         * @tparam T element type
         * @param A input matrix (n×n square matrix)
         * @return {T, Q} — T: quasi-upper-triangular matrix, Q: orthogonal matrix (A = Q*T*Qᵀ)
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        std::pair<Matrix<sangi::element_t<MA>>, Matrix<sangi::element_t<MA>>>
        schur_decomposition(const MA& A) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            const auto n = A.rows();
            if (n != A.cols()) {
                assert(false && "DimensionError: schur_decomposition: matrix must be square");
                throw DimensionError("schur_decomposition: matrix must be square");
            }

            if (n <= 1) {
                return {A, Matrix<T>(1, 1, T(1))};
            }

#if SANGI_HAS_MKL
            if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
                Matrix<T> Sch = A;
                Matrix<T> Q(n, n);
                lapack_int sdim = 0;
                std::vector<T> wr(n), wi(n);
                lapack_int info;
                if constexpr (std::is_same_v<T, float>)
                    info = LAPACKE_sgees(LAPACK_ROW_MAJOR, 'V', 'N', nullptr,
                                         (lapack_int)n, Sch.data(), (lapack_int)n,
                                         &sdim, wr.data(), wi.data(),
                                         Q.data(), (lapack_int)n);
                else
                    info = LAPACKE_dgees(LAPACK_ROW_MAJOR, 'V', 'N', nullptr,
                                         (lapack_int)n, Sch.data(), (lapack_int)n,
                                         &sdim, wr.data(), wi.data(),
                                         Q.data(), (lapack_int)n);
                if (info > 0)
                    throw MathError("schur_decomposition: LAPACKE_gees did not converge");
                return { Sch, Q };
            }
#endif

            // Step 1: Hessenberg decomposition
            auto [Sch, Q] = hessenberg_decomposition(A);

            const int max_iter = 300 * static_cast<int>(n);
            const T eps = std::numeric_limits<T>::epsilon();

            // Step 2: Francis QR iteration
            // The working range is [ilo, ihi) (0-indexed)
            int ihi = static_cast<int>(n);

            int total_iter = 0;
            int iter_since_deflation = 0; // for exceptional-shift decision
            while (ihi > 1 && total_iter < max_iter) {
                // Deflation: find a location where the subdiagonal element is sufficiently small
                int ilo = ihi - 1;
                while (ilo > 0) {
                    T threshold = eps * (std::abs(Sch(ilo - 1, ilo - 1)) +
                                         std::abs(Sch(ilo, ilo)));
                    if (threshold == T(0)) threshold = eps;
                    if (std::abs(Sch(ilo, ilo - 1)) <= threshold) {
                        Sch(ilo, ilo - 1) = T(0);
                        break;
                    }
                    --ilo;
                }

                if (ilo == ihi - 1) {
                    // 1×1 block (real eigenvalue): deflate
                    --ihi;
                    iter_since_deflation = 0;
                    continue;
                }

                if (ilo == ihi - 2) {
                    // 2×2 block: compute the eigenvalues and decide
                    T a11 = Sch(ilo, ilo), a12 = Sch(ilo, ilo + 1);
                    T a21 = Sch(ilo + 1, ilo), a22 = Sch(ilo + 1, ilo + 1);
                    T disc = (a11 - a22) * (a11 - a22) + T(4) * a12 * a21;
                    if (disc >= T(0)) {
                        // Real eigenvalues → diagonalize via QR with Wilkinson shift
                        // Shift: the eigenvalue closer to a22
                        T delta = (a11 - a22) / T(2);
                        T sign_d = (delta >= T(0)) ? T(1) : T(-1);
                        T sigma = a22 - sign_d * a21 * a21
                                  / (std::abs(delta) +
                                     std::sqrt(delta * delta + a21 * a21));
                        // Givens rotation on the shifted matrix
                        T x2 = Sch(ilo, ilo) - sigma;
                        T y2 = Sch(ilo + 1, ilo);
                        T r = std::sqrt(x2 * x2 + y2 * y2);
                        if (r > T(0)) {
                            T c = x2 / r;
                            T s = y2 / r;
                            // Rotate from the left
                            for (int j = ilo; j < static_cast<int>(n); ++j) {
                                T t1 = Sch(ilo, j);
                                T t2 = Sch(ilo + 1, j);
                                Sch(ilo, j) = c * t1 + s * t2;
                                Sch(ilo + 1, j) = -s * t1 + c * t2;
                            }
                            // Rotate from the right
                            for (int i = 0; i <= ilo + 1; ++i) {
                                T t1 = Sch(i, ilo);
                                T t2 = Sch(i, ilo + 1);
                                Sch(i, ilo) = c * t1 + s * t2;
                                Sch(i, ilo + 1) = -s * t1 + c * t2;
                            }
                            // Accumulate Q
                            for (std::size_t i = 0; i < n; ++i) {
                                T t1 = Q(i, ilo);
                                T t2 = Q(i, ilo + 1);
                                Q(i, ilo) = c * t1 + s * t2;
                                Q(i, ilo + 1) = -s * t1 + c * t2;
                            }
                        }
                    }
                    // 2×2 block (complex conjugate pair or already diagonalized): deflate
                    ihi -= 2;
                    iter_since_deflation = 0;
                    continue;
                }

                // Francis double-shift QR step
                auto compute_exceptional_shift = [&]() -> std::pair<T, T> {
                    // Exceptional shift (LAPACK style):
                    // When the usual shift does not converge, break the pattern
                    // with an ad-hoc shift
                    T sub = std::abs(Sch(ihi - 1, ihi - 2));
                    T sub2 = (ihi >= 3)
                        ? std::abs(Sch(ihi - 2, ihi - 3)) : T(0);
                    T dat1 = T(0.75);
                    T ss = dat1 * (sub + sub2)
                           + Sch(ihi - 1, ihi - 1)
                           + Sch(ihi - 2, ihi - 2);
                    T tt = Sch(ihi - 1, ihi - 1) * Sch(ihi - 2, ihi - 2)
                           - dat1 * dat1 * sub * sub;
                    return {ss, tt};
                };

                // Wilkinson shift: eigenvalues of the bottom-right 2×2 block
                T a11 = Sch(ihi - 2, ihi - 2), a12 = Sch(ihi - 2, ihi - 1);
                T a21 = Sch(ihi - 1, ihi - 2), a22 = Sch(ihi - 1, ihi - 1);
                T s_shift = a11 + a22;       // trace
                T t_shift = a11 * a22 - a12 * a21; // determinant

                // Implicit QR step (bulge chasing)
                // Initial vector: the first 3 elements of (H - σ₂I)(H - σ₁I)e₁
                T h11 = Sch(ilo, ilo), h12 = Sch(ilo, ilo + 1);
                T h21 = Sch(ilo + 1, ilo), h22 = Sch(ilo + 1, ilo + 1);
                T x = h11 * h11 + h12 * h21 - s_shift * h11 + t_shift;
                T y = h21 * (h11 + h22 - s_shift);
                T z = h21 * Sch(ilo + 2, ilo + 1);

                // Initial-vector degeneracy detection:
                // If x≈0, y≈0, bulge chasing is essentially just a row/column
                // swap and does not converge → avoid with an exceptional shift
                T xy_mag = std::abs(x) + std::abs(y);
                T z_mag = std::abs(z);
                bool need_exceptional =
                    (iter_since_deflation > 0 &&
                     iter_since_deflation % 10 == 0) ||
                    (z_mag > T(0) &&
                     xy_mag < eps * z_mag * T(1000));

                if (need_exceptional) {
                    auto [ss, tt] = compute_exceptional_shift();
                    s_shift = ss;
                    t_shift = tt;
                    x = h11 * h11 + h12 * h21 - s_shift * h11 + t_shift;
                    y = h21 * (h11 + h22 - s_shift);
                    z = h21 * Sch(ilo + 2, ilo + 1);
                }

                for (int k = ilo; k < ihi - 1; ++k) {
                    // Chase the bulge with a 3×1 (or 2×1 at end) Householder reflection
                    int p = std::min(3, ihi - k);
                    // Householder vector
                    T norm_xyz;
                    if (p == 3) {
                        norm_xyz = std::sqrt(x * x + y * y + z * z);
                    } else {
                        norm_xyz = std::sqrt(x * x + y * y);
                    }
                    if (norm_xyz == T(0)) {
                        if (k + p < ihi) {
                            x = Sch(k + 1, k);
                            y = (k + 2 < ihi) ? Sch(k + 2, k) : T(0);
                            z = (k + 3 < ihi) ? Sch(k + 3, k + 1) : T(0);
                        }
                        continue;
                    }

                    T beta = (x >= T(0)) ? -norm_xyz : norm_xyz;
                    T v1 = x - beta;
                    T inv_v1 = T(1) / v1;
                    T v2 = y * inv_v1;
                    T v3 = (p == 3) ? z * inv_v1 : T(0);
                    T tau = -v1 / beta; // equivalent to 2 / (1 + v2² + v3²)

                    // Reflect from the left: H[k:k+p, :] -= tau * v * (vᵀ * H[k:k+p, :])
                    int jstart = (k > ilo) ? k - 1 : k;
                    for (int j = jstart; j < static_cast<int>(n); ++j) {
                        T w = Sch(k, j) + v2 * Sch(k + 1, j);
                        if (p == 3) w += v3 * Sch(k + 2, j);
                        w *= tau;
                        Sch(k, j) -= w;
                        Sch(k + 1, j) -= w * v2;
                        if (p == 3) Sch(k + 2, j) -= w * v3;
                    }

                    // Reflect from the right: H[:, k:k+p] -= tau * (H[:, k:k+p] * v) * vᵀ
                    int iend = std::min(ihi, k + 4);
                    for (int i = 0; i < iend; ++i) {
                        T w = Sch(i, k) + v2 * Sch(i, k + 1);
                        if (p == 3) w += v3 * Sch(i, k + 2);
                        w *= tau;
                        Sch(i, k) -= w;
                        Sch(i, k + 1) -= w * v2;
                        if (p == 3) Sch(i, k + 2) -= w * v3;
                    }

                    // Accumulate Q: Q[:, k:k+p] -= tau * (Q[:, k:k+p] * v) * vᵀ
                    for (std::size_t i = 0; i < n; ++i) {
                        T w = Q(i, k) + v2 * Q(i, k + 1);
                        if (p == 3) w += v3 * Q(i, k + 2);
                        w *= tau;
                        Q(i, k) -= w;
                        Q(i, k + 1) -= w * v2;
                        if (p == 3) Q(i, k + 2) -= w * v3;
                    }

                    // Update x, y, z for the next iteration
                    if (k + 1 < ihi - 1) {
                        x = Sch(k + 1, k);
                        y = Sch(k + 2, k);
                        z = (k + 3 < ihi) ? Sch(k + 3, k) : T(0);
                    }
                }

                ++total_iter;
                ++iter_since_deflation;
            }

            // Clean up tiny values on the subdiagonal
            for (std::size_t i = 1; i < n; ++i) {
                T threshold = eps * (std::abs(Sch(i - 1, i - 1)) +
                                     std::abs(Sch(i, i)));
                if (threshold == T(0)) threshold = eps;
                if (std::abs(Sch(i, i - 1)) <= threshold) {
                    Sch(i, i - 1) = T(0);
                }
            }

            // Finalize the real Schur form: decompose 2×2 blocks with disc > 0 (real eigenvalue pairs)
            // into two 1×1 blocks via Givens rotation. Neglecting this causes
            // the selector of schur_decomposition_reordered to see only one eigenvalue of the block,
            // breaking the stable/unstable separation of Hamiltonian matrices (LQR/Laub).
            // Since a single Givens rotation perturbs the subdiagonal of other blocks, a single
            // pass may leave some missed (the subdiagonal of an adjacent block is temporarily
            // perturbed → remains naturally small). Iterate for at most n passes
            // (no new cleanup loop is created; it stops based on the disc>0 test alone).
            {
                for (std::size_t outer = 0; outer < n; ++outer) {
                    bool changed = false;
                    for (std::size_t k = 0; k + 1 < n; ++k) {
                        if (Sch(k + 1, k) == T(0)) continue;
                        T a = Sch(k, k), b = Sch(k, k + 1);
                        T c = Sch(k + 1, k), d = Sch(k + 1, k + 1);
                        // Treat a microscopic subdiagonal as "effectively already 1×1"
                        // (a borderline case deflated by the original Francis QR; touching it degrades accuracy)
                        T subdiag_thr = eps * (std::abs(a) + std::abs(d));
                        if (subdiag_thr == T(0)) subdiag_thr = eps;
                        if (std::abs(c) <= subdiag_thr) {
                            Sch(k + 1, k) = T(0);
                            continue;
                        }
                        T disc = (a - d) * (a - d) + T(4) * b * c;
                        // Decompose only when numerically "clearly disc > 0" (when the boundary
                        // with the complex conjugate pair is close, do not touch it). tolerance = ‖block‖² * eps² * 100
                        // (eps²-based: the numerical error for a true 0 disc is ~‖block‖²·eps²).
                        T block_norm_sq = a*a + b*b + c*c + d*d;
                        T tol_sq = block_norm_sq * eps * eps * T(100);
                        if (disc <= tol_sq) continue;  // complex conjugate pair or boundary
                        T sq = std::sqrt(disc);
                        T lam1 = ((a + d) + sq) / T(2);
                        // Build the eigenvector v = (v0, v1) for lam1 in a numerically stable way
                        T v0, v1;
                        if (std::abs(b) > std::abs(c)) {
                            v0 = b;
                            v1 = lam1 - a;
                        } else {
                            v0 = lam1 - d;
                            v1 = c;
                        }
                        T r = std::hypot(v0, v1);
                        if (r == T(0)) continue;
                        T cs = v0 / r;
                        T sn = v1 / r;
                        // Update columns k, k+1 of Sch (all rows)
                        for (std::size_t i = 0; i < n; ++i) {
                            T t1 = Sch(i, k), t2 = Sch(i, k + 1);
                            Sch(i, k)     =  cs * t1 + sn * t2;
                            Sch(i, k + 1) = -sn * t1 + cs * t2;
                        }
                        // Update rows k, k+1 of Sch (all columns)
                        for (std::size_t j = 0; j < n; ++j) {
                            T t1 = Sch(k, j), t2 = Sch(k + 1, j);
                            Sch(k, j)     =  cs * t1 + sn * t2;
                            Sch(k + 1, j) = -sn * t1 + cs * t2;
                        }
                        // Post-multiply Q (Q · G)
                        for (std::size_t i = 0; i < n; ++i) {
                            T q1 = Q(i, k), q2 = Q(i, k + 1);
                            Q(i, k)     =  cs * q1 + sn * q2;
                            Q(i, k + 1) = -sn * q1 + cs * q2;
                        }
                        Sch(k + 1, k) = T(0);
                        changed = true;
                    }
                    if (!changed) break;
                }
            }

            return {Sch, Q};
        }

        //=====================================================================
        // Reordered real Schur decomposition (Bai-Demmel 1993)
        //=====================================================================

        /**
         * @brief Return the size of the k-th diagonal block of the real Schur form (1 or 2)
         *
         * If Tm(k+1, k) != 0 it is a 2×2 block (complex conjugate pair), otherwise a 1×1 block.
         */
        template<typename T>
        std::size_t schur_block_size(const BaseMatrix<T>& Tm, std::size_t k) {
            const std::size_t n = Tm.rows();
            if (k + 1 >= n) return 1;
            return (Tm(k + 1, k) != T(0)) ? 2 : 1;
        }

        /**
         * @brief Return the eigenvalue pair of the k-th diagonal block
         *
         * For a 1×1 block, fill both with the same real eigenvalue.
         * For a 2×2 block, return the complex conjugate pair.
         */
        template<typename T>
        std::pair<Complex<T>, Complex<T>>
        schur_block_eigenvalues(const BaseMatrix<T>& Tm, std::size_t k) {
            if (schur_block_size(Tm, k) == 1) {
                Complex<T> e(Tm(k, k), T(0));
                return {e, e};
            }
            const T a = Tm(k, k);
            const T b = Tm(k, k + 1);
            const T c = Tm(k + 1, k);
            const T d = Tm(k + 1, k + 1);
            const T tr_half = (a + d) / T(2);
            const T disc = (a - d) * (a - d) + T(4) * b * c;
            if (disc >= T(0)) {
                const T sq = std::sqrt(disc);
                return {Complex<T>(tr_half + sq / T(2), T(0)),
                        Complex<T>(tr_half - sq / T(2), T(0))};
            }
            const T im = std::sqrt(-disc) / T(2);
            return {Complex<T>(tr_half, im), Complex<T>(tr_half, -im)};
        }

        namespace detail_schur {

            /**
             * Solve the small Sylvester equation A11 X - X A22 = C (p, q ∈ {1,2})
             *
             * Kronecker-product form: (I_q ⊗ A11 - A22^T ⊗ I_p) vec(X) = vec(C)
             * (vec is the column stack)
             *
             * Return value: the minimum pivot of Gaussian elimination (near zero means near-degenerate, swap impossible)
             */
            template<typename T>
            T solve_sylvester_small(const T* A11, std::size_t p,
                                    const T* A22, std::size_t q,
                                    const T* C, T* X_out) {
                const std::size_t n = p * q; // ≤ 4
                T M[16] = {T(0)};
                T b[4]  = {T(0)};

                auto Aij = [](const T* A_, std::size_t dim,
                              std::size_t i, std::size_t j) -> T {
                    return A_[i * dim + j];
                };

                // Column-stack vec convention: vec(X)[l*p + i] = X(i, l)
                for (std::size_t l = 0; l < q; ++l) {
                    for (std::size_t i = 0; i < p; ++i) {
                        const std::size_t row = l * p + i;
                        for (std::size_t k = 0; k < p; ++k)
                            M[row * n + (l * p + k)] += Aij(A11, p, i, k);
                        for (std::size_t m = 0; m < q; ++m)
                            M[row * n + (m * p + i)] -= Aij(A22, q, m, l);
                        b[row] = C[i * q + l];
                    }
                }

                std::size_t piv[4] = {0, 1, 2, 3};
                T sep_min = std::numeric_limits<T>::max();
                for (std::size_t k = 0; k < n; ++k) {
                    std::size_t pmax = k;
                    T amax = std::abs(M[piv[k] * n + k]);
                    for (std::size_t i = k + 1; i < n; ++i) {
                        T aval = std::abs(M[piv[i] * n + k]);
                        if (aval > amax) { amax = aval; pmax = i; }
                    }
                    if (amax < sep_min) sep_min = amax;
                    if (amax == T(0)) return T(0);
                    if (pmax != k) std::swap(piv[k], piv[pmax]);

                    for (std::size_t i = k + 1; i < n; ++i) {
                        T factor = M[piv[i] * n + k] / M[piv[k] * n + k];
                        for (std::size_t j = k; j < n; ++j)
                            M[piv[i] * n + j] -= factor * M[piv[k] * n + j];
                        b[piv[i]] -= factor * b[piv[k]];
                    }
                }

                T x[4] = {T(0)};
                for (std::size_t ii = n; ii-- > 0;) {
                    T s = b[piv[ii]];
                    for (std::size_t j = ii + 1; j < n; ++j)
                        s -= M[piv[ii] * n + j] * x[j];
                    x[ii] = s / M[piv[ii] * n + ii];
                }

                for (std::size_t l = 0; l < q; ++l)
                    for (std::size_t i = 0; i < p; ++i)
                        X_out[i * q + l] = x[l * p + i];

                return sep_min;
            }

            /**
             * Adjacent block swap of Bai-Demmel 1993
             *
             * Swap Tm[k:k+p, k:k+p] (A11) and Tm[k+p:k+p+q, k+p:k+p+q] (A22).
             * Accumulate the action into the orthogonal Z as well. p, q ∈ {1, 2}.
             * Returns false if it fails due to near-degeneracy.
             */
            template<typename T>
            bool swap_adjacent_blocks(Matrix<T>& Tm, Matrix<T>& Z,
                                      std::size_t k, std::size_t p, std::size_t q,
                                      T tolerance) {
                const std::size_t n = Tm.rows();
                const std::size_t s = p + q;
                if (k + s > n || p == 0 || q == 0 || p > 2 || q > 2)
                    return false;

                T A11[4] = {T(0)}, A12[4] = {T(0)}, A22[4] = {T(0)}, X[4] = {T(0)};
                for (std::size_t i = 0; i < p; ++i)
                    for (std::size_t j = 0; j < p; ++j)
                        A11[i * p + j] = Tm(k + i, k + j);
                for (std::size_t i = 0; i < p; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        A12[i * q + j] = Tm(k + i, k + p + j);
                for (std::size_t i = 0; i < q; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        A22[i * q + j] = Tm(k + p + i, k + p + j);

                T sep = solve_sylvester_small<T>(A11, p, A22, q, A12, X);
                if (sep < tolerance) return false;

                // V = [-X; I_q] (s×q, row-major)
                T V[16] = {T(0)};
                for (std::size_t i = 0; i < p; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        V[i * q + j] = -X[i * q + j];
                for (std::size_t i = 0; i < q; ++i)
                    V[(p + i) * q + i] = T(1);

                // Householder QR (column by column)
                T uvec[2][4]    = {{T(0)}};
                T tau[2]        = {T(0), T(0)};
                std::size_t ulen[2] = {0, 0};

                for (std::size_t j = 0; j < q; ++j) {
                    const std::size_t len = s - j;
                    ulen[j] = len;
                    T xn2 = T(0);
                    for (std::size_t i = 0; i < len; ++i) {
                        T v = V[(j + i) * q + j];
                        xn2 += v * v;
                    }
                    T xn = std::sqrt(xn2);
                    if (xn == T(0)) {
                        tau[j] = T(0);
                        continue;
                    }
                    T sign = (V[j * q + j] >= T(0)) ? T(1) : T(-1);
                    T alpha = -sign * xn;
                    T v0 = V[j * q + j] - alpha;
                    if (std::abs(v0) <
                        std::numeric_limits<T>::epsilon() * (xn + T(1))) {
                        tau[j] = T(0);
                        continue;
                    }
                    uvec[j][0] = T(1);
                    for (std::size_t i = 1; i < len; ++i)
                        uvec[j][i] = V[(j + i) * q + j] / v0;
                    T uu = T(1);
                    for (std::size_t i = 1; i < len; ++i)
                        uu += uvec[j][i] * uvec[j][i];
                    tau[j] = T(2) / uu;

                    for (std::size_t col = j; col < q; ++col) {
                        T w = T(0);
                        for (std::size_t i = 0; i < len; ++i)
                            w += uvec[j][i] * V[(j + i) * q + col];
                        w *= tau[j];
                        for (std::size_t i = 0; i < len; ++i)
                            V[(j + i) * q + col] -= uvec[j][i] * w;
                    }
                }

                // Apply to rows [k..k+s) of Tm from the left
                for (std::size_t j = 0; j < q; ++j) {
                    if (tau[j] == T(0)) continue;
                    const std::size_t len = ulen[j];
                    const std::size_t row_start = k + j;
                    for (std::size_t col = k; col < n; ++col) {
                        T w = T(0);
                        for (std::size_t i = 0; i < len; ++i)
                            w += uvec[j][i] * Tm(row_start + i, col);
                        w *= tau[j];
                        for (std::size_t i = 0; i < len; ++i)
                            Tm(row_start + i, col) -= uvec[j][i] * w;
                    }
                }

                // Apply to columns [k..k+s) of Tm and columns [k..k+s) of Z from the right
                // Since Q = H_0 H_1 ... H_{q-1}, T Q = T H_0 H_1 ... — apply in forward order
                for (std::size_t jj = 0; jj < q; ++jj) {
                    if (tau[jj] == T(0)) continue;
                    const std::size_t len = ulen[jj];
                    const std::size_t col_start = k + jj;
                    for (std::size_t row = 0; row < k + s; ++row) {
                        T w = T(0);
                        for (std::size_t i = 0; i < len; ++i)
                            w += Tm(row, col_start + i) * uvec[jj][i];
                        w *= tau[jj];
                        for (std::size_t i = 0; i < len; ++i)
                            Tm(row, col_start + i) -= w * uvec[jj][i];
                    }
                    for (std::size_t row = 0; row < n; ++row) {
                        T w = T(0);
                        for (std::size_t i = 0; i < len; ++i)
                            w += Z(row, col_start + i) * uvec[jj][i];
                        w *= tau[jj];
                        for (std::size_t i = 0; i < len; ++i)
                            Z(row, col_start + i) -= w * uvec[jj][i];
                    }
                }

                // Cleanup: exactly zero out subdiagonal elements that violate the Schur form
                for (std::size_t i = q; i < s; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        Tm(k + i, k + j) = T(0);

                return true;
            }

        } // namespace detail_schur

        /**
         * @brief Result of the reordered real Schur decomposition
         */
        template<typename T>
        struct ReorderedSchur {
            Matrix<T> T_mat;            ///< quasi-upper-triangular (selected eigenvalues gathered top-left)
            Matrix<T> Z;                ///< orthogonal matrix (A = Z * T_mat * Z^T)
            std::size_t selected_count; ///< total number of selected eigenvalues gathered top-left
        };

        /**
         * @brief Continuous-time stability: Re(λ) < 0
         */
        template<typename T>
        inline bool is_stable_continuous(const Complex<T>& lambda) {
            return lambda.re < T(0);
        }

        /**
         * @brief Discrete-time stability: |λ| < 1
         */
        template<typename T>
        inline bool is_stable_discrete(const Complex<T>& lambda) {
            return std::sqrt(lambda.re * lambda.re + lambda.im * lambda.im) < T(1);
        }

        /**
         * @brief Reordered real Schur decomposition A = Z T Z^T (gather the selected eigenvalues top-left)
         *
         * Selection sort via the adjacent block swap of Bai-Demmel 1993.
         *
         * @tparam T element type (float, double)
         * @tparam Selector a function object satisfying bool(const Complex<T>&)
         * @param A n×n square matrix
         * @param select selection predicate: gather eigenvalues for which it returns true to the top-left
         * @return ReorderedSchur<T>{T_mat, Z, selected_count}
         *
         * @throws MathError if the adjacent block swap fails due to near-degeneracy
         *
         * @note A building block for the Schur-method CARE/DARE solver of Laub (1979).
         */
        template<typename T, typename Selector>
        ReorderedSchur<T> schur_decomposition_reordered(
            const Matrix<T>& A, Selector select)
        {
            auto [Tm, Z] = schur_decomposition(A);
            const std::size_t n = Tm.rows();
            const T tol = std::numeric_limits<T>::epsilon() * T(1e3);

            std::size_t k = 0;
            while (k < n) {
                const std::size_t pk = schur_block_size(Tm, k);
                auto eigs_k = schur_block_eigenvalues(Tm, k);
                if (select(eigs_k.first)) {
                    k += pk;
                    continue;
                }
                std::size_t j = k + pk;
                bool found = false;
                while (j < n) {
                    const std::size_t pj = schur_block_size(Tm, j);
                    auto eigs_j = schur_block_eigenvalues(Tm, j);
                    if (select(eigs_j.first)) { found = true; break; }
                    j += pj;
                }
                if (!found) break;

                std::size_t cur = j;
                while (cur > k) {
                    const std::size_t pj_now = schur_block_size(Tm, cur);
                    std::size_t prev_start, prev_size;
                    if (cur >= 2 && Tm(cur - 1, cur - 2) != T(0)) {
                        prev_start = cur - 2;
                        prev_size = 2;
                    } else {
                        prev_start = cur - 1;
                        prev_size = 1;
                    }
                    bool ok = detail_schur::swap_adjacent_blocks<T>(
                        Tm, Z, prev_start, prev_size, pj_now, tol);
                    if (!ok) {
                        throw MathError(
                            "schur_decomposition_reordered: adjacent block swap failed (eigenvalues too close)");
                    }
                    cur = prev_start;
                }
                k += schur_block_size(Tm, k);
            }

            std::size_t cnt = 0;
            std::size_t pos = 0;
            while (pos < n) {
                const std::size_t bs = schur_block_size(Tm, pos);
                auto eigs = schur_block_eigenvalues(Tm, pos);
                if (!select(eigs.first)) break;
                cnt += bs;
                pos += bs;
            }

            return ReorderedSchur<T>{std::move(Tm), std::move(Z), cnt};
        }

        //=====================================================================
        // Polar decomposition
        //=====================================================================

        /**
         * @brief Polar decomposition: A = U_polar * H_polar
         *
         * U_polar: orthogonal matrix (nearest orthogonal matrix)
         * H_polar: symmetric positive semidefinite matrix
         *
         * SVD-based: A = UΣVᵀ → U_polar = UVᵀ, H_polar = VΣVᵀ
         *
         * @tparam T element type
         * @param A input matrix (m×n, m >= n)
         * @return {U_polar, H_polar}
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        std::pair<Matrix<sangi::element_t<MA>>, Matrix<sangi::element_t<MA>>>
        polar_decomposition(const MA& A) {
            using T = sangi::element_t<MA>;
            // rectangular OK (m >= n)

            const auto m = A.rows();
            const auto n = A.cols();
            if (m < n) {
                assert(false && "DimensionError: polar_decomposition: requires rows >= cols");
                throw DimensionError("polar_decomposition: requires rows >= cols");
            }

            // SVD: A = U * diag(sigma) * Vᵀ
            auto [U, sigma, Vt] = svd_decomposition(A);

            // U_polar = U * Vᵀ (m×n * n×n → m×n ... n×n if square)
            // Only for square matrices
            if (m != n) {
                assert(false && "DimensionError: polar_decomposition: currently supports square matrices only");
                throw DimensionError("polar_decomposition: currently supports square matrices only");
            }

            // U_polar = U * Vt (n×n)
            Matrix<T> U_polar(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) {
                for (std::size_t j = 0; j < n; ++j) {
                    T sum = T(0);
                    for (std::size_t k = 0; k < n; ++k) {
                        sum += U(i, k) * Vt(k, j);
                    }
                    U_polar(i, j) = sum;
                }
            }

            // H_polar = Vᵀᵀ * diag(sigma) * Vᵀ = V * diag(sigma) * Vᵀ
            // V = Vtᵀ
            Matrix<T> H_polar(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) {
                for (std::size_t j = 0; j < n; ++j) {
                    T sum = T(0);
                    for (std::size_t k = 0; k < n; ++k) {
                        sum += Vt(k, i) * sigma[k] * Vt(k, j);
                    }
                    H_polar(i, j) = sum;
                }
            }

            return {U_polar, H_polar};
        }

        //=====================================================================
        // Takagi Factorization
        //=====================================================================

        /**
         * @brief Structure storing the result of the Takagi factorization
         *
         * Complex symmetric matrix A = U * D * U^T (note: U^T, NOT U^H)
         * U: unitary matrix, D: real nonnegative diagonal matrix
         */
        template<typename T>
        struct TakagiResult {
            Matrix<Complex<T>> U;   ///< unitary matrix (n×n)
            Vector<T> d;            ///< real nonnegative diagonal elements (= singular values)
        };

        /**
         * @brief Takagi factorization: A = U * diag(d) * U^T
         *
         * Factorize a complex symmetric matrix A (A^T = A, note: A^H ≠ A in general).
         * d are the singular values of A (real nonnegative), U is a unitary matrix.
         *
         * Algorithm: with A = X + iY, use the SVD of a real 2n×2n matrix.
         *   M = [[X, -Y], [Y, X]]
         * The singular values of M are duplicate pairs of the singular values of A.
         * Reference: Horn & Johnson "Matrix Analysis" 2nd ed., §4.4
         *
         * @param A complex symmetric matrix (n×n, A^T = A)
         * @return TakagiResult<T> {U, d}
         */
        template<typename T>
        TakagiResult<T> takagi_factorization(const BaseMatrix<Complex<T>>& A) {
            const auto n = A.rows();

            if (n != A.cols()) {
                assert(false && "DimensionError: takagi_factorization: matrix must be square");
                throw DimensionError("takagi_factorization: matrix must be square");
            }
            if (n == 0) {
                assert(false && "DimensionError: takagi_factorization: empty matrix");
                throw DimensionError("takagi_factorization: empty matrix");
            }

            // Symmetry check (A^T = A, not A^H = A)
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = i + 1; j < n; ++j) {
                    auto diff = A(i, j) - A(j, i);
                    double d_abs = std::sqrt(static_cast<double>(diff.re * diff.re + diff.im * diff.im));
                    double scale = std::sqrt(static_cast<double>(A(i, j).re * A(i, j).re + A(i, j).im * A(i, j).im));
                    if (d_abs > std::max(scale, 1.0) * static_cast<double>(n) *
                        static_cast<double>(std::numeric_limits<T>::epsilon()) + 1e-14)
                        throw MathError("takagi_factorization: matrix must be symmetric (A^T = A)");
                }

            // Extract the real part X and imaginary part Y
            Matrix<T> X(n, n, T(0));
            Matrix<T> Y(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j) {
                    X(i, j) = A(i, j).re;
                    Y(i, j) = A(i, j).im;
                }

            // Build the real 2n×2n matrix M = [[X, -Y], [Y, X]]
            Matrix<T> M(2 * n, 2 * n, T(0));
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j) {
                    M(i, j) = X(i, j);
                    M(i, n + j) = -Y(i, j);
                    M(n + i, j) = Y(i, j);
                    M(n + i, n + j) = X(i, j);
                }

            // SVD(M) = U_real * Sigma * V_real^T
            auto [U_real, sigma_real, Vt_real] = svd_decomposition(M);

            // Singular values appear in duplicate pairs: σ₁, σ₁, σ₂, σ₂, ...
            // Take the even-indexed singular values
            Vector<T> d(n);
            for (std::size_t i = 0; i < n; ++i)
                d[i] = sigma_real[2 * i];

            // Build the unitary matrix U from the upper and lower halves of U_real
            // Use column 2k of U_real: z_k = U_real(0:n, 2k) + i * U_real(n:2n, 2k)
            Matrix<Complex<T>> U(n, n, Complex<T>(T(0)));
            for (std::size_t k = 0; k < n; ++k) {
                for (std::size_t i = 0; i < n; ++i) {
                    U(i, k) = Complex<T>(U_real(i, 2 * k), U_real(n + i, 2 * k));
                }
            }

            // Phase correction: use the Takagi relation A * conj(u_k) = σ_k * u_k
            // If z_k differs from the correct u_k by a phase φ (z_k = u_k * e^{iφ}) then:
            //   A * conj(z_k) = A * conj(u_k) * e^{-iφ} = σ_k * u_k * e^{-iφ}
            //                 = σ_k * z_k * e^{-2iφ}
            // Hence w_k = A * conj(z_k) / (σ_k * z_k) = e^{-2iφ}
            // → φ = -arg(w_k) / 2, correction: z_k ← z_k * e^{-iφ}
            for (std::size_t k = 0; k < n; ++k) {
                if (d[k] < std::numeric_limits<T>::epsilon() * T(10)) continue;

                // w = A * conj(z_k)
                Vector<Complex<T>> w(n, Complex<T>(T(0)));
                for (std::size_t i = 0; i < n; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        w[i] = w[i] + A(i, j) * conj(U(j, k));

                // Estimate the phase from the largest component of z_k
                std::size_t max_idx = 0;
                T max_abs = T(0);
                for (std::size_t i = 0; i < n; ++i) {
                    T a = sangi::abs(U(i, k));
                    if (a > max_abs) { max_abs = a; max_idx = i; }
                }
                if (max_abs < std::numeric_limits<T>::epsilon()) continue;

                // ratio = w[max_idx] / (σ_k * z_k[max_idx]) = e^{-2iφ}
                Complex<T> ratio = w[max_idx] / (Complex<T>(d[k]) * U(max_idx, k));
                T phase_minus2phi = static_cast<T>(std::atan2(
                    static_cast<double>(ratio.im), static_cast<double>(ratio.re)));
                T phi = -phase_minus2phi / T(2);

                // z_k ← z_k * e^{-iφ}
                Complex<T> rot(static_cast<T>(std::cos(static_cast<double>(-phi))),
                               static_cast<T>(std::sin(static_cast<double>(-phi))));
                for (std::size_t i = 0; i < n; ++i)
                    U(i, k) = U(i, k) * rot;
            }

            return { std::move(U), std::move(d) };
        }

        //=====================================================================
        // CS decomposition (Cosine-Sine Decomposition)
        //=====================================================================

        /**
         * @brief Result of the CS decomposition
         *
         * Decompose an orthogonal matrix Q as follows:
         *   Q = [[U1, 0], [0, U2]] * Σ * [[V1, 0], [0, V2]]^T
         *
         * When p ≤ q, the central matrix Σ (m×m) is:
         *   Σ = [[C,   -S,  0 ],
         *        [S,    C,  0 ],
         *        [0,    0,  I_{q-p}]]
         * where C = diag(cos θ_i), S = diag(sin θ_i), C² + S² = I
         */
        template<typename T>
        struct CSDecompositionResult {
            Matrix<T> U1;       ///< p×p orthogonal matrix
            Matrix<T> U2;       ///< q×q orthogonal matrix (q = m - p)
            Matrix<T> V1;       ///< p×p orthogonal matrix
            Matrix<T> V2;       ///< q×q orthogonal matrix
            Vector<T> theta;    ///< r = min(p,q) angles θ_i ∈ [0, π/2]
        };

        namespace detail_cs {

            // Orthogonal completion: find the remaining columns of M (n×n) orthogonal to the known n_known columns
            template<typename T>
            void orthogonal_completion(Matrix<T>& M, std::size_t n_known) {
                std::size_t n = M.rows();
                std::size_t filled = n_known;
                for (std::size_t trial = 0; trial < n && filled < n; ++trial) {
                    // Candidate vector e_{trial}
                    Vector<T> v(n, T(0));
                    v[trial] = T(1);

                    // Remove the projection onto the known columns
                    for (std::size_t j = 0; j < filled; ++j) {
                        T dot = T(0);
                        for (std::size_t i = 0; i < n; ++i)
                            dot += M(i, j) * v[i];
                        for (std::size_t i = 0; i < n; ++i)
                            v[i] -= dot * M(i, j);
                    }

                    // Compute the norm
                    T norm_sq = T(0);
                    for (std::size_t i = 0; i < n; ++i)
                        norm_sq += v[i] * v[i];
                    T norm = std::sqrt(norm_sq);

                    if (norm > T(1e-10)) {
                        for (std::size_t i = 0; i < n; ++i)
                            M(i, filled) = v[i] / norm;
                        ++filled;
                    }
                }
            }

        } // namespace detail_cs

        /**
         * @brief CS decomposition (Cosine-Sine Decomposition)
         *
         * Decompose an orthogonal matrix Q (m×m) with partition size p.
         * Based on the SVD of Q's top-left block Q11 (p×p),
         * decompose into unitary block-diagonal factors and cosine/sine diagonal matrices.
         *
         * @param Q  orthogonal matrix (m×m)
         * @param p  partition size (m/2 if 0). 0 < p < m
         * @return CSDecompositionResult<T>
         * @throws DimensionError non-square matrix, empty matrix, invalid partition size
         */
        namespace detail_cs {

            // Internal implementation for p ≤ q:
            // Obtain U1, V1, theta from SVD(Q11) and build U2, V2 from Q21, Q12, Q22
            template<typename T>
            void solve_smaller_block(
                const BaseMatrix<T>& Q11_basemat, const BaseMatrix<T>& Q12_basemat,
                const BaseMatrix<T>& Q21_basemat, const BaseMatrix<T>& Q22_basemat,
                std::size_t p, std::size_t q,
                Matrix<T>& U1, Matrix<T>& U2,
                Matrix<T>& V1, Matrix<T>& V2,
                Vector<T>& theta)
            {
                Matrix<T> Q11(Q11_basemat);  // 1 copy via BaseMatrix ctor
                Matrix<T> Q12(Q12_basemat);  // 1 copy via BaseMatrix ctor
                Matrix<T> Q21(Q21_basemat);  // 1 copy via BaseMatrix ctor
                Matrix<T> Q22(Q22_basemat);  // 1 copy via BaseMatrix ctor
                std::size_t r = p; // min(p, q) = p

                // SVD(Q11) = U1 * diag(σ) * V1t
                Vector<T> sigma;
                Matrix<T> V1t;
                std::tie(U1, sigma, V1t) = svd_decomposition(Q11);
                V1 = V1t.transpose();

                // θ_i = arccos(σ_i)
                theta = Vector<T>(r);
                Vector<T> cos_th(r), sin_th(r);
                for (std::size_t i = 0; i < r; ++i) {
                    T c = std::clamp(sigma[i], T(0), T(1));
                    cos_th[i] = c;
                    sin_th[i] = std::sqrt(T(1) - c * c);
                    theta[i] = static_cast<T>(std::acos(static_cast<double>(c)));
                }

                // W = Q21 * V1 (q×p): column i ≈ sin(θ_i) * U2(:,i)
                Matrix<T> W(q, p, T(0));
                for (std::size_t i = 0; i < q; ++i)
                    for (std::size_t j = 0; j < p; ++j)
                        for (std::size_t k = 0; k < p; ++k)
                            W(i, j) += Q21(i, k) * V1(k, j);

                // N = Q12^T * U1 (q×p): column i ≈ -sin(θ_i) * V2(:,i)
                Matrix<T> N(q, p, T(0));
                for (std::size_t i = 0; i < q; ++i)
                    for (std::size_t j = 0; j < p; ++j)
                        for (std::size_t k = 0; k < p; ++k)
                            N(i, j) += Q12(k, i) * U1(k, j);

                const T eps = std::numeric_limits<T>::epsilon() * T(100);

                U2 = Matrix<T>(q, q, T(0));
                V2 = Matrix<T>(q, q, T(0));

                std::size_t n_good = 0;
                std::vector<bool> col_filled(q, false);

                for (std::size_t i = 0; i < r; ++i) {
                    if (sin_th[i] > eps) {
                        for (std::size_t row = 0; row < q; ++row) {
                            U2(row, i) = W(row, i) / sin_th[i];
                            V2(row, i) = -N(row, i) / sin_th[i];
                        }
                        col_filled[i] = true;
                        ++n_good;
                    }
                }

                // Determine the remaining columns from the residual of Q22
                if (n_good < q) {
                    Matrix<T> R(q, q, T(0));
                    for (std::size_t a = 0; a < q; ++a)
                        for (std::size_t b = 0; b < q; ++b)
                            R(a, b) = Q22(a, b);

                    for (std::size_t i = 0; i < r; ++i) {
                        if (col_filled[i]) {
                            T ci = cos_th[i];
                            for (std::size_t a = 0; a < q; ++a)
                                for (std::size_t b = 0; b < q; ++b)
                                    R(a, b) -= ci * U2(a, i) * V2(b, i);
                        }
                    }

                    auto [Ur, sr, Vrt] = svd_decomposition(R);

                    std::size_t svd_idx = 0;
                    for (std::size_t i = 0; i < q && svd_idx < q; ++i) {
                        if (!col_filled[i]) {
                            for (std::size_t row = 0; row < q; ++row) {
                                U2(row, i) = Ur(row, svd_idx);
                                V2(row, i) = Vrt(svd_idx, row);
                            }
                            ++svd_idx;
                        }
                    }
                }
            }

        } // namespace detail_cs (additional part)

        template<typename T>
        CSDecompositionResult<T> cs_decomposition(const BaseMatrix<T>& Q, std::size_t p = 0) {
            std::size_t m = Q.rows();
            if (m < 2 || Q.cols() != m) {
                assert(false && "DimensionError: cs_decomposition: requires a square matrix of size >= 2");
                throw DimensionError("cs_decomposition: requires a square matrix of size >= 2");
            }
            if (p == 0) p = m / 2;
            if (p == 0 || p >= m) {
                assert(false && "DimensionError: cs_decomposition: partition size p must satisfy 0 < p < m");
                throw DimensionError("cs_decomposition: partition size p must satisfy 0 < p < m");
            }

            std::size_t q = m - p;

            // Block extraction
            Matrix<T> Q11(p, p, T(0)), Q12(p, q, T(0));
            Matrix<T> Q21(q, p, T(0)), Q22(q, q, T(0));
            for (std::size_t i = 0; i < p; ++i) {
                for (std::size_t j = 0; j < p; ++j) Q11(i, j) = Q(i, j);
                for (std::size_t j = 0; j < q; ++j) Q12(i, j) = Q(i, p + j);
            }
            for (std::size_t i = 0; i < q; ++i) {
                for (std::size_t j = 0; j < p; ++j) Q21(i, j) = Q(p + i, j);
                for (std::size_t j = 0; j < q; ++j) Q22(i, j) = Q(p + i, p + j);
            }

            Matrix<T> U1, U2, V1, V2;
            Vector<T> theta;

            if (p <= q) {
                // p ≤ q: build U2, V2 starting from SVD(Q11) (p×p)
                detail_cs::solve_smaller_block(Q11, Q12, Q21, Q22, p, q,
                                               U1, U2, V1, V2, theta);
            } else {
                // p > q: build U1, V1 starting from SVD(Q22) (q×q)
                // Q22 = U2 * C * V2^T (C = diag(cos θ), q×q)
                std::size_t r = q;
                Vector<T> sigma;
                Matrix<T> V2t;
                std::tie(U2, sigma, V2t) = svd_decomposition(Q22);
                V2 = V2t.transpose();

                theta = Vector<T>(r);
                Vector<T> cos_th(r), sin_th(r);
                for (std::size_t i = 0; i < r; ++i) {
                    T c = std::clamp(sigma[i], T(0), T(1));
                    cos_th[i] = c;
                    sin_th[i] = std::sqrt(T(1) - c * c);
                    theta[i] = static_cast<T>(std::acos(static_cast<double>(c)));
                }

                // W = Q12 * V2 (p×q): column i = -sin(θ_i) * U1(:,i)
                Matrix<T> W(p, q, T(0));
                for (std::size_t i = 0; i < p; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        for (std::size_t k = 0; k < q; ++k)
                            W(i, j) += Q12(i, k) * V2(k, j);

                // N = Q21^T * U2 (p×q): column i = +sin(θ_i) * V1(:,i)
                Matrix<T> N(p, q, T(0));
                for (std::size_t i = 0; i < p; ++i)
                    for (std::size_t j = 0; j < q; ++j)
                        for (std::size_t k = 0; k < q; ++k)
                            N(i, j) += Q21(k, i) * U2(k, j);

                const T eps = std::numeric_limits<T>::epsilon() * T(100);

                U1 = Matrix<T>(p, p, T(0));
                V1 = Matrix<T>(p, p, T(0));

                std::size_t n_good = 0;
                std::vector<bool> col_filled(p, false);

                for (std::size_t i = 0; i < r; ++i) {
                    if (sin_th[i] > eps) {
                        for (std::size_t row = 0; row < p; ++row) {
                            U1(row, i) = -W(row, i) / sin_th[i]; // -(-sin * u1) / sin = u1
                            V1(row, i) = N(row, i) / sin_th[i];  // (+sin * v1) / sin = v1
                        }
                        col_filled[i] = true;
                        ++n_good;
                    }
                }

                // Determine the remaining columns from the residual of Q11
                // Q11 = U1 * [[C,0],[0,I_{p-q}]] * V1^T
                if (n_good < p) {
                    Matrix<T> R(p, p, T(0));
                    for (std::size_t a = 0; a < p; ++a)
                        for (std::size_t b = 0; b < p; ++b)
                            R(a, b) = Q11(a, b);

                    for (std::size_t i = 0; i < r; ++i) {
                        if (col_filled[i]) {
                            T ci = cos_th[i];
                            for (std::size_t a = 0; a < p; ++a)
                                for (std::size_t b = 0; b < p; ++b)
                                    R(a, b) -= ci * U1(a, i) * V1(b, i);
                        }
                    }

                    auto [Ur, sr, Vrt] = svd_decomposition(R);

                    std::size_t svd_idx = 0;
                    for (std::size_t i = 0; i < p && svd_idx < p; ++i) {
                        if (!col_filled[i]) {
                            for (std::size_t row = 0; row < p; ++row) {
                                U1(row, i) = Ur(row, svd_idx);
                                V1(row, i) = Vrt(svd_idx, row);
                            }
                            ++svd_idx;
                        }
                    }
                }
            }

            return { std::move(U1), std::move(U2),
                     std::move(V1), std::move(V2),
                     std::move(theta) };
        }

        //=====================================================================
        // Rank Decomposition
        //=====================================================================

        /**
         * @brief Result of the rank decomposition
         *
         * A = B * C  (B: m×r, C: r×n, r = rank(A))
         * SVD-based: B = U_r * diag(σ_r), C = V_r^T
         * (U_r: the first r columns, σ_r: the nonzero singular values, V_r^T: the first r rows)
         */
        template<typename T>
        struct RankDecompositionResult {
            Matrix<T> B;        ///< m×r matrix (column-space basis × singular values)
            Matrix<T> C;        ///< r×n matrix (row-space basis)
            std::size_t rank;   ///< numerical rank
        };

        /**
         * @brief Rank Decomposition
         *
         * Decompose a matrix A (m×n) into A = B * C (B: m×r, C: r×n).
         * Uses SVD and determines the numerical rank r by a threshold.
         *
         * @param A       input matrix (m×n)
         * @param rank    specified rank (auto-detected if 0)
         * @param tol     rank-determination threshold (max(m,n) * σ_max * ε if 0)
         * @return RankDecompositionResult<T>
         * @throws DimensionError empty matrix
         * @throws MathError if the specified rank is larger than the actual rank
         */
        template<typename T>
        RankDecompositionResult<T> rank_decomposition(
            const BaseMatrix<T>& A,
            std::size_t rank = 0,
            T tol = T(0))
        {
            std::size_t m = A.rows();
            std::size_t n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: rank_decomposition: empty matrix");
                throw DimensionError("rank_decomposition: empty matrix");
            }

            auto [U, sigma, Vt] = svd_decomposition(A);

            // Rank determination
            std::size_t k = sigma.size(); // min(m, n)
            if (tol <= T(0)) {
                // Default threshold: max(m,n) * σ_max * machine_epsilon
                T sigma_max = (k > 0) ? sigma[0] : T(0);
                tol = static_cast<T>(std::max(m, n)) * sigma_max
                    * std::numeric_limits<T>::epsilon();
            }

            std::size_t r = 0;
            for (std::size_t i = 0; i < k; ++i) {
                if (sigma[i] > tol)
                    ++r;
                else
                    break; // σ is in descending order
            }

            if (rank > 0) {
                if (rank > r)
                    throw MathError("rank_decomposition: specified rank exceeds numerical rank");
                r = rank;
            }

            // Case r == 0 (zero matrix)
            if (r == 0) {
                return { Matrix<T>(m, 0), Matrix<T>(0, n), 0 };
            }

            // B = U(:, 0:r) * diag(σ(0:r))  (m×r)
            Matrix<T> B(m, r, T(0));
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < r; ++j)
                    B(i, j) = U(i, j) * sigma[j];

            // C = Vt(0:r, :)  (r×n)
            Matrix<T> C(r, n, T(0));
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    C(i, j) = Vt(i, j);

            return { std::move(B), std::move(C), r };
        }

        //=====================================================================
        // CUR Decomposition
        //=====================================================================

        /**
         * @brief Result of the CUR decomposition
         *
         * A ≈ C * U * R
         *   C: m×c matrix (the selected c columns of A)
         *   U: c×r linking matrix
         *   R: r×n matrix (the selected r rows of A)
         *   col_indices: indices of the selected columns
         *   row_indices: indices of the selected rows
         */
        template<typename T>
        struct CURDecompositionResult {
            Matrix<T> C;                        ///< m×c matrix (selected columns)
            Matrix<T> U;                        ///< c×r linking matrix
            Matrix<T> R;                        ///< r×n matrix (selected rows)
            std::vector<std::size_t> col_indices;  ///< indices of the selected columns
            std::vector<std::size_t> row_indices;  ///< indices of the selected rows
        };

        /**
         * @brief CUR Decomposition
         *
         * Decompose a matrix A (m×n) into A ≈ C * U * R.
         * Deterministic column/row selection based on SVD leverage scores.
         * C is a column subset of A, R is a row subset of A,
         * U = C⁺ * A * R⁺ (linking via pseudo-inverses).
         *
         * @param A       input matrix (m×n)
         * @param k       number of columns/rows to select (uses the numerical rank if 0)
         * @return CURDecompositionResult<T>
         * @throws DimensionError empty matrix
         * @throws MathError if k exceeds min(m,n)
         */
        template<typename T>
        CURDecompositionResult<T> cur_decomposition(
            const BaseMatrix<T>& A,
            std::size_t k = 0)
        {
            std::size_t m = A.rows();
            std::size_t n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: cur_decomposition: empty matrix");
                throw DimensionError("cur_decomposition: empty matrix");
            }

            auto [U_svd, sigma, Vt] = svd_decomposition(A);
            std::size_t s = sigma.size(); // min(m, n)

            // Rank determination (automatic if k=0)
            if (k == 0) {
                T sigma_max = (s > 0) ? sigma[0] : T(0);
                T tol = static_cast<T>(std::max(m, n)) * sigma_max
                      * std::numeric_limits<T>::epsilon();
                k = 0;
                for (std::size_t i = 0; i < s; ++i) {
                    if (sigma[i] > tol) ++k;
                    else break;
                }
                if (k == 0) k = 1; // at least 1 column/row
            }
            if (k > std::min(m, n))
                throw MathError("cur_decomposition: k exceeds min(m, n)");

            // Column selection by leverage score (sum of squares of the top-k components of the right singular vectors)
            // col_score[j] = Σ_{i<k} Vt(i, j)²
            std::vector<std::pair<T, std::size_t>> col_scores(n);
            for (std::size_t j = 0; j < n; ++j) {
                T score = T(0);
                for (std::size_t i = 0; i < k && i < s; ++i)
                    score += Vt(i, j) * Vt(i, j);
                col_scores[j] = { score, j };
            }
            std::sort(col_scores.begin(), col_scores.end(),
                      [](const auto& a, const auto& b) { return a.first > b.first; });

            std::size_t c = std::min(k, n);
            std::vector<std::size_t> col_idx(c);
            for (std::size_t i = 0; i < c; ++i)
                col_idx[i] = col_scores[i].second;
            std::sort(col_idx.begin(), col_idx.end());

            // Row selection by leverage score (sum of squares of the top-k components of the left singular vectors)
            // row_score[i] = Σ_{j<k} U_svd(i, j)²
            std::vector<std::pair<T, std::size_t>> row_scores(m);
            for (std::size_t i = 0; i < m; ++i) {
                T score = T(0);
                for (std::size_t j = 0; j < k && j < s; ++j)
                    score += U_svd(i, j) * U_svd(i, j);
                row_scores[i] = { score, i };
            }
            std::sort(row_scores.begin(), row_scores.end(),
                      [](const auto& a, const auto& b) { return a.first > b.first; });

            std::size_t r = std::min(k, m);
            std::vector<std::size_t> row_idx(r);
            for (std::size_t i = 0; i < r; ++i)
                row_idx[i] = row_scores[i].second;
            std::sort(row_idx.begin(), row_idx.end());

            // C: the selected columns of A (m×c)
            Matrix<T> C_mat(m, c, T(0));
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < c; ++j)
                    C_mat(i, j) = A(i, col_idx[j]);

            // R: the selected rows of A (r×n)
            Matrix<T> R_mat(r, n, T(0));
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    R_mat(i, j) = A(row_idx[i], j);

            // U = C⁺ * A * R⁺
            // C⁺ (c×m) and R⁺ (n×r) are pseudo-inverses via SVD
            // W = the selected-rows × selected-columns submatrix of A (r×c)
            // U = W⁺ (the pseudo-inverse of W)
            Matrix<T> W(r, c, T(0));
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = 0; j < c; ++j)
                    W(i, j) = A(row_idx[i], col_idx[j]);

            // SVD of W → pseudo-inverse
            auto [Uw, sw, Vwt] = svd_decomposition(W);
            T sw_max = (sw.size() > 0) ? sw[0] : T(0);
            T pinv_tol = static_cast<T>(std::max(r, c)) * sw_max
                       * std::numeric_limits<T>::epsilon();

            // W⁺ = Vw * diag(1/σ) * Uw^T  (c×r)
            std::size_t sw_size = sw.size();
            Matrix<T> U_mat(c, r, T(0));
            for (std::size_t i = 0; i < c; ++i)
                for (std::size_t j = 0; j < r; ++j) {
                    T val = T(0);
                    for (std::size_t t = 0; t < sw_size; ++t) {
                        if (sw[t] > pinv_tol)
                            val += Vwt(t, i) * (T(1) / sw[t]) * Uw(j, t);
                    }
                    U_mat(i, j) = val;
                }

            return { std::move(C_mat), std::move(U_mat), std::move(R_mat),
                     std::move(col_idx), std::move(row_idx) };
        }

        //=====================================================================
        // NMF (Non-negative Matrix Factorization)
        //=====================================================================

        /**
         * @brief Result of NMF
         *
         * A ≈ W * H  (W: m×k, H: k×n, W,H ≥ 0)
         */
        template<typename T>
        struct NMFResult {
            Matrix<T> W;            ///< m×k basis matrix (nonnegative)
            Matrix<T> H;            ///< k×n coefficient matrix (nonnegative)
            T residual;             ///< Frobenius-norm residual ‖A - WH‖_F
            std::size_t iterations; ///< number of iterations performed
        };

        /**
         * @brief NMF (Non-negative Matrix Factorization)
         *
         * Decompose a nonnegative matrix A (m×n) into A ≈ W * H (W: m×k, H: k×n, W,H ≥ 0).
         * Uses the Lee-Seung multiplicative update rule.
         *
         * @param A         input matrix (m×n, all elements ≥ 0)
         * @param k         number of factors (1 ≤ k ≤ min(m, n))
         * @param max_iter  maximum number of iterations (default 200)
         * @param tol       convergence threshold (relative change of residual, default 1e-4)
         * @return NMFResult<T>
         * @throws DimensionError empty matrix, invalid k
         * @throws MathError if negative elements are present
         */
        template<typename T>
        NMFResult<T> nmf(
            const BaseMatrix<T>& A,
            std::size_t k,
            std::size_t max_iter = 200,
            T tol = T(1e-4))
        {
            std::size_t m = A.rows();
            std::size_t n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: nmf: empty matrix");
                throw DimensionError("nmf: empty matrix");
            }
            if (k == 0 || k > std::min(m, n)) {
                assert(false && "DimensionError: nmf: k must satisfy 1 <= k <= min(m, n)");
                throw DimensionError("nmf: k must satisfy 1 <= k <= min(m, n)");
            }

            // Nonnegativity check
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    if (A(i, j) < T(0))
                        throw MathError("nmf: matrix must be non-negative");

            const T eps_small = std::numeric_limits<T>::epsilon();

            // Initialize W, H via SVD-based NNDSVD
            // (Non-Negative Double SVD: Boutsidis & Gallopoulos 2008)
            auto [U_svd, sigma_svd, Vt_svd] = svd_decomposition(A);

            Matrix<T> W(m, k, T(0));
            Matrix<T> H(k, n, T(0));

            // 0-th factor: sqrt(σ_0) * |u_0|, sqrt(σ_0) * |v_0|
            if (sigma_svd.size() > 0 && sigma_svd[0] > T(0)) {
                T sq = std::sqrt(sigma_svd[0]);
                for (std::size_t i = 0; i < m; ++i)
                    W(i, 0) = sq * std::abs(U_svd(i, 0));
                for (std::size_t j = 0; j < n; ++j)
                    H(0, j) = sq * std::abs(Vt_svd(0, j));
            } else {
                // For a zero matrix: initialize with a small positive value
                for (std::size_t i = 0; i < m; ++i) W(i, 0) = eps_small;
                for (std::size_t j = 0; j < n; ++j) H(0, j) = eps_small;
            }

            // t-th factor (t >= 1): NNDSVD
            for (std::size_t t = 1; t < k; ++t) {
                if (t >= sigma_svd.size() || sigma_svd[t] <= T(0)) {
                    for (std::size_t i = 0; i < m; ++i) W(i, t) = eps_small;
                    for (std::size_t j = 0; j < n; ++j) H(t, j) = eps_small;
                    continue;
                }
                // u⁺, u⁻, v⁺, v⁻
                Vector<T> up(m, T(0)), un(m, T(0));
                Vector<T> vp(n, T(0)), vn(n, T(0));
                for (std::size_t i = 0; i < m; ++i) {
                    T val = U_svd(i, t);
                    if (val > T(0)) up[i] = val; else un[i] = -val;
                }
                for (std::size_t j = 0; j < n; ++j) {
                    T val = Vt_svd(t, j);
                    if (val > T(0)) vp[j] = val; else vn[j] = -val;
                }
                T up_norm = T(0), un_norm = T(0);
                T vp_norm = T(0), vn_norm = T(0);
                for (std::size_t i = 0; i < m; ++i) { up_norm += up[i]*up[i]; un_norm += un[i]*un[i]; }
                for (std::size_t j = 0; j < n; ++j) { vp_norm += vp[j]*vp[j]; vn_norm += vn[j]*vn[j]; }
                up_norm = std::sqrt(up_norm); un_norm = std::sqrt(un_norm);
                vp_norm = std::sqrt(vp_norm); vn_norm = std::sqrt(vn_norm);

                T mp = up_norm * vp_norm;
                T mn = un_norm * vn_norm;
                T sq = std::sqrt(sigma_svd[t]);
                if (mp >= mn) {
                    T scale_w = (up_norm > T(0)) ? sq * std::sqrt(mp) / up_norm : T(0);
                    T scale_h = (vp_norm > T(0)) ? sq * std::sqrt(mp) / vp_norm : T(0);
                    for (std::size_t i = 0; i < m; ++i) W(i, t) = std::max(up[i] * scale_w, eps_small);
                    for (std::size_t j = 0; j < n; ++j) H(t, j) = std::max(vp[j] * scale_h, eps_small);
                } else {
                    T scale_w = (un_norm > T(0)) ? sq * std::sqrt(mn) / un_norm : T(0);
                    T scale_h = (vn_norm > T(0)) ? sq * std::sqrt(mn) / vn_norm : T(0);
                    for (std::size_t i = 0; i < m; ++i) W(i, t) = std::max(un[i] * scale_w, eps_small);
                    for (std::size_t j = 0; j < n; ++j) H(t, j) = std::max(vn[j] * scale_h, eps_small);
                }
            }

            // Lee-Seung multiplicative update rule
            T prev_residual = std::numeric_limits<T>::max();
            std::size_t iter = 0;

            for (; iter < max_iter; ++iter) {
                // Update H: H ← H * (W^T A) / (W^T W H + ε)
                // WtA = W^T * A (k×n)
                Matrix<T> WtA(k, n, T(0));
                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        for (std::size_t t = 0; t < m; ++t)
                            WtA(i, j) += W(t, i) * A(t, j);

                // WtW = W^T * W (k×k)
                Matrix<T> WtW(k, k, T(0));
                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        for (std::size_t t = 0; t < m; ++t)
                            WtW(i, j) += W(t, i) * W(t, j);

                // WtWH = WtW * H (k×n)
                Matrix<T> WtWH(k, n, T(0));
                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        for (std::size_t t = 0; t < k; ++t)
                            WtWH(i, j) += WtW(i, t) * H(t, j);

                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = 0; j < n; ++j)
                        H(i, j) = H(i, j) * WtA(i, j) / (WtWH(i, j) + eps_small);

                // Update W: W ← W * (A H^T) / (W H H^T + ε)
                // AHt = A * H^T (m×k)
                Matrix<T> AHt(m, k, T(0));
                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        for (std::size_t t = 0; t < n; ++t)
                            AHt(i, j) += A(i, t) * H(j, t);

                // HHt = H * H^T (k×k)
                Matrix<T> HHt(k, k, T(0));
                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        for (std::size_t t = 0; t < n; ++t)
                            HHt(i, j) += H(i, t) * H(j, t);

                // WHHt = W * HHt (m×k)
                Matrix<T> WHHt(m, k, T(0));
                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        for (std::size_t t = 0; t < k; ++t)
                            WHHt(i, j) += W(i, t) * HHt(t, j);

                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        W(i, j) = W(i, j) * AHt(i, j) / (WHHt(i, j) + eps_small);

                // Compute the residual ‖A - WH‖_F
                T residual = T(0);
                for (std::size_t i = 0; i < m; ++i) {
                    for (std::size_t j = 0; j < n; ++j) {
                        T wh = T(0);
                        for (std::size_t t = 0; t < k; ++t)
                            wh += W(i, t) * H(t, j);
                        T d = A(i, j) - wh;
                        residual += d * d;
                    }
                }
                residual = std::sqrt(residual);

                // Convergence test
                if (prev_residual > T(0) &&
                    std::abs(prev_residual - residual) / prev_residual < tol) {
                    prev_residual = residual;
                    ++iter;
                    break;
                }
                prev_residual = residual;
            }

            return { std::move(W), std::move(H), prev_residual, iter };
        }

        //=====================================================================
        // UTV Decomposition
        //=====================================================================

        /**
         * @brief Result of the UTV decomposition
         *
         * A = U * T * V^T
         *   U: m×m orthogonal matrix
         *   T: m×n upper triangular matrix (diagonal approximates the singular values, descending)
         *   V: n×n orthogonal matrix
         *   rank: numerical rank
         */
        template<typename T>
        struct UTVDecompositionResult {
            Matrix<T> U;        ///< m×m orthogonal matrix
            Matrix<T> T_mat;    ///< m×n upper triangular matrix
            Matrix<T> V;        ///< n×n orthogonal matrix
            std::size_t rank;   ///< numerical rank
        };

        /**
         * @brief UTV Decomposition
         *
         * Decompose a matrix A (m×n) into A = U * T * V^T.
         * Built on column-pivoted QR decomposition,
         * T's diagonal approximates the singular values. A lower-cost rank determination than SVD.
         *
         * Algorithm:
         * 1. Column-pivoted QR: A*P = Q*R
         * 2. Refine V with the right singular vectors of R: R = U_R * Σ * V_R^T
         * 3. U = Q * U_R, T = Σ (singular values on the diagonal), V = P * V_R
         *
         * @param A    input matrix (m×n)
         * @param tol  rank-determination threshold (0: automatic)
         * @return UTVDecompositionResult<T>
         * @throws DimensionError empty matrix
         */
        template<typename T>
        UTVDecompositionResult<T> utv_decomposition(
            const BaseMatrix<T>& A,
            T tol = T(0))
        {
            std::size_t m = A.rows();
            std::size_t n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: utv_decomposition: empty matrix");
                throw DimensionError("utv_decomposition: empty matrix");
            }

            std::size_t k = std::min(m, n);

            // Column-pivoted QR: select columns in order of largest column norm
            // Working copy of A
            Matrix<T> W(A);
            std::vector<std::size_t> piv(n);
            for (std::size_t i = 0; i < n; ++i) piv[i] = i;

            // Compute the column norms
            Vector<T> col_norms(n, T(0));
            for (std::size_t j = 0; j < n; ++j)
                for (std::size_t i = 0; i < m; ++i)
                    col_norms[j] += W(i, j) * W(i, j);

            // Householder QR with column pivoting
            Vector<T> tau(k, T(0)); // Householder coefficients

            for (std::size_t step = 0; step < k; ++step) {
                // Search for the column with the largest norm
                std::size_t max_col = step;
                T max_norm = col_norms[step];
                for (std::size_t j = step + 1; j < n; ++j) {
                    if (col_norms[j] > max_norm) {
                        max_norm = col_norms[j];
                        max_col = j;
                    }
                }

                // Column swap
                if (max_col != step) {
                    for (std::size_t i = 0; i < m; ++i)
                        std::swap(W(i, step), W(i, max_col));
                    std::swap(col_norms[step], col_norms[max_col]);
                    std::swap(piv[step], piv[max_col]);
                }

                // Eliminate the lower triangle of column step via Householder transformation
                // v = W(step:m, step), β = householder_beta
                T norm_x = T(0);
                for (std::size_t i = step; i < m; ++i)
                    norm_x += W(i, step) * W(i, step);
                norm_x = std::sqrt(norm_x);

                if (norm_x < std::numeric_limits<T>::epsilon()) {
                    tau[step] = T(0);
                    continue;
                }

                T sign = (W(step, step) >= T(0)) ? T(1) : T(-1);
                T alpha = -sign * norm_x;
                T v0 = W(step, step) - alpha;
                tau[step] = -v0 / alpha; // equivalent to β = 2 / (v^T v)

                // Normalize v (v[0] = 1)
                if (std::abs(v0) > std::numeric_limits<T>::epsilon()) {
                    T inv_v0 = T(1) / v0;
                    for (std::size_t i = step + 1; i < m; ++i)
                        W(i, step) *= inv_v0;
                }
                W(step, step) = alpha;

                // Apply the Householder transformation to the remaining columns
                for (std::size_t j = step + 1; j < n; ++j) {
                    T dot = W(step, j);
                    for (std::size_t i = step + 1; i < m; ++i)
                        dot += W(i, step) * W(i, j);
                    dot *= tau[step];
                    W(step, j) -= dot;
                    for (std::size_t i = step + 1; i < m; ++i)
                        W(i, j) -= W(i, step) * dot;
                }

                // Update the column norms
                for (std::size_t j = step + 1; j < n; ++j)
                    col_norms[j] -= W(step, j) * W(step, j);
            }

            // Extract R (k×n upper triangular)
            Matrix<T> R(k, n, T(0));
            for (std::size_t i = 0; i < k; ++i)
                for (std::size_t j = i; j < n; ++j)
                    R(i, j) = W(i, j);

            // Build Q (m×m): accumulation of the Householder transformations
            Matrix<T> Q = Matrix<T>::identity(m);
            for (std::size_t step = k; step-- > 0; ) {
                if (std::abs(tau[step]) < std::numeric_limits<T>::epsilon())
                    continue;
                // v = [1, W(step+1:m, step)]
                for (std::size_t j = 0; j < m; ++j) {
                    T dot = Q(step, j);
                    for (std::size_t i = step + 1; i < m; ++i)
                        dot += W(i, step) * Q(i, j);
                    dot *= tau[step];
                    Q(step, j) -= dot;
                    for (std::size_t i = step + 1; i < m; ++i)
                        Q(i, j) -= W(i, step) * dot;
                }
            }

            // SVD of R: R = U_R * Σ * V_R^T
            auto [U_R, sigma, V_Rt] = svd_decomposition(R);

            // Rank determination
            if (tol <= T(0)) {
                T sigma_max = (sigma.size() > 0) ? sigma[0] : T(0);
                tol = static_cast<T>(std::max(m, n)) * sigma_max
                    * std::numeric_limits<T>::epsilon();
            }
            std::size_t rank = 0;
            for (std::size_t i = 0; i < sigma.size(); ++i) {
                if (sigma[i] > tol) ++rank;
                else break;
            }

            // Build the T matrix (m×n): top k×n is Σ (singular values on the diagonal + zeros above)
            Matrix<T> T_mat(m, n, T(0));
            for (std::size_t i = 0; i < sigma.size(); ++i)
                T_mat(i, i) = sigma[i];

            // U = Q * U_R_ext: Q (m×m) * U_R_ext (m×m)
            // U_R is the left singular vectors of SVD(R) (k×k or k×min(k,n))
            // U_R_ext: U_R top-left, I_{m-k} bottom-right
            std::size_t ur_rows = U_R.rows();
            std::size_t ur_cols = U_R.cols();
            Matrix<T> U_full(m, m, T(0));
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < m; ++j) {
                    T val = T(0);
                    for (std::size_t t = 0; t < m; ++t) {
                        T ur_val;
                        if (t < ur_rows && j < ur_cols)
                            ur_val = U_R(t, j);
                        else
                            ur_val = (t == j) ? T(1) : T(0);
                        val += Q(i, t) * ur_val;
                    }
                    U_full(i, j) = val;
                }

            // V = P * V_R: apply the pivot permutation
            // V_Rt is the transposed right singular vectors of SVD(R). When R is k×n,
            // V_Rt is min(k,n)×n or n×n (depending on the SVD implementation)
            // V_R = V_Rt^T: V_R(i,j) = V_Rt(j,i)
            Matrix<T> V_full(n, n, T(0));
            std::size_t vrt_rows = V_Rt.rows();
            std::size_t vrt_cols = V_Rt.cols();
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j) {
                    T vr_val;
                    if (j < vrt_rows && i < vrt_cols)
                        vr_val = V_Rt(j, i);
                    else
                        vr_val = (i == j) ? T(1) : T(0);
                    V_full(piv[i], j) = vr_val;
                }

            return { std::move(U_full), std::move(T_mat),
                     std::move(V_full), rank };
        }

        //=====================================================================
        // RRQR (Rank-Revealing QR)
        //=====================================================================

        /**
         * @brief Result of RRQR
         *
         * A * P = Q * R  (Q: m×m orthogonal, R: m×n upper triangular, P: n×n permutation)
         * R's diagonal approximates the singular values and reveals the rank.
         * A simplified version of Strong RRQR (Gu-Eisenstat): column-pivoted QR +
         * rank determination in post-processing.
         */
        template<typename T>
        struct RRQRResult {
            Matrix<T> Q;                        ///< m×m orthogonal matrix
            Matrix<T> R;                        ///< m×n upper triangular matrix
            std::vector<std::size_t> perm;      ///< column permutation (perm[j] = original column index)
            std::size_t rank;                   ///< numerical rank
        };

        /**
         * @brief RRQR (Rank-Revealing QR)
         *
         * Apply column-pivoted QR decomposition to a matrix A (m×n) so that
         * R(k,k) becomes a good approximation of the singular values.
         *
         * @param A    input matrix (m×n)
         * @param tol  rank-determination threshold (0: automatic = max(m,n) * ‖A‖ * ε)
         * @return RRQRResult<T>
         * @throws DimensionError empty matrix
         */
        template<typename T>
        RRQRResult<T> rrqr(const BaseMatrix<T>& A, T tol = T(0)) {
            std::size_t m = A.rows();
            std::size_t n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: rrqr: empty matrix");
                throw DimensionError("rrqr: empty matrix");
            }

            std::size_t k = std::min(m, n);

            // Working copy
            Matrix<T> W(A);
            std::vector<std::size_t> piv(n);
            for (std::size_t i = 0; i < n; ++i) piv[i] = i;

            // Column norms
            Vector<T> col_norms(n, T(0));
            for (std::size_t j = 0; j < n; ++j)
                for (std::size_t i = 0; i < m; ++i)
                    col_norms[j] += W(i, j) * W(i, j);

            // Householder coefficients
            Vector<T> tau(k, T(0));

            for (std::size_t step = 0; step < k; ++step) {
                // Select the column with the largest norm
                std::size_t max_col = step;
                T max_norm = col_norms[step];
                for (std::size_t j = step + 1; j < n; ++j) {
                    if (col_norms[j] > max_norm) {
                        max_norm = col_norms[j];
                        max_col = j;
                    }
                }

                // Column swap
                if (max_col != step) {
                    for (std::size_t i = 0; i < m; ++i)
                        std::swap(W(i, step), W(i, max_col));
                    std::swap(col_norms[step], col_norms[max_col]);
                    std::swap(piv[step], piv[max_col]);
                }

                // Householder transformation
                T norm_x = T(0);
                for (std::size_t i = step; i < m; ++i)
                    norm_x += W(i, step) * W(i, step);
                norm_x = std::sqrt(norm_x);

                if (norm_x < std::numeric_limits<T>::epsilon()) {
                    tau[step] = T(0);
                    continue;
                }

                T sign = (W(step, step) >= T(0)) ? T(1) : T(-1);
                T alpha = -sign * norm_x;
                T v0 = W(step, step) - alpha;
                tau[step] = -v0 / alpha;

                if (std::abs(v0) > std::numeric_limits<T>::epsilon()) {
                    T inv_v0 = T(1) / v0;
                    for (std::size_t i = step + 1; i < m; ++i)
                        W(i, step) *= inv_v0;
                }
                W(step, step) = alpha;

                // Apply to the remaining columns
                for (std::size_t j = step + 1; j < n; ++j) {
                    T dot = W(step, j);
                    for (std::size_t i = step + 1; i < m; ++i)
                        dot += W(i, step) * W(i, j);
                    dot *= tau[step];
                    W(step, j) -= dot;
                    for (std::size_t i = step + 1; i < m; ++i)
                        W(i, j) -= W(i, step) * dot;
                }

                // Update the column norms
                for (std::size_t j = step + 1; j < n; ++j) {
                    col_norms[j] -= W(step, j) * W(step, j);
                    if (col_norms[j] < T(0)) col_norms[j] = T(0);
                }
            }

            // Extract R (m×n)
            Matrix<T> R_mat(m, n, T(0));
            for (std::size_t i = 0; i < k; ++i)
                for (std::size_t j = i; j < n; ++j)
                    R_mat(i, j) = W(i, j);

            // Build Q (m×m)
            Matrix<T> Q_mat = Matrix<T>::identity(m);
            for (std::size_t step = k; step-- > 0; ) {
                if (std::abs(tau[step]) < std::numeric_limits<T>::epsilon())
                    continue;
                for (std::size_t j = 0; j < m; ++j) {
                    T dot = Q_mat(step, j);
                    for (std::size_t i = step + 1; i < m; ++i)
                        dot += W(i, step) * Q_mat(i, j);
                    dot *= tau[step];
                    Q_mat(step, j) -= dot;
                    for (std::size_t i = step + 1; i < m; ++i)
                        Q_mat(i, j) -= W(i, step) * dot;
                }
            }

            // Rank determination
            if (tol <= T(0)) {
                // |R(0,0)| ≈ σ_max
                T r_max = (k > 0) ? std::abs(R_mat(0, 0)) : T(0);
                tol = static_cast<T>(std::max(m, n)) * r_max
                    * std::numeric_limits<T>::epsilon();
            }
            std::size_t rank = 0;
            for (std::size_t i = 0; i < k; ++i) {
                if (std::abs(R_mat(i, i)) > tol)
                    ++rank;
                else
                    break;
            }

            return { std::move(Q_mat), std::move(R_mat),
                     std::move(piv), rank };
        }

        //=====================================================================
        // Block LU Decomposition
        //=====================================================================

        /**
         * @brief Result of the block LU decomposition
         *
         * A = P * L * U  (L: lower triangular, U: upper triangular, P: row permutation)
         * Computes in block units, leveraging matrix-matrix multiplication equivalent to
         * BLAS Level 3 (gemm) to improve cache efficiency.
         */
        template<typename T>
        struct BlockLUResult {
            Matrix<T> L;                                    ///< n×n lower triangular matrix (diagonal = 1)
            Matrix<T> U;                                    ///< n×n upper triangular matrix
            std::vector<std::size_t> pivots;                ///< row permutation
            std::size_t block_size;                         ///< block size used
        };

        /**
         * @brief Block LU Decomposition
         *
         * Perform LU decomposition of a square matrix A (n×n) in block units.
         * At each step, factor an nb-column panel with ordinary LU, and
         * update the remaining part with matrix-matrix multiplication (gemm-equivalent).
         *
         * @param A         input matrix (n×n, square)
         * @param nb        block size (0: automatic = max(1, n/4))
         * @return BlockLUResult<T>
         * @throws DimensionError empty matrix, non-square matrix
         * @throws MathError singular matrix
         */
        template<typename T>
        BlockLUResult<T> block_lu_decomposition(
            const BaseMatrix<T>& A,
            std::size_t nb = 0)
        {
            std::size_t n = A.rows();
            if (n == 0 || A.cols() != n) {
                assert(false && "DimensionError: block_lu_decomposition: requires a non-empty square matrix");
                throw DimensionError("block_lu_decomposition: requires a non-empty square matrix");
            }

            if (nb == 0)
                nb = std::max<std::size_t>(1, n / 4);
            nb = std::min(nb, n);

            // Working copy: keep LU together
            Matrix<T> LU(A);
            std::vector<std::size_t> piv(n);
            for (std::size_t i = 0; i < n; ++i) piv[i] = i;

            // Relative threshold for singularity: ||A||_max * n * eps
            T a_max = T(0);
            for (std::size_t i = 0; i < n; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    a_max = std::max(a_max, std::abs(A(i, j)));
            const T sing_tol = a_max * static_cast<T>(n) * std::numeric_limits<T>::epsilon();

            for (std::size_t jb = 0; jb < n; jb += nb) {
                std::size_t jend = std::min(jb + nb, n);
                std::size_t bsize = jend - jb;

                // Panel LU: factor columns jb..jend-1 with partial pivoting
                for (std::size_t j = jb; j < jend; ++j) {
                    // Pivot selection
                    std::size_t pivot_row = j;
                    T pivot_val = std::abs(LU(j, j));
                    for (std::size_t i = j + 1; i < n; ++i) {
                        T av = std::abs(LU(i, j));
                        if (av > pivot_val) {
                            pivot_val = av;
                            pivot_row = i;
                        }
                    }
                    piv[j] = pivot_row;

                    // Row swap (across all columns)
                    if (pivot_row != j) {
                        for (std::size_t c = 0; c < n; ++c)
                            std::swap(LU(j, c), LU(pivot_row, c));
                    }

                    // Singularity check
                    if (std::abs(LU(j, j)) <= sing_tol)
                        throw MathError("block_lu_decomposition: singular matrix");

                    // Compute column j of L (rows j+1..n-1)
                    T inv_diag = T(1) / LU(j, j);
                    for (std::size_t i = j + 1; i < n; ++i)
                        LU(i, j) *= inv_diag;

                    // Update the remaining columns in the panel (rank-1 update)
                    for (std::size_t c = j + 1; c < jend; ++c)
                        for (std::size_t i = j + 1; i < n; ++i)
                            LU(i, c) -= LU(i, j) * LU(j, c);
                }

                if (jend < n) {
                    // Forward substitution for U12: L11^{-1} * A12 → U12
                    // The panel LU already applied the pivot row swaps to all columns, but
                    // the rank-1 update is only within the panel columns. Reflect L11's effect on the right columns.
                    for (std::size_t j = jb + 1; j < jend; ++j)
                        for (std::size_t c = jend; c < n; ++c)
                            for (std::size_t k = jb; k < j; ++k)
                                LU(j, c) -= LU(j, k) * LU(k, c);

                    // Update the remaining block (gemm): A22 -= L21 * U12
                    for (std::size_t i = jend; i < n; ++i)
                        for (std::size_t c = jend; c < n; ++c) {
                            T sum = T(0);
                            for (std::size_t t = jb; t < jend; ++t)
                                sum += LU(i, t) * LU(t, c);
                            LU(i, c) -= sum;
                        }
                }
            }

            // Separate L and U
            Matrix<T> L = Matrix<T>::identity(n);
            Matrix<T> U(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) {
                for (std::size_t j = 0; j < i; ++j)
                    L(i, j) = LU(i, j);
                for (std::size_t j = i; j < n; ++j)
                    U(i, j) = LU(i, j);
            }

            return { std::move(L), std::move(U), std::move(piv), nb };
        }

        /**
         * @brief Solve a system of linear equations using block LU: Ax = b
         */
        template<typename T>
        Vector<T> block_lu_solve(
            const BlockLUResult<T>& lu,
            const BaseVector<T>& b_basevec)
        {
            Vector<T> b(b_basevec);  // 1 copy via BaseVector ctor
            std::size_t n = lu.L.rows();
            if (b.size() != n) {
                assert(false && "DimensionError: block_lu_solve: dimension mismatch");
                throw DimensionError("block_lu_solve: dimension mismatch");
            }

            // Apply the pivot permutation
            Vector<T> x(b);
            for (std::size_t i = 0; i < n; ++i) {
                if (lu.pivots[i] != i)
                    std::swap(x[i], x[lu.pivots[i]]);
            }

            // Forward substitution: L * y = P * b
            for (std::size_t i = 1; i < n; ++i)
                for (std::size_t j = 0; j < i; ++j)
                    x[i] -= lu.L(i, j) * x[j];

            // Backward substitution: U * x = y
            for (std::size_t i = n; i-- > 0; ) {
                for (std::size_t j = i + 1; j < n; ++j)
                    x[i] -= lu.U(i, j) * x[j];
                x[i] /= lu.U(i, i);
            }

            return x;
        }

        //=====================================================================
        // HSS decomposition (Hierarchically Semi-Separable)
        //=====================================================================

        /**
         * @brief HSS node: represents each node of the hierarchically semi-separable structure
         *
         * Leaf node: holds the diagonal block D
         * Internal node: holds the generator matrices U, V (off-diagonal low-rank representation) and
         *             the transformation matrix B (sibling coupling), R, W (compression matrices)
         *
         * Off-diagonal block A(i,j) ≈ U_i * B_{ij} * V_j^T (i≠j)
         */
        template<typename T>
        struct HSSNode {
            bool is_leaf = true;
            std::size_t row_start = 0;       ///< starting row index
            std::size_t row_size = 0;         ///< number of rows
            std::size_t col_start = 0;        ///< starting column index
            std::size_t col_size = 0;         ///< number of columns

            // Leaf node: diagonal block
            Matrix<T> D;

            // All nodes: generator matrices (for low-rank representation)
            Matrix<T> U;                      ///< row-direction generator matrix
            Matrix<T> V;                      ///< column-direction generator matrix

            // Internal node: coupling matrix between sibling nodes
            Matrix<T> B;                      ///< sibling coupling matrix

            // Internal node: child-to-parent transformation matrices
            Matrix<T> R;                      ///< compression of U: U_parent = [R_left * U_left; R_right * U_right]
            Matrix<T> W;                      ///< compression of V: V_parent = [W_left * V_left; W_right * V_right]

            // Child nodes
            std::unique_ptr<HSSNode<T>> left;
            std::unique_ptr<HSSNode<T>> right;
        };

        /**
         * @brief Result of the HSS decomposition
         */
        template<typename T>
        struct HSSResult {
            HSSNode<T> root;                  ///< root of the HSS tree
            std::size_t leaf_size;             ///< maximum size of a leaf node
            std::size_t rank;                  ///< rank of the off-diagonal low-rank approximation
        };

        namespace detail_hss {

            /**
             * @brief Build the HSS tree (partitioning only; matrix values not set)
             */
            template<typename T>
            std::unique_ptr<HSSNode<T>> build_tree(
                std::size_t row_start, std::size_t row_size,
                std::size_t col_start, std::size_t col_size,
                std::size_t leaf_size)
            {
                auto node = std::make_unique<HSSNode<T>>();
                node->row_start = row_start;
                node->row_size  = row_size;
                node->col_start = col_start;
                node->col_size  = col_size;

                if (row_size <= leaf_size && col_size <= leaf_size) {
                    node->is_leaf = true;
                } else {
                    node->is_leaf = false;
                    std::size_t rmid = row_size / 2;
                    std::size_t cmid = col_size / 2;
                    node->left  = build_tree<T>(row_start, rmid, col_start, cmid, leaf_size);
                    node->right = build_tree<T>(row_start + rmid, row_size - rmid,
                                                col_start + cmid, col_size - cmid, leaf_size);
                }
                return node;
            }

            /**
             * @brief Extract a subblock of the matrix
             */
            template<typename T>
            Matrix<T> extract_block(const BaseMatrix<T>& A,
                                    std::size_t r0, std::size_t nr,
                                    std::size_t c0, std::size_t nc)
            {
                Matrix<T> B(nr, nc, T(0));
                for (std::size_t i = 0; i < nr; ++i)
                    for (std::size_t j = 0; j < nc; ++j)
                        B(i, j) = A(r0 + i, c0 + j);
                return B;
            }

            /**
             * @brief Low-rank approximation: A ≈ U * S * V^T up to rank via truncated SVD
             * @return {U_r (m×rank), V_r (n×rank)} — U_r includes the singular values
             */
            template<typename T>
            std::pair<Matrix<T>, Matrix<T>> low_rank_approx(
                const BaseMatrix<T>& A, std::size_t rank)
            {
                std::size_t m = A.rows(), n = A.cols();
                if (m == 0 || n == 0) {
                    return { Matrix<T>(m, 0, T(0)), Matrix<T>(n, 0, T(0)) };
                }

                auto [U_svd, sigma, Vt_svd] = svd_decomposition(A);
                std::size_t k = std::min({ rank, m, n,
                    static_cast<std::size_t>(sigma.size()) });

                // Further limit the effective rank: truncate tiny singular values
                T threshold = (k > 0 ? sigma[0] : T(0)) *
                              static_cast<T>(std::max(m, n)) *
                              std::numeric_limits<T>::epsilon();
                std::size_t eff_k = 0;
                for (std::size_t i = 0; i < k; ++i) {
                    if (sigma[i] > threshold) ++eff_k;
                    else break;
                }
                k = std::max<std::size_t>(eff_k, 1);  // at least 1

                // U_r = U(:, 0:k) * diag(S(0:k))
                Matrix<T> Ur(m, k, T(0));
                for (std::size_t i = 0; i < m; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        Ur(i, j) = U_svd(i, j) * sigma[j];

                // V_r = V(:, 0:k)  — Vt_svd is V^T, so V(i,j) = Vt_svd(j,i)
                Matrix<T> Vr(n, k, T(0));
                for (std::size_t i = 0; i < n; ++i)
                    for (std::size_t j = 0; j < k; ++j)
                        Vr(i, j) = Vt_svd(j, i);

                return { std::move(Ur), std::move(Vr) };
            }

            /**
             * @brief Set the matrix values into the HSS tree (bottom-up, leaf→root)
             *
             * Leaf node: D = the diagonal block of A, U/V are the low-rank approximation of the off-diagonal rows
             * Internal node: compress the children's U/V to compute R/W, B is the sibling coupling
             */
            template<typename T>
            void compress_node(
                HSSNode<T>& node,
                const BaseMatrix<T>& A,
                std::size_t rank)
            {
                std::size_t n = A.rows();

                if (node.is_leaf) {
                    // Diagonal block
                    node.D = extract_block(A, node.row_start, node.row_size,
                                              node.col_start, node.col_size);

                    // Off-diagonal rows: among this node's rows, the columns other than the diagonal block
                    // Exclude the diagonal-block columns from all columns of rows row_start..row_start+row_size-1
                    std::size_t off_cols = n - node.col_size;
                    if (off_cols == 0) {
                        // The whole matrix is one block → no off-diagonal
                        node.U = Matrix<T>(node.row_size, 1, T(0));
                        node.V = Matrix<T>(1, 1, T(1));  // dummy
                        return;
                    }

                    // Construct the off-diagonal row block
                    Matrix<T> off_row(node.row_size, off_cols, T(0));
                    std::size_t jj = 0;
                    for (std::size_t j = 0; j < n; ++j) {
                        if (j >= node.col_start && j < node.col_start + node.col_size)
                            continue;
                        for (std::size_t i = 0; i < node.row_size; ++i)
                            off_row(i, jj) = A(node.row_start + i, j);
                        ++jj;
                    }

                    auto [Ur, Vr_unused] = low_rank_approx(off_row, rank);
                    node.U = std::move(Ur);

                    // Off-diagonal column block: rows other than the diagonal block, this node's columns
                    std::size_t off_rows = n - node.row_size;
                    Matrix<T> off_col(off_rows, node.col_size, T(0));
                    std::size_t ii = 0;
                    for (std::size_t i = 0; i < n; ++i) {
                        if (i >= node.row_start && i < node.row_start + node.row_size)
                            continue;
                        for (std::size_t j = 0; j < node.col_size; ++j)
                            off_col(ii, j) = A(i, node.col_start + j);
                        ++ii;
                    }

                    auto [Uc, sigma_c, Vtc] = svd_decomposition(off_col);
                    std::size_t k = std::min({ rank, off_rows, node.col_size,
                        static_cast<std::size_t>(sigma_c.size()) });
                    T threshold = (k > 0 ? sigma_c[0] : T(0)) *
                                  static_cast<T>(std::max(off_rows, node.col_size)) *
                                  std::numeric_limits<T>::epsilon();
                    std::size_t eff_k = 0;
                    for (std::size_t i = 0; i < k; ++i) {
                        if (sigma_c[i] > threshold) ++eff_k;
                        else break;
                    }
                    k = std::max<std::size_t>(eff_k, 1);

                    // V = V(:, 0:k)  — Vtc is V^T, so V(i,j) = Vtc(j,i)
                    Matrix<T> Vr(node.col_size, k, T(0));
                    for (std::size_t i = 0; i < node.col_size; ++i)
                        for (std::size_t j = 0; j < k; ++j)
                            Vr(i, j) = Vtc(j, i);
                    node.V = std::move(Vr);

                    return;
                }

                // Internal node: compress the child nodes first
                compress_node(*node.left, A, rank);
                compress_node(*node.right, A, rank);

                auto& lc = *node.left;
                auto& rc = *node.right;

                // Sibling coupling matrix B: off-diagonal block between siblings
                // A(left_rows, right_cols) ≈ U_left * B_lr * V_right^T
                // → B_lr = U_left^+ * A(left_rows, right_cols) * V_right^{+T}
                // where U_left^+ = (U^T U)^{-1} U^T (pseudo-inverse)
                Matrix<T> A_lr = extract_block(A, lc.row_start, lc.row_size,
                                                  rc.col_start, rc.col_size);

                // B = pinv(U_left) * A_lr * pinv(V_right)^T
                // pinv(X) via SVD
                auto compute_pinv = [](const Matrix<T>& X) -> Matrix<T> {
                    if (X.rows() == 0 || X.cols() == 0)
                        return Matrix<T>(X.cols(), X.rows(), T(0));
                    auto [U_p, sigma_p, Vt_p] = svd_decomposition(X);
                    std::size_t m = X.rows(), nn = X.cols();
                    std::size_t k = std::min(m, nn);
                    T thr = sigma_p[0] * static_cast<T>(std::max(m, nn)) *
                            std::numeric_limits<T>::epsilon();
                    // pinv = V * S^{-1} * U^T = Vt^T * S^{-1} * U^T
                    Matrix<T> result(nn, m, T(0));
                    for (std::size_t i = 0; i < nn; ++i)
                        for (std::size_t j = 0; j < m; ++j) {
                            T sum = T(0);
                            for (std::size_t t = 0; t < k; ++t) {
                                if (sigma_p[t] > thr)
                                    sum += Vt_p(t, i) * (T(1) / sigma_p[t]) * U_p(j, t);
                            }
                            result(i, j) = sum;
                        }
                    return result;
                };

                Matrix<T> Ul_pinv = compute_pinv(lc.U);
                Matrix<T> Vr_pinv = compute_pinv(rc.V);

                // B = Ul_pinv * A_lr * Vr_pinv^T
                std::size_t brows = Ul_pinv.rows(), bcols = Vr_pinv.rows();
                Matrix<T> temp(brows, A_lr.cols(), T(0));
                for (std::size_t i = 0; i < brows; ++i)
                    for (std::size_t j = 0; j < A_lr.cols(); ++j)
                        for (std::size_t t = 0; t < A_lr.rows(); ++t)
                            temp(i, j) += Ul_pinv(i, t) * A_lr(t, j);

                node.B = Matrix<T>(brows, bcols, T(0));
                for (std::size_t i = 0; i < brows; ++i)
                    for (std::size_t j = 0; j < bcols; ++j)
                        for (std::size_t t = 0; t < A_lr.cols(); ++t)
                            node.B(i, j) += temp(i, t) * Vr_pinv(j, t);

                // Synthesize the parent's U, V generator matrices from the children
                // Construct the off-diagonal row block
                std::size_t total_rows = node.row_size;
                std::size_t off_cols = n - node.col_size;
                if (off_cols == 0) {
                    std::size_t uk = lc.U.cols();
                    node.U = Matrix<T>(total_rows, uk, T(0));
                    node.R = Matrix<T>(uk, uk, T(0));
                    node.W = Matrix<T>(uk, uk, T(0));
                    node.V = Matrix<T>(node.col_size, 1, T(0));
                    return;
                }

                Matrix<T> off_row(total_rows, off_cols, T(0));
                std::size_t jj = 0;
                for (std::size_t j = 0; j < n; ++j) {
                    if (j >= node.col_start && j < node.col_start + node.col_size)
                        continue;
                    for (std::size_t i = 0; i < total_rows; ++i)
                        off_row(i, jj) = A(node.row_start + i, j);
                    ++jj;
                }

                auto [Ur_parent, Vr_unused2] = low_rank_approx(off_row, rank);
                node.U = std::move(Ur_parent);

                // Off-diagonal column block
                std::size_t off_rows = n - node.row_size;
                Matrix<T> off_col(off_rows, node.col_size, T(0));
                std::size_t ii = 0;
                for (std::size_t i = 0; i < n; ++i) {
                    if (i >= node.row_start && i < node.row_start + node.row_size)
                        continue;
                    for (std::size_t j = 0; j < node.col_size; ++j)
                        off_col(ii, j) = A(i, node.col_start + j);
                    ++ii;
                }

                auto [Uc2, sigma_c2, Vtc2] = svd_decomposition(off_col);
                std::size_t k2 = std::min({ rank, off_rows, node.col_size,
                    static_cast<std::size_t>(sigma_c2.size()) });
                T thr2 = (k2 > 0 ? sigma_c2[0] : T(0)) *
                         static_cast<T>(std::max(off_rows, node.col_size)) *
                         std::numeric_limits<T>::epsilon();
                std::size_t eff2 = 0;
                for (std::size_t i = 0; i < k2; ++i) {
                    if (sigma_c2[i] > thr2) ++eff2;
                    else break;
                }
                k2 = std::max<std::size_t>(eff2, 1);

                Matrix<T> Vr_parent(node.col_size, k2, T(0));
                for (std::size_t i = 0; i < node.col_size; ++i)
                    for (std::size_t j = 0; j < k2; ++j)
                        Vr_parent(i, j) = Vtc2(j, i);
                node.V = std::move(Vr_parent);

                // R: U_parent ≈ [R_left * U_left; R_right * U_right]
                // R_left = pinv(U_left) * U_parent(left_rows, :)
                std::size_t lrows = lc.row_size;
                Matrix<T> Up_left(lrows, node.U.cols(), T(0));
                for (std::size_t i = 0; i < lrows; ++i)
                    for (std::size_t j = 0; j < node.U.cols(); ++j)
                        Up_left(i, j) = node.U(i, j);

                Matrix<T> Ul_pinv2 = compute_pinv(lc.U);
                node.R = Matrix<T>(Ul_pinv2.rows(), Up_left.cols(), T(0));
                for (std::size_t i = 0; i < node.R.rows(); ++i)
                    for (std::size_t j = 0; j < node.R.cols(); ++j)
                        for (std::size_t t = 0; t < lrows; ++t)
                            node.R(i, j) += Ul_pinv2(i, t) * Up_left(t, j);

                // W: V_parent ≈ [W_left * V_left; W_right * V_right]
                std::size_t lcols = lc.col_size;
                Matrix<T> Vp_left(lcols, node.V.cols(), T(0));
                for (std::size_t i = 0; i < lcols; ++i)
                    for (std::size_t j = 0; j < node.V.cols(); ++j)
                        Vp_left(i, j) = node.V(i, j);

                Matrix<T> Vl_pinv = compute_pinv(lc.V);
                node.W = Matrix<T>(Vl_pinv.rows(), Vp_left.cols(), T(0));
                for (std::size_t i = 0; i < node.W.rows(); ++i)
                    for (std::size_t j = 0; j < node.W.cols(); ++j)
                        for (std::size_t t = 0; t < lcols; ++t)
                            node.W(i, j) += Vl_pinv(i, t) * Vp_left(t, j);
            }

            /**
             * @brief Reconstruct the dense matrix from the HSS representation (for verification)
             */
            template<typename T>
            void reconstruct_node(
                const HSSNode<T>& node,
                Matrix<T>& A)
            {
                if (node.is_leaf) {
                    // Write the diagonal block
                    for (std::size_t i = 0; i < node.row_size; ++i)
                        for (std::size_t j = 0; j < node.col_size; ++j)
                            A(node.row_start + i, node.col_start + j) = node.D(i, j);
                    return;
                }

                // Recursively reconstruct the diagonal parts of the child nodes
                reconstruct_node(*node.left, A);
                reconstruct_node(*node.right, A);

                auto& lc = *node.left;
                auto& rc = *node.right;

                // Off-diagonal block: A(left, right) = U_left * B * V_right^T
                for (std::size_t i = 0; i < lc.row_size; ++i)
                    for (std::size_t j = 0; j < rc.col_size; ++j) {
                        T val = T(0);
                        for (std::size_t p = 0; p < node.B.rows(); ++p)
                            for (std::size_t q = 0; q < node.B.cols(); ++q)
                                val += lc.U(i, p) * node.B(p, q) * rc.V(j, q);
                        A(lc.row_start + i, rc.col_start + j) = val;
                    }

                // A(right, left) = U_right * B^T * V_left^T
                // Sibling coupling of B^T: A_rl = U_right * B_rl * V_left^T
                // B_rl is the coupling matrix of A(right, left)
                Matrix<T> A_rl = extract_block(
                    A,  // note: A does not yet hold the original values (during reconstruction)
                    rc.row_start, rc.row_size,
                    lc.col_start, lc.col_size);

                // Direct computation: low-rank approximation of A(right, left)
                // B_rl = pinv(U_right) * A_orig(right, left) * pinv(V_left)^T
                // However, since the original matrix is unavailable during reconstruction, a different approach is needed

                // Use the symmetric B: A(right, left) = U_right * B^T * V_left^T
                for (std::size_t i = 0; i < rc.row_size; ++i)
                    for (std::size_t j = 0; j < lc.col_size; ++j) {
                        T val = T(0);
                        for (std::size_t p = 0; p < node.B.cols(); ++p)
                            for (std::size_t q = 0; q < node.B.rows(); ++q)
                                val += rc.U(i, p) * node.B(q, p) * lc.V(j, q);
                        A(rc.row_start + i, lc.col_start + j) = val;
                    }
            }

        } // namespace detail_hss

        /**
         * @brief HSS decomposition (Hierarchically Semi-Separable)
         *
         * Decompose a square matrix A into a hierarchically semi-separable structure.
         * Approximate the off-diagonal blocks with low rank and represent them as a hierarchical tree.
         *
         * Effective for matrices with low-rank structure such as BEM, integral equations, and kernel matrices.
         *
         * @param A          input matrix (n×n, square)
         * @param leaf_size  maximum size of a leaf node (0: automatic = max(2, n/4))
         * @param rank       maximum rank of the off-diagonal low-rank approximation (0: automatic = max(1, leaf_size/2))
         * @return HSSResult<T>
         * @throws DimensionError empty matrix, non-square matrix
         */
        template<typename T>
        HSSResult<T> hss_decomposition(
            const BaseMatrix<T>& A,
            std::size_t leaf_size = 0,
            std::size_t rank = 0)
        {
            std::size_t n = A.rows();
            if (n == 0 || A.cols() != n) {
                assert(false && "DimensionError: hss_decomposition: requires a non-empty square matrix");
                throw DimensionError("hss_decomposition: requires a non-empty square matrix");
            }

            if (n == 1) {
                // 1×1 matrix: single leaf
                HSSResult<T> result;
                result.leaf_size = 1;
                result.rank = 1;
                result.root.is_leaf = true;
                result.root.row_start = 0;
                result.root.row_size = 1;
                result.root.col_start = 0;
                result.root.col_size = 1;
                result.root.D = Matrix<T>(A);  // 1×1 deep copy
                result.root.U = Matrix<T>(1, 1, T(1));
                result.root.V = Matrix<T>(1, 1, T(1));
                return result;
            }

            if (leaf_size == 0)
                leaf_size = std::max<std::size_t>(2, n / 4);
            leaf_size = std::min(leaf_size, n);

            if (rank == 0)
                rank = std::max<std::size_t>(1, leaf_size / 2);

            // Build the HSS tree
            auto root = detail_hss::build_tree<T>(0, n, 0, n, leaf_size);

            // Bottom-up compression
            detail_hss::compress_node(*root, A, rank);

            HSSResult<T> result;
            result.root = std::move(*root);
            result.leaf_size = leaf_size;
            result.rank = rank;
            return result;
        }

        /**
         * @brief Reconstruct the dense matrix from the HSS representation (for verification)
         *
         * @param hss  the result of the HSS decomposition
         * @return the reconstructed dense matrix
         */
        template<typename T>
        Matrix<T> hss_reconstruct(const HSSResult<T>& hss)
        {
            std::size_t n = hss.root.row_size;
            Matrix<T> A(n, n, T(0));
            detail_hss::reconstruct_node(hss.root, A);
            return A;
        }

        //=====================================================================
        // Dulmage-Mendelsohn decomposition (sparse matrix structural decomposition)
        //=====================================================================

        /**
         * @brief Result of the Dulmage-Mendelsohn decomposition
         *
         * Reorder a matrix A(m×n) with row permutation P and column permutation Q so that
         *   P * A * Q^T = block upper triangular (BTF)
         * .
         *
         * Coarse decomposition:
         *   HR (horizontal rough): the part containing unmatched rows
         *   SR (square rough):     the square part with a perfect matching
         *   HC (horizontal coarse): the part containing unmatched columns
         *
         * The square part SR is further block-upper-triangularized via strongly connected components (SCC).
         */
        template<typename T>
        struct DulmageMendelsohnResult {
            std::vector<std::size_t> row_perm;     ///< row permutation (P)
            std::vector<std::size_t> col_perm;     ///< column permutation (Q)
            std::vector<std::size_t> block_starts;  ///< block boundaries [0, b1, b2, ..., n_blocks]
            std::size_t n_blocks;                   ///< number of blocks

            // Boundaries of the coarse decomposition
            std::size_t hr_end;     ///< end row of the HR part (0 = no HR)
            std::size_t sr_end;     ///< end row of the SR part
            std::size_t hc_start;   ///< start column of the HC part (n = no HC)
        };

        namespace detail_dm {

            /**
             * @brief Find the maximum matching of a bipartite graph via Hopcroft-Karp
             *
             * @param adj      adj[row] = {col1, col2, ...} (nonzero columns)
             * @param m        number of rows
             * @param n        number of columns
             * @param match_r  match_r[row] = matched column (SIZE_MAX = unmatched)
             * @param match_c  match_c[col] = matched row (SIZE_MAX = unmatched)
             * @return matching size
             */
            inline std::size_t hopcroft_karp(
                const std::vector<std::vector<std::size_t>>& adj,
                std::size_t m, std::size_t n,
                std::vector<std::size_t>& match_r,
                std::vector<std::size_t>& match_c)
            {
                const std::size_t NIL = SIZE_MAX;
                match_r.assign(m, NIL);
                match_c.assign(n, NIL);

                std::vector<std::size_t> dist(m + 1);
                std::size_t matching = 0;

                // BFS: assign alternating-path distance labels from unmatched rows
                auto bfs = [&]() -> bool {
                    std::queue<std::size_t> Q;
                    for (std::size_t r = 0; r < m; ++r) {
                        if (match_r[r] == NIL) {
                            dist[r] = 0;
                            Q.push(r);
                        } else {
                            dist[r] = SIZE_MAX;
                        }
                    }
                    dist[m] = SIZE_MAX;  // distance of the NIL node

                    while (!Q.empty()) {
                        std::size_t r = Q.front(); Q.pop();
                        if (dist[r] < dist[m]) {
                            for (std::size_t c : adj[r]) {
                                std::size_t r2 = match_c[c];
                                std::size_t idx = (r2 == NIL) ? m : r2;
                                if (dist[idx] == SIZE_MAX) {
                                    dist[idx] = dist[r] + 1;
                                    if (idx != m) Q.push(idx);
                                }
                            }
                        }
                    }
                    return dist[m] != SIZE_MAX;
                };

                // DFS: follow alternating paths to find augmenting paths
                std::function<bool(std::size_t)> dfs = [&](std::size_t r) -> bool {
                    if (r == m) return true;  // reached NIL
                    for (std::size_t c : adj[r]) {
                        std::size_t r2 = match_c[c];
                        std::size_t idx = (r2 == NIL) ? m : r2;
                        if (dist[idx] == dist[r] + 1) {
                            if (dfs(idx)) {
                                match_c[c] = r;
                                match_r[r] = c;
                                return true;
                            }
                        }
                    }
                    dist[r] = SIZE_MAX;
                    return false;
                };

                while (bfs()) {
                    for (std::size_t r = 0; r < m; ++r) {
                        if (match_r[r] == NIL) {
                            if (dfs(r)) ++matching;
                        }
                    }
                }
                return matching;
            }

            /**
             * @brief Find rows/columns reachable from the matching via BFS
             */
            inline void reachable_from_unmatched_rows(
                const std::vector<std::vector<std::size_t>>& adj,
                std::size_t m,
                const std::vector<std::size_t>& match_r,
                const std::vector<std::size_t>& match_c,
                std::vector<bool>& row_reached,
                std::vector<bool>& col_reached)
            {
                const std::size_t NIL = SIZE_MAX;
                std::queue<std::size_t> Q;

                // Start from unmatched rows
                for (std::size_t r = 0; r < m; ++r) {
                    if (match_r[r] == NIL) {
                        row_reached[r] = true;
                        Q.push(r);
                    }
                }

                // Search for rows/columns reachable via alternating paths
                while (!Q.empty()) {
                    std::size_t r = Q.front(); Q.pop();
                    for (std::size_t c : adj[r]) {
                        if (!col_reached[c]) {
                            col_reached[c] = true;
                            // Traverse the matched edge in reverse
                            if (match_c[c] != NIL && !row_reached[match_c[c]]) {
                                row_reached[match_c[c]] = true;
                                Q.push(match_c[c]);
                            }
                        }
                    }
                }
            }

            /**
             * @brief Find rows/columns reachable in the column→row direction from unmatched columns via BFS
             */
            inline void reachable_from_unmatched_cols(
                const std::vector<std::vector<std::size_t>>& adj_t,
                std::size_t n,
                const std::vector<std::size_t>& match_r,
                const std::vector<std::size_t>& match_c,
                std::vector<bool>& row_reached,
                std::vector<bool>& col_reached)
            {
                const std::size_t NIL = SIZE_MAX;
                std::queue<std::size_t> Q;

                for (std::size_t c = 0; c < n; ++c) {
                    if (match_c[c] == NIL) {
                        col_reached[c] = true;
                        Q.push(c);
                    }
                }

                while (!Q.empty()) {
                    std::size_t c = Q.front(); Q.pop();
                    for (std::size_t r : adj_t[c]) {
                        if (!row_reached[r]) {
                            row_reached[r] = true;
                            if (match_r[r] != NIL && !col_reached[match_r[r]]) {
                                col_reached[match_r[r]] = true;
                                Q.push(match_r[r]);
                            }
                        }
                    }
                }
            }

            /**
             * @brief Strongly connected components of the graph composed of matched rows only (Tarjan)
             *
             * Nodes = matched rows (in 1:1 correspondence with matched columns)
             * Edge: row r → column match_r[r] → (nonzero) row r'   (r' ≠ r, r' is a matched row)
             */
            inline std::vector<std::size_t> scc_order(
                const std::vector<std::vector<std::size_t>>& adj,
                const std::vector<std::size_t>& sq_rows,
                const std::vector<std::size_t>& match_r,
                const std::vector<std::size_t>& match_c,
                std::vector<std::size_t>& block_starts)
            {
                std::size_t ns = sq_rows.size();
                if (ns == 0) return {};

                // Mapping from row number → index within SQ
                std::unordered_map<std::size_t, std::size_t> row_to_idx;
                for (std::size_t i = 0; i < ns; ++i)
                    row_to_idx[sq_rows[i]] = i;

                // Adjacency list of the SQ graph
                std::vector<std::vector<std::size_t>> g(ns);
                for (std::size_t i = 0; i < ns; ++i) {
                    std::size_t r = sq_rows[i];
                    for (std::size_t c : adj[r]) {
                        std::size_t r2 = match_c[c];
                        if (r2 != SIZE_MAX && r2 != r) {
                            auto it = row_to_idx.find(r2);
                            if (it != row_to_idx.end())
                                g[i].push_back(it->second);
                        }
                    }
                }

                // Tarjan's SCC
                std::vector<std::size_t> order;
                order.reserve(ns);
                std::vector<int> idx_arr(ns, -1), low(ns, -1);
                std::vector<bool> on_stack(ns, false);
                std::stack<std::size_t> st;
                int counter = 0;

                std::function<void(std::size_t)> strongconnect = [&](std::size_t v) {
                    idx_arr[v] = low[v] = counter++;
                    st.push(v);
                    on_stack[v] = true;

                    for (std::size_t w : g[v]) {
                        if (idx_arr[w] < 0) {
                            strongconnect(w);
                            low[v] = std::min(low[v], low[w]);
                        } else if (on_stack[w]) {
                            low[v] = std::min(low[v], idx_arr[w]);
                        }
                    }

                    if (low[v] == idx_arr[v]) {
                        std::size_t start = order.size();
                        while (true) {
                            std::size_t w = st.top(); st.pop();
                            on_stack[w] = false;
                            order.push_back(w);
                            if (w == v) break;
                        }
                        block_starts.push_back(start);
                    }
                };

                for (std::size_t i = 0; i < ns; ++i) {
                    if (idx_arr[i] < 0)
                        strongconnect(i);
                }

                // Tarjan outputs SCCs in reverse topological order, so reverse them
                std::reverse(order.begin(), order.end());
                // Recompute block_starts with reversal too
                std::reverse(block_starts.begin(), block_starts.end());
                std::size_t nb = block_starts.size();
                for (std::size_t i = 0; i < nb; ++i)
                    block_starts[i] = ns - block_starts[i];
                // block_starts is a cumulative value like [size1, size1+size2, ...]
                // Fix to the correct form
                std::vector<std::size_t> bs_new;
                bs_new.push_back(0);
                // Compute the size of each block
                std::vector<std::size_t> sizes(nb);
                for (std::size_t i = 0; i < nb; ++i) {
                    sizes[i] = block_starts[i] - (i > 0 ? block_starts[i - 1] : 0);
                }
                // Simplification: rebuild order in SCC order
                // After reversing Tarjan's output, order[0..] is in topological order
                // Rebuild block_starts from order
                bs_new.clear();
                bs_new.push_back(0);

                // Reimplement Tarjan: produce correct block boundaries iteratively
                // The current order is already reversed. Recompute the block boundaries
                std::vector<int> comp(ns, -1);
                {
                    // Recompute the SCCs (concisely with Kosaraju)
                    // 1st pass: compute the finish order via DFS
                    std::vector<std::size_t> finish_order;
                    finish_order.reserve(ns);
                    std::vector<bool> visited(ns, false);

                    std::function<void(std::size_t)> dfs1 = [&](std::size_t v) {
                        visited[v] = true;
                        for (std::size_t w : g[v])
                            if (!visited[w]) dfs1(w);
                        finish_order.push_back(v);
                    };

                    for (std::size_t i = 0; i < ns; ++i)
                        if (!visited[i]) dfs1(i);

                    // Transposed graph
                    std::vector<std::vector<std::size_t>> gt(ns);
                    for (std::size_t v = 0; v < ns; ++v)
                        for (std::size_t w : g[v])
                            gt[w].push_back(v);

                    // 2nd pass: DFS the transposed graph in reverse finish order
                    int comp_id = 0;
                    std::fill(visited.begin(), visited.end(), false);

                    std::function<void(std::size_t, int)> dfs2 = [&](std::size_t v, int c) {
                        visited[v] = true;
                        comp[v] = c;
                        for (std::size_t w : gt[v])
                            if (!visited[w]) dfs2(w, c);
                    };

                    for (std::size_t i = ns; i-- > 0; ) {
                        std::size_t v = finish_order[i];
                        if (!visited[v]) {
                            dfs2(v, comp_id);
                            ++comp_id;
                        }
                    }
                }

                // comp[i] is the SCC ID in topological order
                // Reorder by SCC ID
                int max_comp = *std::max_element(comp.begin(), comp.end());
                std::vector<std::vector<std::size_t>> scc_groups(max_comp + 1);
                for (std::size_t i = 0; i < ns; ++i)
                    scc_groups[comp[i]].push_back(i);

                order.clear();
                block_starts.clear();
                block_starts.push_back(0);
                for (int c = 0; c <= max_comp; ++c) {
                    if (scc_groups[c].empty()) continue;
                    for (std::size_t idx : scc_groups[c])
                        order.push_back(idx);
                    block_starts.push_back(order.size());
                }

                return order;
            }

        } // namespace detail_dm

        /**
         * @brief Dulmage-Mendelsohn decomposition
         *
         * Construct a bipartite graph from the nonzero structure of a matrix A (m×n), and
         * reorder into block upper triangular form via maximum matching + strongly connected component decomposition.
         *
         * @param A    input matrix
         * @param tol  nonzero-detection threshold (nonzero when |a_{ij}| > tol)
         * @return DulmageMendelsohnResult<T>
         * @throws DimensionError empty matrix
         */
        template<typename T>
        DulmageMendelsohnResult<T> dulmage_mendelsohn(
            const BaseMatrix<T>& A,
            T tol = std::numeric_limits<T>::epsilon())
        {
            std::size_t m = A.rows(), n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: dulmage_mendelsohn: requires a non-empty matrix");
                throw DimensionError("dulmage_mendelsohn: requires a non-empty matrix");
            }

            // Adjacency list (row → nonzero columns)
            std::vector<std::vector<std::size_t>> adj(m);
            std::vector<std::vector<std::size_t>> adj_t(n);  // transposed
            for (std::size_t i = 0; i < m; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    if (std::abs(A(i, j)) > tol) {
                        adj[i].push_back(j);
                        adj_t[j].push_back(i);
                    }

            // Maximum matching
            std::vector<std::size_t> match_r, match_c;
            detail_dm::hopcroft_karp(adj, m, n, match_r, match_c);

            // Coarse decomposition: HR (reachable from unmatched rows), HC (reachable from unmatched columns)
            std::vector<bool> hr_rows(m, false), hr_cols(n, false);
            detail_dm::reachable_from_unmatched_rows(adj, m, match_r, match_c,
                                                      hr_rows, hr_cols);

            std::vector<bool> hc_rows(m, false), hc_cols(n, false);
            detail_dm::reachable_from_unmatched_cols(adj_t, n, match_r, match_c,
                                                      hc_rows, hc_cols);

            // Classification: HR, SR, HC
            std::vector<std::size_t> hr_r, sr_r, hc_r;
            std::vector<std::size_t> hr_c, sr_c, hc_c;

            for (std::size_t r = 0; r < m; ++r) {
                if (hr_rows[r])       hr_r.push_back(r);
                else if (hc_rows[r])  hc_r.push_back(r);
                else                  sr_r.push_back(r);
            }
            for (std::size_t c = 0; c < n; ++c) {
                if (hr_cols[c])       hr_c.push_back(c);
                else if (hc_cols[c])  hc_c.push_back(c);
                else                  sr_c.push_back(c);
            }

            // Block-upper-triangularize the SR part via SCC
            std::vector<std::size_t> scc_block_starts;
            std::vector<std::size_t> scc_order;

            if (!sr_r.empty()) {
                scc_order = detail_dm::scc_order(adj, sr_r, match_r, match_c, scc_block_starts);
            }

            // Construct the row/column permutations
            std::vector<std::size_t> row_perm, col_perm;
            std::vector<std::size_t> block_starts;

            // HR part
            for (std::size_t r : hr_r) row_perm.push_back(r);
            for (std::size_t c : hr_c) col_perm.push_back(c);

            std::size_t hr_end_row = hr_r.size();
            std::size_t hr_end_col = hr_c.size();

            // SR part (SCC order)
            block_starts.push_back(row_perm.size());
            if (!sr_r.empty()) {
                for (std::size_t i = 0; i < scc_order.size(); ++i) {
                    std::size_t sq_idx = scc_order[i];
                    std::size_t r = sr_r[sq_idx];
                    row_perm.push_back(r);
                    col_perm.push_back(match_r[r]);
                }
                // Add a block boundary
                for (std::size_t i = 1; i < scc_block_starts.size(); ++i)
                    block_starts.push_back(hr_end_row + scc_block_starts[i]);
            }
            std::size_t sr_end_row = row_perm.size();

            // HC part
            for (std::size_t r : hc_r) row_perm.push_back(r);
            for (std::size_t c : hc_c) col_perm.push_back(c);

            // Add a trailing boundary only if there is an HC block
            if (!hc_r.empty())
                block_starts.push_back(row_perm.size());

            DulmageMendelsohnResult<T> result;
            result.row_perm = std::move(row_perm);
            result.col_perm = std::move(col_perm);
            result.block_starts = std::move(block_starts);
            result.n_blocks = result.block_starts.size() - 1;
            result.hr_end = hr_end_row;
            result.sr_end = sr_end_row;
            result.hc_start = hr_end_col + sr_r.size();  // HC start column
            return result;
        }

        //=====================================================================
        // Smith Normal Form
        //=====================================================================

        /**
         * @brief Result of the Smith Normal Form
         *
         * Decompose an integer matrix A (m×n) into A = U * D * V.
         * - U: m×m unimodular matrix (det = ±1)
         * - D: m×n diagonal matrix (invariant factors: d_1 | d_2 | ... | d_r, the rest are 0)
         * - V: n×n unimodular matrix (det = ±1)
         *
         * Exact computation with integer types (int, long long, etc.).
         */
        template<typename T>
        struct SmithNormalFormResult {
            Matrix<T> U;                          ///< left unimodular matrix
            Matrix<T> D;                          ///< diagonal matrix (invariant factors)
            Matrix<T> V;                          ///< right unimodular matrix
            std::vector<T> invariant_factors;      ///< invariant factors d_1, d_2, ..., d_r
            std::size_t rank;                      ///< rank (number of nonzero invariant factors)
        };

        /**
         * @brief Smith Normal Form
         *
         * Transform an integer matrix A (m×n) into diagonal form via elementary row/column operations.
         * A = U * D * V (U, V: unimodular, D: diagonal)
         *
         * Algorithm: successive diagonalization via the extended Euclidean algorithm
         *
         * @param A    input matrix (integer elements)
         * @return SmithNormalFormResult<T>
         * @throws DimensionError empty matrix
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        SmithNormalFormResult<sangi::element_t<MA>> smith_normal_form(const MA& A)
        {
            using T = sangi::element_t<MA>;
            // rectangular OK (m×n integer matrix)

            std::size_t m = A.rows(), n = A.cols();
            if (m == 0 || n == 0) {
                assert(false && "DimensionError: smith_normal_form: requires a non-empty matrix");
                throw DimensionError("smith_normal_form: requires a non-empty matrix");
            }

            // Working copy
            Matrix<T> D(A);
            Matrix<T> U = Matrix<T>::identity(m);
            Matrix<T> V = Matrix<T>::identity(n);

            std::size_t min_mn = std::min(m, n);

            for (std::size_t k = 0; k < min_mn; ++k) {
                // Pivot selection: find the nonzero element with the smallest absolute value in D(k:m, k:n)
                bool found_nonzero = true;
                while (found_nonzero) {
                    // Bring the smallest nonzero element to the pivot
                    found_nonzero = false;
                    T min_val = T(0);
                    std::size_t pi = k, pj = k;
                    for (std::size_t i = k; i < m; ++i)
                        for (std::size_t j = k; j < n; ++j) {
                            T av = D(i, j);
                            if (av < T(0)) av = -av;
                            if (av > T(0) && (min_val == T(0) || av < min_val)) {
                                min_val = av;
                                pi = i; pj = j;
                            }
                        }

                    if (min_val == T(0)) break;  // the rest are all zero

                    // Move the pivot to (k, k)
                    if (pi != k) {
                        for (std::size_t j = 0; j < n; ++j)
                            std::swap(D(k, j), D(pi, j));
                        for (std::size_t j = 0; j < m; ++j)
                            std::swap(U(j, k), U(j, pi));
                    }
                    if (pj != k) {
                        for (std::size_t i = 0; i < m; ++i)
                            std::swap(D(i, k), D(i, pj));
                        // Column swap of D → V_new = C^{-1}*V, C is a column permutation → row swap of V
                        for (std::size_t j = 0; j < n; ++j)
                            std::swap(V(k, j), V(pj, j));
                    }

                    // If D(k,k) is negative, sign-flip the row
                    if (D(k, k) < T(0)) {
                        for (std::size_t j = 0; j < n; ++j)
                            D(k, j) = -D(k, j);
                        for (std::size_t j = 0; j < m; ++j)
                            U(j, k) = -U(j, k);
                    }

                    T pivot = D(k, k);

                    // Eliminate column k: make D(i, k) divisible by D(k, k)
                    bool changed = false;
                    for (std::size_t i = k + 1; i < m; ++i) {
                        if (D(i, k) == T(0)) continue;
                        T q = D(i, k) / pivot;
                        // row i -= q * row k
                        for (std::size_t j = 0; j < n; ++j)
                            D(i, j) -= q * D(k, j);
                        for (std::size_t j = 0; j < m; ++j)
                            U(j, k) += q * U(j, i);
                        if (D(i, k) != T(0)) changed = true;
                    }

                    // Eliminate row k: make D(k, j) divisible by D(k, k)
                    for (std::size_t j = k + 1; j < n; ++j) {
                        if (D(k, j) == T(0)) continue;
                        T q = D(k, j) / pivot;
                        // column j -= q * column k  → row j of V += q * row k
                        for (std::size_t i = 0; i < m; ++i)
                            D(i, j) -= q * D(i, k);
                        for (std::size_t c = 0; c < n; ++c)
                            V(k, c) += q * V(j, c);
                        if (D(k, j) != T(0)) changed = true;
                    }

                    if (changed) {
                        found_nonzero = true;  // a remainder appeared → loop again
                    } else {
                        // Check whether columns and rows from k+1 onward are zero
                        bool all_zero = true;
                        for (std::size_t i = k + 1; i < m && all_zero; ++i)
                            if (D(i, k) != T(0)) all_zero = false;
                        for (std::size_t j = k + 1; j < n && all_zero; ++j)
                            if (D(k, j) != T(0)) all_zero = false;
                        if (all_zero) break;
                        found_nonzero = true;
                    }
                }
            }

            // Guarantee the divisibility condition: d_i | d_{i+1}
            // Extended GCD + 2×2 row/column transform to map diag(a,b) → diag(gcd, lcm)
            //
            // R * diag(a,b) * C = diag(g, l)
            // R = [[s, t], [-b/g, a/g]],  C = [[1, c01], [1, c11]]
            // c01 = -t*b/g,  c11 = s*a/g
            // R^{-1} = [[a/g, -t], [b/g, s]]
            // C^{-1} = [[c11, -c01], [-1, 1]]
            //
            // A = U*D*V → A = (U*R^{-1}) * diag(g,l) * (C^{-1}*V)
            auto ext_gcd = [](T a, T b, T& s, T& t) -> T {
                T old_r = a, r = b;
                T old_s = T(1); s = T(0);
                T old_t = T(0); t = T(1);
                while (r != T(0)) {
                    T q = old_r / r;
                    T tmp;
                    tmp = r; r = old_r - q * r; old_r = tmp;
                    tmp = s; s = old_s - q * s; old_s = tmp;
                    tmp = t; t = old_t - q * t; old_t = tmp;
                }
                s = old_s; t = old_t;
                return old_r;
            };

            bool modified = true;
            while (modified) {
                modified = false;
                for (std::size_t i = 0; i + 1 < min_mn; ++i) {
                    T a = D(i, i), b = D(i + 1, i + 1);
                    if (a == T(0) || b == T(0)) continue;

                    T abs_a = (a < T(0)) ? -a : a;
                    T abs_b = (b < T(0)) ? -b : b;
                    if (abs_b % abs_a == T(0)) continue;

                    T s_coeff, t_coeff;
                    T g = ext_gcd(a, b, s_coeff, t_coeff);
                    if (g < T(0)) { g = -g; s_coeff = -s_coeff; t_coeff = -t_coeff; }
                    T l = a / g * b;
                    if (l < T(0)) l = -l;

                    // Update D's diagonal elements directly
                    D(i, i) = g;
                    D(i + 1, i + 1) = l;

                    // U_new = U_old * R^{-1}, R^{-1} = [[a/g, -t], [b/g, s]]
                    T ri00 = a / g, ri01 = -t_coeff;
                    T ri10 = b / g, ri11 = s_coeff;
                    for (std::size_t r = 0; r < m; ++r) {
                        T ui = U(r, i), uj = U(r, i + 1);
                        U(r, i)     = ui * ri00 + uj * ri10;
                        U(r, i + 1) = ui * ri01 + uj * ri11;
                    }

                    // V_new = C^{-1} * V_old  (2×2 transform on rows i, i+1)
                    // C = [[1, -tb/g], [1, sa/g]]
                    // C^{-1} = [[sa/g, tb/g], [-1, 1]]
                    T ci00 = (s_coeff * a) / g;
                    T ci01 = (t_coeff * b) / g;
                    for (std::size_t c = 0; c < n; ++c) {
                        T vi = V(i, c), vj = V(i + 1, c);
                        V(i, c)     = ci00 * vi + ci01 * vj;
                        V(i + 1, c) = -vi + vj;
                    }

                    modified = true;
                }
            }

            // Normalize the diagonal elements to positive
            for (std::size_t i = 0; i < min_mn; ++i) {
                if (D(i, i) < T(0)) {
                    for (std::size_t j = 0; j < n; ++j) D(i, j) = -D(i, j);
                    for (std::size_t j = 0; j < m; ++j) U(j, i) = -U(j, i);
                }
            }

            // Extract the invariant factors
            std::vector<T> inv_factors;
            std::size_t r = 0;
            for (std::size_t i = 0; i < min_mn; ++i) {
                if (D(i, i) != T(0)) {
                    inv_factors.push_back(D(i, i));
                    ++r;
                }
            }

            return { std::move(U), std::move(D), std::move(V),
                     std::move(inv_factors), r };
        }

        //=====================================================================
        // Jordan Normal Form
        //=====================================================================

        /**
         * @brief Result of the Jordan Normal Form
         *
         * A = P * J * P^{-1}
         * J is a Jordan-block diagonal matrix (real approximation)
         *
         * Note: numerically unstable. For educational use / small matrices.
         */
        template<typename T>
        struct JordanResult {
            Matrix<T> J;                         ///< Jordan matrix
            Matrix<T> P;                         ///< transformation matrix (generalized eigenvectors)
            std::vector<T> eigenvalues;           ///< eigenvalues (real part only, with multiplicity)
            std::vector<std::size_t> block_sizes; ///< size of each Jordan block
        };

        namespace detail_jordan {

            /**
             * @brief Extract eigenvalues from the Schur form (1×1 real blocks, 2×2 complex blocks)
             * @return list of eigenvalues (real part only) and the corresponding algebraic multiplicities
             */
            template<typename T>
            std::vector<std::pair<T, std::size_t>> extract_eigenvalues(
                const BaseMatrix<T>& Sch, T tol)
            {
                std::size_t n = Sch.rows();
                std::vector<T> eigs;

                std::size_t i = 0;
                while (i < n) {
                    if (i + 1 < n && std::abs(Sch(i + 1, i)) > tol) {
                        // 2×2 block: complex eigenvalue → add the real part twice
                        T a = Sch(i, i), b = Sch(i, i + 1);
                        T c = Sch(i + 1, i), d = Sch(i + 1, i + 1);
                        T real_part = (a + d) / T(2);
                        eigs.push_back(real_part);
                        eigs.push_back(real_part);
                        i += 2;
                    } else {
                        eigs.push_back(Sch(i, i));
                        i += 1;
                    }
                }

                // Grouping (merge nearby eigenvalues)
                std::vector<std::pair<T, std::size_t>> grouped;
                std::vector<bool> used(eigs.size(), false);
                T group_tol = tol * T(100);

                for (std::size_t j = 0; j < eigs.size(); ++j) {
                    if (used[j]) continue;
                    T val = eigs[j];
                    std::size_t count = 1;
                    used[j] = true;
                    for (std::size_t k = j + 1; k < eigs.size(); ++k) {
                        if (!used[k] && std::abs(eigs[k] - val) < group_tol) {
                            count++;
                            used[k] = true;
                        }
                    }
                    grouped.push_back({val, count});
                }
                return grouped;
            }

            /**
             * @brief Compute the dimension of the matrix's null space (rank deficiency)
             */
            template<typename T>
            std::size_t null_space_dim(const BaseMatrix<T>& M, T tol)
            {
                std::size_t m = M.rows(), n = M.cols();
                if (m == 0 || n == 0) return n;

                auto [U, sigma, Vt] = svd_decomposition(M);
                std::size_t rank = 0;
                T threshold = sigma[0] * static_cast<T>(std::max(m, n)) *
                              std::numeric_limits<T>::epsilon() * T(10);
                threshold = std::max(threshold, tol);
                for (std::size_t i = 0; i < sigma.size(); ++i) {
                    if (sigma[i] > threshold) ++rank;
                }
                return n - rank;
            }

            /**
             * @brief Compute a basis of the null space of (A - λI)^k via SVD
             * @return basis vectors of the null space (column vectors)
             */
            template<typename T>
            Matrix<T> null_space_basis(const BaseMatrix<T>& M, T tol)
            {
                std::size_t m = M.rows(), n = M.cols();
                if (m == 0 || n == 0) return Matrix<T>(n, n, T(0));

                auto [U, sigma, Vt] = svd_decomposition(M);
                T threshold = sigma[0] * static_cast<T>(std::max(m, n)) *
                              std::numeric_limits<T>::epsilon() * T(10);
                threshold = std::max(threshold, tol);

                std::size_t rank = 0;
                for (std::size_t i = 0; i < sigma.size(); ++i)
                    if (sigma[i] > threshold) ++rank;

                std::size_t null_dim = n - rank;
                if (null_dim == 0) return Matrix<T>(n, 0, T(0));

                // The last null_dim columns of V = the transpose of the last null_dim rows of Vt
                Matrix<T> basis(n, null_dim, T(0));
                for (std::size_t j = 0; j < null_dim; ++j)
                    for (std::size_t i = 0; i < n; ++i)
                        basis(i, j) = Vt(rank + j, i);

                return basis;
            }

            /**
             * @brief Compute the matrix power M^k
             */
            template<typename T>
            Matrix<T> mat_power(const BaseMatrix<T>& M_basemat, std::size_t k)
            {
                Matrix<T> M(M_basemat);  // 1 copy via BaseMatrix ctor
                std::size_t n = M.rows();
                if (k == 0) return Matrix<T>::identity(n);
                Matrix<T> result = M;
                for (std::size_t i = 1; i < k; ++i)
                    result = result * M;
                return result;
            }

        } // namespace detail_jordan

        /**
         * @brief Jordan Normal Form
         *
         * Decompose a square matrix A into A = P * J * P^{-1}.
         * J is a Jordan-block diagonal matrix.
         *
         * Numerically unstable, so for educational use / small matrices (n ≤ ~20).
         * Complex eigenvalues are approximated by their real part only.
         *
         * @param A    input matrix (n×n, square)
         * @return JordanResult<T>
         * @throws DimensionError empty matrix, non-square matrix
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        JordanResult<sangi::element_t<MA>> jordan_normal_form(const MA& A)
        {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            std::size_t n = A.rows();
            if (n == 0 || A.cols() != n) {
                assert(false && "DimensionError: jordan_normal_form: requires a non-empty square matrix");
                throw DimensionError("jordan_normal_form: requires a non-empty square matrix");
            }

            const T eps = std::numeric_limits<T>::epsilon();
            const T tol = eps * T(n) * T(100);

            // Case 1×1
            if (n == 1) {
                Matrix<T> J(1, 1, A(0, 0));
                Matrix<T> P = Matrix<T>::identity(1);
                return { std::move(J), std::move(P), {A(0, 0)}, {1} };
            }

            // Obtain the eigenvalues via Schur decomposition
            auto [Sch, Q] = schur_decomposition(A);
            auto eig_groups = detail_jordan::extract_eigenvalues(Sch, tol);

            // Construct a Jordan chain for each eigenvalue
            Matrix<T> J(n, n, T(0));
            Matrix<T> P(n, n, T(0));
            std::vector<T> all_eigenvalues;
            std::vector<std::size_t> block_sizes;

            std::size_t col_offset = 0;

            for (auto& [lambda, alg_mult] : eig_groups) {
                // A - λI
                Matrix<T> AmlI(A);
                for (std::size_t i = 0; i < n; ++i)
                    AmlI(i, i) -= lambda;

                // Compute the null-space dimension of (A - λI)^k and
                // determine the structure of the Jordan blocks
                // null_dim(k) - null_dim(k-1) = the number of blocks of size k or larger
                std::vector<std::size_t> null_dims;
                null_dims.push_back(0);  // k=0: null_dim = 0

                Matrix<T> Mk = Matrix<T>::identity(n);
                for (std::size_t k = 1; k <= alg_mult; ++k) {
                    Mk = Mk * AmlI;
                    std::size_t nd = detail_jordan::null_space_dim(Mk, tol);
                    nd = std::min(nd, alg_mult);  // do not exceed the algebraic multiplicity
                    null_dims.push_back(nd);
                    if (nd >= alg_mult) break;
                }

                // Derive the Jordan block structure
                // Δ_k = null_dim(k) - null_dim(k-1)
                // Number of blocks of size s = Δ_s - Δ_{s+1}
                std::size_t max_k = null_dims.size() - 1;
                std::vector<std::size_t> deltas(max_k + 2, 0);
                for (std::size_t k = 1; k <= max_k; ++k)
                    deltas[k] = null_dims[k] - null_dims[k - 1];

                std::vector<std::pair<std::size_t, std::size_t>> blocks; // (size, count)
                for (std::size_t s = max_k; s >= 1; --s) {
                    std::size_t count = deltas[s] - (s + 1 <= max_k ? deltas[s + 1] : 0);
                    if (count > 0)
                        blocks.push_back({s, count});
                }

                // Fallback when the block sizes cannot be determined
                std::size_t total_in_blocks = 0;
                for (auto& [sz, cnt] : blocks) total_in_blocks += sz * cnt;
                if (total_in_blocks < alg_mult) {
                    // Fill the rest with 1×1 blocks
                    std::size_t remaining = alg_mult - total_in_blocks;
                    bool found_size1 = false;
                    for (auto& [sz, cnt] : blocks) {
                        if (sz == 1) { cnt += remaining; found_size1 = true; break; }
                    }
                    if (!found_size1)
                        blocks.push_back({1, remaining});
                }

                // Construct the basis vectors of the Jordan chain
                // For each block size s:
                //   choose the head vector of the chain from the null space of (A-λI)^s, and
                //   construct the basis with v, (A-λI)v, (A-λI)²v, ...
                std::vector<Vector<T>> chain_vectors;

                // Orthogonal projection to manage the already-used directions
                std::vector<Vector<T>> used_vectors;

                // Process from large to small block size (blocks is already large→small)
                for (auto& [sz, cnt] : blocks) {
                    Matrix<T> Mk_sz = detail_jordan::mat_power(AmlI, sz);
                    Matrix<T> null_basis = detail_jordan::null_space_basis(Mk_sz, tol);

                    // To exclude the directions used in the previous stage,
                    // subtract the projection of null_basis onto used_vectors
                    for (std::size_t b = 0; b < cnt; ++b) {
                        // From the columns of null_basis, find a direction orthogonal to used_vectors
                        Vector<T> seed(n, T(0));
                        bool found = false;

                        for (std::size_t ci = 0; ci < null_basis.cols(); ++ci) {
                            // Candidate vector
                            Vector<T> v(n, T(0));
                            for (std::size_t i = 0; i < n; ++i)
                                v[i] = null_basis(i, ci);

                            // Subtract the projection onto the used vectors (Gram-Schmidt)
                            for (auto& u : used_vectors) {
                                T dot = T(0), norm_sq = T(0);
                                for (std::size_t i = 0; i < n; ++i) {
                                    dot += v[i] * u[i];
                                    norm_sq += u[i] * u[i];
                                }
                                if (norm_sq > tol) {
                                    T coeff = dot / norm_sq;
                                    for (std::size_t i = 0; i < n; ++i)
                                        v[i] -= coeff * u[i];
                                }
                            }

                            // Norm check
                            T norm = T(0);
                            for (std::size_t i = 0; i < n; ++i) norm += v[i] * v[i];
                            if (norm > tol * tol) {
                                // Normalize
                                norm = std::sqrt(norm);
                                for (std::size_t i = 0; i < n; ++i) v[i] /= norm;
                                seed = v;
                                found = true;
                                break;
                            }
                        }

                        if (!found) {
                            // Fallback: choose from the unit vectors
                            for (std::size_t i = 0; i < n; ++i) {
                                Vector<T> ei(n, T(0));
                                ei[i] = T(1);
                                for (auto& u : used_vectors) {
                                    T dot = T(0), norm_sq = T(0);
                                    for (std::size_t k = 0; k < n; ++k) {
                                        dot += ei[k] * u[k];
                                        norm_sq += u[k] * u[k];
                                    }
                                    if (norm_sq > tol) {
                                        T coeff = dot / norm_sq;
                                        for (std::size_t k = 0; k < n; ++k)
                                            ei[k] -= coeff * u[k];
                                    }
                                }
                                T norm = T(0);
                                for (std::size_t k = 0; k < n; ++k) norm += ei[k] * ei[k];
                                if (norm > tol * tol) {
                                    norm = std::sqrt(norm);
                                    for (std::size_t k = 0; k < n; ++k) ei[k] /= norm;
                                    seed = ei;
                                    found = true;
                                    break;
                                }
                            }
                        }

                        // Jordan chain: v_s = seed, v_{s-1} = (A-λI)*v_s, ...
                        // Do not normalize, to preserve the relation A*v_k = λ*v_k + v_{k-1}
                        std::vector<Vector<T>> chain(sz);
                        chain[sz - 1] = seed;
                        for (std::size_t k = sz - 1; k > 0; --k) {
                            Vector<T> next(n, T(0));
                            for (std::size_t i = 0; i < n; ++i)
                                for (std::size_t j = 0; j < n; ++j)
                                    next[i] += AmlI(i, j) * chain[k][j];
                            chain[k - 1] = next;
                        }

                        // Store into P and set J
                        for (std::size_t k = 0; k < sz; ++k) {
                            for (std::size_t i = 0; i < n; ++i)
                                P(i, col_offset + k) = chain[k][i];
                            J(col_offset + k, col_offset + k) = lambda;
                            if (k + 1 < sz)
                                J(col_offset + k, col_offset + k + 1) = T(1);
                            all_eigenvalues.push_back(lambda);
                        }
                        block_sizes.push_back(sz);

                        // Record the used vectors
                        for (auto& cv : chain)
                            used_vectors.push_back(cv);

                        col_offset += sz;
                    }
                }

                if (col_offset > n) break;
            }

            return { std::move(J), std::move(P), std::move(all_eigenvalues),
                     std::move(block_sizes) };
        }

        //=====================================================================
        // QZ decomposition (generalized real Schur decomposition)
        //=====================================================================

        namespace detail_qz {

            // Givens rotation parameters: c*a + s*b = r, -s*a + c*b = 0
            template<typename T>
            struct GivensParams { T c, s; };

            template<typename T>
            GivensParams<T> givens_rotation(T a, T b) {
                // c*a + s*b = r (> 0), -s*a + c*b = 0
                // c = a/r, s = b/r, r = sqrt(a² + b²)
                if (a == T(0) && b == T(0)) {
                    return {T(1), T(0)};
                }
                T r = std::sqrt(a * a + b * b);
                return {a / r, b / r};
            }

            // Left Givens rotation: apply to rows i1, i2
            template<typename T>
            void apply_givens_left(Matrix<T>& M, int i1, int i2,
                                   T c, T s, int col_start, int col_end) {
                for (int j = col_start; j < col_end; ++j) {
                    T t1 = M(i1, j);
                    T t2 = M(i2, j);
                    M(i1, j) =  c * t1 + s * t2;
                    M(i2, j) = -s * t1 + c * t2;
                }
            }

            // Right Givens rotation: apply to columns j1, j2
            template<typename T>
            void apply_givens_right(Matrix<T>& M, int j1, int j2,
                                    T c, T s, int row_start, int row_end) {
                for (int i = row_start; i < row_end; ++i) {
                    T t1 = M(i, j1);
                    T t2 = M(i, j2);
                    M(i, j1) = c * t1 + s * t2;
                    M(i, j2) = -s * t1 + c * t2;
                }
            }

        } // namespace detail_qz

        /**
         * @brief QZ decomposition (generalized real Schur decomposition)
         *
         * A = Q·S·Zᵀ, B = Q·T·Zᵀ
         *   S: quasi-upper-triangular (real Schur form)
         *   T: upper triangular
         *   Q, Z: orthogonal matrices
         *
         * Generalized eigenvalues λᵢ = S(i,i)/T(i,i)  (1×1 block)
         * Complex conjugate pairs correspond to 2×2 blocks of S
         *
         * Algorithm:
         *   Phase 1: Hessenberg-Triangular reduction (Moler-Stewart)
         *   Phase 2: Francis double-shift QZ iteration
         *
         * @tparam T element type
         * @param A input matrix (n×n)
         * @param B input matrix (n×n)
         * @return {S, T, Q, Z}
         */
        template<typename MA, typename MB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseMatrixLike<MB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<MB>>
        std::tuple<Matrix<sangi::element_t<MA>>, Matrix<sangi::element_t<MA>>,
                   Matrix<sangi::element_t<MA>>, Matrix<sangi::element_t<MA>>>
        qz_decomposition(const MA& A, const MB& B) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_SQUARE(MB);

            const int n = static_cast<int>(A.rows());
            if (A.rows() != A.cols() || B.rows() != B.cols() ||
                A.rows() != B.rows()) {
                assert(false && "DimensionError: qz_decomposition: requires square matrices of same size");
                throw DimensionError("qz_decomposition: requires square matrices of same size");
            }

            if (n == 0) {
                return {A, B, Matrix<T>(0, 0), Matrix<T>(0, 0)};
            }
            if (n == 1) {
                Matrix<T> I1(1, 1, T(1));
                return {A, B, I1, I1};
            }

            const T eps = std::numeric_limits<T>::epsilon();

            // ============================================================
            // Phase 1: Hessenberg-Triangular reduction
            // ============================================================

            // Step 1a: QR decomposition of B
            auto [Q0, R] = qr_decomposition(B);
            // H = Q0ᵀ * A
            Matrix<T> H(n, n, T(0));
            for (int i = 0; i < n; ++i) {
                for (int j = 0; j < n; ++j) {
                    T sum = T(0);
                    for (int k = 0; k < n; ++k) {
                        sum += Q0(k, i) * A(k, j); // Q0ᵀ(i,k) = Q0(k,i)
                    }
                    H(i, j) = sum;
                }
            }

            // Initialize Q, Z
            // Q = Q0 (accumulation of left orthogonal transforms)
            Matrix<T> Q = Q0;
            Matrix<T> Z(n, n, T(0));
            for (int i = 0; i < n; ++i) Z(i, i) = T(1);

            // Step 1b: reduce H to upper Hessenberg (preserving R's upper-triangularity)
            for (int k = 0; k < n - 2; ++k) {
                for (int i = n - 1; i >= k + 2; --i) {
                    // Left Givens: zero out H(i, k) (rows i-1, i)
                    auto [c, s] = detail_qz::givens_rotation(H(i - 1, k), H(i, k));
                    detail_qz::apply_givens_left(H, i - 1, i, c, s, k, n);
                    detail_qz::apply_givens_left(R, i - 1, i, c, s, 0, n);
                    // Accumulate Q: Q ← Q · G (from the right)
                    detail_qz::apply_givens_right(Q, i - 1, i, c, s, 0, n);

                    // Fill-in at R(i, i-1) → zero out with a right Givens (columns i-1, i)
                    auto [c2, s2] = detail_qz::givens_rotation(R(i, i), R(i, i - 1));
                    // Rotate columns i, i-1 → R(i, i-1) = 0
                    detail_qz::apply_givens_right(R, i, i - 1, c2, s2, 0, n);
                    detail_qz::apply_givens_right(H, i, i - 1, c2, s2, 0, n);
                    // Accumulate Z: Z ← Z · G (from the right)
                    detail_qz::apply_givens_right(Z, i, i - 1, c2, s2, 0, n);
                }
            }

            // Clean up tiny values
            for (int j = 0; j < n; ++j) {
                for (int i = j + 2; i < n; ++i) {
                    H(i, j) = T(0);
                }
            }
            for (int j = 0; j < n; ++j) {
                for (int i = j + 1; i < n; ++i) {
                    R(i, j) = T(0);
                }
            }

            // ============================================================
            // Phase 2: single-shift QZ iteration (Givens-based)
            // ============================================================

            const int max_iter = 300 * n;
            int ihi = n;
            int total_iter = 0;
            int iter_since_deflation = 0;

            while (ihi > 1 && total_iter < max_iter) {
                // Deflation: find a location where H's subdiagonal element is sufficiently small
                int ilo = ihi - 1;
                while (ilo > 0) {
                    T threshold = eps * (std::abs(H(ilo - 1, ilo - 1)) +
                                         std::abs(H(ilo, ilo)));
                    if (threshold == T(0)) threshold = eps;
                    if (std::abs(H(ilo, ilo - 1)) <= threshold) {
                        H(ilo, ilo - 1) = T(0);
                        break;
                    }
                    --ilo;
                }

                if (ilo == ihi - 1) {
                    // 1×1 block: deflate
                    --ihi;
                    iter_since_deflation = 0;
                    continue;
                }

                if (ilo == ihi - 2) {
                    // 2×2 block
                    int p = ilo;
                    T b11 = R(p, p), b22 = R(p + 1, p + 1);
                    if (std::abs(b11) > eps && std::abs(b22) > eps) {
                        T b12 = R(p, p + 1);
                        T m11 = H(p, p) / b11;
                        T m12 = H(p, p + 1) / b22 - H(p, p) * b12 / (b11 * b22);
                        T m21 = H(p + 1, p) / b11;
                        T m22 = H(p + 1, p + 1) / b22 - H(p + 1, p) * b12 / (b11 * b22);
                        T disc = (m11 - m22) * (m11 - m22) + T(4) * m12 * m21;
                        if (disc >= T(0)) {
                            // Real eigenvalues: one QZ step with the Wilkinson shift
                            T delta = (m11 - m22) / T(2);
                            T sign_d = (delta >= T(0)) ? T(1) : T(-1);
                            T sq = std::sqrt(delta * delta + m12 * m21);
                            T sigma = m22 - m21 * m12 / (delta + sign_d * sq);
                            T x2 = H(p, p) - sigma * R(p, p);
                            T y2 = H(p + 1, p);
                            auto [c, s] = detail_qz::givens_rotation(x2, y2);
                            detail_qz::apply_givens_left(H, p, p + 1, c, s, 0, n);
                            detail_qz::apply_givens_left(R, p, p + 1, c, s, 0, n);
                            detail_qz::apply_givens_right(Q, p, p + 1, c, s, 0, n);
                            // R fill-in: zero out R(p+1, p)
                            auto [c2, s2] = detail_qz::givens_rotation(R(p + 1, p + 1), R(p + 1, p));
                            detail_qz::apply_givens_right(R, p + 1, p, c2, s2, 0, n);
                            detail_qz::apply_givens_right(H, p + 1, p, c2, s2, 0, n);
                            detail_qz::apply_givens_right(Z, p + 1, p, c2, s2, 0, n);
                            R(p + 1, p) = T(0);
                        }
                    }
                    ihi -= 2;
                    iter_since_deflation = 0;
                    continue;
                }

                // Single-shift QZ step
                // Wilkinson shift: among the eigenvalues of the bottom-right 2×2 M = R⁻¹H,
                // the one closer to M(ihi-1,ihi-1)
                T sigma;
                {
                    int p = ihi - 2;
                    T r11 = R(p, p), r12 = R(p, p + 1), r22 = R(p + 1, p + 1);
                    if (std::abs(r11) < eps) r11 = eps;
                    if (std::abs(r22) < eps) r22 = eps;
                    T m11 = H(p, p) / r11;
                    T m12 = H(p, p + 1) / r22 - H(p, p) * r12 / (r11 * r22);
                    T m21 = H(p + 1, p) / r11;
                    T m22 = H(p + 1, p + 1) / r22 - H(p + 1, p) * r12 / (r11 * r22);

                    // Eigenvalues of the 2×2
                    T tr = m11 + m22;
                    T det = m11 * m22 - m12 * m21;
                    T disc = tr * tr - T(4) * det;

                    if (disc >= T(0)) {
                        T sq = std::sqrt(disc);
                        T ev1 = (tr + sq) / T(2);
                        T ev2 = (tr - sq) / T(2);
                        sigma = (std::abs(ev1 - m22) < std::abs(ev2 - m22)) ? ev1 : ev2;
                    } else {
                        // Complex eigenvalues: use the real part as the shift
                        sigma = tr / T(2);
                    }

                    // Exceptional shift
                    if (iter_since_deflation > 0 && iter_since_deflation % 10 == 0) {
                        T sub = std::abs(H(ihi - 1, ihi - 2));
                        sigma = m22 + T(0.75) * sub;
                    }
                }

                // Initial Givens: the first column of (H - σR)
                T x = H(ilo, ilo) - sigma * R(ilo, ilo);
                T y = H(ilo + 1, ilo);

                for (int k = ilo; k < ihi - 1; ++k) {
                    // Left Givens: rows (k, k+1), [x, y] → [r, 0]
                    auto [c, s] = detail_qz::givens_rotation(x, y);
                    detail_qz::apply_givens_left(H, k, k + 1, c, s, 0, n);
                    detail_qz::apply_givens_left(R, k, k + 1, c, s, 0, n);
                    detail_qz::apply_givens_right(Q, k, k + 1, c, s, 0, n);

                    // Right Givens: zero out the R(k+1, k) fill-in
                    // apply_givens_right(M, j1, j2, c, s):
                    //   M(:,j2) = -s*M(:,j1) + c*M(:,j2)
                    // R(k+1, j2=k) = -s*R(k+1, j1=k+1) + c*R(k+1, k) = 0
                    // → givens(R(k+1,k+1), R(k+1,k))
                    auto [c2, s2] = detail_qz::givens_rotation(R(k + 1, k + 1), R(k + 1, k));
                    detail_qz::apply_givens_right(R, k + 1, k, c2, s2, 0, n);
                    detail_qz::apply_givens_right(H, k + 1, k, c2, s2, 0, n);
                    detail_qz::apply_givens_right(Z, k + 1, k, c2, s2, 0, n);
                    R(k + 1, k) = T(0);

                    // For the next step: bulge (H(k+2, k) is nonzero)
                    if (k + 2 < ihi) {
                        x = H(k + 1, k);
                        y = H(k + 2, k);
                    }
                }

                ++total_iter;
                ++iter_since_deflation;
            }

            // Clean up tiny values
            for (int i = 1; i < n; ++i) {
                T threshold = eps * (std::abs(H(i - 1, i - 1)) + std::abs(H(i, i)));
                if (threshold == T(0)) threshold = eps;
                if (std::abs(H(i, i - 1)) <= threshold) {
                    H(i, i - 1) = T(0);
                }
            }
            for (int j = 0; j < n; ++j) {
                for (int i = j + 1; i < n; ++i) {
                    R(i, j) = T(0);
                }
            }

            return {H, R, Q, Z};
        }

        //=====================================================================
        // Iwasawa Decomposition
        //=====================================================================

        /**
         * @brief Result of the Iwasawa decomposition
         *
         * Decompose a nonsingular matrix G in GL(n,R) into G = K * A * N.
         * - K: n×n orthogonal matrix (maximal compact subgroup O(n))
         * - A: n×n positive diagonal matrix (abelian subgroup)
         * - N: n×n upper triangular unipotent matrix (diagonal = 1, nilpotent subgroup)
         *
         * Algorithm: separate the diagonal signs from R in the QR decomposition G = Q*R
         * K = Q*D, A = |diag(R)|, N = A^{-1}*D*R (D = diag(sign(R_ii)))
         */
        template<typename T>
        struct IwasawaResult {
            Matrix<T> K;    ///< orthogonal matrix
            Matrix<T> A;    ///< positive diagonal matrix
            Matrix<T> N;    ///< upper triangular unipotent matrix (diagonal = 1)
        };

        /**
         * @brief Iwasawa Decomposition
         *
         * Decompose a nonsingular matrix G (n×n) into G = K * A * N.
         *
         * @param G    input matrix (nonsingular square matrix)
         * @return IwasawaResult<T>
         * @throws DimensionError empty matrix or non-square matrix
         * @throws MathError singular matrix
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        IwasawaResult<sangi::element_t<MA>> iwasawa_decomposition(const MA& G)
        {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);

            std::size_t n = G.rows();
            if (n == 0) {
                assert(false && "DimensionError: iwasawa_decomposition: requires a non-empty matrix");
                throw DimensionError("iwasawa_decomposition: requires a non-empty matrix");
            }
            if (n != G.cols()) {
                assert(false && "DimensionError: iwasawa_decomposition: requires a square matrix");
                throw DimensionError("iwasawa_decomposition: requires a square matrix");
            }

            // QR decomposition: G = Q * R
            auto [Q, R] = qr_decomposition(G);

            // Singularity check: singular if a diagonal element of R is zero
            T eps = std::numeric_limits<T>::epsilon() * T(100);
            T max_diag = T(0);
            for (std::size_t i = 0; i < n; ++i) {
                T ad = std::abs(R(i, i));
                if (ad > max_diag) max_diag = ad;
            }
            T sing_tol = max_diag * T(n) * eps;
            for (std::size_t i = 0; i < n; ++i) {
                if (std::abs(R(i, i)) <= sing_tol)
                    throw MathError("iwasawa_decomposition: matrix is singular or near-singular");
            }

            // D = diag(sign(R_ii)), K = Q * D
            // A = diag(|R_ii|)
            // N = A^{-1} * D * R (upper triangular unipotent)
            Matrix<T> K(Q);
            Matrix<T> A(n, n, T(0));
            Matrix<T> N(n, n, T(0));

            // Absorb the diagonal signs into Q → K, the diagonal absolute values → A
            for (std::size_t j = 0; j < n; ++j) {
                T sign_j = (R(j, j) >= T(0)) ? T(1) : T(-1);
                T abs_rjj = std::abs(R(j, j));
                A(j, j) = abs_rjj;

                // K(:, j) = Q(:, j) * sign_j
                for (std::size_t i = 0; i < n; ++i)
                    K(i, j) = Q(i, j) * sign_j;

                // N(j, :) = (1/abs_rjj) * sign_j * R(j, :)
                // N(j, j) = 1 (diagonal), N(j, k) = sign_j * R(j, k) / abs_rjj for k > j
                T inv_a = T(1) / abs_rjj;
                N(j, j) = T(1);
                for (std::size_t k = j + 1; k < n; ++k)
                    N(j, k) = sign_j * R(j, k) * inv_a;
            }

            return { std::move(K), std::move(A), std::move(N) };
        }

        // ====================================================================
        // ColPivHouseholderQR — QR decomposition with column pivoting
        // ====================================================================

        /**
         * @brief Result of the column-pivoted QR decomposition
         *
         * A * P = Q * R (P is a column permutation matrix)
         * The rank can be obtained via rank().
         */
        template<typename T>
        struct ColPivQRResult {
            Matrix<T> Q;                     ///< orthogonal matrix (m×m)
            Matrix<T> R;                     ///< upper triangular matrix (m×n)
            std::vector<std::size_t> perm;   ///< column permutation (perm[j] = original column number)

            /// numerical rank (|R(i,i)| > threshold)
            [[nodiscard]] std::size_t rank(T threshold = T(-1)) const {
                const auto k = std::min(R.rows(), R.cols());
                if (k == 0) return 0;
                if (threshold < T(0)) {
                    threshold = std::abs(R(0, 0)) *
                        static_cast<T>(std::max(R.rows(), R.cols())) *
                        std::numeric_limits<T>::epsilon();
                }
                std::size_t r = 0;
                for (std::size_t i = 0; i < k; ++i) {
                    if (std::abs(R(i, i)) > threshold) ++r;
                    else break;
                }
                return r;
            }

            /// Obtain the permutation matrix P as a dense matrix
            [[nodiscard]] Matrix<T> permutationMatrix() const {
                const auto n = perm.size();
                Matrix<T> P(n, n, T(0));
                for (std::size_t j = 0; j < n; ++j)
                    P(perm[j], j) = T(1);
                return P;
            }
        };

        /**
         * @brief Column-pivoted Householder QR decomposition
         *
         * Select the column with the largest column norm as the pivot and compute a rank-revealing QR.
         * A * P = Q * R where P is a column permutation.
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        ColPivQRResult<sangi::element_t<MA>> col_piv_qr(const MA& a) {
            using T = sangi::element_t<MA>;
            // rectangular OK

            const auto m = a.rows();
            const auto n = a.cols();
            const auto k = std::min(m, n);

            Matrix<T> R = a;
            Matrix<T> Q(m, m, T(0));
            for (std::size_t i = 0; i < m; ++i) Q(i, i) = T(1);

            std::vector<std::size_t> perm(n);
            std::iota(perm.begin(), perm.end(), std::size_t(0));

            // Column norms (cached)
            std::vector<T> col_norms(n);
            for (std::size_t j = 0; j < n; ++j) {
                T s = T(0);
                for (std::size_t i = 0; i < m; ++i) s += R(i, j) * R(i, j);
                col_norms[j] = s;
            }

            for (std::size_t step = 0; step < k; ++step) {
                // Select the column with the largest residual column norm
                std::size_t best = step;
                T best_norm = col_norms[step];
                for (std::size_t j = step + 1; j < n; ++j) {
                    if (col_norms[j] > best_norm) {
                        best_norm = col_norms[j];
                        best = j;
                    }
                }

                // Swap the columns
                if (best != step) {
                    std::swap(perm[step], perm[best]);
                    std::swap(col_norms[step], col_norms[best]);
                    for (std::size_t i = 0; i < m; ++i)
                        std::swap(R(i, step), R(i, best));
                }

                // Householder reflector
                T sigma = T(0);
                for (std::size_t i = step; i < m; ++i) sigma += R(i, step) * R(i, step);
                sigma = std::sqrt(sigma);

                if (sigma < std::numeric_limits<T>::epsilon()) continue;

                T alpha = (R(step, step) >= T(0)) ? -sigma : sigma;
                T beta = R(step, step) - alpha;
                R(step, step) = alpha;

                // v = [1, R(step+1:m, step) / beta]
                T inv_beta = T(1) / beta;
                for (std::size_t i = step + 1; i < m; ++i)
                    R(i, step) *= inv_beta;

                T tau = -beta / alpha;  // 2 / (v^T v)

                // Update R: R -= tau * v * (v^T * R)
                for (std::size_t j = step + 1; j < n; ++j) {
                    T d = R(step, j);
                    for (std::size_t i = step + 1; i < m; ++i)
                        d += R(i, step) * R(i, j);
                    d *= tau;
                    R(step, j) -= d;
                    for (std::size_t i = step + 1; i < m; ++i)
                        R(i, j) -= R(i, step) * d;
                }

                // Update Q: Q -= tau * Q * v * v^T (multiply from the right)
                for (std::size_t i = 0; i < m; ++i) {
                    T d = Q(i, step);
                    for (std::size_t j = step + 1; j < m; ++j)
                        d += Q(i, j) * R(j, step);
                    d *= tau;
                    Q(i, step) -= d;
                    for (std::size_t j = step + 1; j < m; ++j)
                        Q(i, j) -= R(j, step) * d;
                }

                // Clear R's lower triangle (free the storage for the reflector vectors)
                for (std::size_t i = step + 1; i < m; ++i) R(i, step) = T(0);

                // Update the column norms
                for (std::size_t j = step + 1; j < n; ++j)
                    col_norms[j] -= R(step, j) * R(step, j);
            }

            return { std::move(Q), std::move(R), std::move(perm) };
        }

        /// Compute the least-squares solution via column-pivoted QR (handles rank deficiency)
        /// Do not build Q explicitly; keep the Householder vectors and compute Q^T*b directly
        /// Block Householder (WY representation) + AVX2 FMA
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> col_piv_qr_solve(const MA& A, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (A.rows() != b.size()) {
                assert(false && "DimensionError: col_piv_qr_solve: dimension mismatch");
                throw DimensionError("col_piv_qr_solve: dimension mismatch");
            }

            const auto m = A.rows();
            const auto n = A.cols();
            const auto k = std::min(m, n);

            using PT = PacketTraits<T>;
            constexpr std::size_t W = PT::size;

            Matrix<T> R = A;
            T* Rp = R.data();

            std::vector<std::size_t> perm(n);
            std::iota(perm.begin(), perm.end(), std::size_t(0));
            std::vector<T> tau_vec(k, T(0));

            // Column norms
            std::vector<T> col_norms(n);
            for (std::size_t j = 0; j < n; ++j) {
                T s = T(0);
                const T* col = Rp + j;
                for (std::size_t i = 0; i < m; ++i) s += col[i * n] * col[i * n];
                col_norms[j] = s;
            }

            // Select unblocked/panel factorization depending on size
            constexpr std::size_t NB = 16;
            constexpr std::size_t PANEL_THRESHOLD = 384;

            if (n < PANEL_THRESHOLD) {
                // --- Unblocked version: apply directly to all subsequent columns (AVX2) ---
                for (std::size_t step = 0; step < k; ++step) {
                    std::size_t best = step;
                    T best_norm = col_norms[step];
                    for (std::size_t j = step + 1; j < n; ++j)
                        if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; }

                    if (best != step) {
                        std::swap(perm[step], perm[best]);
                        std::swap(col_norms[step], col_norms[best]);
                        for (std::size_t i = 0; i < m; ++i)
                            std::swap(Rp[i * n + step], Rp[i * n + best]);
                    }

                    T sigma = T(0);
                    for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step] * Rp[i*n+step];
                    sigma = std::sqrt(sigma);
                    if (sigma < std::numeric_limits<T>::epsilon()) continue;

                    T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma;
                    T beta_v = Rp[step*n+step] - alpha;
                    Rp[step*n+step] = alpha;
                    T inv_beta = T(1) / beta_v;
                    for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta;
                    T tau = -beta_v / alpha;
                    tau_vec[step] = tau;

                    std::size_t j = step + 1;
                    if constexpr (W > 1) {
                        auto vtau = PT::set1(tau);
                        for (; j + W - 1 < n; j += W) {
                            auto d_vec = PT::load(Rp + step*n + j);
                            for (std::size_t i = step+1; i < m; ++i)
                                d_vec = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                  PT::load(Rp + i*n + j), d_vec);
                            d_vec = PT::mul(d_vec, vtau);
                            PT::store(Rp + step*n + j,
                                PT::sub(PT::load(Rp + step*n + j), d_vec));
                            for (std::size_t i = step+1; i < m; ++i) {
                                auto vi = PT::set1(Rp[i*n+step]);
                                PT::store(Rp + i*n + j,
                                    PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec)));
                            }
                        }
                    }
                    for (; j < n; ++j) {
                        T d = Rp[step*n + j];
                        for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step] * Rp[i*n+j];
                        d *= tau;
                        Rp[step*n + j] -= d;
                        for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step] * d;
                    }
                    for (std::size_t jj = step+1; jj < n; ++jj)
                        col_norms[jj] -= Rp[step*n+jj] * Rp[step*n+jj];
                }
            } else {
                // --- Panel factorization (F matrix) + trailing GEMM ---
                std::vector<T> F_t(NB * n, T(0));

                for (std::size_t panel_start = 0; panel_start < k; panel_start += NB) {
                    std::size_t panel_end = std::min(panel_start + NB, k);
                    std::size_t nb = panel_end - panel_start;

                    for (std::size_t ll = 0; ll < nb; ++ll)
                        std::fill(&F_t[ll * n], &F_t[ll * n + n], T(0));

                    for (std::size_t l = 0; l < nb; ++l) {
                        std::size_t step = panel_start + l;

                        std::size_t best = step;
                        T best_norm = col_norms[step];
                        for (std::size_t j = step + 1; j < n; ++j)
                            if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; }

                        // When swapping a subsequent column into the panel: apply the lazy update first
                        if (best != step) {
                            if (best >= panel_end && l > 0) {
                                // Apply the accumulated F to the best column before swapping
                                for (std::size_t i = panel_start; i < step; ++i) {
                                    std::size_t i_local = i - panel_start;
                                    T c = F_t[i_local * n + best]; // V(i,ps+i_local)=1
                                    for (std::size_t s = 0; s < i_local; ++s)
                                        c += Rp[i * n + (panel_start+s)] * F_t[s * n + best];
                                    Rp[i * n + best] -= c;
                                }
                                for (std::size_t i = step; i < m; ++i) {
                                    T c = T(0);
                                    for (std::size_t s = 0; s < l; ++s)
                                        c += Rp[i * n + (panel_start+s)] * F_t[s * n + best];
                                    Rp[i * n + best] -= c;
                                }
                                for (std::size_t s = 0; s < l; ++s)
                                    F_t[s * n + best] = T(0);
                            }
                            std::swap(perm[step], perm[best]);
                            std::swap(col_norms[step], col_norms[best]);
                            for (std::size_t i = 0; i < m; ++i)
                                std::swap(Rp[i * n + step], Rp[i * n + best]);
                            for (std::size_t s = 0; s < l; ++s)
                                std::swap(F_t[s * n + step], F_t[s * n + best]);
                        }

                        // Householder
                        T sigma = T(0);
                        for (std::size_t i = step; i < m; ++i)
                            sigma += Rp[i * n + step] * Rp[i * n + step];
                        sigma = std::sqrt(sigma);
                        if (sigma < std::numeric_limits<T>::epsilon()) continue;

                        T alpha = (Rp[step * n + step] >= T(0)) ? -sigma : sigma;
                        T beta_v = Rp[step * n + step] - alpha;
                        Rp[step * n + step] = alpha;
                        T inv_beta = T(1) / beta_v;
                        for (std::size_t i = step + 1; i < m; ++i)
                            Rp[i * n + step] *= inv_beta;
                        T tau = -beta_v / alpha;
                        tau_vec[step] = tau;

                        // Apply directly to the remaining columns in the panel
                        {
                            std::size_t j = step + 1;
                            if constexpr (W > 1) {
                                auto vtau = PT::set1(tau);
                                for (; j + W - 1 < panel_end; j += W) {
                                    auto d_vec = PT::load(Rp + step*n + j);
                                    for (std::size_t i = step+1; i < m; ++i)
                                        d_vec = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                          PT::load(Rp + i*n + j), d_vec);
                                    d_vec = PT::mul(d_vec, vtau);
                                    PT::store(Rp + step*n + j,
                                        PT::sub(PT::load(Rp + step*n + j), d_vec));
                                    for (std::size_t i = step+1; i < m; ++i) {
                                        auto vi = PT::set1(Rp[i*n+step]);
                                        PT::store(Rp + i*n + j,
                                            PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec)));
                                    }
                                }
                            }
                            for (; j < panel_end; ++j) {
                                T d = Rp[step*n + j];
                                for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step] * Rp[i*n+j];
                                d *= tau;
                                Rp[step*n + j] -= d;
                                for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step] * d;
                            }
                        }

                        // Build F: subsequent columns j ∈ [panel_end, n)
                        if (panel_end < n) {
                            T y_buf[NB];
                            for (std::size_t s = 0; s < l; ++s) {
                                std::size_t vs_col = panel_start + s;
                                T dot = Rp[step * n + vs_col];
                                for (std::size_t i = step + 1; i < m; ++i)
                                    dot += Rp[i * n + vs_col] * Rp[i * n + step];
                                y_buf[s] = dot;
                            }

                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                auto vtau = PT::set1(tau);
                                for (; j + W - 1 < n; j += W) {
                                    auto z = PT::load(Rp + step*n + j);
                                    for (std::size_t i = step+1; i < m; ++i)
                                        z = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                      PT::load(Rp + i*n + j), z);
                                    for (std::size_t s = 0; s < l; ++s)
                                        z = PT::sub(z, PT::mul(PT::set1(y_buf[s]),
                                                               PT::load(&F_t[s * n + j])));
                                    PT::store(&F_t[l * n + j], PT::mul(vtau, z));
                                }
                            }
                            for (; j < n; ++j) {
                                T z = Rp[step*n + j];
                                for (std::size_t i = step+1; i < m; ++i) z += Rp[i*n+step] * Rp[i*n+j];
                                for (std::size_t s = 0; s < l; ++s) z -= y_buf[s] * F_t[s * n + j];
                                F_t[l * n + j] = tau * z;
                            }
                        }

                        // Norm update: panel columns
                        for (std::size_t j = step + 1; j < panel_end; ++j)
                            col_norms[j] -= Rp[step*n + j] * Rp[step*n + j];
                        // Norm update: subsequent columns (virtual R_eff)
                        for (std::size_t j = panel_end; j < n; ++j) {
                            T r_eff = Rp[step*n + j];
                            for (std::size_t s = 0; s < l; ++s)
                                r_eff -= Rp[step*n + (panel_start+s)] * F_t[s * n + j];
                            r_eff -= F_t[l * n + j];
                            col_norms[j] -= r_eff * r_eff;
                        }
                    }

                    // Trailing GEMM: R -= V * F_t^T (branchless optimization)
                    if (panel_end < n) {
                        // Triangular part: rows [panel_start, panel_end)
                        for (std::size_t i = panel_start; i < panel_end; ++i) {
                            std::size_t i_local = i - panel_start;
                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                for (; j + W - 1 < n; j += W) {
                                    auto acc = PT::load(&F_t[i_local * n + j]); // V(i,ps+i_local)=1
                                    for (std::size_t ll = 0; ll < i_local; ++ll)
                                        acc = PT::fmadd(PT::set1(Rp[i*n + panel_start+ll]),
                                                        PT::load(&F_t[ll * n + j]), acc);
                                    PT::store(Rp + i*n + j,
                                        PT::sub(PT::load(Rp + i*n + j), acc));
                                }
                            }
                            for (; j < n; ++j) {
                                T s = F_t[i_local * n + j];
                                for (std::size_t ll = 0; ll < i_local; ++ll)
                                    s += Rp[i*n + panel_start+ll] * F_t[ll * n + j];
                                Rp[i*n + j] -= s;
                            }
                        }
                        // Rectangular part: rows [panel_end, m) — all V(i,l) = R(i,ps+l)
                        for (std::size_t i = panel_end; i < m; ++i) {
                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                for (; j + W - 1 < n; j += W) {
                                    auto acc = PT::set1(T(0));
                                    for (std::size_t ll = 0; ll < nb; ++ll)
                                        acc = PT::fmadd(PT::set1(Rp[i*n + panel_start+ll]),
                                                        PT::load(&F_t[ll * n + j]), acc);
                                    PT::store(Rp + i*n + j,
                                        PT::sub(PT::load(Rp + i*n + j), acc));
                                }
                            }
                            for (; j < n; ++j) {
                                T s = T(0);
                                for (std::size_t ll = 0; ll < nb; ++ll)
                                    s += Rp[i*n + panel_start+ll] * F_t[ll * n + j];
                                Rp[i*n + j] -= s;
                            }
                        }
                    }
                }
            }

            // Rank determination
            T threshold = std::abs(Rp[0]) * T(m) * std::numeric_limits<T>::epsilon();
            std::size_t r = 0;
            for (std::size_t i = 0; i < k; ++i) {
                if (std::abs(Rp[i * n + i]) > threshold) ++r;
                else break;
            }

            // Compute Q^T * b directly via Householder reflections (AVX2)
            Vector<T> qtb = b;
            T* qp = qtb.data();
            for (std::size_t step = 0; step < k; ++step) {
                if (tau_vec[step] == T(0)) continue;
                const std::size_t len = m - step - 1;
                std::size_t i = step + 1;
                T d;
                if constexpr (W > 1) {
                    auto acc = PT::set1(T(0));
                    std::size_t endw = step + 1 + (len & ~(W - 1));
                    for (; i < endw; i += W)
                        acc = PT::fmadd(PT::load(Rp + i * n + step), // stride-n... non-contiguous
                                        PT::load(qp + i), acc);  // this is contiguous but Rp is non-contiguous
                    // Rp[i*n+step] is stride-n → AVX2 gather would be needed
                    // → scalar fallback
                    i = step + 1;
                    d = qp[step];
                    for (; i < m; ++i)
                        d += Rp[i * n + step] * qp[i];
                } else {
                    d = qp[step];
                    for (; i < m; ++i)
                        d += Rp[i * n + step] * qp[i];
                }
                d *= tau_vec[step];
                qp[step] -= d;
                for (std::size_t i2 = step + 1; i2 < m; ++i2)
                    qp[i2] -= Rp[i2 * n + step] * d;
            }

            // Backward substitution (AVX2 inner product)
            Vector<T> y(n, T(0));
            T* yp = y.data();
            for (std::size_t ii = 0; ii < r; ++ii) {
                std::size_t i = r - 1 - ii;
                const T* row = Rp + i * n;
                std::size_t j = i + 1;
                if constexpr (W > 1) {
                    auto acc = PT::set1(T(0));
                    std::size_t endw = i + 1 + ((r - i - 1) & ~(W - 1));
                    for (; j < endw; j += W)
                        acc = PT::fmadd(PT::load(row + j), PT::load(yp + j), acc);
                    T sum = PT::reduce_add(acc);
                    for (; j < r; ++j) sum += row[j] * yp[j];
                    yp[i] = (qtb[i] - sum) / row[i];
                } else {
                    T sum = T(0);
                    for (; j < r; ++j) sum += row[j] * yp[j];
                    yp[i] = (qtb[i] - sum) / row[i];
                }
            }

            // Apply the inverse permutation
            Vector<T> x(n, T(0));
            for (std::size_t j = 0; j < n; ++j)
                x[perm[j]] = yp[j];

            return x;
        }

        // ====================================================================
        // FullPivLU — full-pivoting LU decomposition
        // ====================================================================

        /**
         * @brief Result of the full-pivoting LU decomposition
         *
         * P * A * Q = L * U (P is a row permutation, Q is a column permutation)
         */
        template<typename T>
        struct FullPivLUResult {
            Matrix<T> LU;                     ///< L and U combined (the diagonal is U)
            std::vector<std::size_t> row_perm; ///< row permutation
            std::vector<std::size_t> col_perm; ///< column permutation
            std::size_t pivots;                ///< number of pivots (for sign computation)

            /// numerical rank
            [[nodiscard]] std::size_t rank(T threshold = T(-1)) const {
                const auto k = std::min(LU.rows(), LU.cols());
                if (k == 0) return 0;
                if (threshold < T(0)) {
                    threshold = std::abs(LU(0, 0)) *
                        static_cast<T>(std::max(LU.rows(), LU.cols())) *
                        std::numeric_limits<T>::epsilon();
                }
                std::size_t r = 0;
                for (std::size_t i = 0; i < k; ++i) {
                    if (std::abs(LU(i, i)) > threshold) ++r;
                    else break;
                }
                return r;
            }

            /// determinant (square matrices only)
            [[nodiscard]] T determinant() const {
                const auto n = LU.rows();
                if (n != LU.cols()) {
                    assert(false && "DimensionError: FullPivLU::determinant: matrix must be square");
                    throw DimensionError("FullPivLU::determinant: matrix must be square");
                }
                T det = (pivots % 2 == 0) ? T(1) : T(-1);
                for (std::size_t i = 0; i < n; ++i) det *= LU(i, i);
                return det;
            }

            /// Obtain the L matrix
            [[nodiscard]] Matrix<T> matrixL() const {
                const auto m = LU.rows();
                const auto k = std::min(m, LU.cols());
                Matrix<T> L(m, k, T(0));
                for (std::size_t i = 0; i < m; ++i) {
                    for (std::size_t j = 0; j < std::min(i, k); ++j)
                        L(i, j) = LU(i, j);
                    if (i < k) L(i, i) = T(1);
                }
                return L;
            }

            /// Obtain the U matrix
            [[nodiscard]] Matrix<T> matrixU() const {
                const auto k = std::min(LU.rows(), LU.cols());
                const auto n = LU.cols();
                Matrix<T> U(k, n, T(0));
                for (std::size_t i = 0; i < k; ++i)
                    for (std::size_t j = i; j < n; ++j)
                        U(i, j) = LU(i, j);
                return U;
            }
        };

        /**
         * @brief Full-pivoting LU decomposition
         *
         * Select pivots in both rows and columns to maximize numerical stability.
         * P * A * Q = L * U
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        FullPivLUResult<sangi::element_t<MA>> full_piv_lu(const MA& a) {
            using T = sangi::element_t<MA>;
            // rectangular OK

            const auto m = a.rows();
            const auto n = a.cols();
            const auto k = std::min(m, n);

            Matrix<T> LU = a;
            T* Lp = LU.data();
            std::vector<std::size_t> rperm(m), cperm(n);
            std::iota(rperm.begin(), rperm.end(), std::size_t(0));
            std::iota(cperm.begin(), cperm.end(), std::size_t(0));
            std::size_t swaps = 0;

            for (std::size_t step = 0; step < k; ++step) {
                // Search for the maximum over all elements
                T best_val = T(0);
                std::size_t best_i = step, best_j = step;
                for (std::size_t i = step; i < m; ++i) {
                    const T* row = Lp + i * n;
                    for (std::size_t j = step; j < n; ++j) {
                        T v = std::abs(row[j]);
                        if (v > best_val) {
                            best_val = v;
                            best_i = i;
                            best_j = j;
                        }
                    }
                }

                if (best_val < std::numeric_limits<T>::epsilon()) break;

                // Row swap
                if (best_i != step) {
                    std::swap(rperm[step], rperm[best_i]);
                    T* r1 = Lp + step * n;
                    T* r2 = Lp + best_i * n;
                    for (std::size_t j = 0; j < n; ++j)
                        std::swap(r1[j], r2[j]);
                    ++swaps;
                }

                // Column swap
                if (best_j != step) {
                    std::swap(cperm[step], cperm[best_j]);
                    for (std::size_t i = 0; i < m; ++i)
                        std::swap(Lp[i * n + step], Lp[i * n + best_j]);
                    ++swaps;
                }

                // Elimination
                T* pivot_row = Lp + step * n;
                T inv_pivot = T(1) / pivot_row[step];
                for (std::size_t i = step + 1; i < m; ++i) {
                    T* row = Lp + i * n;
                    T factor = row[step] * inv_pivot;
                    row[step] = factor;  // element of L
                    for (std::size_t j = step + 1; j < n; ++j)
                        row[j] -= factor * pivot_row[j];
                }
            }

            return { std::move(LU), std::move(rperm), std::move(cperm), swaps };
        }

        /// Solve a system of linear equations via full-pivoting LU
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> full_piv_lu_solve(const MA& A, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            const auto m = A.rows();
            const auto n = A.cols();
            if (m != n) {
                assert(false && "DimensionError: full_piv_lu_solve: matrix must be square");
                throw DimensionError("full_piv_lu_solve: matrix must be square");
            }
            if (b.size() != m) {
                assert(false && "DimensionError: full_piv_lu_solve: dimension mismatch");
                throw DimensionError("full_piv_lu_solve: dimension mismatch");
            }

            auto result = full_piv_lu(A);
            const T* Lp = result.LU.data();

            // P * b (apply the row permutation)
            Vector<T> pb(m);
            for (std::size_t i = 0; i < m; ++i)
                pb[i] = b[result.row_perm[i]];
            T* pp = pb.data();

            // Forward substitution: L * y = P * b
            for (std::size_t i = 1; i < m; ++i) {
                const T* row = Lp + i * m;
                T sum = T(0);
                for (std::size_t j = 0; j < i; ++j)
                    sum += row[j] * pp[j];
                pp[i] -= sum;
            }

            // Backward substitution: U * z = y
            for (std::size_t ii = 0; ii < m; ++ii) {
                std::size_t i = m - 1 - ii;
                const T* row = Lp + i * m;
                T sum = T(0);
                for (std::size_t j = i + 1; j < m; ++j)
                    sum += row[j] * pp[j];
                pp[i] = (pp[i] - sum) / row[i];
            }

            // Q^T * z → x (apply the inverse of the column permutation)
            Vector<T> x(n);
            for (std::size_t j = 0; j < n; ++j)
                x[result.col_perm[j]] = pb[j];

            return x;
        }

        // ====================================================================
        // CompleteOrthogonalDecomposition — complete orthogonal decomposition
        // ====================================================================

        /**
         * @brief Result of the complete orthogonal decomposition
         *
         * A * P = Q * [T 0; 0 0] * Z
         * Here T is r×r upper triangular, Q is m×m orthogonal, Z is n×n orthogonal, and P is a column permutation.
         * Used for the minimum-norm least-squares solution of a rank-deficient matrix.
         */
        template<typename T>
        struct CODResult {
            Matrix<T> Q;                      ///< orthogonal matrix (m×m)
            Matrix<T> T_mat;                  ///< upper triangular (r×r)
            Matrix<T> Z;                      ///< orthogonal matrix (n×n)
            std::vector<std::size_t> perm;    ///< column permutation
            std::size_t r;                    ///< rank

            [[nodiscard]] std::size_t rank() const { return r; }
        };

        /**
         * @brief Complete orthogonal decomposition
         *
         * After ColPivQR, apply a right Householder to the first r rows of R so that
         * R[:r,:] = T * Z^T (T is r×r upper triangular).
         */
        template<typename MA>
            requires sangi::BaseMatrixLike<MA>
        CODResult<sangi::element_t<MA>> complete_orthogonal_decomposition(const MA& A) {
            using T = sangi::element_t<MA>;
            // rectangular OK
            auto qr = col_piv_qr(A);
            const auto m = A.rows();
            const auto n = A.cols();
            const auto r = qr.rank();

            if (r == 0 || r == n) {
                // Rank 0 or full rank → Z = I
                Matrix<T> Zmat(n, n, T(0));
                for (std::size_t i = 0; i < n; ++i) Zmat(i, i) = T(1);
                Matrix<T> Tmat(r, r, T(0));
                for (std::size_t i = 0; i < r; ++i)
                    for (std::size_t j = i; j < r; ++j)
                        Tmat(i, j) = qr.R(i, j);
                return { std::move(qr.Q), std::move(Tmat), std::move(Zmat),
                         std::move(qr.perm), r };
            }

            // RZ decomposition: zero out the right part R[:r, r:] of R[:r,:]
            // For each row k (k = r-1 → 0), apply a Householder to the vector [R(k,k), R(k,r:n)]
            // to zero out R(k,r:n)
            Matrix<T> Rwork(r, n, T(0));
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = 0; j < n; ++j)
                    Rwork(i, j) = qr.R(i, j);

            Matrix<T> Zmat(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) Zmat(i, i) = T(1);

            for (std::size_t kk = 0; kk < r; ++kk) {
                std::size_t k = r - 1 - kk;
                if (r >= n) continue;

                // Via Householder, [R(k,k), R(k,r), R(k,r+1), ...] → [sigma, 0, 0, ...]
                // The reflector vector v is scattered over positions k and r:n
                const std::size_t tail = n - r;
                T x0 = Rwork(k, k);
                T sigma_sq = x0 * x0;
                for (std::size_t j = r; j < n; ++j)
                    sigma_sq += Rwork(k, j) * Rwork(k, j);
                T sigma = std::sqrt(sigma_sq);
                if (sigma < std::numeric_limits<T>::epsilon()) continue;

                T alpha = (x0 >= T(0)) ? -sigma : sigma;
                T beta = x0 - alpha;
                if (std::abs(beta) < std::numeric_limits<T>::epsilon()) continue;

                // v = [1, Rwork(k, r:n) / beta], v[0] corresponds to position k
                T inv_beta = T(1) / beta;
                std::vector<T> vtail(tail);
                for (std::size_t j = 0; j < tail; ++j)
                    vtail[j] = Rwork(k, r + j) * inv_beta;

                T tau = -beta / alpha;  // = 2 / (1 + ||vtail||^2)

                Rwork(k, k) = alpha;
                for (std::size_t j = r; j < n; ++j) Rwork(k, j) = T(0);

                // Apply the reflector to the other rows of Rwork from the right (rows 0..k-1)
                // row[k] += tau * (row[k] + sum(vtail[j]*row[r+j])) → corrected
                for (std::size_t i = 0; i < k; ++i) {
                    T d = Rwork(i, k);
                    for (std::size_t j = 0; j < tail; ++j)
                        d += vtail[j] * Rwork(i, r + j);
                    d *= tau;
                    Rwork(i, k) -= d;
                    for (std::size_t j = 0; j < tail; ++j)
                        Rwork(i, r + j) -= vtail[j] * d;
                }

                // Update Z: apply the reflector to column k and columns r:n of Z
                for (std::size_t i = 0; i < n; ++i) {
                    T d = Zmat(i, k);
                    for (std::size_t j = 0; j < tail; ++j)
                        d += vtail[j] * Zmat(i, r + j);
                    d *= tau;
                    Zmat(i, k) -= d;
                    for (std::size_t j = 0; j < tail; ++j)
                        Zmat(i, r + j) -= vtail[j] * d;
                }
            }

            // T = Rwork[:r, :r]
            Matrix<T> Tmat(r, r, T(0));
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = i; j < r; ++j)
                    Tmat(i, j) = Rwork(i, j);

            return { std::move(qr.Q), std::move(Tmat), std::move(Zmat),
                     std::move(qr.perm), r };
        }

        /// Compute the minimum-norm least-squares solution via complete orthogonal decomposition
        /// Do not build Q, Z explicitly; compute directly with the Householder vectors
        template<typename MA, typename VB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseVectorLike<VB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<VB>>
        Vector<sangi::element_t<MA>> cod_solve(const MA& A, const VB& b) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB);

            if (A.rows() != b.size()) {
                assert(false && "DimensionError: cod_solve: dimension mismatch");
                throw DimensionError("cod_solve: dimension mismatch");
            }

            const auto m = A.rows();
            const auto n = A.cols();
            const auto k = std::min(m, n);

            // --- Phase 1: ColPivQR + AVX2 (keep the Householder vectors in R's lower triangle) ---
            using PT = PacketTraits<T>;
            constexpr std::size_t W = PT::size;

            Matrix<T> R = A;
            T* Rp = R.data();
            std::vector<std::size_t> perm(n);
            std::iota(perm.begin(), perm.end(), std::size_t(0));
            std::vector<T> tau_vec(k, T(0));

            std::vector<T> col_norms(n);
            for (std::size_t j = 0; j < n; ++j) {
                T s = T(0);
                for (std::size_t i = 0; i < m; ++i) s += Rp[i * n + j] * Rp[i * n + j];
                col_norms[j] = s;
            }

            // Select unblocked/panel factorization depending on size
            constexpr std::size_t NB_COD = 16;
            constexpr std::size_t COD_PANEL_THRESHOLD = 384;

            if (n < COD_PANEL_THRESHOLD) {
                // --- Unblocked version ---
                for (std::size_t step = 0; step < k; ++step) {
                    std::size_t best = step;
                    T best_norm = col_norms[step];
                    for (std::size_t j = step + 1; j < n; ++j)
                        if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; }
                    if (best != step) {
                        std::swap(perm[step], perm[best]);
                        std::swap(col_norms[step], col_norms[best]);
                        for (std::size_t i = 0; i < m; ++i)
                            std::swap(Rp[i*n+step], Rp[i*n+best]);
                    }
                    T sigma = T(0);
                    for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step]*Rp[i*n+step];
                    sigma = std::sqrt(sigma);
                    if (sigma < std::numeric_limits<T>::epsilon()) continue;
                    T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma;
                    T beta_v = Rp[step*n+step] - alpha;
                    Rp[step*n+step] = alpha;
                    T inv_beta = T(1) / beta_v;
                    for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta;
                    T tau = -beta_v / alpha;
                    tau_vec[step] = tau;
                    std::size_t j = step + 1;
                    if constexpr (W > 1) {
                        auto vtau = PT::set1(tau);
                        for (; j + W - 1 < n; j += W) {
                            auto d_vec = PT::load(Rp + step*n + j);
                            for (std::size_t i = step+1; i < m; ++i)
                                d_vec = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                  PT::load(Rp + i*n + j), d_vec);
                            d_vec = PT::mul(d_vec, vtau);
                            PT::store(Rp + step*n + j,
                                PT::sub(PT::load(Rp + step*n + j), d_vec));
                            for (std::size_t i = step+1; i < m; ++i) {
                                auto vi = PT::set1(Rp[i*n+step]);
                                PT::store(Rp + i*n + j,
                                    PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec)));
                            }
                        }
                    }
                    for (; j < n; ++j) {
                        T d = Rp[step*n+j];
                        for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step]*Rp[i*n+j];
                        d *= tau;
                        Rp[step*n+j] -= d;
                        for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step]*d;
                    }
                    for (std::size_t jj = step+1; jj < n; ++jj)
                        col_norms[jj] -= Rp[step*n+jj]*Rp[step*n+jj];
                }
            } else {
                // --- Panel factorization (F matrix) + trailing GEMM ---
                std::vector<T> F_t_cod(NB_COD * n, T(0));

                for (std::size_t panel_start = 0; panel_start < k; panel_start += NB_COD) {
                    std::size_t panel_end = std::min(panel_start + NB_COD, k);
                    std::size_t nb = panel_end - panel_start;

                    for (std::size_t ll = 0; ll < nb; ++ll)
                        std::fill(&F_t_cod[ll*n], &F_t_cod[ll*n+n], T(0));

                    for (std::size_t l = 0; l < nb; ++l) {
                        std::size_t step = panel_start + l;
                        std::size_t best = step;
                        T best_norm = col_norms[step];
                        for (std::size_t j = step + 1; j < n; ++j)
                            if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; }

                        if (best != step) {
                            if (best >= panel_end && l > 0) {
                                for (std::size_t i = panel_start; i < step; ++i) {
                                    std::size_t i_local = i - panel_start;
                                    T c = F_t_cod[i_local * n + best];
                                    for (std::size_t s = 0; s < i_local; ++s)
                                        c += Rp[i*n+(panel_start+s)] * F_t_cod[s*n+best];
                                    Rp[i*n+best] -= c;
                                }
                                for (std::size_t i = step; i < m; ++i) {
                                    T c = T(0);
                                    for (std::size_t s = 0; s < l; ++s)
                                        c += Rp[i*n+(panel_start+s)] * F_t_cod[s*n+best];
                                    Rp[i*n+best] -= c;
                                }
                                for (std::size_t s = 0; s < l; ++s) F_t_cod[s*n+best] = T(0);
                            }
                            std::swap(perm[step], perm[best]);
                            std::swap(col_norms[step], col_norms[best]);
                            for (std::size_t i = 0; i < m; ++i)
                                std::swap(Rp[i*n+step], Rp[i*n+best]);
                            for (std::size_t s = 0; s < l; ++s)
                                std::swap(F_t_cod[s*n+step], F_t_cod[s*n+best]);
                        }

                        T sigma = T(0);
                        for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step]*Rp[i*n+step];
                        sigma = std::sqrt(sigma);
                        if (sigma < std::numeric_limits<T>::epsilon()) continue;
                        T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma;
                        T beta_v = Rp[step*n+step] - alpha;
                        Rp[step*n+step] = alpha;
                        T inv_beta = T(1) / beta_v;
                        for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta;
                        T tau = -beta_v / alpha;
                        tau_vec[step] = tau;

                        {
                            std::size_t j = step + 1;
                            if constexpr (W > 1) {
                                auto vtau = PT::set1(tau);
                                for (; j + W - 1 < panel_end; j += W) {
                                    auto d_vec = PT::load(Rp + step*n + j);
                                    for (std::size_t i = step+1; i < m; ++i)
                                        d_vec = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                          PT::load(Rp + i*n + j), d_vec);
                                    d_vec = PT::mul(d_vec, vtau);
                                    PT::store(Rp + step*n + j,
                                        PT::sub(PT::load(Rp + step*n + j), d_vec));
                                    for (std::size_t i = step+1; i < m; ++i) {
                                        auto vi = PT::set1(Rp[i*n+step]);
                                        PT::store(Rp + i*n + j,
                                            PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec)));
                                    }
                                }
                            }
                            for (; j < panel_end; ++j) {
                                T d = Rp[step*n+j];
                                for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step]*Rp[i*n+j];
                                d *= tau;
                                Rp[step*n+j] -= d;
                                for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step]*d;
                            }
                        }

                        if (panel_end < n) {
                            T y_buf[NB_COD];
                            for (std::size_t s = 0; s < l; ++s) {
                                std::size_t vs_col = panel_start + s;
                                T dot = Rp[step*n+vs_col];
                                for (std::size_t i = step+1; i < m; ++i)
                                    dot += Rp[i*n+vs_col] * Rp[i*n+step];
                                y_buf[s] = dot;
                            }
                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                auto vtau = PT::set1(tau);
                                for (; j + W - 1 < n; j += W) {
                                    auto z = PT::load(Rp + step*n + j);
                                    for (std::size_t i = step+1; i < m; ++i)
                                        z = PT::fmadd(PT::set1(Rp[i*n+step]),
                                                      PT::load(Rp + i*n + j), z);
                                    for (std::size_t s = 0; s < l; ++s)
                                        z = PT::sub(z, PT::mul(PT::set1(y_buf[s]),
                                                               PT::load(&F_t_cod[s*n + j])));
                                    PT::store(&F_t_cod[l*n + j], PT::mul(vtau, z));
                                }
                            }
                            for (; j < n; ++j) {
                                T z = Rp[step*n+j];
                                for (std::size_t i = step+1; i < m; ++i) z += Rp[i*n+step]*Rp[i*n+j];
                                for (std::size_t s = 0; s < l; ++s) z -= y_buf[s] * F_t_cod[s*n+j];
                                F_t_cod[l*n+j] = tau * z;
                            }
                        }

                        for (std::size_t j = step+1; j < panel_end; ++j)
                            col_norms[j] -= Rp[step*n+j]*Rp[step*n+j];
                        for (std::size_t j = panel_end; j < n; ++j) {
                            T r_eff = Rp[step*n+j];
                            for (std::size_t s = 0; s < l; ++s)
                                r_eff -= Rp[step*n+(panel_start+s)] * F_t_cod[s*n+j];
                            r_eff -= F_t_cod[l*n+j];
                            col_norms[j] -= r_eff * r_eff;
                        }
                    }

                    if (panel_end < n) {
                        for (std::size_t i = panel_start; i < panel_end; ++i) {
                            std::size_t i_local = i - panel_start;
                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                for (; j + W - 1 < n; j += W) {
                                    auto acc = PT::load(&F_t_cod[i_local*n + j]);
                                    for (std::size_t ll = 0; ll < i_local; ++ll)
                                        acc = PT::fmadd(PT::set1(Rp[i*n+panel_start+ll]),
                                                        PT::load(&F_t_cod[ll*n + j]), acc);
                                    PT::store(Rp + i*n + j,
                                        PT::sub(PT::load(Rp + i*n + j), acc));
                                }
                            }
                            for (; j < n; ++j) {
                                T s = F_t_cod[i_local*n+j];
                                for (std::size_t ll = 0; ll < i_local; ++ll)
                                    s += Rp[i*n+panel_start+ll] * F_t_cod[ll*n+j];
                                Rp[i*n+j] -= s;
                            }
                        }
                        for (std::size_t i = panel_end; i < m; ++i) {
                            std::size_t j = panel_end;
                            if constexpr (W > 1) {
                                for (; j + W - 1 < n; j += W) {
                                    auto acc = PT::set1(T(0));
                                    for (std::size_t ll = 0; ll < nb; ++ll)
                                        acc = PT::fmadd(PT::set1(Rp[i*n+panel_start+ll]),
                                                        PT::load(&F_t_cod[ll*n + j]), acc);
                                    PT::store(Rp + i*n + j,
                                        PT::sub(PT::load(Rp + i*n + j), acc));
                                }
                            }
                            for (; j < n; ++j) {
                                T s = T(0);
                                for (std::size_t ll = 0; ll < nb; ++ll)
                                    s += Rp[i*n+panel_start+ll] * F_t_cod[ll*n+j];
                                Rp[i*n+j] -= s;
                            }
                        }
                    }
                }
            }

            // Rank determination
            T threshold = std::abs(Rp[0]) * T(m) * std::numeric_limits<T>::epsilon();
            std::size_t r = 0;
            for (std::size_t i = 0; i < k; ++i) {
                if (std::abs(Rp[i * n + i]) > threshold) ++r;
                else break;
            }

            // --- Phase 2: Q^T * b (computed directly via Householder reflections) ---
            Vector<T> qtb = b;
            T* qp = qtb.data();
            for (std::size_t step = 0; step < k; ++step) {
                if (tau_vec[step] == T(0)) continue;
                T d = qp[step];
                for (std::size_t i = step + 1; i < m; ++i)
                    d += Rp[i * n + step] * qp[i];
                d *= tau_vec[step];
                qp[step] -= d;
                for (std::size_t i = step + 1; i < m; ++i)
                    qp[i] -= Rp[i * n + step] * d;
            }

            if (r == 0) {
                Vector<T> x(n, T(0));
                return x;
            }

            if (r == n) {
                // Full rank: no RZ needed, T^{-1} * qtb + inverse permutation
                Vector<T> y(n, T(0));
                for (std::size_t ii = 0; ii < r; ++ii) {
                    std::size_t i = r - 1 - ii;
                    y[i] = qtb[i];
                    for (std::size_t j = i + 1; j < r; ++j)
                        y[i] -= Rp[i * n + j] * y[j];
                    y[i] /= Rp[i * n + i];
                }
                Vector<T> x(n, T(0));
                for (std::size_t j = 0; j < n; ++j)
                    x[perm[j]] = y[j];
                return x;
            }

            // --- Phase 3: RZ decomposition + T^{-1} + Z application (keeping the Householder vectors) ---
            // Copy the upper triangular part of R[:r, :] into Rwork
            // (Rp's lower triangle holds the QR Householder vectors, so use a separate buffer)
            Matrix<T> Rwork(r, n, T(0));
            T* Rwp = Rwork.data();
            for (std::size_t i = 0; i < r; ++i)
                for (std::size_t j = i; j < n; ++j)
                    Rwp[i * n + j] = Rp[i * n + j];

            // Save the RZ Householder vectors and tau
            std::vector<std::vector<T>> rz_vtail(r);
            std::vector<T> rz_tau(r, T(0));
            const std::size_t tail = n - r;

            for (std::size_t kk = 0; kk < r; ++kk) {
                std::size_t kv = r - 1 - kk;

                T x0 = Rwp[kv * n + kv];
                T sigma_sq = x0 * x0;
                for (std::size_t j = r; j < n; ++j)
                    sigma_sq += Rwp[kv * n + j] * Rwp[kv * n + j];
                T sig = std::sqrt(sigma_sq);
                if (sig < std::numeric_limits<T>::epsilon()) continue;

                T alph = (x0 >= T(0)) ? -sig : sig;
                T bet = x0 - alph;
                if (std::abs(bet) < std::numeric_limits<T>::epsilon()) continue;

                T inv_b = T(1) / bet;
                rz_vtail[kv].resize(tail);
                for (std::size_t j = 0; j < tail; ++j)
                    rz_vtail[kv][j] = Rwp[kv * n + r + j] * inv_b;

                T tau_rz = -bet / alph;
                rz_tau[kv] = tau_rz;

                Rwp[kv * n + kv] = alph;
                for (std::size_t j = r; j < n; ++j) Rwp[kv * n + j] = T(0);

                // Apply the reflector to the other rows of Rwork from the right
                for (std::size_t i = 0; i < kv; ++i) {
                    T d = Rwp[i * n + kv];
                    for (std::size_t j = 0; j < tail; ++j)
                        d += rz_vtail[kv][j] * Rwp[i * n + r + j];
                    d *= tau_rz;
                    Rwp[i * n + kv] -= d;
                    for (std::size_t j = 0; j < tail; ++j)
                        Rwp[i * n + r + j] -= rz_vtail[kv][j] * d;
                }
            }

            // T^{-1} * qtb[:r]
            Vector<T> w(n, T(0));
            for (std::size_t ii = 0; ii < r; ++ii) {
                std::size_t i = r - 1 - ii;
                w[i] = qtb[i];
                for (std::size_t j = i + 1; j < r; ++j)
                    w[i] -= Rwp[i * n + j] * w[j];
                w[i] /= Rwp[i * n + i];
            }

            // Compute Z * w via Householder reflections (applied in reverse order)
            // Z = H_0 * H_1 * ... * H_{r-1}
            // Z * w = H_0(H_1(...(H_{r-1} * w)...))
            // Each H_k acts on position k and r:n
            for (std::size_t kv = 0; kv < r; ++kv) {
                if (rz_tau[kv] == T(0)) continue;
                T d = w[kv];
                for (std::size_t j = 0; j < tail; ++j)
                    d += rz_vtail[kv][j] * w[r + j];
                d *= rz_tau[kv];
                w[kv] -= d;
                for (std::size_t j = 0; j < tail; ++j)
                    w[r + j] -= rz_vtail[kv][j] * d;
            }

            // Inverse of the column permutation
            Vector<T> x(n, T(0));
            for (std::size_t j = 0; j < n; ++j)
                x[perm[j]] = w[j];

            return x;
        }

        // ====================================================================
        // RealQZ — generalized Schur (QZ) decomposition
        // ====================================================================

        /**
         * @brief Result of the QZ decomposition
         *
         * Q^T * A * Z = S (quasi-upper triangular)
         * Q^T * B * Z = T (upper triangular)
         * Generalized eigenvalues = diag(S) / diag(T)
         */
        template<typename T>
        struct QZResult {
            Matrix<T> S;   ///< quasi-upper-triangular (real Schur form)
            Matrix<T> TT;  ///< upper triangular
            Matrix<T> Q;   ///< left orthogonal matrix
            Matrix<T> Z;   ///< right orthogonal matrix

            /// generalized eigenvalues (alpha/beta pairs)
            [[nodiscard]] std::vector<std::pair<Complex<T>, T>>
            eigenvalues() const {
                const auto n = S.rows();
                std::vector<std::pair<Complex<T>, T>> eigs;
                eigs.reserve(n);
                for (std::size_t i = 0; i < n; ) {
                    if (i + 1 < n && std::abs(S(i + 1, i)) >
                        std::numeric_limits<T>::epsilon() * 100 *
                        (std::abs(S(i, i)) + std::abs(S(i + 1, i + 1)))) {
                        // 2×2 block → complex eigenvalue pair
                        T a = S(i, i), b = S(i, i + 1), c = S(i + 1, i), d = S(i + 1, i + 1);
                        T tr = a + d, det = a * d - b * c;
                        T disc = tr * tr - T(4) * det;
                        T beta1 = TT(i, i), beta2 = TT(i + 1, i + 1);
                        if (disc < T(0)) {
                            T re = tr / T(2);
                            T im = std::sqrt(-disc) / T(2);
                            eigs.push_back({ {re, im}, beta1 });
                            eigs.push_back({ {re, -im}, beta2 });
                        } else {
                            T sq = std::sqrt(disc);
                            eigs.push_back({ {(tr + sq) / T(2), T(0)}, beta1 });
                            eigs.push_back({ {(tr - sq) / T(2), T(0)}, beta2 });
                        }
                        i += 2;
                    } else {
                        eigs.push_back({ {S(i, i), T(0)}, TT(i, i) });
                        ++i;
                    }
                }
                return eigs;
            }
        };

        /**
         * @brief QZ decomposition (generalized Schur decomposition)
         *
         * Implementation: Hessenberg-triangularization + QZ iteration (single shift)
         * Q^T * A * Z = S, Q^T * B * Z = T
         */
        template<typename MA, typename MB>
            requires sangi::BaseMatrixLike<MA>
                  && sangi::BaseMatrixLike<MB>
                  && std::same_as<sangi::element_t<MA>, sangi::element_t<MB>>
        QZResult<sangi::element_t<MA>> real_qz(const MA& A, const MB& B) {
            using T = sangi::element_t<MA>;
            SANGI_STATIC_ASSERT_SQUARE(MA);
            SANGI_STATIC_ASSERT_SQUARE(MB);

            const auto n = A.rows();
            if (n != A.cols() || n != B.rows() || n != B.cols()) {
                assert(false && "DimensionError: real_qz: A and B must be square with same size");
                throw DimensionError("real_qz: A and B must be square with same size");
            }

            if (n == 0) return { Matrix<T>(), Matrix<T>(), Matrix<T>(), Matrix<T>() };
            if (n == 1) return { A, B, Matrix<T>({{T(1)}}), Matrix<T>({{T(1)}}) };

            Matrix<T> H = A;
            Matrix<T> R = B;
            Matrix<T> Q(n, n, T(0)), Z(n, n, T(0));
            for (std::size_t i = 0; i < n; ++i) { Q(i, i) = T(1); Z(i, i) = T(1); }

            // Givens rotation helper: a,b → (c,s,r) s.t. [c s; -s c]^T [a;b] = [r;0]
            auto givens = [](T a, T b) -> std::tuple<T,T,T> {
                T r = std::sqrt(a*a + b*b);
                if (r < std::numeric_limits<T>::epsilon())
                    return {T(1), T(0), T(0)};
                return {a/r, b/r, r};
            };

            // Left Givens: apply to rows i,j
            auto apply_left = [&](T cs, T sn, std::size_t i, std::size_t j,
                                   Matrix<T>& M, std::size_t col_start, std::size_t col_end) {
                for (std::size_t c = col_start; c < col_end; ++c) {
                    T t1 = M(i, c), t2 = M(j, c);
                    M(i, c) = cs * t1 + sn * t2;
                    M(j, c) = -sn * t1 + cs * t2;
                }
            };

            // Right Givens: apply to columns i,j
            auto apply_right = [&](T cs, T sn, std::size_t i, std::size_t j,
                                    Matrix<T>& M, std::size_t row_start, std::size_t row_end) {
                for (std::size_t r = row_start; r < row_end; ++r) {
                    T t1 = M(r, i), t2 = M(r, j);
                    M(r, i) = cs * t1 + sn * t2;
                    M(r, j) = -sn * t1 + cs * t2;
                }
            };

            // Step 1: upper-triangularize B (left Givens)
            for (std::size_t k = 0; k + 1 < n; ++k) {
                auto [cs, sn, r] = givens(R(k, k), R(k+1, k));
                apply_left(cs, sn, k, k+1, R, k, n);
                apply_left(cs, sn, k, k+1, H, 0, n);
                apply_right(cs, sn, k, k+1, Q, 0, n);
            }

            // Step 2: reduce H to upper Hessenberg (right Givens + left Givens to re-triangularize R)
            for (std::size_t k = 0; k + 2 < n; ++k) {
                for (std::size_t i = n - 1; i > k + 1; --i) {
                    // Zero out H(i, k): right Givens (columns i-1, i)
                    auto [cs, sn, r] = givens(H(i-1, k), H(i, k));
                    apply_right(cs, sn, i-1, i, H, 0, n);
                    apply_right(cs, sn, i-1, i, R, 0, n);
                    apply_right(cs, sn, i-1, i, Z, 0, n);

                    // Zero out R(i, i-1): left Givens (rows i-1, i)
                    if (std::abs(R(i, i-1)) > std::numeric_limits<T>::epsilon()) {
                        auto [c2, s2, r2] = givens(R(i-1, i-1), R(i, i-1));
                        apply_left(c2, s2, i-1, i, R, i-1, n);
                        apply_left(c2, s2, i-1, i, H, 0, n);
                        apply_right(c2, s2, i-1, i, Q, 0, n);
                    }
                }
            }

            // Step 3: QZ iteration
            const std::size_t max_qz_iter = 100 * n;
            std::size_t p = n;

            for (std::size_t iter = 0; iter < max_qz_iter && p > 1; ++iter) {
                // deflation check
                T tol = std::numeric_limits<T>::epsilon() *
                    (std::abs(H(p-2, p-2)) + std::abs(H(p-1, p-1)));
                if (std::abs(H(p-1, p-2)) <= std::max(tol, std::numeric_limits<T>::min())) {
                    H(p-1, p-2) = T(0);
                    --p;
                    continue;
                }

                // Wilkinson shift: the one closer to the generalized eigenvalue a22/b22
                T b22_inv = (std::abs(R(p-1, p-1)) > std::numeric_limits<T>::epsilon())
                    ? T(1) / R(p-1, p-1) : T(0);
                T shift = H(p-1, p-1) * b22_inv;

                // QZ step: start the Givens from the leading element of (H - shift * R)
                for (std::size_t k = 0; k + 1 < p; ++k) {
                    T x, y;
                    if (k == 0) {
                        x = H(0, 0) - shift * R(0, 0);
                        y = H(1, 0);
                    } else {
                        x = H(k, k-1);
                        y = H(k+1, k-1);
                    }
                    auto [cs, sn, rv] = givens(x, y);

                    // Left Givens (rows k, k+1)
                    apply_left(cs, sn, k, k+1, H, 0, n);
                    apply_left(cs, sn, k, k+1, R, 0, n);
                    apply_right(cs, sn, k, k+1, Q, 0, n);

                    // Zero out R(k+1, k): right Givens (columns k+1, k)
                    auto [c2, s2, r2] = givens(R(k+1, k+1), R(k+1, k));
                    apply_right(c2, s2, k+1, k, R, 0, n);
                    apply_right(c2, s2, k+1, k, H, 0, n);
                    apply_right(c2, s2, k+1, k, Z, 0, n);
                }
            }

            return { std::move(H), std::move(R), std::move(Q), std::move(Z) };
        }

    } // namespace algorithms
} // namespace sangi

#endif // SANGI_DECOMPOSITION_HPP