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

// optimization_1d_impl.hpp — golden_section_search, brent_minimize の実装

#ifndef SANGI_OPTIMIZATION_1D_IMPL_HPP
#define SANGI_OPTIMIZATION_1D_IMPL_HPP

#include <math/optimization/optimization_1d.hpp>

namespace sangi {

template<concepts::OrderedField T>
OptimizationResult1D<T> golden_section_search(
    const std::function<T(T)>& f,
    T a, T b,
    const OptimizationOptions<T>& options) {

    const T golden_ratio = (std::sqrt(T(5)) - 1) / 2;
    const T inv_golden_ratio = 1 - golden_ratio;

    // 初期区間内の2点を選択
    T c = a + inv_golden_ratio * (b - a);
    T d = a + golden_ratio * (b - a);
    T fc = f(c);
    T fd = f(d);

    for (size_t i = 0; i < options.max_iterations; ++i) {
        if (std::abs(b - a) < options.tolerance) {
            // 区間の中点を返す
            T x_min = (a + b) / 2;
            T f_min = f(x_min);
            return OptimizationResult1D<T>::makeSuccess(x_min, f_min, i);
        }

        if (fc < fd) {
            b = d;
            d = c;
            fd = fc;
            c = a + inv_golden_ratio * (b - a);
            fc = f(c);
        }
        else {
            a = c;
            c = d;
            fc = fd;
            d = a + golden_ratio * (b - a);
            fd = f(d);
        }
    }

    // 最大反復回数に達した場合、現在の最良点を返す
    T x_min = (fc < fd) ? c : d;
    T f_min = (fc < fd) ? fc : fd;
    return OptimizationResult1D<T>::makeFailure(x_min, f_min, options.max_iterations);
}

template<concepts::OrderedField T>
OptimizationResult1D<T> brent_minimize(
    const std::function<T(T)>& f,
    T a, T b,
    const OptimizationOptions<T>& options) {

    const T golden_ratio = (3 - std::sqrt(T(5))) / 2;  // ≈ 0.3819...

    // 初期化
    T x = (a + b) / 2;
    T v = x;
    T w = x;
    T fx = f(x);
    T fv = fx;
    T fw = fx;

    T d = 0, e = 0;

    for (size_t i = 0; i < options.max_iterations; ++i) {
        T midpoint = (a + b) / 2;
        T tol = std::sqrt(std::numeric_limits<T>::epsilon()) * std::abs(x) + options.tolerance;

        // 収束判定
        if (std::abs(x - midpoint) <= 2 * tol - (b - a) / 2) {
            return OptimizationResult1D<T>::makeSuccess(x, fx, i);
        }

        // 放物線補間の準備
        bool do_golden_section = true;

        if (std::abs(e) > tol) {
            // 放物線補間を試みる
            T r = (x - w) * (fx - fv);
            T q = (x - v) * (fx - fw);
            T p = (x - v) * q - (x - w) * r;
            q = 2 * (q - r);

            if (q > 0) {
                p = -p;
            }
            else {
                q = -q;
            }

            T old_e = e;
            e = d;

            // 放物線補間が有効かチェック
            if (std::abs(p) < std::abs(q * old_e / 2) && p > q * (a - x) && p < q * (b - x)) {
                // 放物線補間を使用
                d = p / q;
                T u = x + d;

                // 更新点が境界または前回の点に近すぎないようにする
                if (u - a < 2 * tol || b - u < 2 * tol || std::abs(u - x) >= std::abs(old_e) / 2) {
                    d = (x < midpoint) ? tol : -tol;
                }
                else {
                    do_golden_section = false;
                }
            }
        }

        // 放物線補間が使用できない場合は黄金分割法を使用
        if (do_golden_section) {
            e = (x < midpoint) ? b - x : a - x;
            d = golden_ratio * e;
        }

        // 更新点を計算
        T u = x + ((std::abs(d) >= tol) ? d : ((d > 0) ? tol : -tol));
        T fu = f(u);

        // 点を更新
        if (fu <= fx) {
            if (u < x) {
                b = x;
            }
            else {
                a = x;
            }
            v = w;
            w = x;
            x = u;
            fv = fw;
            fw = fx;
            fx = fu;
        }
        else {
            if (u < x) {
                a = u;
            }
            else {
                b = u;
            }

            if (fu <= fw || w == x) {
                v = w;
                w = u;
                fv = fw;
                fw = fu;
            }
            else if (fu <= fv || v == x || v == w) {
                v = u;
                fv = fu;
            }
        }
    }

    // 最大反復回数に達した場合
    return OptimizationResult1D<T>::makeFailure(x, fx, options.max_iterations);
}

} // namespace sangi

#endif // SANGI_OPTIMIZATION_1D_IMPL_HPP