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

// verified.hpp
// 精度保証付き数値計算 (Verified Numerical Computation)
//
// Interval<T> ベースの計算結果に数学的保証を付与。
//
// 提供する機能:
//   VerificationResult — 存在・一意性の検証結果
//   krawczyk1d(f, df, X)     — Krawczyk 法 (1次元)
//   hansenSengupta1d(f, df, X) — Hansen-Sengupta 法 (1次元)
//   verifyRoot1d(f, df, X)   — 1次元根の存在・一意性証明
//   krawczykNd(F, J, X, C)   — Krawczyk 法 (n次元)
//   verifyRootNd(F, J, X)    — n次元根の存在・一意性証明
//   verifyAllRoots1d(f, df, initial) — 全根の列挙と検証

#ifndef CALX_VERIFIED_HPP
#define CALX_VERIFIED_HPP

#include <math/core/Interval.hpp>
#include <vector>
#include <functional>
#include <cmath>
#include <algorithm>
#include <numeric>

namespace calx {

// ================================================================
// 検証結果
// ================================================================

enum class VerificationStatus {
    Verified,       // 存在と一意性が証明された
    ExistenceOnly,  // 存在は示されたが一意性は未確認
    Unverified,     // 検証できなかった (根が存在しない可能性)
    NoRoot          // 根が存在しないことが証明された
};

template<std::floating_point T>
struct VerificationResult {
    VerificationStatus status;
    Interval<T> enclosure;     // 根の包含区間
    int iterations;            // 反復回数

    bool verified() const { return status == VerificationStatus::Verified; }
    bool exists() const {
        return status == VerificationStatus::Verified
            || status == VerificationStatus::ExistenceOnly;
    }
};

template<std::floating_point T>
struct VerificationResultNd {
    VerificationStatus status;
    std::vector<Interval<T>> enclosure;  // 根の包含区間ベクトル
    int iterations;

    bool verified() const { return status == VerificationStatus::Verified; }
    bool exists() const {
        return status == VerificationStatus::Verified
            || status == VerificationStatus::ExistenceOnly;
    }
};

// ================================================================
// 区間ベクトル・区間行列のユーティリティ
// ================================================================

namespace detail {

template<std::floating_point T>
using IVec = std::vector<Interval<T>>;

template<std::floating_point T>
using IMat = std::vector<std::vector<Interval<T>>>;

template<std::floating_point T>
std::vector<T> midpoint(const IVec<T>& x) {
    std::vector<T> m(x.size());
    for (size_t i = 0; i < x.size(); ++i)
        m[i] = x[i].midpoint();
    return m;
}

template<std::floating_point T>
T maxWidth(const IVec<T>& x) {
    T w = T(0);
    for (auto& xi : x)
        w = std::max(w, xi.width());
    return w;
}

template<std::floating_point T>
bool isStrictSubset(const IVec<T>& a, const IVec<T>& b) {
    if (a.size() != b.size()) return false;
    for (size_t i = 0; i < a.size(); ++i) {
        if (a[i].lo() <= b[i].lo() || a[i].hi() >= b[i].hi())
            return false;
    }
    return true;
}

template<std::floating_point T>
IVec<T> intersect(const IVec<T>& a, const IVec<T>& b) {
    IVec<T> result(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        result[i] = a[i].intersect(b[i]);
    return result;
}

template<std::floating_point T>
IMat<T> identityIM(size_t n) {
    IMat<T> I(n, std::vector<Interval<T>>(n));
    for (size_t i = 0; i < n; ++i)
        I[i][i] = Interval<T>::degenerate(T(1));
    return I;
}

template<std::floating_point T>
IVec<T> matVecMul(const IMat<T>& A, const IVec<T>& x) {
    size_t n = A.size();
    IVec<T> result(n);
    for (size_t i = 0; i < n; ++i) {
        Interval<T> s;
        for (size_t j = 0; j < x.size(); ++j)
            s = s + A[i][j] * x[j];
        result[i] = s;
    }
    return result;
}

template<std::floating_point T>
IMat<T> matMatMul(const IMat<T>& A, const IMat<T>& B) {
    size_t n = A.size();
    size_t m = B[0].size();
    size_t p = B.size();
    IMat<T> C(n, std::vector<Interval<T>>(m));
    for (size_t i = 0; i < n; ++i)
        for (size_t j = 0; j < m; ++j) {
            Interval<T> s;
            for (size_t k = 0; k < p; ++k)
                s = s + A[i][k] * B[k][j];
            C[i][j] = s;
        }
    return C;
}

template<std::floating_point T>
IMat<T> toIntervalMatrix(const std::vector<std::vector<T>>& A) {
    size_t n = A.size();
    IMat<T> result(n);
    for (size_t i = 0; i < n; ++i) {
        result[i].resize(A[i].size());
        for (size_t j = 0; j < A[i].size(); ++j)
            result[i][j] = Interval<T>::degenerate(A[i][j]);
    }
    return result;
}

template<std::floating_point T>
IVec<T> toIntervalVector(const std::vector<T>& v) {
    IVec<T> result(v.size());
    for (size_t i = 0; i < v.size(); ++i)
        result[i] = Interval<T>::degenerate(v[i]);
    return result;
}

template<std::floating_point T>
std::vector<std::vector<T>> invertMatrix(const std::vector<std::vector<T>>& A) {
    size_t n = A.size();
    std::vector<std::vector<T>> aug(n, std::vector<T>(2 * n, T(0)));
    for (size_t i = 0; i < n; ++i) {
        for (size_t j = 0; j < n; ++j)
            aug[i][j] = A[i][j];
        aug[i][n + i] = T(1);
    }

    for (size_t k = 0; k < n; ++k) {
        size_t maxRow = k;
        T maxVal = std::abs(aug[k][k]);
        for (size_t i = k + 1; i < n; ++i) {
            if (std::abs(aug[i][k]) > maxVal) {
                maxVal = std::abs(aug[i][k]);
                maxRow = i;
            }
        }
        if (maxVal < std::numeric_limits<T>::epsilon() * T(100))
            return {};

        if (maxRow != k)
            std::swap(aug[maxRow], aug[k]);

        T pivot = aug[k][k];
        for (size_t j = k; j < 2 * n; ++j)
            aug[k][j] /= pivot;

        for (size_t i = 0; i < n; ++i) {
            if (i == k) continue;
            T factor = aug[i][k];
            for (size_t j = k; j < 2 * n; ++j)
                aug[i][j] -= factor * aug[k][j];
        }
    }

    std::vector<std::vector<T>> inv(n, std::vector<T>(n));
    for (size_t i = 0; i < n; ++i)
        for (size_t j = 0; j < n; ++j)
            inv[i][j] = aug[i][n + j];
    return inv;
}

} // namespace detail

// ================================================================
// Krawczyk 法 (1次元)
// ================================================================
//
// K(X) = x̃ - f(x̃)/f'(x̃) + (1 - f'(X)/f'(x̃)) · (X - x̃)
// K(X) ⊂ X ならば X 内に唯一の根が存在する (Brouwer 不動点定理)

template<std::floating_point T, typename F, typename DF>
Interval<T> krawczyk1d(const F& f, const DF& df, Interval<T> X)
{
    T xm = X.midpoint();
    auto x_mid = Interval<T>::degenerate(xm);

    auto f_mid = f(x_mid);
    auto df_mid = df(x_mid);
    auto df_X = df(X);

    if (df_mid.containsZero()) return X;

    auto newton_corr = x_mid - f_mid / df_mid;
    auto one = Interval<T>::degenerate(T(1));
    auto krawczyk_corr = (one - df_X / df_mid) * (X - x_mid);

    return newton_corr + krawczyk_corr;
}

// ================================================================
// Hansen-Sengupta 法 (1次元)
// ================================================================
//
// HS(X) = x̃ - f(x̃)/f'(X)  ∩  X
// 区間 Newton 法の一種。f'(X) が 0 を含む場合は拡張除算で二分割。

template<std::floating_point T, typename F, typename DF>
std::vector<Interval<T>> hansenSengupta1d(const F& f, const DF& df, Interval<T> X)
{
    T xm = X.midpoint();
    auto x_mid = Interval<T>::degenerate(xm);
    auto f_mid = f(x_mid);
    auto df_X = df(X);

    std::vector<Interval<T>> results;

    if (!df_X.containsZero()) {
        auto nStep = x_mid - f_mid / df_X;
        auto refined = X.intersect(nStep);
        if (!refined.isEmpty())
            results.push_back(refined);
    } else if (df_X.lo() == T(0) && df_X.hi() == T(0)) {
        if (f_mid.containsZero())
            results.push_back(X);
    } else {
        // 拡張除算: f'(X) が 0 を含む → 二分割して各半区間で通常 Newton
        T splitPt = xm;
        Interval<T> left(X.lo(), splitPt);
        Interval<T> right(splitPt, X.hi());

        // 左半区間
        if (f(left).containsZero()) {
            auto df_left = df(left);
            if (!df_left.containsZero()) {
                auto nStep = Interval<T>::degenerate(left.midpoint()) - f(Interval<T>::degenerate(left.midpoint())) / df_left;
                auto r = left.intersect(nStep);
                if (!r.isEmpty()) results.push_back(r);
                else results.push_back(left);
            } else {
                results.push_back(left);
            }
        }
        // 右半区間
        if (f(right).containsZero()) {
            auto df_right = df(right);
            if (!df_right.containsZero()) {
                auto nStep = Interval<T>::degenerate(right.midpoint()) - f(Interval<T>::degenerate(right.midpoint())) / df_right;
                auto r = right.intersect(nStep);
                if (!r.isEmpty()) results.push_back(r);
                else results.push_back(right);
            } else {
                results.push_back(right);
            }
        }
    }

    return results;
}

// ================================================================
// 1次元根の検証 (Krawczyk + Hansen-Sengupta 併用)
// ================================================================

template<std::floating_point T, typename F, typename DF>
VerificationResult<T> verifyRoot1d(const F& f, const DF& df, Interval<T> X,
                                    int maxIter = 50, T tol = T(0))
{
    if (tol == T(0))
        tol = std::numeric_limits<T>::epsilon() * T(100);

    if (!f(X).containsZero()) {
        return {VerificationStatus::NoRoot, X, 0};
    }

    Interval<T> current = X;
    int stagnation = 0;

    for (int iter = 0; iter < maxIter; ++iter) {
        T xm = current.midpoint();
        auto x_mid = Interval<T>::degenerate(xm);

        Interval<T> f_mid, df_cur;
        try {
            f_mid = f(x_mid);
            df_cur = df(current);
        } catch (...) {
            if (f(current).containsZero())
                return {VerificationStatus::ExistenceOnly, current, iter};
            return {VerificationStatus::Unverified, current, iter};
        }

        Interval<T> refined = current;
        if (!df_cur.containsZero()) {
            auto nStep = x_mid - f_mid / df_cur;
            refined = current.intersect(nStep);
            if (refined.isEmpty()) {
                return {VerificationStatus::NoRoot, current, iter};
            }
        } else {
            // 導関数が 0 を含む → 二分割
            T splitPt = xm;
            if (current.lo() < splitPt && splitPt < current.hi()) {
                Interval<T> left(current.lo(), splitPt);
                Interval<T> right(splitPt, current.hi());
                bool leftOk = f(left).containsZero();
                bool rightOk = f(right).containsZero();
                if (leftOk && !rightOk) refined = left;
                else if (!leftOk && rightOk) refined = right;
                else if (!leftOk && !rightOk) return {VerificationStatus::NoRoot, current, iter};
                // 両方 → refined = current のまま
            }
        }

        // Krawczyk 検証
        auto K = krawczyk1d<T>(f, df, refined);
        if (refined.contains(K) && K.lo() > refined.lo() && K.hi() < refined.hi()) {
            return {VerificationStatus::Verified, refined.intersect(K), iter + 1};
        }

        auto narrowed = refined.intersect(K);
        if (!narrowed.isEmpty() && narrowed.width() < refined.width()) {
            refined = narrowed;
        }

        if (refined.width() < tol) {
            T eps = std::max(refined.width() * T(0.1),
                             std::numeric_limits<T>::epsilon() * T(10) * (std::abs(xm) + T(1)));
            T inf_lo = refined.lo() - eps;
            T inf_hi = refined.hi() + eps;
            if (inf_lo < inf_hi) {
                auto inflated = Interval<T>(inf_lo, inf_hi);
                auto K2 = krawczyk1d<T>(f, df, inflated);
                if (inflated.contains(K2) && K2.lo() > inflated.lo() && K2.hi() < inflated.hi()) {
                    return {VerificationStatus::Verified, inflated.intersect(K2), iter + 1};
                }
            }
            if (f(refined).containsZero()) {
                return {VerificationStatus::ExistenceOnly, refined, iter + 1};
            }
        }

        if (refined.width() >= current.width() * T(0.95)) {
            ++stagnation;
            if (stagnation >= 5) {
                if (f(refined).containsZero()) {
                    return {VerificationStatus::ExistenceOnly, refined, iter + 1};
                }
                return {VerificationStatus::Unverified, refined, iter + 1};
            }
        } else {
            stagnation = 0;
        }

        current = refined;
    }

    if (f(current).containsZero()) {
        return {VerificationStatus::ExistenceOnly, current, maxIter};
    }
    return {VerificationStatus::Unverified, current, maxIter};
}

// ================================================================
// Krawczyk 法 (n次元)
// ================================================================
//
// K(X) = x̃ - C·F(x̃) + (I - C·J(X)) · (X - x̃)
// C ≈ J(x̃)^{-1} (前処理行列)
// K(X) ⊂ int(X) ならば X 内に唯一の根が存在

template<std::floating_point T, typename FFunc, typename JFunc>
detail::IVec<T> krawczykNd(const FFunc& F, const JFunc& J,
                            const detail::IVec<T>& X,
                            const std::vector<std::vector<T>>& C)
{
    size_t n = X.size();
    auto xm = detail::midpoint(X);

    auto f_mid = F(xm);
    auto f_mid_iv = detail::toIntervalVector<T>(f_mid);

    auto C_iv = detail::toIntervalMatrix<T>(C);
    auto Cf = detail::matVecMul<T>(C_iv, f_mid_iv);

    auto J_X = J(X);
    auto CJ = detail::matMatMul<T>(C_iv, J_X);

    auto I_mat = detail::identityIM<T>(n);
    detail::IMat<T> I_minus_CJ(n, std::vector<Interval<T>>(n));
    for (size_t i = 0; i < n; ++i)
        for (size_t j = 0; j < n; ++j)
            I_minus_CJ[i][j] = I_mat[i][j] - CJ[i][j];

    detail::IVec<T> dx(n);
    auto xm_iv = detail::toIntervalVector<T>(xm);
    for (size_t i = 0; i < n; ++i)
        dx[i] = X[i] - xm_iv[i];

    auto correction = detail::matVecMul<T>(I_minus_CJ, dx);

    detail::IVec<T> K(n);
    for (size_t i = 0; i < n; ++i)
        K[i] = xm_iv[i] - Cf[i] + correction[i];

    return K;
}

// ================================================================
// n次元根の検証
// ================================================================

template<std::floating_point T, typename FFunc, typename JFunc, typename JPFunc>
VerificationResultNd<T> verifyRootNd(const FFunc& F, const JFunc& J, const JPFunc& J_point,
                                      const detail::IVec<T>& X,
                                      int maxIter = 50, T tol = T(0))
{
    if (tol == T(0))
        tol = std::numeric_limits<T>::epsilon() * T(100);

    auto current = X;

    for (int iter = 0; iter < maxIter; ++iter) {
        auto xm = detail::midpoint(current);
        auto J_mid = J_point(xm);
        auto C = detail::invertMatrix(J_mid);
        if (C.empty()) {
            return {VerificationStatus::Unverified, current, iter};
        }

        auto K = krawczykNd<T>(F, J, current, C);

        if (detail::isStrictSubset(K, current)) {
            auto refined = detail::intersect(K, current);
            return {VerificationStatus::Verified, refined, iter + 1};
        }

        auto refined = detail::intersect(K, current);
        bool anyEmpty = false;
        for (auto& ri : refined) {
            if (ri.isEmpty()) { anyEmpty = true; break; }
        }
        if (anyEmpty) {
            return {VerificationStatus::NoRoot, current, iter};
        }

        if (detail::maxWidth(refined) < tol) {
            return {VerificationStatus::ExistenceOnly, refined, iter + 1};
        }

        if (detail::maxWidth(refined) >= detail::maxWidth(current) * T(0.999)) {
            return {VerificationStatus::Unverified, refined, iter + 1};
        }

        current = refined;
    }

    return {VerificationStatus::ExistenceOnly, current, maxIter};
}

// ================================================================
// 全根の列挙と検証 (1次元)
// ================================================================

template<std::floating_point T, typename F, typename DF>
std::vector<VerificationResult<T>> verifyAllRoots1d(F& f, DF& df,
                                                     Interval<T> initial,
                                                     int subdivisions = 64,
                                                     int maxIter = 50,
                                                     T tol = T(0))
{
    T dx = initial.width() / T(subdivisions);
    std::vector<VerificationResult<T>> results;

    for (int i = 0; i < subdivisions; ++i) {
        T lo = initial.lo() + T(i) * dx;
        T hi = initial.lo() + T(i + 1) * dx;
        if (hi > initial.hi()) hi = initial.hi();
        if (lo >= hi) continue;
        Interval<T> sub(lo, hi);

        if (!f(sub).containsZero()) continue;

        VerificationResult<T> vr;
        try {
            vr = verifyRoot1d<T, F, DF>(f, df, sub, maxIter, tol);
        } catch (...) {
            continue; // 数値的に不安定な部分区間をスキップ
        }
        if (vr.exists()) {
            bool duplicate = false;
            for (auto& prev : results) {
                if (prev.enclosure.overlaps(vr.enclosure)) {
                    if (vr.enclosure.width() < prev.enclosure.width()) {
                        prev = vr;
                    }
                    duplicate = true;
                    break;
                }
            }
            if (!duplicate)
                results.push_back(vr);
        }
    }

    return results;
}

} // namespace calx

#endif // CALX_VERIFIED_HPP
