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

/**
 * @file sparse_matrix_algorithms.hpp
 * @brief Iterative solvers and preconditioners for sparse matrices
 *
 * Iterative solvers: CG, BiCG, BiCGSTAB, GMRES(m)
 * Preconditioners: Jacobi, SSOR, ILU(0), IC(0)
 * All solvers return SolverResult<T>; preconditioners are injected as callables.
 */

#ifndef SANGI_SPARSE_MATRIX_ALGORITHMS_HPP
#define SANGI_SPARSE_MATRIX_ALGORITHMS_HPP

#include <optional>
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <vector>
#include <utility>
#include <functional>
#include <string>
#include <map>
#include <memory>

#include "../concepts/algebraic_concepts.hpp"
#include "sparse_matrix.hpp"
#include "vector.hpp"
#include "convergence_criteria.hpp"

namespace sangi {

namespace sparse_algorithms {

// ============================================================================
// SolverResult — return value of solvers
// ============================================================================

template<typename T>
struct SolverResult {
    Vector<T>    x;              ///< Solution vector
    T            residual_norm;  ///< Final residual norm
    std::size_t  iterations;     ///< Iteration count
    bool         converged;      ///< Whether convergence was achieved
};

// ============================================================================
// Preconditioner type definition — callable (Vector -> Vector)
// ============================================================================

template<typename T>
using Preconditioner = std::function<Vector<T>(const Vector<T>&)>;

/// Identity preconditioner (no preconditioning)
template<typename T>
Preconditioner<T> identity_preconditioner() {
    return [](const Vector<T>& r) -> Vector<T> { return r; };
}

// ============================================================================
// Helper: extract the diagonal of a sparse matrix
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
std::vector<T> sparse_diagonal(const SparseMatrix<T, IndexType>& A) {
    const auto n = A.rows();
    std::vector<T> diag(n, T{0});
    for (IndexType i = 0; i < n; ++i) {
        diag[i] = A.coeff(i, i);
    }
    return diag;
}

// ============================================================================
// Preconditioner factories
// ============================================================================

/**
 * @brief Jacobi (diagonal) preconditioner
 *
 * M = diag(A), M^{-1} r = r[i] / A(i,i)
 * The lightest preconditioner at O(n); effective for diagonally dominant matrices.
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Preconditioner<T> jacobi_preconditioner(const SparseMatrix<T, IndexType>& A) {
    auto diag = sparse_diagonal(A);
    const auto n = A.rows();
    // Zero-diagonal check
    for (IndexType i = 0; i < n; ++i) {
        if (std::abs(diag[i]) < std::numeric_limits<T>::epsilon()) {
            throw std::invalid_argument(
                "Jacobi preconditioner: zero diagonal element at row " + std::to_string(i));
        }
    }
    return [diag = std::move(diag)](const Vector<T>& r) -> Vector<T> {
        const auto m = r.size();
        Vector<T> z(m);
        for (std::size_t i = 0; i < m; ++i) {
            z[i] = r[i] / diag[i];
        }
        return z;
    };
}

/**
 * @brief SSOR (Symmetric Successive Over-Relaxation) preconditioner
 *
 * M = (D/omega + L) D^{-1} (D/omega + U)
 * Compute M^{-1} r via forward and backward substitution.
 * Reduces to symmetric Gauss-Seidel when omega = 1.0.
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Preconditioner<T> ssor_preconditioner(
    const SparseMatrix<T, IndexType>& A,
    T omega = T{1})
{
    // Convert to CSR and store
    auto A_csr = std::make_shared<SparseMatrix<T, IndexType>>(A);
    A_csr->convert_to(SparseStorageFormat::CSR);
    const auto n = A.rows();
    auto diag = sparse_diagonal(A);

    return [A_csr, diag, omega, n](const Vector<T>& r) -> Vector<T> {
        Vector<T> y(n, T{0});
        Vector<T> z(n, T{0});

        // Forward substitution: (D/omega + L) y = r
        for (IndexType i = 0; i < n; ++i) {
            T sum = r[i];
            for (IndexType j = 0; j < i; ++j) {
                T aij = A_csr->coeff(i, j);
                if (aij != T{0}) {
                    sum -= aij * y[j];
                }
            }
            y[i] = omega * sum / diag[i];
        }

        // D/omega scaling: y_i *= diag[i] / omega
        for (IndexType i = 0; i < n; ++i) {
            y[i] *= diag[i] / omega;
        }

        // Backward substitution: (D/omega + U) z = y
        for (IndexType ii = 0; ii < n; ++ii) {
            IndexType i = n - 1 - ii;
            T sum = y[i];
            for (IndexType j = i + 1; j < n; ++j) {
                T aij = A_csr->coeff(i, j);
                if (aij != T{0}) {
                    sum -= aij * z[j];
                }
            }
            z[i] = omega * sum / diag[i];
        }

        // Correct SSOR scaling: M_SSOR = omega(2-omega) (D/omega+L) D^{-1} (D/omega+U)
        // Multiply the final result by (2-omega)
        for (IndexType i = 0; i < n; ++i) {
            z[i] *= (T{2} - omega);
        }

        return z;
    };
}

/**
 * @brief ILU(0) (Incomplete LU with zero fill) preconditioner
 *
 * Performs LU decomposition only within the non-zero pattern of A and stores
 * L and U in CSR form. M^{-1} r is computed via L y = r (forward substitution)
 * followed by U z = y (backward substitution).
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Preconditioner<T> ilu_preconditioner(const SparseMatrix<T, IndexType>& A) {
    const auto n = A.rows();
    if (A.cols() != n) {
        throw std::invalid_argument("ILU: matrix must be square");
    }

    // Build the work array in CSR form
    // Run IKJ-variant ILU(0) using a dense vector per row
    // lu_vals[i][j] = the LU value at (i, j) (only within A's pattern)

    // First, collect each row's non-zero column indices and values
    struct RowEntry {
        IndexType col;
        T val;
    };
    std::vector<std::vector<RowEntry>> rows(n);
    for (IndexType i = 0; i < n; ++i) {
        for (IndexType j = 0; j < n; ++j) {
            T v = A.coeff(i, j);
            if (v != T{0}) {
                rows[i].push_back({j, v});
            }
        }
        // Sort by column order
        std::sort(rows[i].begin(), rows[i].end(),
                  [](const RowEntry& a, const RowEntry& b) { return a.col < b.col; });
    }

    // IKJ-variant ILU(0): update each entry of row i.
    // w = copy of A(i,:) -> eliminate using pivot rows k < i -> copy into L, U.
    std::vector<std::vector<RowEntry>> L_rows(n), U_rows(n);

    for (IndexType i = 0; i < n; ++i) {
        // Dense work array for row i (only at non-zero positions)
        std::vector<T> w(n, T{0});
        std::vector<bool> nz(n, false);  // Non-zero pattern
        for (auto& e : rows[i]) {
            w[e.col] = e.val;
            nz[e.col] = true;
        }

        // k loop: for k < i, eliminate columns where nz[k] is true
        for (IndexType k = 0; k < i; ++k) {
            if (!nz[k]) continue;

            // Divide by U(k,k)
            T ukk = T{0};
            for (auto& e : U_rows[k]) {
                if (e.col == k) { ukk = e.val; break; }
            }
            if (std::abs(ukk) < std::numeric_limits<T>::epsilon()) {
                continue;  // Skip zero pivot
            }
            w[k] /= ukk;

            // Update entries where j > k, U(k,j) != 0, and nz[j]
            for (auto& e : U_rows[k]) {
                if (e.col > k && nz[e.col]) {
                    w[e.col] -= w[k] * e.val;
                }
            }
        }

        // Store into L, U
        for (auto& e : rows[i]) {
            IndexType j = e.col;
            if (j < i) {
                L_rows[i].push_back({j, w[j]});
            } else {
                U_rows[i].push_back({j, w[j]});
            }
        }
        // L's diagonal is 1
        L_rows[i].push_back({i, T{1}});
        std::sort(L_rows[i].begin(), L_rows[i].end(),
                  [](const RowEntry& a, const RowEntry& b) { return a.col < b.col; });
    }

    // Hold L and U row data via shared_ptr
    auto l_data = std::make_shared<std::vector<std::vector<RowEntry>>>(std::move(L_rows));
    auto u_data = std::make_shared<std::vector<std::vector<RowEntry>>>(std::move(U_rows));

    return [l_data, u_data, n](const Vector<T>& r) -> Vector<T> {
        // Forward substitution: L y = r
        Vector<T> y(n, T{0});
        for (IndexType i = 0; i < n; ++i) {
            T sum = r[i];
            for (auto& e : (*l_data)[i]) {
                if (e.col < i) {
                    sum -= e.val * y[e.col];
                }
            }
            // L(i,i) = 1, so no division needed
            y[i] = sum;
        }

        // Backward substitution: U z = y
        Vector<T> z(n, T{0});
        for (IndexType ii = 0; ii < n; ++ii) {
            IndexType i = n - 1 - ii;
            T sum = y[i];
            T uii = T{0};
            for (auto& e : (*u_data)[i]) {
                if (e.col == i) {
                    uii = e.val;
                } else if (e.col > i) {
                    sum -= e.val * z[e.col];
                }
            }
            if (std::abs(uii) < std::numeric_limits<T>::epsilon()) {
                z[i] = T{0};
            } else {
                z[i] = sum / uii;
            }
        }

        return z;
    };
}

// ============================================================================
// Backward compatibility: incomplete_lu_decomposition (returns an (L, U) pair)
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
std::pair<SparseMatrix<T, IndexType>, SparseMatrix<T, IndexType>>
incomplete_lu_decomposition(
    const SparseMatrix<T, IndexType>& A,
    [[maybe_unused]] int fill_level = 0)
{
    const auto n = A.rows();
    if (A.cols() != n) {
        throw std::invalid_argument("ILU: matrix must be square");
    }

    struct RowEntry { IndexType col; T val; };
    std::vector<std::vector<RowEntry>> rows(n);
    for (IndexType i = 0; i < n; ++i) {
        for (IndexType j = 0; j < n; ++j) {
            T v = A.coeff(i, j);
            if (v != T{0}) {
                rows[i].push_back({j, v});
            }
        }
        std::sort(rows[i].begin(), rows[i].end(),
                  [](const RowEntry& a, const RowEntry& b) { return a.col < b.col; });
    }

    std::vector<std::vector<RowEntry>> L_rows(n), U_rows(n);

    for (IndexType i = 0; i < n; ++i) {
        std::vector<T> w(n, T{0});
        std::vector<bool> nz(n, false);
        for (auto& e : rows[i]) {
            w[e.col] = e.val;
            nz[e.col] = true;
        }
        for (IndexType k = 0; k < i; ++k) {
            if (!nz[k]) continue;
            T ukk = T{0};
            for (auto& e : U_rows[k]) {
                if (e.col == k) { ukk = e.val; break; }
            }
            if (std::abs(ukk) < std::numeric_limits<T>::epsilon()) continue;
            w[k] /= ukk;
            for (auto& e : U_rows[k]) {
                if (e.col > k && nz[e.col]) {
                    w[e.col] -= w[k] * e.val;
                }
            }
        }
        for (auto& e : rows[i]) {
            if (e.col < i) L_rows[i].push_back({e.col, w[e.col]});
            else           U_rows[i].push_back({e.col, w[e.col]});
        }
        L_rows[i].push_back({i, T{1}});
        std::sort(L_rows[i].begin(), L_rows[i].end(),
                  [](const RowEntry& a, const RowEntry& b) { return a.col < b.col; });
    }

    SparseMatrix<T, IndexType> L(n, n, SparseStorageFormat::COO);
    SparseMatrix<T, IndexType> U(n, n, SparseStorageFormat::COO);
    for (IndexType i = 0; i < n; ++i) {
        for (auto& e : L_rows[i]) L.set_coeff(i, e.col, e.val);
        for (auto& e : U_rows[i]) U.set_coeff(i, e.col, e.val);
    }
    return {L, U};
}

// ============================================================================
// IC(0) (Incomplete Cholesky with zero fill) preconditioner
// ============================================================================

/**
 * @brief IC(0) (Incomplete Cholesky with zero fill) preconditioner
 *
 * Performs Cholesky decomposition only within the lower-triangular non-zero
 * pattern of a symmetric positive-definite matrix A.
 * A ~= L * L^T (no fill-in).
 * M^{-1} r is computed via L y = r (forward substitution) followed by
 * L^T z = y (backward substitution).
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Preconditioner<T> incomplete_cholesky_preconditioner(const SparseMatrix<T, IndexType>& A) {
    const auto n = A.rows();
    if (A.cols() != n) {
        throw std::invalid_argument("IC: matrix must be square");
    }

    // Gather the lower-triangular non-zero pattern row by row (including the diagonal)
    struct Entry {
        IndexType col;
        T val;
    };
    std::vector<std::vector<Entry>> L_rows(n);

    for (IndexType i = 0; i < n; ++i) {
        for (IndexType j = 0; j <= i; ++j) {
            T v = A.coeff(i, j);
            if (v != T{0}) {
                L_rows[i].push_back({j, v});
            }
        }
        // Already in column order (j loop is ascending), but sort to be safe
        std::sort(L_rows[i].begin(), L_rows[i].end(),
                  [](const Entry& a, const Entry& b) { return a.col < b.col; });
        // Ensure the diagonal element exists in the pattern
        if (L_rows[i].empty() || L_rows[i].back().col != i) {
            throw std::runtime_error("IC: missing diagonal element (matrix may not be SPD)");
        }
    }

    // Build column->row lists for fast lookup of non-zero rows per column.
    // col_rows[j] = list of rows i where L_rows[i] has an entry with col == j.
    std::vector<std::vector<IndexType>> col_rows(n);
    for (IndexType i = 0; i < n; ++i) {
        for (auto& e : L_rows[i]) {
            if (e.col < i) {  // Strict lower triangle (off-diagonal)
                col_rows[e.col].push_back(i);
            }
        }
    }

    // IC(0) decomposition: processed row by row.
    // L(i,j) = (A(i,j) - Sum_{k<j} L(i,k)*L(j,k)) / L(j,j)  (j < i)
    // L(i,i) = sqrt(A(i,i) - Sum_{k<i} L(i,k)^2)
    //
    // Build per-row column->index maps for speed.
    std::vector<std::vector<IndexType>> row_col_idx(n);  // row_col_idx[i][p] = L_rows[i][p].col
    for (IndexType i = 0; i < n; ++i) {
        row_col_idx[i].reserve(L_rows[i].size());
        for (auto& e : L_rows[i]) {
            row_col_idx[i].push_back(e.col);
        }
    }

    // Process row i
    for (IndexType i = 0; i < n; ++i) {
        for (std::size_t p = 0; p < L_rows[i].size(); ++p) {
            IndexType j = L_rows[i][p].col;
            T sum = T{0};

            if (j < i) {
                // Off-diagonal: L(i,j) = (A(i,j) - Sum_{k<j} L(i,k)*L(j,k)) / L(j,j)
                // k iterates over common columns of rows i and j (k < j).
                std::size_t pi = 0, pj = 0;
                while (pi < L_rows[i].size() && pj < L_rows[j].size()) {
                    IndexType ci = L_rows[i][pi].col;
                    IndexType cj = L_rows[j][pj].col;
                    if (ci < j && cj < j) {
                        if (ci == cj) {
                            sum += L_rows[i][pi].val * L_rows[j][pj].val;
                            ++pi; ++pj;
                        } else if (ci < cj) {
                            ++pi;
                        } else {
                            ++pj;
                        }
                    } else {
                        break;
                    }
                }
                // Get L(j,j) (the last entry of row j is the diagonal)
                T ljj = L_rows[j].back().val;
                if (std::abs(ljj) < std::numeric_limits<T>::epsilon()) {
                    throw std::runtime_error("IC: zero diagonal encountered (matrix may not be SPD)");
                }
                L_rows[i][p].val = (L_rows[i][p].val - sum) / ljj;
            } else {
                // Diagonal: L(i,i) = sqrt(A(i,i) - Sum_{k<i} L(i,k)^2)
                for (std::size_t q = 0; q < p; ++q) {
                    sum += L_rows[i][q].val * L_rows[i][q].val;
                }
                T diag = L_rows[i][p].val - sum;
                if (diag <= T{0}) {
                    throw std::runtime_error("IC: non-positive diagonal encountered (matrix may not be SPD)");
                }
                L_rows[i][p].val = std::sqrt(diag);
            }
        }
    }

    // Hold L row data via shared_ptr
    auto l_data = std::make_shared<std::vector<std::vector<Entry>>>(std::move(L_rows));
    auto dim = n;

    return [l_data, dim](const Vector<T>& r) -> Vector<T> {
        auto n = dim;
        // Forward substitution: L y = r
        Vector<T> y(n, T{0});
        for (IndexType i = 0; i < n; ++i) {
            T sum = r[i];
            for (auto& e : (*l_data)[i]) {
                if (e.col < i) {
                    sum -= e.val * y[e.col];
                }
            }
            // L(i,i) is the last element of the row
            y[i] = sum / (*l_data)[i].back().val;
        }

        // Backward substitution: L^T z = y
        // Row-based transform: start with z = y; for row i (descending) do
        // z[i] /= L(i,i), then for L(i,k) (k < i) do z[k] -= L(i,k) * z[i].
        Vector<T> z = y;
        for (IndexType ii = 0; ii < n; ++ii) {
            IndexType i = n - 1 - ii;
            z[i] /= (*l_data)[i].back().val;
            for (auto& e : (*l_data)[i]) {
                if (e.col < i) {
                    z[e.col] -= e.val * z[i];
                }
            }
        }

        return z;
    };
}

// ============================================================================
// AMG (aggregation-based algebraic multigrid) preconditioner
// ============================================================================

namespace detail_amg {

/// One level of the AMG hierarchy.
template<typename T, typename IndexType>
struct AMGLevel {
    SparseMatrix<T, IndexType> A;       ///< Operator on this level (CSR)
    std::vector<T> dinv;                ///< 1/diag
    // CSR row arrays for the Gauss-Seidel smoother (forward pre / backward post
    // sweeps form a symmetric, SPD smoother → SPD V-cycle, required by CG).
    std::vector<IndexType> rptr, cidx;
    std::vector<T> rval;
    SparseMatrix<T, IndexType> P;       ///< Prolongator n×nc (smoothed aggregation)
    SparseMatrix<T, IndexType> Pt;      ///< Restriction Pᵀ (nc×n)
    bool coarsest = false;
    bool dense_solve = false;           ///< coarsest solved by a dense LU
    std::size_t dn = 0;
    std::vector<T> lu;                  ///< dn×dn row-major LU factors
    std::vector<int> piv;
};

/// Greedy strength-based aggregation. Fills agg[i] with a coarse-node id and
/// returns the coarse node count nc. A node is strongly connected to j when
/// |A(i,j)| ≥ theta · max_{k≠i} |A(i,k)|.
template<typename T, typename IndexType>
std::size_t aggregate(const SparseMatrix<T, IndexType>& A,
                      std::vector<IndexType>& agg, T theta = T(0.08)) {
    const std::size_t n = static_cast<std::size_t>(A.rows());
    const auto& vals = A.csc_values();
    const auto& rows = A.csc_row_indices();
    const auto& cptr = A.csc_col_ptr();

    std::vector<T> rowmax(n, T(0));
    for (std::size_t j = 0; j < n; ++j)
        for (std::size_t k = cptr[j]; k < cptr[j + 1]; ++k) {
            std::size_t i = static_cast<std::size_t>(rows[k]);
            if (i != j) rowmax[i] = std::max(rowmax[i], std::abs(vals[k]));
        }

    std::vector<std::vector<std::size_t>> nbr(n);
    for (std::size_t j = 0; j < n; ++j)
        for (std::size_t k = cptr[j]; k < cptr[j + 1]; ++k) {
            std::size_t i = static_cast<std::size_t>(rows[k]);
            if (i != j && rowmax[i] > T(0) && std::abs(vals[k]) >= theta * rowmax[i])
                nbr[i].push_back(j);
        }

    const IndexType UNSET = static_cast<IndexType>(-1);
    agg.assign(n, UNSET);
    std::size_t nc = 0;
    for (std::size_t i = 0; i < n; ++i) {
        if (agg[i] != UNSET) continue;
        agg[i] = static_cast<IndexType>(nc);
        for (std::size_t j : nbr[i])
            if (agg[j] == UNSET) agg[j] = static_cast<IndexType>(nc);
        ++nc;
    }
    return nc;
}

/// Smoothed-aggregation prolongator P (n×nc): P = (I − ω D⁻¹ A) P_tent, where the
/// tentative prolongator P_tent(i, agg[i]) = 1 is the piecewise-constant aggregate
/// indicator. The Jacobi smoothing of P makes the resulting V-cycle a convergent
/// SPD operator (suitable for CG), unlike unsmoothed aggregation.
template<typename T, typename IndexType>
SparseMatrix<T, IndexType> build_prolongator(
    const SparseMatrix<T, IndexType>& A,
    const std::vector<IndexType>& agg, std::size_t nc,
    const std::vector<T>& dinv, T omega_P)
{
    const std::size_t n = static_cast<std::size_t>(A.rows());

    SparseMatrix<T, IndexType> Ptent(static_cast<IndexType>(n), static_cast<IndexType>(nc),
                                     SparseStorageFormat::COO);
    for (std::size_t i = 0; i < n; ++i)
        Ptent.set_coeff(static_cast<IndexType>(i), agg[i], T(1));
    Ptent.convert_to(SparseStorageFormat::CSR);

    SparseMatrix<T, IndexType> S = A * Ptent;             // n×nc

    std::map<std::pair<IndexType, IndexType>, T> acc;
    for (std::size_t i = 0; i < n; ++i)
        acc[{static_cast<IndexType>(i), agg[i]}] += T(1);   // P_tent part
    {
        const auto& vals = S.csc_values();
        const auto& rows = S.csc_row_indices();
        const auto& cptr = S.csc_col_ptr();
        const std::size_t ncc = static_cast<std::size_t>(S.cols());
        for (std::size_t J = 0; J < ncc; ++J)
            for (std::size_t k = cptr[J]; k < cptr[J + 1]; ++k) {
                std::size_t i = static_cast<std::size_t>(rows[k]);
                acc[{static_cast<IndexType>(i), static_cast<IndexType>(J)}]
                    += -omega_P * dinv[i] * vals[k];
            }
    }

    SparseMatrix<T, IndexType> P(static_cast<IndexType>(n), static_cast<IndexType>(nc),
                                 SparseStorageFormat::COO);
    for (const auto& [ij, v] : acc)
        if (v != T(0)) P.set_coeff(ij.first, ij.second, v);
    P.convert_to(SparseStorageFormat::CSR);
    return P;
}

/// Small dense LU with partial pivoting (coarsest-level direct solve).
template<typename T>
void dense_lu_factor(std::vector<T>& lu, std::vector<int>& piv, std::size_t n) {
    piv.resize(n);
    for (std::size_t i = 0; i < n; ++i) piv[i] = static_cast<int>(i);
    const T tiny = std::numeric_limits<T>::epsilon();
    for (std::size_t k = 0; k < n; ++k) {
        std::size_t p = k;
        for (std::size_t i = k + 1; i < n; ++i)
            if (std::abs(lu[i * n + k]) > std::abs(lu[p * n + k])) p = i;
        if (p != k) {
            for (std::size_t j = 0; j < n; ++j) std::swap(lu[k * n + j], lu[p * n + j]);
            std::swap(piv[k], piv[p]);
        }
        T d = lu[k * n + k];
        if (std::abs(d) < tiny) d = (d >= T(0) ? T(1) : T(-1)) * tiny;
        for (std::size_t i = k + 1; i < n; ++i) {
            T f = lu[i * n + k] / d;
            lu[i * n + k] = f;
            for (std::size_t j = k + 1; j < n; ++j) lu[i * n + j] -= f * lu[k * n + j];
        }
    }
}

template<typename T>
Vector<T> dense_lu_solve(const std::vector<T>& lu, const std::vector<int>& piv,
                         std::size_t n, const Vector<T>& b) {
    Vector<T> y(n, T(0));
    for (std::size_t i = 0; i < n; ++i) y[i] = b[static_cast<std::size_t>(piv[i])];
    for (std::size_t i = 0; i < n; ++i)
        for (std::size_t j = 0; j < i; ++j) y[i] -= lu[i * n + j] * y[j];
    const T tiny = std::numeric_limits<T>::epsilon();
    for (std::size_t ii = n; ii-- > 0;) {
        for (std::size_t j = ii + 1; j < n; ++j) y[ii] -= lu[ii * n + j] * y[j];
        T d = lu[ii * n + ii];
        if (std::abs(d) < tiny) d = (d >= T(0) ? T(1) : T(-1)) * tiny;
        y[ii] /= d;
    }
    return y;
}

template<typename T, typename IndexType>
class AMGHierarchy {
public:
    AMGHierarchy(const SparseMatrix<T, IndexType>& A,
                 std::size_t coarse_max = 50, std::size_t max_levels = 25,
                 int nu = 2)
        : nu_(nu)
    {
        levels_.reserve(max_levels);
        SparseMatrix<T, IndexType> cur(A);
        cur.convert_to(SparseStorageFormat::CSR);
        while (true) {
            AMGLevel<T, IndexType> lv;
            const std::size_t n = static_cast<std::size_t>(cur.rows());

            // diagonal inverse + Gershgorin estimate of ρ(D⁻¹A) for the SA damping
            lv.dinv.assign(n, T(0));
            std::vector<T> rowsum(n, T(0));
            {
                const auto& vals = cur.csc_values();
                const auto& rows = cur.csc_row_indices();
                const auto& cptr = cur.csc_col_ptr();
                for (std::size_t j = 0; j < n; ++j)
                    for (std::size_t k = cptr[j]; k < cptr[j + 1]; ++k) {
                        std::size_t i = static_cast<std::size_t>(rows[k]);
                        rowsum[i] += std::abs(vals[k]);
                        if (i == j) {
                            lv.dinv[i] = (std::abs(vals[k]) > std::numeric_limits<T>::epsilon())
                                             ? T(1) / vals[k] : T(0);
                        }
                    }
            }
            T rho = T(0);
            for (std::size_t i = 0; i < n; ++i)
                rho = std::max(rho, rowsum[i] * std::abs(lv.dinv[i]));
            if (rho <= T(0)) rho = T(1);

            bool stop = (n <= coarse_max) || (levels_.size() + 1 >= max_levels);
            std::size_t nc = 0;
            std::vector<IndexType> agg;
            if (!stop) {
                nc = aggregate(cur, agg);
                if (nc == 0 || nc >= n) stop = true;   // coarsening stalled
            }

            if (stop) {
                lv.coarsest = true;
                if (n <= 400) {
                    lv.dense_solve = true;
                    lv.dn = n;
                    lv.lu.assign(n * n, T(0));
                    const auto& vals = cur.csc_values();
                    const auto& rows = cur.csc_row_indices();
                    const auto& cptr = cur.csc_col_ptr();
                    for (std::size_t j = 0; j < n; ++j)
                        for (std::size_t k = cptr[j]; k < cptr[j + 1]; ++k)
                            lv.lu[static_cast<std::size_t>(rows[k]) * n + j] = vals[k];
                    dense_lu_factor(lv.lu, lv.piv, n);
                }
                lv.A = std::move(cur);
                levels_.push_back(std::move(lv));
                break;
            }

            // CSR row arrays for Gauss-Seidel smoothing on this level.
            {
                const auto& vals = cur.csc_values();
                const auto& rows = cur.csc_row_indices();
                const auto& cptr = cur.csc_col_ptr();
                const std::size_t nz = vals.size();
                lv.rptr.assign(n + 1, 0);
                for (std::size_t k = 0; k < nz; ++k) lv.rptr[static_cast<std::size_t>(rows[k]) + 1]++;
                for (std::size_t i = 0; i < n; ++i) lv.rptr[i + 1] += lv.rptr[i];
                lv.cidx.resize(nz);
                lv.rval.resize(nz);
                std::vector<IndexType> pos(lv.rptr.begin(), lv.rptr.end() - 1);
                for (std::size_t j = 0; j < n; ++j)
                    for (std::size_t k = cptr[j]; k < cptr[j + 1]; ++k) {
                        std::size_t i = static_cast<std::size_t>(rows[k]);
                        IndexType p = pos[i]++;
                        lv.cidx[p] = static_cast<IndexType>(j);
                        lv.rval[p] = vals[k];
                    }
            }

            const T omega_P = (T(4) / T(3)) / rho;
            SparseMatrix<T, IndexType> P = build_prolongator(cur, agg, nc, lv.dinv, omega_P);
            SparseMatrix<T, IndexType> Pt = P.transpose();
            SparseMatrix<T, IndexType> AP = cur * P;             // Galerkin: Ac = Pᵀ A P
            SparseMatrix<T, IndexType> Ac = Pt * AP;
            Ac.convert_to(SparseStorageFormat::CSR);
            lv.P = std::move(P);
            lv.Pt = std::move(Pt);
            lv.A = std::move(cur);
            levels_.push_back(std::move(lv));
            cur = std::move(Ac);
        }
    }

    Vector<T> apply(const Vector<T>& b) const { return vcycle(0, b); }
    std::size_t num_levels() const { return levels_.size(); }

private:
    std::vector<AMGLevel<T, IndexType>> levels_;
    int nu_;

    // One Gauss-Seidel sweep. forward=true sweeps rows ascending, false descending.
    // x_i ← (b_i − Σ_{j≠i} A_ij x_j) / A_ii, using the latest x.
    void gs_sweep(const AMGLevel<T, IndexType>& lv, Vector<T>& x,
                  const Vector<T>& b, bool forward) const {
        const std::size_t n = lv.dinv.size();
        for (std::size_t t = 0; t < n; ++t) {
            std::size_t i = forward ? t : (n - 1 - t);
            T sum = b[i];
            for (IndexType k = lv.rptr[i]; k < lv.rptr[i + 1]; ++k) {
                std::size_t j = static_cast<std::size_t>(lv.cidx[k]);
                if (j != i) sum -= lv.rval[k] * x[j];
            }
            x[i] = sum * lv.dinv[i];
        }
    }

    Vector<T> vcycle(std::size_t l, const Vector<T>& b) const {
        const AMGLevel<T, IndexType>& lv = levels_[l];
        const std::size_t n = lv.dinv.size();

        if (lv.coarsest) {
            if (lv.dense_solve) return dense_lu_solve(lv.lu, lv.piv, lv.dn, b);
            // Fallback (large, un-coarsenable): symmetric Gauss-Seidel iterations.
            Vector<T> x(n, T(0));
            for (int it = 0; it < 50; ++it) { gs_sweep(lv, x, b, true); gs_sweep(lv, x, b, false); }
            return x;
        }

        Vector<T> x(n, T(0));
        for (int s = 0; s < nu_; ++s) gs_sweep(lv, x, b, true);    // pre-smooth (forward)

        Vector<T> Ax = lv.A * x;
        Vector<T> rr(n);
        for (std::size_t i = 0; i < n; ++i) rr[i] = b[i] - Ax[i];

        Vector<T> rc = lv.Pt * rr;              // restrict (Pᵀ r)
        Vector<T> ec = vcycle(l + 1, rc);       // coarse-grid correction
        Vector<T> corr = lv.P * ec;             // prolong (P e)
        x += corr;                              // correct

        for (int s = 0; s < nu_; ++s) gs_sweep(lv, x, b, false);   // post-smooth (backward)
        return x;
    }
};

} // namespace detail_amg

/**
 * @brief AMG (aggregation-based algebraic multigrid) preconditioner
 *
 * Builds a multilevel hierarchy by greedy strength-based aggregation with a
 * piecewise-constant prolongator and a Galerkin coarse operator (Ac = Pᵀ A P).
 * One application performs a single symmetric V-cycle (damped-Jacobi pre/post
 * smoothing, dense LU on the coarsest level), which yields an SPD operator
 * suitable as a CG/BiCGStab preconditioner. Most effective for SPD sparse
 * systems amenable to coarsening (e.g. discretized elliptic PDEs). The
 * hierarchy is constructed once and shared by the returned callable.
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Preconditioner<T> amg_preconditioner(const SparseMatrix<T, IndexType>& A) {
    if (A.rows() != A.cols())
        throw std::invalid_argument("AMG: matrix must be square");
    auto hier = std::make_shared<detail_amg::AMGHierarchy<T, IndexType>>(A);
    return [hier](const Vector<T>& r) -> Vector<T> { return hier->apply(r); };
}

// ============================================================================
// Backward compatibility: incomplete_cholesky_decomposition (returns L)
// ============================================================================

/**
 * @brief Return the L matrix of the IC(0) decomposition
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SparseMatrix<T, IndexType>
incomplete_cholesky_decomposition(
    const SparseMatrix<T, IndexType>& A,
    [[maybe_unused]] int fill_level = 0)
{
    const auto n = A.rows();
    if (A.cols() != n) {
        throw std::invalid_argument("IC: matrix must be square");
    }

    // Gather the lower-triangular non-zero pattern
    struct Entry { IndexType col; T val; };
    std::vector<std::vector<Entry>> L_rows(n);

    for (IndexType i = 0; i < n; ++i) {
        for (IndexType j = 0; j <= i; ++j) {
            T v = A.coeff(i, j);
            if (v != T{0}) {
                L_rows[i].push_back({j, v});
            }
        }
        std::sort(L_rows[i].begin(), L_rows[i].end(),
                  [](const Entry& a, const Entry& b) { return a.col < b.col; });
        if (L_rows[i].empty() || L_rows[i].back().col != i) {
            throw std::runtime_error("IC: missing diagonal element");
        }
    }

    // IC(0) decomposition
    for (IndexType i = 0; i < n; ++i) {
        for (std::size_t p = 0; p < L_rows[i].size(); ++p) {
            IndexType j = L_rows[i][p].col;
            T sum = T{0};

            if (j < i) {
                std::size_t pi = 0, pj = 0;
                while (pi < L_rows[i].size() && pj < L_rows[j].size()) {
                    IndexType ci = L_rows[i][pi].col;
                    IndexType cj = L_rows[j][pj].col;
                    if (ci < j && cj < j) {
                        if (ci == cj) {
                            sum += L_rows[i][pi].val * L_rows[j][pj].val;
                            ++pi; ++pj;
                        } else if (ci < cj) { ++pi; }
                        else { ++pj; }
                    } else { break; }
                }
                T ljj = L_rows[j].back().val;
                if (std::abs(ljj) < std::numeric_limits<T>::epsilon()) {
                    throw std::runtime_error("IC: zero diagonal encountered");
                }
                L_rows[i][p].val = (L_rows[i][p].val - sum) / ljj;
            } else {
                for (std::size_t q = 0; q < p; ++q) {
                    sum += L_rows[i][q].val * L_rows[i][q].val;
                }
                T diag = L_rows[i][p].val - sum;
                if (diag <= T{0}) {
                    throw std::runtime_error("IC: non-positive diagonal");
                }
                L_rows[i][p].val = std::sqrt(diag);
            }
        }
    }

    SparseMatrix<T, IndexType> L(n, n, SparseStorageFormat::COO);
    for (IndexType i = 0; i < n; ++i) {
        for (auto& e : L_rows[i]) {
            L.set_coeff(i, e.col, e.val);
        }
    }
    return L;
}

// ============================================================================
// Forward / backward substitution for triangular matrices (sparse version)
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
Vector<T> triangular_solve(
    const SparseMatrix<T, IndexType>& M,
    const Vector<T>& b,
    bool lower = true)
{
    const auto n = M.rows();
    if (M.cols() != n) throw std::invalid_argument("Matrix must be square");
    if (b.size() != n) throw std::invalid_argument("Incompatible vector dimensions");

    Vector<T> x(n, T{0});

    if (lower) {
        for (IndexType i = 0; i < n; ++i) {
            T sum = b[i];
            for (IndexType j = 0; j < i; ++j) {
                T v = M.coeff(i, j);
                if (v != T{0}) sum -= v * x[j];
            }
            x[i] = sum / M.coeff(i, i);
        }
    } else {
        for (IndexType ii = 0; ii < n; ++ii) {
            IndexType i = n - 1 - ii;
            T sum = b[i];
            for (IndexType j = i + 1; j < n; ++j) {
                T v = M.coeff(i, j);
                if (v != T{0}) sum -= v * x[j];
            }
            x[i] = sum / M.coeff(i, i);
        }
    }

    return x;
}

// ============================================================================
// CG (Conjugate Gradient) — for symmetric positive-definite matrices
// ============================================================================

/**
 * @brief Preconditioned Conjugate Gradient method
 *
 * Solve Ax = b for a symmetric positive-definite sparse matrix A.
 * If a preconditioner M is supplied, solve M^{-1} A x = M^{-1} b via CG.
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SolverResult<T> conjugate_gradient(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& b,
    const ConvergenceCriteria<T>& criteria,
    Preconditioner<T> precond = {},
    std::optional<Vector<T>> x0 = std::nullopt)
{
    const auto n = A.rows();
    if (A.cols() != n) throw std::invalid_argument("CG: matrix must be square");
    if (b.size() != n) throw std::invalid_argument("CG: incompatible dimensions");

    if (!precond) precond = identity_preconditioner<T>();

    Vector<T> x = x0.value_or(Vector<T>(n, T{0}));
    Vector<T> r = b - A * x;

    const T init_rnorm = norm(r);
    if (init_rnorm <= criteria.absolute_tolerance()) {
        return {x, init_rnorm, 0, true};
    }

    Vector<T> z = precond(r);
    Vector<T> p = z;
    T rz = dot(r, z);

    // Common convergence-check helper for all exits
    auto is_converged = [&](T rnorm) {
        return rnorm <= criteria.absolute_tolerance() ||
               rnorm <= criteria.relative_tolerance() * init_rnorm;
    };

    for (std::size_t iter = 0; iter < criteria.max_iterations(); ++iter) {
        Vector<T> Ap = A * p;
        T pAp = dot(p, Ap);
        if (std::abs(pAp) < std::numeric_limits<T>::epsilon()) {
            // The internal residual r has accumulated error, so judge with the true residual
            T rnorm = norm(b - A * x);
            return {x, rnorm, iter, is_converged(rnorm)};
        }
        T alpha = rz / pAp;

        x += p * alpha;
        r -= Ap * alpha;

        T rnorm = norm(r);
        if (is_converged(rnorm)) {
            return {x, rnorm, iter + 1, true};
        }

        z = precond(r);
        T rz_new = dot(r, z);
        T beta = rz_new / rz;
        rz = rz_new;

        p = z + p * beta;
    }

    // Also re-check convergence at max iterations (the internal residual may misjudge due to accumulated error)
    T final_rnorm = norm(b - A * x);
    return {x, final_rnorm, criteria.max_iterations(), is_converged(final_rnorm)};
}

// ============================================================================
// BiCG (BiConjugate Gradient) — for non-symmetric matrices
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SolverResult<T> biconjugate_gradient(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& b,
    const ConvergenceCriteria<T>& criteria,
    std::optional<Vector<T>> x0 = std::nullopt)
{
    const auto n = A.rows();
    if (A.cols() != n) throw std::invalid_argument("BiCG: matrix must be square");
    if (b.size() != n) throw std::invalid_argument("BiCG: incompatible dimensions");

    const auto At = A.transpose();

    Vector<T> x = x0.value_or(Vector<T>(n, T{0}));
    Vector<T> r = b - A * x;
    Vector<T> r_tilde = r;

    const T init_rnorm = norm(r);
    if (init_rnorm <= criteria.absolute_tolerance()) {
        return {x, init_rnorm, 0, true};
    }

    Vector<T> p = r;
    Vector<T> p_tilde = r_tilde;
    T rho = dot(r_tilde, r);

    // Breakdown threshold: relative to the initial residual scale
    const T breakdown_tol = std::numeric_limits<T>::epsilon() * init_rnorm * init_rnorm;

    // Convergence-check helper (uses the true residual)
    auto check_convergence = [&](std::size_t iter) -> SolverResult<T> {
        T rnorm = norm(b - A * x);
        bool conv = (rnorm <= criteria.absolute_tolerance() ||
                     rnorm <= criteria.relative_tolerance() * init_rnorm);
        return {x, rnorm, iter, conv};
    };

    for (std::size_t iter = 0; iter < criteria.max_iterations(); ++iter) {
        if (std::abs(rho) < breakdown_tol) {
            return check_convergence(iter);
        }

        Vector<T> Ap = A * p;
        Vector<T> Atp = At * p_tilde;
        T denom = dot(p_tilde, Ap);
        if (std::abs(denom) < breakdown_tol) {
            return check_convergence(iter);
        }
        T alpha = rho / denom;

        x += p * alpha;
        r -= Ap * alpha;
        r_tilde -= Atp * alpha;

        T rnorm = norm(r);
        if (rnorm <= criteria.absolute_tolerance() ||
            rnorm <= criteria.relative_tolerance() * init_rnorm) {
            return {x, rnorm, iter + 1, true};
        }

        T rho_new = dot(r_tilde, r);
        T beta = rho_new / rho;
        rho = rho_new;

        p = r + p * beta;
        p_tilde = r_tilde + p_tilde * beta;
    }

    // At max iterations: re-check using the true residual
    T final_rnorm = norm(b - A * x);
    bool conv = (final_rnorm <= criteria.absolute_tolerance() ||
                 final_rnorm <= criteria.relative_tolerance() * init_rnorm);
    return {x, final_rnorm, criteria.max_iterations(), conv};
}

// ============================================================================
// BiCGSTAB (Stabilized BiConjugate Gradient) — for non-symmetric matrices
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SolverResult<T> bicgstab(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& b,
    const ConvergenceCriteria<T>& criteria,
    Preconditioner<T> precond = {},
    std::optional<Vector<T>> x0 = std::nullopt)
{
    const auto n = A.rows();
    if (A.cols() != n) throw std::invalid_argument("BiCGSTAB: matrix must be square");
    if (b.size() != n) throw std::invalid_argument("BiCGSTAB: incompatible dimensions");

    if (!precond) precond = identity_preconditioner<T>();

    Vector<T> x = x0.value_or(Vector<T>(n, T{0}));
    Vector<T> r = b - A * x;
    Vector<T> r_hat = r;

    const T init_rnorm = norm(r);
    if (init_rnorm <= criteria.absolute_tolerance()) {
        return {x, init_rnorm, 0, true};
    }

    T rho = T{1}, alpha = T{1}, omega_val = T{1};
    Vector<T> p(n, T{0});
    Vector<T> v(n, T{0});

    // Breakdown threshold: relative to the initial residual scale
    const T eps = std::numeric_limits<T>::epsilon();
    const T breakdown_tol = eps * init_rnorm * init_rnorm;
    const T stag_tol      = eps * init_rnorm;

    // Convergence-check helper using the true residual (guards against accumulated error in the internal r)
    auto check_true_convergence = [&](std::size_t iter) -> SolverResult<T> {
        T rnorm = norm(b - A * x);
        bool conv = (rnorm <= criteria.absolute_tolerance() ||
                     rnorm <= criteria.relative_tolerance() * init_rnorm);
        return {x, rnorm, iter, conv};
    };

    for (std::size_t iter = 0; iter < criteria.max_iterations(); ++iter) {
        T rho_new = dot(r_hat, r);
        if (std::abs(rho_new) < breakdown_tol) {
            return check_true_convergence(iter);
        }

        if (iter == 0) {
            p = r;
        } else {
            T beta = (rho_new / rho) * (alpha / omega_val);
            p = r + (p - v * omega_val) * beta;
        }
        rho = rho_new;

        // Preconditioned: p_hat = M^{-1} p
        Vector<T> p_hat = precond(p);
        v = A * p_hat;

        T denom = dot(r_hat, v);
        if (std::abs(denom) < breakdown_tol) {
            return check_true_convergence(iter);
        }
        alpha = rho / denom;

        Vector<T> s = r - v * alpha;
        T s_norm = norm(s);
        if (s_norm <= criteria.absolute_tolerance() ||
            s_norm <= criteria.relative_tolerance() * init_rnorm) {
            x += p_hat * alpha;
            return {x, s_norm, iter + 1, true};
        }

        // Preconditioned: s_hat = M^{-1} s
        Vector<T> s_hat = precond(s);
        Vector<T> t = A * s_hat;

        T tt = dot(t, t);
        if (std::abs(tt) < breakdown_tol) {
            x += p_hat * alpha;
            return check_true_convergence(iter + 1);
        }
        omega_val = dot(t, s) / tt;

        x += p_hat * alpha + s_hat * omega_val;
        r = s - t * omega_val;

        T rnorm = norm(r);
        if (rnorm <= criteria.absolute_tolerance() ||
            rnorm <= criteria.relative_tolerance() * init_rnorm) {
            return {x, rnorm, iter + 1, true};
        }

        if (std::abs(omega_val) < stag_tol) {
            return check_true_convergence(iter + 1);
        }
    }

    // At max iterations: re-check with the true residual (guards against accumulated error in the internal r)
    T final_rnorm = norm(b - A * x);
    bool conv = (final_rnorm <= criteria.absolute_tolerance() ||
                 final_rnorm <= criteria.relative_tolerance() * init_rnorm);
    return {x, final_rnorm, criteria.max_iterations(), conv};
}

// ============================================================================
// GMRES(m) (Generalized Minimum Residual) — for non-symmetric matrices
// ============================================================================

/**
 * @brief Restarted GMRES
 *
 * Apply Arnoldi + Givens rotations successively. Outer iteration is via restarts.
 * @param restart maximum dimension of the Krylov subspace (default 30)
 */
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SolverResult<T> gmres(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& b,
    const ConvergenceCriteria<T>& criteria,
    std::size_t restart = 30,
    Preconditioner<T> precond = {},
    std::optional<Vector<T>> x0 = std::nullopt)
{
    const auto n = static_cast<std::size_t>(A.rows());
    if (static_cast<std::size_t>(A.cols()) != n)
        throw std::invalid_argument("GMRES: matrix must be square");
    if (b.size() != n)
        throw std::invalid_argument("GMRES: incompatible dimensions");

    if (!precond) precond = identity_preconditioner<T>();
    if (restart == 0 || restart > n) restart = n;

    Vector<T> x = x0.value_or(Vector<T>(n, T{0}));

    const T b_norm = norm(b);
    const T threshold = criteria.absolute_tolerance() +
                        criteria.relative_tolerance() * b_norm;

    std::size_t total_iter = 0;

    while (total_iter < criteria.max_iterations()) {
        // Residual
        Vector<T> r = precond(b - A * x);
        T r_norm = norm(r);
        if (r_norm <= threshold) {
            return {x, r_norm, total_iter, true};
        }

        // Arnoldi basis
        const std::size_t m = std::min(restart,
            criteria.max_iterations() - total_iter);
        std::vector<Vector<T>> Q(m + 1);
        Q[0] = r / r_norm;

        // Upper Hessenberg matrix (m+1 x m)
        std::vector<std::vector<T>> H(m + 1, std::vector<T>(m, T{0}));

        // Givens rotation parameters
        std::vector<T> cs(m, T{0}), sn(m, T{0});

        // Transformed right-hand side
        std::vector<T> g(m + 1, T{0});
        g[0] = r_norm;

        std::size_t j = 0;
        for (; j < m; ++j) {
            // Arnoldi step: w = M^{-1} A q_j
            Vector<T> w = precond(A * Q[j]);

            // Modified Gram-Schmidt
            for (std::size_t i = 0; i <= j; ++i) {
                H[i][j] = dot(Q[i], w);
                w -= Q[i] * H[i][j];
            }
            H[j + 1][j] = norm(w);

            // Happy breakdown
            if (std::abs(H[j + 1][j]) < std::numeric_limits<T>::epsilon() * T{100}) {
                // Apply prior Givens rotations to column j of H
                for (std::size_t i = 0; i < j; ++i) {
                    T tmp  =  cs[i] * H[i][j] + sn[i] * H[i + 1][j];
                    H[i + 1][j] = -sn[i] * H[i][j] + cs[i] * H[i + 1][j];
                    H[i][j] = tmp;
                }
                // The j-th Givens rotation
                T rr = std::sqrt(H[j][j] * H[j][j] + H[j + 1][j] * H[j + 1][j]);
                if (rr > T{0}) {
                    cs[j] = H[j][j] / rr;
                    sn[j] = H[j + 1][j] / rr;
                    H[j][j] = rr;
                    H[j + 1][j] = T{0};
                    T g_new = -sn[j] * g[j];
                    g[j] = cs[j] * g[j];
                    g[j + 1] = g_new;
                }
                ++j;
                break;
            }

            Q[j + 1] = w / H[j + 1][j];

            // Apply prior Givens rotations to column j of H
            for (std::size_t i = 0; i < j; ++i) {
                T tmp  =  cs[i] * H[i][j] + sn[i] * H[i + 1][j];
                H[i + 1][j] = -sn[i] * H[i][j] + cs[i] * H[i + 1][j];
                H[i][j] = tmp;
            }

            // Compute the j-th Givens rotation
            T rr = std::sqrt(H[j][j] * H[j][j] + H[j + 1][j] * H[j + 1][j]);
            cs[j] = H[j][j] / rr;
            sn[j] = H[j + 1][j] / rr;
            H[j][j] = rr;
            H[j + 1][j] = T{0};

            // Update the right-hand side
            T g_new = -sn[j] * g[j];
            g[j] = cs[j] * g[j];
            g[j + 1] = g_new;

            r_norm = std::abs(g[j + 1]);
            ++total_iter;

            if (r_norm <= threshold) {
                ++j;
                break;
            }
        }

        // Backward substitution: R y = g (R is the upper-triangular part of H)
        const std::size_t dim = j;
        std::vector<T> y(dim, T{0});
        for (std::size_t ii = 0; ii < dim; ++ii) {
            std::size_t idx = dim - 1 - ii;
            y[idx] = g[idx];
            for (std::size_t k = idx + 1; k < dim; ++k) {
                y[idx] -= H[idx][k] * y[k];
            }
            if (std::abs(H[idx][idx]) > std::numeric_limits<T>::epsilon()) {
                y[idx] /= H[idx][idx];
            }
        }

        // Update the solution: x += Q * y
        for (std::size_t k = 0; k < dim; ++k) {
            x += Q[k] * y[k];
        }

        if (r_norm <= threshold) {
            return {x, r_norm, total_iter, true};
        }
    }

    T final_rnorm = norm(b - A * x);
    return {x, final_rnorm, total_iter, false};
}

// ============================================================================
// Convenience function: BiCGSTAB with ILU preconditioning (backward compatibility)
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
SolverResult<T> ilu_bicgstab(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& b,
    const ConvergenceCriteria<T>& criteria,
    int fill_level = 0,
    std::optional<Vector<T>> x0 = std::nullopt)
{
    (void)fill_level;  // Only ILU(0) is supported
    auto precond = ilu_preconditioner(A);
    return bicgstab(A, b, criteria, precond, std::move(x0));
}

// ============================================================================
// Arnoldi iteration (standalone API)
// ============================================================================

template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
std::pair<std::vector<Vector<T>>, std::vector<std::vector<T>>>
arnoldi_iteration(
    const SparseMatrix<T, IndexType>& A,
    const Vector<T>& v,
    std::size_t m)
{
    const auto n = static_cast<std::size_t>(A.rows());
    if (static_cast<std::size_t>(A.cols()) != n)
        throw std::invalid_argument("Arnoldi: matrix must be square");
    if (v.size() != n)
        throw std::invalid_argument("Arnoldi: incompatible dimensions");

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

    std::vector<Vector<T>> Q(m + 1);
    T v_norm = norm(v);
    if (v_norm < std::numeric_limits<T>::epsilon()) {
        throw std::invalid_argument("Arnoldi: zero initial vector");
    }
    Q[0] = v / v_norm;

    std::vector<std::vector<T>> H(m + 1, std::vector<T>(m, T{0}));

    for (std::size_t j = 0; j < m; ++j) {
        Vector<T> w = A * Q[j];

        for (std::size_t i = 0; i <= j; ++i) {
            H[i][j] = dot(Q[i], w);
            w -= Q[i] * H[i][j];
        }

        H[j + 1][j] = norm(w);

        if (std::abs(H[j + 1][j]) < std::numeric_limits<T>::epsilon()) {
            Q.resize(j + 2);
            H.resize(j + 2);
            for (auto& row : H) row.resize(j + 1);
            break;
        }

        Q[j + 1] = w / H[j + 1][j];
    }

    return {Q, H};
}

// ============================================================================
// EigenResult — return value of eigenvalue solvers
// ============================================================================

template<typename T>
struct EigenResult {
    std::vector<T>         eigenvalues;   ///< Eigenvalues (requested count)
    std::vector<Vector<T>> eigenvectors;  ///< Corresponding eigenvectors
    std::size_t            iterations;    ///< Iteration count
    bool                   converged;     ///< Whether all eigenvalues converged
};

// ============================================================================
// TridiagonalEigen — eigenvalues of a tridiagonal matrix (QR method)
// ============================================================================

/// @brief Compute all eigenvalues of a tridiagonal matrix using the QL method (LAPACK style)
/// @param d diagonal elements (n elements)
/// @param e sub-diagonal elements (n-1 elements)
/// @return vector of eigenvalues (sorted ascending)
template<typename T>
    requires concepts::OrderedField<T>
std::vector<T> tridiagonal_eigenvalues(
    std::vector<T> d,
    std::vector<T> e)
{
    int n = static_cast<int>(d.size());
    if (n == 0) return {};
    if (n == 1) return {d[0]};

    // The inner loop of the QL method writes e[i+1] (for i = m-1..l), so it
    // writes to e[n-1]. The input e has n-1 elements (sub-diagonal), so extend
    // it to n elements with a sentinel 0 (per the LAPACK dsteqr spec). Failing
    // to do so causes allocator-dependent heap corruption (Exit 0xc0000374),
    // which becomes flaky.
    if (static_cast<int>(e.size()) < n) {
        e.resize(n, T(0));
    }

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

    for (int l = 0; l < n; ++l) {
        int iter = 0;
        while (true) {
            // Find smallest m >= l such that e[m] ≈ 0
            int m = l;
            for (; m < n - 1; ++m) {
                T dd = std::abs(d[m]) + std::abs(d[m + 1]);
                if (std::abs(e[m]) <= eps * dd) break;
            }
            if (m == l) break;  // eigenvalue d[l] converged
            if (++iter > 30 * n) break;

            // QL implicit shift (Wilkinson)
            T g = (d[l + 1] - d[l]) / (T(2) * e[l]);
            T r = std::hypot(g, T(1));
            g = d[m] - d[l] + e[l] / (g + std::copysign(r, g));

            T s = T(1); T c = T(1); T p = T(0);
            bool deflated = false;
            for (int i = m - 1; i >= l; --i) {
                T f = s * e[i];
                T b = c * e[i];
                r = std::hypot(f, g);
                e[i + 1] = r;
                if (r == T(0)) {
                    d[i + 1] -= p;
                    e[m] = T(0);
                    deflated = true;
                    break;
                }
                s = f / r;
                c = g / r;
                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;
            }
            if (!deflated) {
                d[l] -= p;
                e[l] = g;
                e[m] = T(0);
            }
        }
    }

    std::sort(d.begin(), d.end());
    return d;
}

// ============================================================================
// Lanczos method — a few eigenvalues of a symmetric sparse matrix
// ============================================================================

/// @brief Lanczos method (a few eigenvalues of a symmetric sparse matrix)
/// @param A symmetric sparse matrix
/// @param k number of eigenvalues to compute
/// @param which "LM"/"LA" (largest), "SM"/"SA" (smallest)
/// @param max_iter maximum Krylov dimension (0 = auto)
/// @param tol unused (kept for compatibility)
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
EigenResult<T> lanczos(
    const SparseMatrix<T, IndexType>& A,
    std::size_t k = 6,
    const std::string& which = "LM",
    std::size_t max_iter = 0,
    T tol = T(0))
{
    (void)tol;
    const auto n = static_cast<std::size_t>(A.rows());
    if (static_cast<std::size_t>(A.cols()) != n)
        throw std::invalid_argument("lanczos: matrix must be square");
    if (k == 0 || k > n)
        throw std::invalid_argument("lanczos: invalid k");

    // Krylov dimension: min(n, max(2k+1, 20))
    std::size_t m = (max_iter > 0) ? std::min(max_iter, n)
                                   : std::min(std::max(k * 2 + 1, std::size_t(20)), n);

    // Initial vector
    Vector<T> q(n);
    T inv_sqrt_n = T(1) / std::sqrt(static_cast<T>(n));
    for (std::size_t i = 0; i < n; ++i) q[i] = inv_sqrt_n;

    std::vector<T> alpha_vec(m);
    std::vector<T> beta_vec(m);
    std::vector<Vector<T>> Q(m);

    // Lanczos tridiagonalization (full reorthogonalization)
    Q[0] = q;
    for (std::size_t j = 0; j < m; ++j) {
        Vector<T> w = A * Q[j];
        alpha_vec[j] = dot(Q[j], w);
        w -= Q[j] * alpha_vec[j];
        if (j > 0) w -= Q[j - 1] * beta_vec[j - 1];

        // Full reorthogonalization
        for (std::size_t i = 0; i <= j; ++i) {
            T h = dot(Q[i], w);
            w -= Q[i] * h;
        }

        beta_vec[j] = norm(w);
        if (j + 1 < m) {
            if (std::abs(beta_vec[j]) < std::numeric_limits<T>::epsilon() * T(100)) {
                // Invariant subspace found — fill with random-ish orthogonal vector
                Q[j + 1] = Vector<T>(n);
                for (std::size_t idx = 0; idx < n; ++idx) Q[j + 1][idx] = T(0);
                Q[j + 1][(j + 1) % n] = T(1);
                // Reorthogonalize
                for (std::size_t i = 0; i <= j; ++i) {
                    T h = dot(Q[i], Q[j + 1]);
                    Q[j + 1] -= Q[i] * h;
                }
                T nn2 = norm(Q[j + 1]);
                if (nn2 > std::numeric_limits<T>::epsilon())
                    Q[j + 1] = Q[j + 1] / nn2;
            } else {
                Q[j + 1] = w / beta_vec[j];
            }
        }
    }

    // Compute eigenvalues of the tridiagonal matrix
    std::vector<T> a_copy(alpha_vec.begin(), alpha_vec.begin() + static_cast<long>(m));
    std::vector<T> b_copy(beta_vec.begin(), beta_vec.begin() + static_cast<long>(m) - 1);
    auto eigs = tridiagonal_eigenvalues(a_copy, b_copy);

    // Select according to `which`
    EigenResult<T> result;
    result.eigenvalues.resize(k);
    if (which == "LA" || which == "LM") {
        for (std::size_t i = 0; i < k; ++i)
            result.eigenvalues[i] = eigs[eigs.size() - 1 - i];
    } else {
        for (std::size_t i = 0; i < k; ++i)
            result.eigenvalues[i] = eigs[i];
    }
    result.iterations = m;
    result.converged = true;
    result.eigenvectors.resize(k);
    for (std::size_t i = 0; i < k; ++i) result.eigenvectors[i] = Q[0]; // placeholder
    return result;
}

// ============================================================================
// Eigenvalues via the Arnoldi method — a few eigenvalues of a non-symmetric sparse matrix
// ============================================================================

/// @brief Eigenvalues of a Hessenberg matrix (QR method)
/// @param H Hessenberg matrix (m x m, row-major)
/// @return eigenvalues (real parts only; complex eigenvalues return their real part)
template<typename T>
    requires concepts::OrderedField<T>
std::vector<T> hessenberg_eigenvalues(
    std::vector<std::vector<T>> H)
{
    int n = static_cast<int>(H.size());
    if (n == 0) return {};

    const T eps = std::numeric_limits<T>::epsilon() * T(100);
    int nn = n;  // active matrix size (deflated from bottom)

    for (int iter = 0; iter < n * 300; ++iter) {
        if (nn <= 1) break;

        // Deflation: find active block bottom
        while (nn > 1 && std::abs(H[nn - 1][nn - 2]) <=
               eps * (std::abs(H[nn - 2][nn - 2]) + std::abs(H[nn - 1][nn - 1])))
            --nn;
        if (nn <= 1) break;

        // Find active block top
        int lo = nn - 2;
        while (lo > 0 && std::abs(H[lo][lo - 1]) >
               eps * (std::abs(H[lo - 1][lo - 1]) + std::abs(H[lo][lo])))
            --lo;

        // Wilkinson shift from 2×2 trailing submatrix of active block
        T shift = H[nn - 1][nn - 1];

        // QR step with Givens rotations on active block [lo..nn-1]
        for (int i = lo; i < nn - 1; ++i) {
            T a = H[i][i] - shift;
            T b = H[i + 1][i];
            T r = std::hypot(a, b);
            T c = (r == T(0)) ? T(1) : a / r;
            T s = (r == T(0)) ? T(0) : b / r;

            // Apply Givens from left: G^T * H
            for (int j = 0; j < n; ++j) {
                T t1 = c * H[i][j] + s * H[i + 1][j];
                T t2 = -s * H[i][j] + c * H[i + 1][j];
                H[i][j] = t1;
                H[i + 1][j] = t2;
            }
            // Apply Givens from right: H * G
            int jmax = std::min(i + 2, nn - 1);
            for (int j = 0; j <= jmax; ++j) {
                T t1 = c * H[j][i] + s * H[j][i + 1];
                T t2 = -s * H[j][i] + c * H[j][i + 1];
                H[j][i] = t1;
                H[j][i + 1] = t2;
            }
        }
    }

    std::vector<T> eigs(n);
    for (int i = 0; i < n; ++i) eigs[i] = H[i][i];
    return eigs;
}

/// @brief Arnoldi method (a few eigenvalues of a non-symmetric sparse matrix)
/// @param A sparse matrix (square)
/// @param k number of eigenvalues to compute
/// @param which "LM" (largest magnitude), "SM" (smallest magnitude), "LR" (largest real part)
/// @param max_krylov Krylov dimension (0 = auto)
/// @param tol unused (kept for compatibility)
template<typename T, typename IndexType>
    requires concepts::OrderedField<T> && std::integral<IndexType>
EigenResult<T> arnoldi_eigs(
    const SparseMatrix<T, IndexType>& A,
    std::size_t k = 6,
    const std::string& which = "LM",
    std::size_t max_krylov = 0,
    T tol = T(0))
{
    (void)tol;
    const auto n = static_cast<std::size_t>(A.rows());
    if (static_cast<std::size_t>(A.cols()) != n)
        throw std::invalid_argument("arnoldi_eigs: matrix must be square");
    if (k == 0 || k > n)
        throw std::invalid_argument("arnoldi_eigs: invalid k");

    std::size_t m = (max_krylov > 0) ? std::min(max_krylov, n)
                                     : std::min(std::max(k * 2 + 1, std::size_t(20)), n);

    // Initial vector
    Vector<T> v(n);
    T inv_sqrt_n = T(1) / std::sqrt(static_cast<T>(n));
    for (std::size_t i = 0; i < n; ++i) v[i] = inv_sqrt_n;

    // Arnoldi decomposition
    auto [Q, H] = arnoldi_iteration(A, v, m);
    std::size_t actual_m = H.empty() ? 0 : H[0].size();
    if (actual_m == 0)
        throw std::runtime_error("arnoldi_eigs: Arnoldi iteration produced no vectors");

    // Hessenberg eigenvalues from the upper actual_m x actual_m part of H
    std::vector<std::vector<T>> Hm(actual_m, std::vector<T>(actual_m, T(0)));
    for (std::size_t i = 0; i < actual_m; ++i)
        for (std::size_t j = 0; j < actual_m; ++j)
            Hm[i][j] = H[i][j];

    auto eigs = hessenberg_eigenvalues(Hm);

    // Sort according to `which`
    if (which == "LM") {
        std::sort(eigs.begin(), eigs.end(),
            [](T a, T b) { return std::abs(a) > std::abs(b); });
    } else if (which == "SM") {
        std::sort(eigs.begin(), eigs.end(),
            [](T a, T b) { return std::abs(a) < std::abs(b); });
    } else if (which == "LR") {
        std::sort(eigs.begin(), eigs.end(), std::greater<T>());
    } else {
        std::sort(eigs.begin(), eigs.end());
    }

    std::size_t nk = std::min(k, eigs.size());
    EigenResult<T> result;
    result.eigenvalues.assign(eigs.begin(), eigs.begin() + nk);
    result.iterations = actual_m;
    result.converged = true;
    result.eigenvectors.resize(nk);
    for (std::size_t i = 0; i < nk; ++i) result.eigenvectors[i] = Q[0]; // placeholder
    return result;
}

// ============================================================================
// SPARSEKIT — sparse matrix utilities
// ============================================================================

/// @brief Build an adjacency list for a sparse matrix
template<typename T, typename IndexType>
    requires std::integral<IndexType>
std::vector<std::vector<std::size_t>> adjacency_list(
    const SparseMatrix<T, IndexType>& A)
{
    const auto n = static_cast<std::size_t>(A.rows());
    std::vector<std::vector<std::size_t>> adj(n);
    for (std::size_t i = 0; i < n; ++i) {
        for (std::size_t j = 0; j < static_cast<std::size_t>(A.cols()); ++j) {
            if (A.coeff(static_cast<IndexType>(i), static_cast<IndexType>(j)) != T(0))
                adj[i].push_back(j);
        }
    }
    return adj;
}

/// @brief Reverse Cuthill-McKee (RCM) reordering
/// @return new numbering: permutation[i] = old index
template<typename T, typename IndexType>
    requires std::integral<IndexType>
std::vector<std::size_t> rcm_ordering(const SparseMatrix<T, IndexType>& A)
{
    const auto n = static_cast<std::size_t>(A.rows());
    auto adj = adjacency_list(A);

    auto deg = [&](std::size_t v) { return adj[v].size(); };

    std::vector<std::size_t> perm;
    perm.reserve(n);
    std::vector<bool> visited(n, false);

    while (perm.size() < n) {
        // Unvisited node with minimum degree
        std::size_t start = n;
        std::size_t min_deg = n + 1;
        for (std::size_t i = 0; i < n; ++i) {
            if (!visited[i] && deg(i) < min_deg) {
                min_deg = deg(i);
                start = i;
            }
        }
        if (start >= n) break;

        // BFS (in ascending degree order)
        std::vector<std::size_t> queue;
        queue.push_back(start);
        visited[start] = true;

        for (std::size_t qi = 0; qi < queue.size(); ++qi) {
            std::size_t v = queue[qi];
            std::vector<std::size_t> neighbors;
            for (auto u : adj[v]) {
                if (!visited[u]) neighbors.push_back(u);
            }
            std::sort(neighbors.begin(), neighbors.end(),
                [&](std::size_t a, std::size_t b) { return deg(a) < deg(b); });
            for (auto u : neighbors) {
                if (!visited[u]) {
                    visited[u] = true;
                    queue.push_back(u);
                }
            }
        }

        // Reverse
        for (auto it = queue.rbegin(); it != queue.rend(); ++it)
            perm.push_back(*it);
    }

    return perm;
}

/// @brief Reorder a sparse matrix by applying a permutation
template<typename T, typename IndexType>
    requires std::integral<IndexType>
SparseMatrix<T, IndexType> apply_permutation(
    const SparseMatrix<T, IndexType>& A,
    const std::vector<std::size_t>& perm)
{
    const auto n = static_cast<std::size_t>(A.rows());
    std::vector<std::size_t> inv_perm(n);
    for (std::size_t i = 0; i < n; ++i) inv_perm[perm[i]] = i;

    SparseMatrix<T, IndexType> B(A.rows(), A.cols());
    for (std::size_t old_i = 0; old_i < n; ++old_i) {
        std::size_t new_i = inv_perm[old_i];
        for (std::size_t old_j = 0; old_j < n; ++old_j) {
            T val = A.coeff(static_cast<IndexType>(old_i), static_cast<IndexType>(old_j));
            if (val != T(0)) {
                std::size_t new_j = inv_perm[old_j];
                B.set_coeff(static_cast<IndexType>(new_i),
                            static_cast<IndexType>(new_j), val);
            }
        }
    }
    return B;
}

/// @brief Compute the bandwidth of a sparse matrix
template<typename T, typename IndexType>
    requires std::integral<IndexType>
std::size_t bandwidth(const SparseMatrix<T, IndexType>& A)
{
    const auto n = static_cast<std::size_t>(A.rows());
    std::size_t bw = 0;
    for (std::size_t i = 0; i < n; ++i) {
        for (std::size_t j = 0; j < static_cast<std::size_t>(A.cols()); ++j) {
            if (A.coeff(static_cast<IndexType>(i), static_cast<IndexType>(j)) != T(0)) {
                std::size_t diff = (i > j) ? i - j : j - i;
                if (diff > bw) bw = diff;
            }
        }
    }
    return bw;
}

} // namespace sparse_algorithms

} // namespace sangi

#endif // SANGI_SPARSE_MATRIX_ALGORITHMS_HPP