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

// matrix_functions.hpp
//
// 行列関数: expm (行列指数), sqrtm (行列平方根), logm (行列対数)
//
// アルゴリズム:
//   expm  — Scaling-and-squaring + Padé [13/13] (Higham 2005)
//   sqrtm — Schur 分解 + Parlett 再帰 (Björck-Hammarling 1983)
//   logm  — Schur 分解 + Parlett 再帰

#ifndef CALX_MATRIX_FUNCTIONS_HPP
#define CALX_MATRIX_FUNCTIONS_HPP

#include <math/core/common.hpp>
#include <math/core/matrix.hpp>
#include <math/core/traits.hpp>
#include <math/linalg/decomposition.hpp>
#include <math/linalg/solvers.hpp>

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <vector>

namespace calx {
    namespace algorithms {

        //=====================================================================
        // ヘルパー関数 (内部用)
        //=====================================================================

        namespace detail_matfun {

            // 行列 1-ノルム: max_j Σ_i |A_{ij}| (最大列絶対値和)
            template<typename T>
            T matrix_one_norm(const Matrix<T>& A) {
                const auto rows = A.rows();
                const auto cols = A.cols();
                T max_col_sum = T(0);
                for (std::size_t j = 0; j < cols; ++j) {
                    T col_sum = T(0);
                    for (std::size_t i = 0; i < rows; ++i) {
                        col_sum += std::abs(A(i, j));
                    }
                    if (col_sum > max_col_sum) {
                        max_col_sum = col_sum;
                    }
                }
                return max_col_sum;
            }

            // 準上三角行列のブロック構造を検出
            // block_starts[k] = k 番目のブロックの開始行
            // block_sizes[k]  = 1 or 2
            template<typename T>
            void detect_blocks(const Matrix<T>& T_mat, std::size_t n,
                               std::vector<std::size_t>& block_starts,
                               std::vector<std::size_t>& block_sizes) {
                block_starts.clear();
                block_sizes.clear();
                std::size_t i = 0;
                while (i < n) {
                    if (i + 1 < n && T_mat(i + 1, i) != T(0)) {
                        // 2×2 ブロック
                        block_starts.push_back(i);
                        block_sizes.push_back(2);
                        i += 2;
                    } else {
                        // 1×1 ブロック
                        block_starts.push_back(i);
                        block_sizes.push_back(1);
                        i += 1;
                    }
                }
            }

            // 2×2 ブロック B の固有値 α±iβ を計算
            // B = [[a,b],[c,d]], α = (a+d)/2, β = sqrt(|bc + ((a-d)/2)²|)
            // ただし複素共役対の場合のみ (disc < 0)
            template<typename T>
            void block2_eigenvalues(const Matrix<T>& T_mat, std::size_t i,
                                     T& alpha, T& beta) {
                T a = T_mat(i, i), b = T_mat(i, i + 1);
                T c = T_mat(i + 1, i), d = T_mat(i + 1, i + 1);
                alpha = (a + d) / T(2);
                T half_diff = (a - d) / T(2);
                T disc = half_diff * half_diff + b * c;
                if (disc < T(0)) {
                    beta = std::sqrt(-disc);
                } else {
                    // 実固有値のケース (α ± sqrt(disc))
                    beta = T(0);
                }
            }

            // 2×2 ブロック B に対する f(B) の計算
            // f(B) = u·I + (v/β)·(B - α·I)
            // func_re, func_im: f(α+iβ) = func_re + i·func_im
            template<typename T>
            void apply_block2_function(
                const Matrix<T>& T_mat, std::size_t i,
                Matrix<T>& F, T alpha, T beta,
                T func_re, T func_im) {
                if (beta == T(0)) {
                    // 実固有値ケース: 2×2 ブロックだが β=0
                    // 対角要素に f(α) を設定
                    F(i, i) = func_re;
                    F(i + 1, i + 1) = func_re;
                    // 上三角要素は f'(α) * T_ij で近似
                    // (実際にはこのケースは稀)
                    F(i, i + 1) = T(0);
                    F(i + 1, i) = T(0);
                    return;
                }
                T u = func_re;
                T v_over_beta = func_im / beta;
                // F の (i,i), (i,i+1), (i+1,i), (i+1,i+1) を設定
                F(i, i)         = u + v_over_beta * (T_mat(i, i)     - alpha);
                F(i, i + 1)     =     v_over_beta *  T_mat(i, i + 1);
                F(i + 1, i)     =     v_over_beta *  T_mat(i + 1, i);
                F(i + 1, i + 1) = u + v_over_beta * (T_mat(i + 1, i + 1) - alpha);
            }

            // 小ブロック Sylvester 方程式ソルバー: A·X - X·B = C
            // A: p×p, B: q×q, C: p×q, X: p×q (p,q ∈ {1,2})
            // F の部分行列 F[ri:ri+p, ci:ci+q] に解を書き込む
            template<typename T>
            void solve_sylvester_block(
                const Matrix<T>& F,      // F 行列 (A_block と B_block の読み出し元)
                const Matrix<T>& T_mat,  // 元の準上三角行列
                Matrix<T>& result,       // 解を書き込む先
                std::size_t ri, std::size_t p,  // 行ブロック (開始, サイズ)
                std::size_t ci, std::size_t q,  // 列ブロック (開始, サイズ)
                std::size_t n) {
                // RHS を計算: C = (F_ii · T_ij - T_ij · F_jj) + Σ_k (F_ik · T_kj - T_ik · F_kj)
                // ここで、F_ii = result[ri:ri+p, ri:ri+p] (対角ブロック)
                //         F_jj = result[ci:ci+q, ci:ci+q] (対角ブロック)
                //         T_ij = T_mat[ri:ri+p, ci:ci+q] (上三角ブロック)

                // まず T_ij · (F_ii - F_jj) 項を計算
                // 次に中間ブロックの cross terms を加算

                // C = F_ii · T_ij
                T C[2][2] = {};
                for (std::size_t a = 0; a < p; ++a) {
                    for (std::size_t b = 0; b < q; ++b) {
                        T sum = T(0);
                        for (std::size_t k = 0; k < p; ++k) {
                            sum += result(ri + a, ri + k) * T_mat(ri + k, ci + b);
                        }
                        C[a][b] = sum;
                    }
                }

                // C -= T_ij · F_jj
                for (std::size_t a = 0; a < p; ++a) {
                    for (std::size_t b = 0; b < q; ++b) {
                        T sum = T(0);
                        for (std::size_t k = 0; k < q; ++k) {
                            sum += T_mat(ri + a, ci + k) * result(ci + k, ci + b);
                        }
                        C[a][b] -= sum;
                    }
                }

                // 中間ブロックの cross terms を加算
                // Σ_{k: ri+p ≤ k < ci} (F[ri:,k:] · T[k:,ci:] - T[ri:,k:] · F[k:,ci:])
                for (std::size_t k = ri + p; k < ci; ++k) {
                    for (std::size_t a = 0; a < p; ++a) {
                        for (std::size_t b = 0; b < q; ++b) {
                            C[a][b] += result(ri + a, k) * T_mat(k, ci + b)
                                     - T_mat(ri + a, k) * result(k, ci + b);
                        }
                    }
                }

                // Sylvester 方程式 A·X - X·B = C を解く
                // A = T_mat[ri:ri+p, ri:ri+p] (T の対角ブロック、F_ii ではない！)
                // 注意: TF=FT の関係から導出すると、
                //   T_ii · F_ij - F_ij · T_jj = RHS
                // なので A = T_ii, B = T_jj

                if (p == 1 && q == 1) {
                    // スカラー: x = c / (t_ii - t_jj)
                    T denom = T_mat(ri, ri) - T_mat(ci, ci);
                    if (std::abs(denom) < std::numeric_limits<T>::epsilon() * T(100)) {
                        result(ri, ci) = T(0); // 縮退ケース
                    } else {
                        result(ri, ci) = C[0][0] / denom;
                    }
                } else if (p == 1 && q == 2) {
                    // 1×2 Sylvester: a·X - X·B = C
                    // [a*x0 - x0*b00 - x1*b10, a*x1 - x0*b01 - x1*b11] = [c0, c1]
                    T a = T_mat(ri, ri);
                    T b00 = T_mat(ci, ci), b01 = T_mat(ci, ci + 1);
                    T b10 = T_mat(ci + 1, ci), b11 = T_mat(ci + 1, ci + 1);
                    // M · [x0, x1]^T = [c0, c1]^T
                    T m00 = a - b00, m01 = -b10;
                    T m10 = -b01,    m11 = a - b11;
                    T det = m00 * m11 - m01 * m10;
                    if (std::abs(det) < std::numeric_limits<T>::epsilon() * T(100)) {
                        result(ri, ci) = T(0);
                        result(ri, ci + 1) = T(0);
                    } else {
                        result(ri, ci)     = ( m11 * C[0][0] - m01 * C[0][1]) / det;
                        result(ri, ci + 1) = (-m10 * C[0][0] + m00 * C[0][1]) / det;
                    }
                } else if (p == 2 && q == 1) {
                    // 2×1 Sylvester: A·X - X·b = C
                    T b = T_mat(ci, ci);
                    T a00 = T_mat(ri, ri),     a01 = T_mat(ri, ri + 1);
                    T a10 = T_mat(ri + 1, ri), a11 = T_mat(ri + 1, ri + 1);
                    // [a00-b, a01; a10, a11-b] · [x0; x1] = [c0; c1]
                    T m00 = a00 - b, m01 = a01;
                    T m10 = a10,     m11 = a11 - b;
                    T det = m00 * m11 - m01 * m10;
                    if (std::abs(det) < std::numeric_limits<T>::epsilon() * T(100)) {
                        result(ri, ci) = T(0);
                        result(ri + 1, ci) = T(0);
                    } else {
                        result(ri, ci)     = ( m11 * C[0][0] - m01 * C[1][0]) / det;
                        result(ri + 1, ci) = (-m10 * C[0][0] + m00 * C[1][0]) / det;
                    }
                } else {
                    // 2×2 Sylvester: A·X - X·B = C → 4×4 線形系
                    T a00 = T_mat(ri, ri),     a01 = T_mat(ri, ri + 1);
                    T a10 = T_mat(ri + 1, ri), a11 = T_mat(ri + 1, ri + 1);
                    T b00 = T_mat(ci, ci),     b01 = T_mat(ci, ci + 1);
                    T b10 = T_mat(ci + 1, ci), b11 = T_mat(ci + 1, ci + 1);

                    // vec(X) = [x00, x10, x01, x11]^T (列優先)
                    // M · vec(X) = vec(C) where M = I⊗A - B^T⊗I
                    // M = [[a00-b00, a01,    -b10,    0    ],
                    //      [a10,     a11-b00, 0,     -b10  ],
                    //      [-b01,    0,       a00-b11, a01  ],
                    //      [0,      -b01,     a10,    a11-b11]]
                    T M[4][4] = {
                        {a00 - b00, a01,       -b10,      T(0)},
                        {a10,       a11 - b00,  T(0),     -b10},
                        {-b01,      T(0),       a00 - b11, a01},
                        {T(0),     -b01,        a10,       a11 - b11}
                    };
                    T rhs[4] = {C[0][0], C[1][0], C[0][1], C[1][1]};

                    // 4×4 ガウス消去法 (部分ピボット)
                    int piv[4] = {0, 1, 2, 3};
                    for (int col = 0; col < 4; ++col) {
                        // ピボット選択
                        int max_row = col;
                        T max_val = std::abs(M[piv[col]][col]);
                        for (int row = col + 1; row < 4; ++row) {
                            T v = std::abs(M[piv[row]][col]);
                            if (v > max_val) {
                                max_val = v;
                                max_row = row;
                            }
                        }
                        std::swap(piv[col], piv[max_row]);

                        if (max_val < std::numeric_limits<T>::epsilon() * T(100)) {
                            // 特異ケース
                            result(ri, ci) = T(0);
                            result(ri + 1, ci) = T(0);
                            result(ri, ci + 1) = T(0);
                            result(ri + 1, ci + 1) = T(0);
                            return;
                        }

                        // 消去
                        for (int row = col + 1; row < 4; ++row) {
                            T factor = M[piv[row]][col] / M[piv[col]][col];
                            for (int k = col + 1; k < 4; ++k) {
                                M[piv[row]][k] -= factor * M[piv[col]][k];
                            }
                            rhs[piv[row]] -= factor * rhs[piv[col]];
                        }
                    }

                    // 後退代入
                    T x[4];
                    for (int i = 3; i >= 0; --i) {
                        T sum = rhs[piv[i]];
                        for (int k = i + 1; k < 4; ++k) {
                            sum -= M[piv[i]][k] * x[k];
                        }
                        x[i] = sum / M[piv[i]][i];
                    }

                    result(ri, ci)         = x[0];
                    result(ri + 1, ci)     = x[1];
                    result(ri, ci + 1)     = x[2];
                    result(ri + 1, ci + 1) = x[3];
                }
            }

        } // namespace detail_matfun

        //=====================================================================
        // expm — 行列指数関数
        //=====================================================================

        /**
         * @brief 行列指数関数 exp(A)
         *
         * Scaling-and-squaring 法 + Padé [13/13] 近似。
         * Higham (2005) "The Scaling and Squaring Method for
         * the Matrix Exponential Revisited" に基づく。
         *
         * @tparam T 要素の型 (float, double)
         * @param A 入力行列 (n×n 正方行列)
         * @return exp(A)
         * @throws DimensionError A が正方行列でない場合
         */
        template<typename T>
        Matrix<T> expm(const Matrix<T>& A) {
            const auto n = A.rows();
            if (n != A.cols()) {
                throw DimensionError("expm: matrix must be square");
            }
            if (n == 0) {
                return A;
            }
            if (n == 1) {
                Matrix<T> result(1, 1);
                result(0, 0) = std::exp(A(0, 0));
                return result;
            }

            auto I = Matrix<T>::identity(n);

            // Padé 近似の θ 値 (Higham Table 10.2)
            constexpr T theta3  = T(1.495585217958292e-02);
            constexpr T theta5  = T(2.539398330063230e-01);
            constexpr T theta7  = T(9.504178996162932e-01);
            constexpr T theta9  = T(2.097847961257068e+00);
            constexpr T theta13 = T(5.371920351148152e+00);

            T norm1 = detail_matfun::matrix_one_norm(A);

            // A² を事前計算 (全次数で使用)
            Matrix<T> A2 = A * A;

            // 低次 Padé が使える場合 (||A||₁ が十分小さい)
            auto pade_3 = [&]() -> Matrix<T> {
                // b₀=120, b₁=60, b₂=12, b₃=1
                constexpr T b0 = T(120), b1 = T(60), b2 = T(12), b3 = T(1);
                Matrix<T> U = A * (b3 * A2 + b1 * I);
                Matrix<T> V = b2 * A2 + b0 * I;
                return solve_multi(Matrix<T>(V - U), Matrix<T>(V + U));
            };

            auto pade_5 = [&]() -> Matrix<T> {
                constexpr T b0 = T(30240), b1 = T(15120), b2 = T(3360);
                constexpr T b3 = T(420), b4 = T(30), b5 = T(1);
                Matrix<T> A4 = A2 * A2;
                Matrix<T> U = A * (b5 * A4 + b3 * A2 + b1 * I);
                Matrix<T> V = b4 * A4 + b2 * A2 + b0 * I;
                return solve_multi(Matrix<T>(V - U), Matrix<T>(V + U));
            };

            auto pade_7 = [&]() -> Matrix<T> {
                constexpr T b0 = T(17297280), b1 = T(8648640), b2 = T(1995840);
                constexpr T b3 = T(277200), b4 = T(25200), b5 = T(1512);
                constexpr T b6 = T(56), b7 = T(1);
                Matrix<T> A4 = A2 * A2;
                Matrix<T> A6 = A4 * A2;
                Matrix<T> U = A * (b7 * A6 + b5 * A4 + b3 * A2 + b1 * I);
                Matrix<T> V = b6 * A6 + b4 * A4 + b2 * A2 + b0 * I;
                return solve_multi(Matrix<T>(V - U), Matrix<T>(V + U));
            };

            auto pade_9 = [&]() -> Matrix<T> {
                constexpr T b0 = T(17643225600.0), b1 = T(8821612800.0);
                constexpr T b2 = T(2075673600.0), b3 = T(302702400.0);
                constexpr T b4 = T(30270240.0), b5 = T(2162160.0);
                constexpr T b6 = T(110880.0), b7 = T(3960.0);
                constexpr T b8 = T(90.0), b9 = T(1);
                Matrix<T> A4 = A2 * A2;
                Matrix<T> A6 = A4 * A2;
                Matrix<T> A8 = A4 * A4;
                Matrix<T> U = A * (b9 * A8 + b7 * A6 + b5 * A4 + b3 * A2 + b1 * I);
                Matrix<T> V = b8 * A8 + b6 * A6 + b4 * A4 + b2 * A2 + b0 * I;
                return solve_multi(Matrix<T>(V - U), Matrix<T>(V + U));
            };

            if (norm1 <= theta3) return pade_3();
            if (norm1 <= theta5) return pade_5();
            if (norm1 <= theta7) return pade_7();
            if (norm1 <= theta9) return pade_9();

            // Padé [13/13] with scaling
            int s = std::max(0, static_cast<int>(std::ceil(std::log2(norm1 / theta13))));
            T scale = std::ldexp(T(1), -s);
            Matrix<T> As = A * scale;
            Matrix<T> As2 = As * As;
            Matrix<T> As4 = As2 * As2;
            Matrix<T> As6 = As4 * As2;

            // Padé [13/13] 係数
            constexpr T b0  = T(64764752532480000.0);
            constexpr T b1  = T(32382376266240000.0);
            constexpr T b2  = T(7771770303897600.0);
            constexpr T b3  = T(1187353796428800.0);
            constexpr T b4  = T(129060195264000.0);
            constexpr T b5  = T(10559470521600.0);
            constexpr T b6  = T(670442572800.0);
            constexpr T b7  = T(33522128640.0);
            constexpr T b8  = T(1323241920.0);
            constexpr T b9  = T(40840800.0);
            constexpr T b10 = T(960960.0);
            constexpr T b11 = T(16380.0);
            constexpr T b12 = T(182.0);
            constexpr T b13 = T(1.0);

            // U = As · (As6·(b13·As6 + b11·As4 + b9·As2) + b7·As6 + b5·As4 + b3·As2 + b1·I)
            Matrix<T> inner_u = b13 * As6 + b11 * As4 + b9 * As2;
            Matrix<T> U = As * (As6 * inner_u + b7 * As6 + b5 * As4 + b3 * As2 + b1 * I);

            // V = As6·(b12·As6 + b10·As4 + b8·As2) + b6·As6 + b4·As4 + b2·As2 + b0·I
            Matrix<T> inner_v = b12 * As6 + b10 * As4 + b8 * As2;
            Matrix<T> V = As6 * inner_v + b6 * As6 + b4 * As4 + b2 * As2 + b0 * I;

            // (V - U) · F = V + U を解く
            Matrix<T> F = solve_multi(Matrix<T>(V - U), Matrix<T>(V + U));

            // s 回自乗
            for (int i = 0; i < s; ++i) {
                F = F * F;
            }

            return F;
        }

        //=====================================================================
        // sqrtm — 行列平方根
        //=====================================================================

        /**
         * @brief 行列平方根 A^{1/2} (主値)
         *
         * Schur 分解 + Parlett 再帰 (Björck-Hammarling 1983)。
         * A の全固有値が負の実軸上にないとき、主値平方根が存在する。
         *
         * @tparam T 要素の型 (float, double)
         * @param A 入力行列 (n×n 正方行列)
         * @return A^{1/2}
         * @throws DimensionError A が正方行列でない場合
         * @throws MathError 負の実固有値が存在する場合
         */
        template<typename T>
        Matrix<T> sqrtm(const Matrix<T>& A) {
            const auto n = A.rows();
            if (n != A.cols()) {
                throw DimensionError("sqrtm: matrix must be square");
            }
            if (n == 0) {
                return A;
            }
            if (n == 1) {
                if (A(0, 0) < T(0)) {
                    throw MathError("sqrtm: matrix has negative real eigenvalue");
                }
                Matrix<T> result(1, 1);
                result(0, 0) = std::sqrt(A(0, 0));
                return result;
            }

            // Schur 分解: A = Q·T·Qᵀ
            auto [T_mat, Q] = schur_decomposition(A);

            // ブロック構造を検出
            std::vector<std::size_t> block_starts, block_sizes;
            detail_matfun::detect_blocks(T_mat, n, block_starts, block_sizes);

            // 結果行列 F = sqrtm(T) を初期化
            Matrix<T> F(n, n, T(0));

            // Step 1: 対角ブロックの計算
            for (std::size_t bi = 0; bi < block_starts.size(); ++bi) {
                std::size_t i = block_starts[bi];
                std::size_t sz = block_sizes[bi];

                if (sz == 1) {
                    // 1×1 ブロック: sqrt(T_ii)
                    if (T_mat(i, i) < T(0)) {
                        throw MathError("sqrtm: matrix has negative real eigenvalue");
                    }
                    F(i, i) = std::sqrt(T_mat(i, i));
                } else {
                    // 2×2 ブロック: 複素共役固有値 α±iβ
                    // √(α+iβ) = √r · (cos(θ/2) + i·sin(θ/2))
                    // r = |λ| = √(α²+β²), θ = arg(λ) = atan2(β, α)
                    T alpha, beta;
                    detail_matfun::block2_eigenvalues(T_mat, i, alpha, beta);

                    T r = std::sqrt(alpha * alpha + beta * beta);
                    T theta = std::atan2(beta, alpha);
                    T sqrt_r = std::sqrt(r);
                    T u = sqrt_r * std::cos(theta / T(2));
                    T v = sqrt_r * std::sin(theta / T(2));

                    detail_matfun::apply_block2_function(T_mat, i, F, alpha, beta, u, v);
                }
            }

            // Step 2: 上対角ブロックの計算 (Parlett 再帰)
            // 対角から離れる方向にブロック列を処理
            for (std::size_t d = 1; d < block_starts.size(); ++d) {
                for (std::size_t bi = 0; bi + d < block_starts.size(); ++bi) {
                    std::size_t bj = bi + d;
                    std::size_t ri = block_starts[bi];
                    std::size_t p  = block_sizes[bi];
                    std::size_t ci = block_starts[bj];
                    std::size_t q  = block_sizes[bj];

                    detail_matfun::solve_sylvester_block(F, T_mat, F, ri, p, ci, q, n);
                }
            }

            // Step 3: 復元 sqrtm(A) = Q·F·Qᵀ
            Matrix<T> Qt = Q.transpose();
            return Q * F * Qt;
        }

        //=====================================================================
        // logm — 行列対数関数
        //=====================================================================

        /**
         * @brief 行列対数 log(A) (主値)
         *
         * Schur 分解 + Parlett 再帰。
         * A の全固有値が負の実軸上にないとき、主値対数が存在する。
         *
         * @tparam T 要素の型 (float, double)
         * @param A 入力行列 (n×n 正方行列)
         * @return log(A)
         * @throws DimensionError A が正方行列でない場合
         * @throws MathError 非正の実固有値が存在する場合
         */
        template<typename T>
        Matrix<T> logm(const Matrix<T>& A) {
            const auto n = A.rows();
            if (n != A.cols()) {
                throw DimensionError("logm: matrix must be square");
            }
            if (n == 0) {
                return A;
            }
            if (n == 1) {
                if (A(0, 0) <= T(0)) {
                    throw MathError("logm: matrix has non-positive real eigenvalue");
                }
                Matrix<T> result(1, 1);
                result(0, 0) = std::log(A(0, 0));
                return result;
            }

            // Schur 分解: A = Q·T·Qᵀ
            auto [T_mat, Q] = schur_decomposition(A);

            // ブロック構造を検出
            std::vector<std::size_t> block_starts, block_sizes;
            detail_matfun::detect_blocks(T_mat, n, block_starts, block_sizes);

            // 結果行列 F = logm(T) を初期化
            Matrix<T> F(n, n, T(0));

            // Step 1: 対角ブロックの計算
            for (std::size_t bi = 0; bi < block_starts.size(); ++bi) {
                std::size_t i = block_starts[bi];
                std::size_t sz = block_sizes[bi];

                if (sz == 1) {
                    // 1×1 ブロック: log(T_ii)
                    if (T_mat(i, i) <= T(0)) {
                        throw MathError("logm: matrix has non-positive real eigenvalue");
                    }
                    F(i, i) = std::log(T_mat(i, i));
                } else {
                    // 2×2 ブロック: 複素共役固有値 α±iβ
                    T alpha, beta;
                    detail_matfun::block2_eigenvalues(T_mat, i, alpha, beta);

                    T r2 = alpha * alpha + beta * beta;
                    T u = std::log(r2) / T(2); // ½·log(|λ|²)
                    T v = std::atan2(beta, alpha);

                    detail_matfun::apply_block2_function(T_mat, i, F, alpha, beta, u, v);
                }
            }

            // Step 2: 上対角ブロックの計算 (Parlett 再帰)
            for (std::size_t d = 1; d < block_starts.size(); ++d) {
                for (std::size_t bi = 0; bi + d < block_starts.size(); ++bi) {
                    std::size_t bj = bi + d;
                    std::size_t ri = block_starts[bi];
                    std::size_t p  = block_sizes[bi];
                    std::size_t ci = block_starts[bj];
                    std::size_t q  = block_sizes[bj];

                    detail_matfun::solve_sylvester_block(F, T_mat, F, ri, p, ci, q, n);
                }
            }

            // Step 3: 復元 logm(A) = Q·F·Qᵀ
            Matrix<T> Qt = Q.transpose();
            return Q * F * Qt;
        }

    } // namespace algorithms
} // namespace calx

#endif // CALX_MATRIX_FUNCTIONS_HPP