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

// optimization_nd_impl.hpp — optimization_nd.hpp の実装
#ifndef SANGI_OPTIMIZATION_ND_IMPL_HPP
#define SANGI_OPTIMIZATION_ND_IMPL_HPP

#include "optimization_nd.hpp"

namespace sangi {

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
std::tuple<T, V, T, bool> backtracking_line_search(
    const std::function<T(const V&)>& f,
    const V& x,
    const V& direction,
    T fx,
    const V& gradient,
    const LineSearchParams<T>& params) {

    T alpha = params.alpha_init;
    T dot_grad_dir = dot(gradient, direction);

    for (size_t i = 0; i < params.max_iterations; ++i) {
        V x_new = x + alpha * direction;
        T fx_new = f(x_new);

        if (fx_new <= fx + params.armijo_const * alpha * dot_grad_dir) {
            return {alpha, x_new, fx_new, true};
        }

        alpha *= params.reduction_factor;
    }

    return {alpha, x + alpha * direction, f(x + alpha * direction), false};
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> steepest_descent(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const V& x0,
    const OptimizationOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    V x = x0;
    T fx = f(x);
    V gradient = df(x);
    T gradient_norm = norm2(gradient);

    if (gradient_norm < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    for (size_t iter = 0; iter < options.max_iterations; ++iter) {
        V direction = -gradient;

        auto [alpha, x_new, fx_new, line_search_success] =
            backtracking_line_search(f, x, direction, fx, gradient, line_search_params);

        if (!line_search_success) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        x = x_new;
        fx = fx_new;
        gradient = df(x);
        gradient_norm = norm2(gradient);

        if (gradient_norm < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V, typename M>
    requires concepts::VectorOf<V, T> && concepts::MatrixOf<M, T>
OptimizationResult<V, T> bfgs_minimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const V& x0,
    const OptimizationOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    V x = x0;
    T fx = f(x);
    V gradient = df(x);
    T gradient_norm = norm2(gradient);

    if (gradient_norm < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    M H = eye<T>(n);

    for (size_t iter = 0; iter < options.max_iterations; ++iter) {
        V direction = -(H * gradient);

        auto [alpha, x_new, fx_new, line_search_success] =
            backtracking_line_search(f, x, direction, fx, gradient, line_search_params);

        if (!line_search_success) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        V s = alpha * direction;
        V gradient_new = df(x_new);
        V y = gradient_new - gradient;

        T s_dot_y = dot(s, y);

        if (s_dot_y > std::numeric_limits<T>::epsilon()) {
            V Hy = H * y;
            T y_dot_Hy = dot(y, Hy);

            M term1 = outer_product<T, V, M>(s, s) * (T(1) + y_dot_Hy / s_dot_y) / s_dot_y;
            M term2 = (outer_product<T, V, M>(s, Hy) + outer_product<T, V, M>(Hy, s)) / s_dot_y;

            H = H + term1 - term2;
        }

        x = x_new;
        fx = fx_new;
        gradient = gradient_new;
        gradient_norm = norm2(gradient);

        if (gradient_norm < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> newton_cg_minimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const std::function<V(const V&, const V&)>& d2f,
    const V& x0,
    const OptimizationOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    V x = x0;
    T fx = f(x);
    V gradient = df(x);
    T gradient_norm = norm2(gradient);

    auto solve_cg_internal = [](
        const std::function<V(const V&)>& Ax,
        const V& b,
        size_t n,
        size_t max_iterations,
        T tolerance) -> V {

        V x(n);
        for (size_t i = 0; i < n; ++i) {
            x[i] = T(0);
        }

        V r = b;
        V p = r;
        T r_dot_r = dot(r, r);
        T r0_dot_r0 = r_dot_r;

        for (size_t iter = 0; iter < max_iterations; ++iter) {
            V Ap = Ax(p);
            T p_dot_Ap = dot(p, Ap);

            if (std::abs(p_dot_Ap) < std::numeric_limits<T>::epsilon()) {
                if (norm2(p) < tolerance) {
                    return x;
                }

                p = r;
                Ap = Ax(p);
                p_dot_Ap = dot(p, Ap);

                if (std::abs(p_dot_Ap) < std::numeric_limits<T>::epsilon()) {
                    return x;
                }
            }

            T alpha = r_dot_r / p_dot_Ap;

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

            T r_new_dot_r_new = dot(r, r);

            if (std::sqrt(r_new_dot_r_new) < tolerance * std::sqrt(r0_dot_r0)) {
                return x;
            }

            T beta = r_new_dot_r_new / r_dot_r;
            p = r + beta * p;
            r_dot_r = r_new_dot_r_new;
        }

        return x;
    };

    if (gradient_norm < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    for (size_t iter = 0; iter < options.max_iterations; ++iter) {
        V p = solve_cg_internal(
            [&](const V& v) { return d2f(x, v); },
            -gradient,
            x.size(),
            std::min<size_t>(50, x.size()),
            options.tolerance
        );

        auto [alpha, x_new, fx_new, line_search_success] =
            backtracking_line_search(f, x, p, fx, gradient, line_search_params);

        if (!line_search_success) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        x = x_new;
        fx = fx_new;
        gradient = df(x);
        gradient_norm = norm2(gradient);

        if (gradient_norm < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::Field T, typename V, typename M>
    requires concepts::VectorOf<V, T> && concepts::MatrixOf<M, T>
M outer_product(const V& a, const V& b) {
    std::size_t m = a.size();
    std::size_t n = b.size();

    M result(m, n);

    for (std::size_t i = 0; i < m; ++i) {
        for (std::size_t j = 0; j < n; ++j) {
            result(i, j) = a[i] * b[j];
        }
    }

    return result;
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> nelderMeadMinimize(
    const std::function<T(const V&)>& f,
    const V& x0,
    const NelderMeadOptions<T>& options) {

    std::size_t n = x0.size();

    std::vector<V> simplex(n + 1);
    std::vector<T> fvals(n + 1);

    simplex[0] = x0;
    fvals[0] = f(x0);

    for (std::size_t i = 0; i < n; ++i) {
        V vertex = x0;
        vertex[i] += options.initial_step;
        simplex[i + 1] = vertex;
        fvals[i + 1] = f(vertex);
    }

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        std::size_t idx_best = 0, idx_worst = 0, idx_second_worst = 0;
        for (std::size_t i = 1; i < n + 1; ++i) {
            if (fvals[i] < fvals[idx_best]) idx_best = i;
            if (fvals[i] > fvals[idx_worst]) idx_worst = i;
        }
        for (std::size_t i = 0; i < n + 1; ++i) {
            if (i != idx_worst) {
                if (idx_second_worst == idx_worst || fvals[i] > fvals[idx_second_worst]) {
                    idx_second_worst = i;
                }
            }
        }

        T f_spread = T(0);
        for (std::size_t i = 0; i < n + 1; ++i) {
            if (i != idx_best) {
                T diff = fvals[i] - fvals[idx_best];
                f_spread += diff * diff;
            }
        }

        T diameter = T(0);
        for (std::size_t i = 0; i < n + 1; ++i) {
            if (i != idx_best) {
                T dist_sq = T(0);
                for (std::size_t j = 0; j < n; ++j) {
                    T d = simplex[i][j] - simplex[idx_best][j];
                    dist_sq += d * d;
                }
                if (dist_sq > diameter) diameter = dist_sq;
            }
        }

        if (f_spread < options.tolerance * options.tolerance &&
            diameter < options.tolerance * options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(
                simplex[idx_best], fvals[idx_best], iter + 1);
        }

        V centroid(n);
        for (std::size_t j = 0; j < n; ++j) centroid[j] = T(0);
        for (std::size_t i = 0; i < n + 1; ++i) {
            if (i != idx_worst) {
                for (std::size_t j = 0; j < n; ++j) {
                    centroid[j] += simplex[i][j];
                }
            }
        }
        for (std::size_t j = 0; j < n; ++j) {
            centroid[j] /= static_cast<T>(n);
        }

        V xr(n);
        for (std::size_t j = 0; j < n; ++j) {
            xr[j] = centroid[j] + options.reflection * (centroid[j] - simplex[idx_worst][j]);
        }
        T fr = f(xr);

        if (fr < fvals[idx_second_worst] && fr >= fvals[idx_best]) {
            simplex[idx_worst] = xr;
            fvals[idx_worst] = fr;
        } else if (fr < fvals[idx_best]) {
            V xe(n);
            for (std::size_t j = 0; j < n; ++j) {
                xe[j] = centroid[j] + options.expansion * (xr[j] - centroid[j]);
            }
            T fe = f(xe);
            if (fe < fr) {
                simplex[idx_worst] = xe;
                fvals[idx_worst] = fe;
            } else {
                simplex[idx_worst] = xr;
                fvals[idx_worst] = fr;
            }
        } else {
            if (fr < fvals[idx_worst]) {
                V xc(n);
                for (std::size_t j = 0; j < n; ++j) {
                    xc[j] = centroid[j] + options.contraction * (xr[j] - centroid[j]);
                }
                T fc = f(xc);
                if (fc <= fr) {
                    simplex[idx_worst] = xc;
                    fvals[idx_worst] = fc;
                } else {
                    goto shrink;
                }
            } else {
                V xc(n);
                for (std::size_t j = 0; j < n; ++j) {
                    xc[j] = centroid[j] + options.contraction * (simplex[idx_worst][j] - centroid[j]);
                }
                T fc = f(xc);
                if (fc < fvals[idx_worst]) {
                    simplex[idx_worst] = xc;
                    fvals[idx_worst] = fc;
                } else {
                    goto shrink;
                }
            }
            continue;

            shrink:
            for (std::size_t i = 0; i < n + 1; ++i) {
                if (i != idx_best) {
                    for (std::size_t j = 0; j < n; ++j) {
                        simplex[i][j] = simplex[idx_best][j] +
                            options.shrink * (simplex[i][j] - simplex[idx_best][j]);
                    }
                    fvals[i] = f(simplex[i]);
                }
            }
        }
    }

    std::size_t idx_best = 0;
    for (std::size_t i = 1; i < n + 1; ++i) {
        if (fvals[i] < fvals[idx_best]) idx_best = i;
    }
    return OptimizationResult<V, T>::makeFailure(
        simplex[idx_best], fvals[idx_best], options.max_iterations);
}

template<concepts::OrderedField T, typename V, typename M>
    requires concepts::VectorOf<V, T> && concepts::MatrixOf<M, T>
OptimizationResult<V, T> gaussNewtonMinimize(
    const std::function<V(const V&)>& residuals,
    const std::function<M(const V&)>& jacobian,
    const V& x0,
    const GaussNewtonOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    V x = x0;
    V r = residuals(x);
    T fx = dot(r, r);

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        M J = jacobian(x);

        std::size_t m = J.cols();
        M JT(m, J.rows());
        for (std::size_t i = 0; i < m; ++i) {
            for (std::size_t j = 0; j < J.rows(); ++j) {
                JT(i, j) = J(j, i);
            }
        }

        M JTJ = JT * J;

        for (std::size_t i = 0; i < m; ++i) {
            JTJ(i, i) += options.damping;
        }

        Vector<T> JTr(m);
        for (std::size_t i = 0; i < m; ++i) {
            T sum = T(0);
            for (std::size_t j = 0; j < J.rows(); ++j) {
                sum += JT(i, j) * r[j];
            }
            JTr[i] = sum;
        }

        Vector<T> dX_vec;
        try {
            dX_vec = algorithms::solve(JTJ, JTr, algorithms::SolverType::LU);
        } catch (...) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        V dX(m);
        for (std::size_t i = 0; i < m; ++i) {
            dX[i] = dX_vec[i];
        }

        V x_new = x;
        T fx_new;

        if (options.use_line_search) {
            T alpha = T(1);
            for (std::size_t ls = 0; ls < line_search_params.max_iterations; ++ls) {
                V x_try(m);
                for (std::size_t j = 0; j < m; ++j) {
                    x_try[j] = x[j] - alpha * dX[j];
                }
                V r_try = residuals(x_try);
                T fx_try = dot(r_try, r_try);
                if (fx_try < fx) {
                    x_new = x_try;
                    fx_new = fx_try;
                    break;
                }
                alpha *= line_search_params.reduction_factor;
                if (ls == line_search_params.max_iterations - 1) {
                    for (std::size_t j = 0; j < m; ++j) {
                        x_new[j] = x[j] - dX[j];
                    }
                    V r_new = residuals(x_new);
                    fx_new = dot(r_new, r_new);
                }
            }
        } else {
            for (std::size_t j = 0; j < m; ++j) {
                x_new[j] = x[j] - dX[j];
            }
            V r_new = residuals(x_new);
            fx_new = dot(r_new, r_new);
        }

        T step_norm_sq = T(0);
        for (std::size_t j = 0; j < m; ++j) {
            T d = x_new[j] - x[j];
            step_norm_sq += d * d;
        }

        x = x_new;
        r = residuals(x);
        fx = dot(r, r);

        if (step_norm_sq < options.tolerance * options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> conjugateGradientMinimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const V& x0,
    const ConjugateGradientOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    std::size_t restart = (options.restart_interval > 0) ? options.restart_interval : n;

    V x = x0;
    T fx = f(x);
    V g = df(x);
    T g_norm_sq = dot(g, g);

    if (std::sqrt(g_norm_sq) < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    V p = -g;

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        auto [alpha, x_new, fx_new, ls_success] =
            backtracking_line_search(f, x, p, fx, g, line_search_params);

        if (!ls_success) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        V g_new = df(x_new);
        T g_new_norm_sq = dot(g_new, g_new);

        if (std::sqrt(g_new_norm_sq) < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x_new, fx_new, iter + 1);
        }

        T beta;
        if (g_norm_sq < std::numeric_limits<T>::epsilon()) {
            beta = T(0);
        } else if (options.use_polak_ribiere) {
            V g_diff = g_new - g;
            beta = dot(g_new, g_diff) / g_norm_sq;
            if (beta < T(0)) beta = T(0);
        } else {
            beta = g_new_norm_sq / g_norm_sq;
        }

        if ((iter + 1) % restart == 0) {
            beta = T(0);
        }

        p = -g_new + beta * p;

        x = x_new;
        fx = fx_new;
        g = g_new;
        g_norm_sq = g_new_norm_sq;
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> lbfgsMinimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const V& x0,
    const LBFGSOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    std::size_t m = options.memory_size;

    V x = x0;
    T fx = f(x);
    V g = df(x);

    if (norm2(g) < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    std::vector<V> s_history, y_history;
    std::vector<T> rho_history;
    s_history.reserve(m);
    y_history.reserve(m);
    rho_history.reserve(m);

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        V q = g;
        std::size_t hist_size = s_history.size();
        std::vector<T> alpha_vec(hist_size);

        for (std::size_t i = hist_size; i > 0; --i) {
            std::size_t idx = i - 1;
            alpha_vec[idx] = rho_history[idx] * dot(s_history[idx], q);
            q = q - alpha_vec[idx] * y_history[idx];
        }

        V r(n);
        if (hist_size > 0) {
            T sy = dot(s_history[hist_size - 1], y_history[hist_size - 1]);
            T yy = dot(y_history[hist_size - 1], y_history[hist_size - 1]);
            T gamma = (yy > std::numeric_limits<T>::epsilon()) ? sy / yy : T(1);
            r = gamma * q;
        } else {
            r = q;
        }

        for (std::size_t i = 0; i < hist_size; ++i) {
            T beta_val = rho_history[i] * dot(y_history[i], r);
            r = r + (alpha_vec[i] - beta_val) * s_history[i];
        }

        V direction = -r;

        auto [alpha, x_new, fx_new, ls_success] =
            backtracking_line_search(f, x, direction, fx, g, line_search_params);

        if (!ls_success) {
            return OptimizationResult<V, T>::makeFailure(x, fx, iter);
        }

        V g_new = df(x_new);

        V s = x_new - x;
        V y = g_new - g;
        T sy = dot(s, y);

        if (sy > std::numeric_limits<T>::epsilon()) {
            T rho = T(1) / sy;

            if (s_history.size() == m) {
                s_history.erase(s_history.begin());
                y_history.erase(y_history.begin());
                rho_history.erase(rho_history.begin());
            }
            s_history.push_back(std::move(s));
            y_history.push_back(std::move(y));
            rho_history.push_back(rho);
        }

        x = x_new;
        fx = fx_new;
        g = g_new;

        if (norm2(g) < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> lbfgsbMinimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const V& x0,
    const V& lower,
    const V& upper,
    const LBFGSBOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    std::size_t m = options.memory_size;
    T inf_val = std::numeric_limits<T>::max();

    auto project = [&](const V& v) -> V {
        V result(n);
        for (std::size_t i = 0; i < n; ++i) {
            result[i] = v[i];
            if (lower[i] > -inf_val && result[i] < lower[i]) result[i] = lower[i];
            if (upper[i] < inf_val && result[i] > upper[i]) result[i] = upper[i];
        }
        return result;
    };

    auto projectedGradient = [&](const V& x, const V& g) -> V {
        V pg(n);
        for (std::size_t i = 0; i < n; ++i) {
            if (x[i] <= lower[i] + std::numeric_limits<T>::epsilon() && g[i] > T(0)) {
                pg[i] = T(0);
            } else if (x[i] >= upper[i] - std::numeric_limits<T>::epsilon() && g[i] < T(0)) {
                pg[i] = T(0);
            } else {
                pg[i] = g[i];
            }
        }
        return pg;
    };

    V x = project(x0);
    T fx = f(x);
    V g = df(x);

    V pg = projectedGradient(x, g);
    if (norm2(pg) < options.tolerance) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, 0);
    }

    std::vector<V> s_history, y_history;
    std::vector<T> rho_history;
    s_history.reserve(m);
    y_history.reserve(m);
    rho_history.reserve(m);

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        V q = g;
        std::size_t hist_size = s_history.size();
        std::vector<T> alpha_vec(hist_size);

        for (std::size_t i = hist_size; i > 0; --i) {
            std::size_t idx = i - 1;
            alpha_vec[idx] = rho_history[idx] * dot(s_history[idx], q);
            q = q - alpha_vec[idx] * y_history[idx];
        }

        V r(n);
        if (hist_size > 0) {
            T sy = dot(s_history[hist_size - 1], y_history[hist_size - 1]);
            T yy = dot(y_history[hist_size - 1], y_history[hist_size - 1]);
            T gamma = (yy > std::numeric_limits<T>::epsilon()) ? sy / yy : T(1);
            r = gamma * q;
        } else {
            r = q;
        }

        for (std::size_t i = 0; i < hist_size; ++i) {
            T beta_val = rho_history[i] * dot(y_history[i], r);
            r = r + (alpha_vec[i] - beta_val) * s_history[i];
        }

        V direction = -r;

        T alpha = line_search_params.alpha_init;
        V x_new = x;
        T fx_new = fx;
        bool ls_found = false;

        for (std::size_t ls = 0; ls < line_search_params.max_iterations; ++ls) {
            V x_try = project(x + alpha * direction);
            T fx_try = f(x_try);

            V step = x_try - x;
            T directional = dot(g, step);
            if (fx_try <= fx + line_search_params.armijo_const * directional) {
                x_new = x_try;
                fx_new = fx_try;
                ls_found = true;
                break;
            }
            alpha *= line_search_params.reduction_factor;
        }

        if (!ls_found) {
            x_new = project(x + alpha * direction);
            fx_new = f(x_new);
            if (fx_new >= fx) {
                return OptimizationResult<V, T>::makeFailure(x, fx, iter);
            }
        }

        V g_new = df(x_new);

        V s = x_new - x;
        V y = g_new - g;
        T sy = dot(s, y);

        if (sy > std::numeric_limits<T>::epsilon()) {
            if (s_history.size() == m) {
                s_history.erase(s_history.begin());
                y_history.erase(y_history.begin());
                rho_history.erase(rho_history.begin());
            }
            s_history.push_back(std::move(s));
            y_history.push_back(std::move(y));
            rho_history.push_back(T(1) / sy);
        }

        x = x_new;
        fx = fx_new;
        g = g_new;

        pg = projectedGradient(x, g);
        if (norm2(pg) < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> penaltyMinimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const std::vector<std::function<T(const V&)>>& constraints,
    const V& x0,
    const ConstrainedOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    T mu = options.initial_penalty;
    V x = x0;
    T fx = f(x);
    T h = std::sqrt(std::numeric_limits<T>::epsilon());

    for (std::size_t outer = 0; outer < options.max_iterations; ++outer) {
        T current_mu = mu;

        auto penalized_f = [&](const V& v) -> T {
            T val = f(v);
            for (auto& ci : constraints) {
                T cv = ci(v);
                if (cv > T(0)) {
                    val += current_mu * cv * cv;
                }
            }
            return val;
        };

        auto penalized_df = [&](const V& v) -> V {
            V grad = df(v);
            for (auto& ci : constraints) {
                T cv = ci(v);
                if (cv > T(0)) {
                    for (std::size_t j = 0; j < n; ++j) {
                        V v_plus = v;
                        v_plus[j] += h;
                        T cv_plus = ci(v_plus);
                        T dc = (cv_plus - cv) / h;
                        grad[j] += current_mu * T(2) * cv * dc;
                    }
                }
            }
            return grad;
        };

        LBFGSOptions<T> inner_opts;
        inner_opts.max_iterations = options.inner_max_iterations;
        inner_opts.tolerance = options.tolerance;

        auto result = lbfgsMinimize<T, V>(penalized_f, penalized_df, x, inner_opts, line_search_params);
        x = result.point;
        fx = f(x);

        T max_violation = T(0);
        for (auto& ci : constraints) {
            T cv = ci(x);
            if (cv > max_violation) max_violation = cv;
        }

        if (max_violation < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, fx, outer + 1);
        }

        mu *= options.penalty_growth;
    }

    return OptimizationResult<V, T>::makeFailure(x, fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> augmentedLagrangianMinimize(
    const std::function<T(const V&)>& f,
    const std::function<V(const V&)>& df,
    const std::vector<std::function<T(const V&)>>& eq_constraints,
    const std::vector<std::function<T(const V&)>>& ineq_constraints,
    const V& x0,
    const ConstrainedOptions<T>& options,
    const LineSearchParams<T>& line_search_params) {

    std::size_t n = x0.size();
    std::size_t n_eq = eq_constraints.size();
    std::size_t n_ineq = ineq_constraints.size();

    V x = x0;
    T mu = options.initial_penalty;
    T h = std::sqrt(std::numeric_limits<T>::epsilon());

    std::vector<T> lambda(n_eq, T(0));
    std::vector<T> nu(n_ineq, T(0));

    for (std::size_t outer = 0; outer < options.max_iterations; ++outer) {
        T current_mu = mu;
        std::vector<T> current_lambda = lambda;
        std::vector<T> current_nu = nu;

        auto aug_f = [&](const V& v) -> T {
            T val = f(v);

            for (std::size_t i = 0; i < n_eq; ++i) {
                T hi = eq_constraints[i](v);
                val += current_lambda[i] * hi + (current_mu / T(2)) * hi * hi;
            }

            for (std::size_t i = 0; i < n_ineq; ++i) {
                T gi = ineq_constraints[i](v);
                T shifted = current_nu[i] / current_mu + gi;
                if (shifted > T(0)) {
                    val += (current_mu / T(2)) * shifted * shifted;
                }
            }

            return val;
        };

        auto aug_df = [&](const V& v) -> V {
            V grad = df(v);

            for (std::size_t i = 0; i < n_eq; ++i) {
                T hi = eq_constraints[i](v);
                T coeff = current_lambda[i] + current_mu * hi;
                for (std::size_t j = 0; j < n; ++j) {
                    V v_plus = v;
                    v_plus[j] += h;
                    T dhi = (eq_constraints[i](v_plus) - hi) / h;
                    grad[j] += coeff * dhi;
                }
            }

            for (std::size_t i = 0; i < n_ineq; ++i) {
                T gi = ineq_constraints[i](v);
                T shifted = current_nu[i] / current_mu + gi;
                if (shifted > T(0)) {
                    T coeff = current_mu * shifted;
                    for (std::size_t j = 0; j < n; ++j) {
                        V v_plus = v;
                        v_plus[j] += h;
                        T dgi = (ineq_constraints[i](v_plus) - gi) / h;
                        grad[j] += coeff * dgi;
                    }
                }
            }

            return grad;
        };

        LBFGSOptions<T> inner_opts;
        inner_opts.max_iterations = options.inner_max_iterations;
        inner_opts.tolerance = options.tolerance;

        auto result = lbfgsMinimize<T, V>(aug_f, aug_df, x, inner_opts, line_search_params);
        x = result.point;

        T max_violation = T(0);

        for (std::size_t i = 0; i < n_eq; ++i) {
            T hi = eq_constraints[i](x);
            lambda[i] += mu * hi;
            T viol = std::abs(hi);
            if (viol > max_violation) max_violation = viol;
        }

        for (std::size_t i = 0; i < n_ineq; ++i) {
            T gi = ineq_constraints[i](x);
            nu[i] = std::max(T(0), nu[i] + mu * gi);
            T viol = std::max(T(0), gi);
            if (viol > max_violation) max_violation = viol;
        }

        if (max_violation < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(x, f(x), outer + 1);
        }

        mu *= options.penalty_growth;
    }

    return OptimizationResult<V, T>::makeFailure(x, f(x), options.max_iterations);
}


template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> hookeJeevesMinimize(
    const std::function<T(const V&)>& f,
    const V& x0,
    const HookeJeevesOptions<T>& options) {

    std::size_t n = x0.size();
    T step_grow = std::sqrt(T(2));
    T step_shrink = T(1) / step_grow;

    std::vector<T> dx(n, options.initial_step);
    for (auto& d : dx) {
        if (d <= options.min_step) d = options.min_step * T(2);
    }

    auto isConverged = [&]() -> bool {
        for (std::size_t i = 0; i < n; ++i) {
            if (dx[i] > options.min_step) return false;
        }
        return true;
    };

    V x = x0;
    T fx = f(x);
    std::size_t count = 1;

    auto explore = [&]() -> bool {
        bool found = false;
        for (std::size_t i = 0; i < n; ++i) {
            T x_orig = x[i];

            x[i] = x_orig - dx[i];
            T f_try = f(x);
            ++count;

            if (f_try < fx) {
                T diff = fx - f_try;
                fx = f_try;
                found = true;
                if (diff < options.tolerance) return true;
            } else {
                x[i] = x_orig + dx[i];
                f_try = f(x);
                ++count;

                if (f_try < fx) {
                    fx = f_try;
                    found = true;
                } else {
                    x[i] = x_orig;
                }
            }
        }
        return found;
    };

    while (!isConverged() && count < options.max_iterations) {
        V x_prev = x;

        while (true) {
            if (isConverged()) {
                return OptimizationResult<V, T>::makeSuccess(x, fx, count);
            }

            if (explore()) {
                break;
            } else {
                for (auto& d : dx) d *= step_shrink;
            }

            if (count >= options.max_iterations) {
                return OptimizationResult<V, T>::makeFailure(x, fx, count);
            }
        }

        if (!isConverged()) {
            while (true) {
                V x_try(n);
                for (std::size_t j = 0; j < n; ++j) {
                    x_try[j] = T(2) * x[j] - x_prev[j];
                }
                T f_try = f(x_try);
                ++count;

                if (f_try < fx) {
                    x_prev = x;
                    x = x_try;
                    fx = f_try;
                    for (auto& d : dx) d *= step_grow;
                } else {
                    for (auto& d : dx) d *= step_shrink;
                    break;
                }

                if (count >= options.max_iterations) break;
            }
        }
    }

    if (isConverged()) {
        return OptimizationResult<V, T>::makeSuccess(x, fx, count);
    }
    return OptimizationResult<V, T>::makeFailure(x, fx, count);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> geneticAlgorithmMinimize(
    const std::function<T(const V&)>& f,
    const V& lower,
    const V& upper,
    const GeneticAlgorithmOptions<T>& options) {

    std::size_t n = lower.size();
    std::size_t pop_size = options.population_size;
    std::mt19937 rng(options.seed);
    std::uniform_real_distribution<double> uniform01(0.0, 1.0);

    struct Individual {
        V point;
        T fitness;
    };

    auto randomIndividual = [&]() -> Individual {
        V p(n);
        for (std::size_t j = 0; j < n; ++j) {
            p[j] = lower[j] + static_cast<T>(uniform01(rng)) * (upper[j] - lower[j]);
        }
        return { p, f(p) };
    };

    auto clamp = [&](V& p) {
        for (std::size_t j = 0; j < n; ++j) {
            if (p[j] < lower[j]) p[j] = lower[j];
            if (p[j] > upper[j]) p[j] = upper[j];
        }
    };

    std::vector<Individual> population(pop_size);
    for (std::size_t i = 0; i < pop_size; ++i) {
        population[i] = randomIndividual();
    }

    auto findBest = [&]() -> std::size_t {
        std::size_t best = 0;
        for (std::size_t i = 1; i < population.size(); ++i) {
            if (population[i].fitness < population[best].fitness) best = i;
        }
        return best;
    };

    auto tournament = [&]() -> std::size_t {
        std::size_t best = static_cast<std::size_t>(uniform01(rng) * pop_size) % pop_size;
        for (std::size_t k = 1; k < options.tournament_size; ++k) {
            std::size_t idx = static_cast<std::size_t>(uniform01(rng) * pop_size) % pop_size;
            if (population[idx].fitness < population[best].fitness) best = idx;
        }
        return best;
    };

    std::size_t best_idx = findBest();
    T best_fitness = population[best_idx].fitness;

    for (std::size_t gen = 0; gen < options.max_generations; ++gen) {
        std::sort(population.begin(), population.end(),
            [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; });

        std::vector<Individual> new_pop;
        new_pop.reserve(pop_size);

        for (std::size_t i = 0; i < options.elite_count && i < pop_size; ++i) {
            new_pop.push_back(population[i]);
        }

        while (new_pop.size() < pop_size) {
            if (uniform01(rng) < static_cast<double>(options.crossover_rate)) {
                std::size_t p1 = tournament();
                std::size_t p2 = tournament();
                V child(n);
                for (std::size_t j = 0; j < n; ++j) {
                    T lo = population[p1].point[j];
                    T hi = population[p2].point[j];
                    if (lo > hi) { T tmp = lo; lo = hi; hi = tmp; }
                    T range = hi - lo;
                    T alpha = T(0.5);
                    T min_val = lo - alpha * range;
                    T max_val = hi + alpha * range;
                    child[j] = min_val + static_cast<T>(uniform01(rng)) * (max_val - min_val);
                }
                clamp(child);
                new_pop.push_back({ child, f(child) });
            } else {
                std::size_t idx = tournament();
                new_pop.push_back(population[idx]);
            }
        }

        std::normal_distribution<double> gaussian(0.0, 1.0);
        for (std::size_t i = options.elite_count; i < new_pop.size(); ++i) {
            bool mutated = false;
            for (std::size_t j = 0; j < n; ++j) {
                if (uniform01(rng) < static_cast<double>(options.mutation_rate)) {
                    T range = upper[j] - lower[j];
                    new_pop[i].point[j] += static_cast<T>(gaussian(rng))
                                         * options.mutation_scale * range;
                    mutated = true;
                }
            }
            if (mutated) {
                clamp(new_pop[i].point);
                new_pop[i].fitness = f(new_pop[i].point);
            }
        }

        population = std::move(new_pop);

        best_idx = findBest();
        T new_best = population[best_idx].fitness;
        if (gen > 0) {
            T diff = best_fitness - new_best;
            if (diff < T(0)) diff = -diff;
            if (diff < options.tolerance && new_best <= best_fitness) {
                return OptimizationResult<V, T>::makeSuccess(
                    population[best_idx].point, new_best, gen + 1);
            }
        }
        best_fitness = (new_best < best_fitness) ? new_best : best_fitness;
    }

    best_idx = findBest();
    return OptimizationResult<V, T>::makeFailure(
        population[best_idx].point, population[best_idx].fitness,
        options.max_generations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> simulatedAnnealingMinimize(
    const std::function<T(const V&)>& f,
    const V& lower, const V& upper,
    const SimulatedAnnealingOptions<T>& options) {

    std::size_t n = lower.size();
    std::mt19937 rng(options.seed);
    std::uniform_real_distribution<T> uniform01(T(0), T(1));
    std::normal_distribution<T> normal(T(0), T(1));

    V x(n);
    for (std::size_t i = 0; i < n; ++i) {
        x[i] = (lower[i] + upper[i]) / T(2);
    }
    T fx = f(x);

    V best = x;
    T best_fx = fx;
    T temp = options.initial_temperature;

    auto clamp = [&](V& v) {
        for (std::size_t i = 0; i < n; ++i) {
            if (v[i] < lower[i]) v[i] = lower[i];
            if (v[i] > upper[i]) v[i] = upper[i];
        }
    };

    for (std::size_t iter = 0; iter < options.max_iterations; ++iter) {
        V candidate(n);
        for (std::size_t i = 0; i < n; ++i) {
            T scale = options.neighbor_scale * (upper[i] - lower[i]) * std::sqrt(temp / options.initial_temperature);
            candidate[i] = x[i] + scale * normal(rng);
        }
        clamp(candidate);

        T fc = f(candidate);
        T delta = fc - fx;

        if (delta < T(0) || uniform01(rng) < std::exp(-delta / temp)) {
            x = candidate;
            fx = fc;
        }

        if (fx < best_fx) {
            best = x;
            best_fx = fx;
        }

        temp *= options.cooling_rate;

        if (temp < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(best, best_fx, iter + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(best, best_fx, options.max_iterations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> differentialEvolutionMinimize(
    const std::function<T(const V&)>& f,
    const V& lower, const V& upper,
    const DifferentialEvolutionOptions<T>& options) {

    std::size_t n = lower.size();
    std::size_t NP = options.population_size;
    std::mt19937 rng(options.seed);
    std::uniform_real_distribution<T> uniform01(T(0), T(1));

    struct Individual { V point; T fitness; };
    std::vector<Individual> population(NP);

    for (std::size_t i = 0; i < NP; ++i) {
        population[i].point.resize(n);
        for (std::size_t j = 0; j < n; ++j) {
            population[i].point[j] = lower[j] + uniform01(rng) * (upper[j] - lower[j]);
        }
        population[i].fitness = f(population[i].point);
    }

    auto findBest = [&]() -> std::size_t {
        std::size_t idx = 0;
        for (std::size_t i = 1; i < NP; ++i) {
            if (population[i].fitness < population[idx].fitness) idx = i;
        }
        return idx;
    };

    std::size_t best_idx = findBest();
    T best_fitness = population[best_idx].fitness;

    auto clamp = [&](V& v) {
        for (std::size_t i = 0; i < n; ++i) {
            if (v[i] < lower[i]) v[i] = lower[i];
            if (v[i] > upper[i]) v[i] = upper[i];
        }
    };

    for (std::size_t gen = 0; gen < options.max_generations; ++gen) {
        std::vector<Individual> new_pop = population;

        for (std::size_t i = 0; i < NP; ++i) {
            std::size_t r1, r2, r3;
            do { r1 = rng() % NP; } while (r1 == i);
            do { r2 = rng() % NP; } while (r2 == i || r2 == r1);
            do { r3 = rng() % NP; } while (r3 == i || r3 == r1 || r3 == r2);

            V mutant(n);
            for (std::size_t j = 0; j < n; ++j) {
                mutant[j] = population[r1].point[j] +
                    options.mutation_factor * (population[r2].point[j] - population[r3].point[j]);
            }

            V trial(n);
            std::size_t j_rand = rng() % n;
            for (std::size_t j = 0; j < n; ++j) {
                if (uniform01(rng) < options.crossover_rate || j == j_rand) {
                    trial[j] = mutant[j];
                } else {
                    trial[j] = population[i].point[j];
                }
            }
            clamp(trial);

            T trial_fitness = f(trial);
            if (trial_fitness <= population[i].fitness) {
                new_pop[i].point = trial;
                new_pop[i].fitness = trial_fitness;
            }
        }

        population = std::move(new_pop);

        best_idx = findBest();
        T new_best = population[best_idx].fitness;
        if (gen > 0) {
            T diff = best_fitness - new_best;
            if (diff < T(0)) diff = -diff;
            if (diff < options.tolerance && new_best <= best_fitness) {
                return OptimizationResult<V, T>::makeSuccess(
                    population[best_idx].point, new_best, gen + 1);
            }
        }
        best_fitness = (new_best < best_fitness) ? new_best : best_fitness;
    }

    best_idx = findBest();
    return OptimizationResult<V, T>::makeFailure(
        population[best_idx].point, population[best_idx].fitness,
        options.max_generations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> cmaesMinimize(
    const std::function<T(const V&)>& f,
    const V& lower, const V& upper,
    const CMAESOptions<T>& options) {

    const std::size_t n = lower.size();
    if (n == 0)
        throw std::invalid_argument("cmaesMinimize: dimension must be > 0");

    const std::size_t lambda = (options.population_size > 0)
        ? options.population_size
        : static_cast<std::size_t>(4 + std::floor(3.0 * std::log(static_cast<double>(n))));
    const std::size_t mu = lambda / 2;

    std::vector<double> weights(mu);
    double sum_w = 0.0;
    for (std::size_t i = 0; i < mu; ++i) {
        weights[i] = std::log(static_cast<double>(mu) + 0.5) - std::log(static_cast<double>(i + 1));
        sum_w += weights[i];
    }
    for (std::size_t i = 0; i < mu; ++i) weights[i] /= sum_w;

    double sum_w2 = 0.0;
    for (std::size_t i = 0; i < mu; ++i) sum_w2 += weights[i] * weights[i];
    double mu_eff = 1.0 / sum_w2;

    double dn = static_cast<double>(n);

    double c_sigma = (mu_eff + 2.0) / (dn + mu_eff + 5.0);
    double d_sigma = 1.0 + 2.0 * std::max(0.0, std::sqrt((mu_eff - 1.0) / (dn + 1.0)) - 1.0) + c_sigma;

    double c_c = (4.0 + mu_eff / dn) / (dn + 4.0 + 2.0 * mu_eff / dn);
    double c_1 = 2.0 / ((dn + 1.3) * (dn + 1.3) + mu_eff);
    double c_mu_cov = std::min(1.0 - c_1,
        2.0 * (mu_eff - 2.0 + 1.0 / mu_eff) / ((dn + 2.0) * (dn + 2.0) + mu_eff));

    double chi_n = std::sqrt(dn) * (1.0 - 1.0 / (4.0 * dn) + 1.0 / (21.0 * dn * dn));

    V m(n);
    for (std::size_t j = 0; j < n; ++j)
        m[j] = (lower[j] + upper[j]) / T(2);

    T max_range = upper[0] - lower[0];
    for (std::size_t j = 1; j < n; ++j) {
        T r = upper[j] - lower[j];
        if (r > max_range) max_range = r;
    }
    double sigma = static_cast<double>(options.initial_sigma) * static_cast<double>(max_range);

    Matrix<T> C = Matrix<T>::identity(n);
    Matrix<T> B = Matrix<T>::identity(n);
    std::vector<double> D_diag(n, 1.0);

    std::vector<double> p_sigma(n, 0.0);
    std::vector<double> p_c(n, 0.0);

    std::mt19937 rng(options.seed);
    std::normal_distribution<double> normal01(0.0, 1.0);

    std::size_t eigen_interval = std::max<std::size_t>(1,
        static_cast<std::size_t>(1.0 / (10.0 * dn * (c_1 + c_mu_cov))));
    std::size_t count_eval = 0;

    T best_ever_value = std::numeric_limits<T>::max();
    V best_ever_point(n);

    for (std::size_t gen = 0; gen < options.max_generations; ++gen) {
        struct Individual { V point; T fitness; };
        std::vector<Individual> offspring(lambda);

        for (std::size_t k = 0; k < lambda; ++k) {
            V x(n);
            std::vector<double> z(n);
            for (std::size_t j = 0; j < n; ++j) z[j] = normal01(rng);

            for (std::size_t i = 0; i < n; ++i) {
                double y_i = 0.0;
                for (std::size_t j = 0; j < n; ++j)
                    y_i += static_cast<double>(B(i, j)) * D_diag[j] * z[j];
                x[i] = m[i] + static_cast<T>(sigma * y_i);
            }

            for (std::size_t j = 0; j < n; ++j) {
                if (x[j] < lower[j]) x[j] = lower[j];
                if (x[j] > upper[j]) x[j] = upper[j];
            }

            T fx = f(x);
            offspring[k] = { std::move(x), fx };
            ++count_eval;
        }

        std::sort(offspring.begin(), offspring.end(),
            [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; });

        if (offspring[0].fitness < best_ever_value) {
            best_ever_value = offspring[0].fitness;
            best_ever_point = offspring[0].point;
        }

        V m_old = m;
        for (std::size_t j = 0; j < n; ++j) {
            T sum = T(0);
            for (std::size_t i = 0; i < mu; ++i)
                sum += static_cast<T>(weights[i]) * offspring[i].point[j];
            m[j] = sum;
        }

        std::vector<double> y_w(n);
        for (std::size_t j = 0; j < n; ++j)
            y_w[j] = static_cast<double>(m[j] - m_old[j]) / sigma;

        std::vector<double> Bty(n, 0.0);
        for (std::size_t i = 0; i < n; ++i)
            for (std::size_t j = 0; j < n; ++j)
                Bty[i] += static_cast<double>(B(j, i)) * y_w[j];

        for (std::size_t i = 0; i < n; ++i)
            Bty[i] /= std::max(D_diag[i], 1e-300);

        std::vector<double> invsqrtC_yw(n, 0.0);
        for (std::size_t i = 0; i < n; ++i)
            for (std::size_t j = 0; j < n; ++j)
                invsqrtC_yw[i] += static_cast<double>(B(i, j)) * Bty[j];

        double sqrt_cs = std::sqrt(c_sigma * (2.0 - c_sigma) * mu_eff);
        for (std::size_t j = 0; j < n; ++j)
            p_sigma[j] = (1.0 - c_sigma) * p_sigma[j] + sqrt_cs * invsqrtC_yw[j];

        double ps_norm = 0.0;
        for (std::size_t j = 0; j < n; ++j) ps_norm += p_sigma[j] * p_sigma[j];
        ps_norm = std::sqrt(ps_norm);

        sigma *= std::exp((c_sigma / d_sigma) * (ps_norm / chi_n - 1.0));
        sigma = std::max(sigma, 1e-300);
        sigma = std::min(sigma, 1e18);

        double gen_factor = 1.0 - std::pow(1.0 - c_sigma, 2.0 * static_cast<double>(gen + 1));
        double threshold = (1.4 + 2.0 / (dn + 1.0)) * chi_n;
        double h_sigma = (ps_norm / std::sqrt(gen_factor) < threshold) ? 1.0 : 0.0;

        double sqrt_cc = std::sqrt(c_c * (2.0 - c_c) * mu_eff);
        for (std::size_t j = 0; j < n; ++j)
            p_c[j] = (1.0 - c_c) * p_c[j] + h_sigma * sqrt_cc * y_w[j];

        double delta_h = (1.0 - h_sigma) * c_c * (2.0 - c_c);
        double coeff_old = 1.0 - c_1 - c_mu_cov + delta_h * c_1;

        for (std::size_t i = 0; i < n; ++i) {
            for (std::size_t j = 0; j <= i; ++j) {
                double rank1 = c_1 * p_c[i] * p_c[j];
                double rank_mu = 0.0;
                for (std::size_t k = 0; k < mu; ++k) {
                    double yi = (static_cast<double>(offspring[k].point[i]) - static_cast<double>(m_old[i])) / sigma;
                    double yj = (static_cast<double>(offspring[k].point[j]) - static_cast<double>(m_old[j])) / sigma;
                    rank_mu += weights[k] * yi * yj;
                }
                rank_mu *= c_mu_cov;

                double new_val = coeff_old * static_cast<double>(C(i, j)) + rank1 + rank_mu;
                C(i, j) = static_cast<T>(new_val);
                C(j, i) = C(i, j);
            }
        }

        if ((gen + 1) % eigen_interval == 0) {
            try {
                auto [eigenvalues, eigenvectors] = algorithms::symmetric_eigen(C);
                B = eigenvectors;
                for (std::size_t j = 0; j < n; ++j) {
                    double ev = static_cast<double>(eigenvalues[j]);
                    D_diag[j] = std::sqrt(std::max(ev, 1e-300));
                }
            } catch (...) {
                C = Matrix<T>::identity(n);
                B = Matrix<T>::identity(n);
                std::fill(D_diag.begin(), D_diag.end(), 1.0);
            }
        }

        double max_D = *std::max_element(D_diag.begin(), D_diag.end());
        double max_pc = 0.0;
        for (std::size_t j = 0; j < n; ++j)
            max_pc = std::max(max_pc, std::abs(p_c[j]));
        if (sigma * std::max(max_D, max_pc) < static_cast<double>(options.tolx)) {
            return OptimizationResult<V, T>::makeSuccess(
                best_ever_point, best_ever_value, gen + 1);
        }

        T f_range = offspring[lambda - 1].fitness - offspring[0].fitness;
        if (f_range < T(0)) f_range = -f_range;
        if (f_range < options.tolerance) {
            return OptimizationResult<V, T>::makeSuccess(
                best_ever_point, best_ever_value, gen + 1);
        }

        double min_D = *std::min_element(D_diag.begin(), D_diag.end());
        if (min_D > 0.0 && (max_D / min_D) * (max_D / min_D) > static_cast<double>(options.condition_limit)) {
            return OptimizationResult<V, T>::makeSuccess(
                best_ever_point, best_ever_value, gen + 1);
        }
    }

    return OptimizationResult<V, T>::makeFailure(
        best_ever_point, best_ever_value, options.max_generations);
}

template<concepts::OrderedField T, typename V>
    requires concepts::VectorOf<V, T>
OptimizationResult<V, T> jsoMinimize(
    const std::function<T(const V&)>& f,
    const V& lower, const V& upper,
    const JSOOptions<T>& options) {

    const std::size_t n = lower.size();
    if (n == 0)
        throw std::invalid_argument("jsoMinimize: dimension must be > 0");

    const std::size_t NP_init = (options.population_size > 0)
        ? options.population_size : std::max<std::size_t>(25 * n, 10);
    const std::size_t NP_min = std::max<std::size_t>(options.min_population_size, 4);
    const std::size_t max_fes = (options.max_evaluations > 0)
        ? options.max_evaluations : 10000 * n;
    const std::size_t H = std::max<std::size_t>(options.history_size, 1);
    const std::size_t archive_max = static_cast<std::size_t>(
        static_cast<double>(NP_init) * static_cast<double>(options.archive_rate));

    std::mt19937 rng(options.seed);
    std::uniform_real_distribution<double> uniform01(0.0, 1.0);
    std::normal_distribution<double> normal01(0.0, 1.0);

    auto cauchy_sample = [&](double location, double scale) -> double {
        double u;
        do { u = uniform01(rng); } while (u == 0.0 || u == 1.0);
        return location + scale * std::tan(std::numbers::pi * (u - 0.5));
    };

    auto clamp = [&](V& v) {
        for (std::size_t j = 0; j < n; ++j) {
            if (v[j] < lower[j]) v[j] = lower[j];
            if (v[j] > upper[j]) v[j] = upper[j];
        }
    };

    struct Individual { V point; T fitness; };
    std::vector<Individual> population(NP_init);
    for (std::size_t i = 0; i < NP_init; ++i) {
        V x(n);
        for (std::size_t j = 0; j < n; ++j)
            x[j] = lower[j] + static_cast<T>(uniform01(rng)) * (upper[j] - lower[j]);
        T fx = f(x);
        population[i] = { std::move(x), fx };
    }

    auto sortPop = [](std::vector<Individual>& pop) {
        std::sort(pop.begin(), pop.end(),
            [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; });
    };
    sortPop(population);
    std::size_t NP = NP_init;

    std::vector<double> M_F(H, 0.5);
    std::vector<double> M_CR(H, 0.8);
    std::size_t k = 0;

    std::vector<V> archive;
    archive.reserve(archive_max);

    std::size_t fes = NP_init;
    T best_fitness = population[0].fitness;
    std::size_t stagnation = 0;
    const std::size_t stag_limit = 20;

    while (fes < max_fes) {
        std::vector<double> S_F, S_CR;
        std::vector<double> delta_f;

        double progress = static_cast<double>(fes) / static_cast<double>(max_fes);

        double p = std::max(static_cast<double>(options.p_best_rate),
                            2.0 / static_cast<double>(NP));

        std::vector<Individual> new_pop = population;

        for (std::size_t i = 0; i < NP; ++i) {
            std::size_t r_idx = rng() % H;
            double mu_F = M_F[r_idx];
            double mu_CR = M_CR[r_idx];

            double F_i;
            do { F_i = cauchy_sample(mu_F, 0.1); } while (F_i <= 0.0);
            if (F_i > 1.0) F_i = 1.0;

            if (progress < 0.25 && F_i > 0.7) F_i = 0.7;
            else if (progress < 0.5 && F_i > 0.8) F_i = 0.8;

            double CR_i;
            if (mu_CR < 0.0) {
                CR_i = 0.0;
            } else {
                CR_i = mu_CR + 0.1 * normal01(rng);
                if (CR_i < 0.0) CR_i = 0.0;
                if (CR_i > 1.0) CR_i = 1.0;
            }

            std::size_t p_count = std::max<std::size_t>(1,
                static_cast<std::size_t>(p * static_cast<double>(NP)));
            std::size_t pbest_idx = rng() % p_count;

            std::size_t r1;
            do { r1 = rng() % NP; } while (r1 == i || r1 == pbest_idx);

            std::size_t total = NP + archive.size();
            std::size_t r2;
            do { r2 = rng() % total; } while (r2 == i || r2 == r1);

            const V& x_r2 = (r2 < NP) ? population[r2].point : archive[r2 - NP];

            V mutant(n);
            T F_t = static_cast<T>(F_i);
            for (std::size_t j = 0; j < n; ++j) {
                mutant[j] = population[i].point[j]
                    + F_t * (population[pbest_idx].point[j] - population[i].point[j])
                    + F_t * (population[r1].point[j] - x_r2[j]);
            }

            V trial(n);
            std::size_t j_rand = rng() % n;
            T CR_t = static_cast<T>(CR_i);
            for (std::size_t j = 0; j < n; ++j) {
                if (static_cast<T>(uniform01(rng)) < CR_t || j == j_rand)
                    trial[j] = mutant[j];
                else
                    trial[j] = population[i].point[j];
            }
            clamp(trial);

            T trial_fitness = f(trial);
            ++fes;

            if (trial_fitness <= population[i].fitness) {
                if (trial_fitness < population[i].fitness) {
                    archive.push_back(population[i].point);
                    S_F.push_back(F_i);
                    S_CR.push_back(CR_i);
                    delta_f.push_back(static_cast<double>(
                        population[i].fitness - trial_fitness));
                }
                new_pop[i] = { std::move(trial), trial_fitness };
            }

            if (fes >= max_fes) break;
        }

        population = std::move(new_pop);
        sortPop(population);

        if (!S_F.empty()) {
            double sum_delta = 0.0;
            for (double d : delta_f) sum_delta += d;

            double sum_wF = 0.0, sum_wF2 = 0.0;
            double sum_wCR = 0.0;
            for (std::size_t j = 0; j < S_F.size(); ++j) {
                double w = delta_f[j] / sum_delta;
                sum_wF  += w * S_F[j];
                sum_wF2 += w * S_F[j] * S_F[j];
                sum_wCR += w * S_CR[j];
            }

            M_F[k] = (sum_wF > 0.0) ? (sum_wF2 / sum_wF) : 0.5;
            M_CR[k] = sum_wCR;

            if (M_CR[k] < 0.0) M_CR[k] = -1.0;

            k = (k + 1) % H;
        }

        std::size_t NP_new = static_cast<std::size_t>(std::round(
            static_cast<double>(NP_init)
            + (static_cast<double>(NP_min) - static_cast<double>(NP_init))
              * static_cast<double>(fes) / static_cast<double>(max_fes)));
        if (NP_new < NP_min) NP_new = NP_min;

        if (NP_new < NP) {
            population.resize(NP_new);
            NP = NP_new;
        }

        while (archive.size() > archive_max) {
            std::size_t del = rng() % archive.size();
            archive.erase(archive.begin() + static_cast<std::ptrdiff_t>(del));
        }

        T new_best = population[0].fitness;
        T improvement = best_fitness - new_best;
        if (improvement > options.tolerance) {
            best_fitness = new_best;
            stagnation = 0;
        } else {
            ++stagnation;
            if (new_best < best_fitness) best_fitness = new_best;
            if (stagnation >= stag_limit) {
                return OptimizationResult<V, T>::makeSuccess(
                    population[0].point, best_fitness, fes);
            }
        }
    }

    return OptimizationResult<V, T>::makeFailure(
        population[0].point, population[0].fitness, fes);
}

template<concepts::OrderedField T>
LeastSquaresFitResult<T> leastSquaresFit(
    const std::function<Vector<T>(const Vector<T>&)>& residuals,
    const std::function<Matrix<T>(const Vector<T>&)>& jacobian,
    const Vector<T>& p0,
    const LeastSquaresOptions<T>& options) {

    const std::size_t n = p0.size();
    if (n == 0)
        throw std::invalid_argument("leastSquaresFit: parameter vector must not be empty");

    Vector<T> p = p0;
    Vector<T> r = residuals(p);
    const std::size_t m = r.size();

    if (m <= n)
        throw std::invalid_argument("leastSquaresFit: data points must exceed parameters");

    T cost = r.dot(r);
    T lambda = options.lambdaInit;
    bool converged = false;
    std::size_t iter = 0;

    for (; iter < options.maxIterations; ++iter) {
        Matrix<T> J = jacobian(p);
        Matrix<T> JT = J.transpose();
        Matrix<T> JTJ = JT * J;
        Vector<T> JTr = JT * r;

        T gradMax = T(0);
        for (std::size_t i = 0; i < n; ++i)
            gradMax = std::max(gradMax, std::abs(JTr[i]));
        if (gradMax < options.gtol) {
            converged = true;
            break;
        }

        Matrix<T> damped = JTJ;
        for (std::size_t i = 0; i < n; ++i)
            damped(i, i) += lambda;

        Vector<T> negJTr(n);
        for (std::size_t i = 0; i < n; ++i)
            negJTr[i] = -JTr[i];

        Vector<T> delta;
        try {
            delta = algorithms::solve(damped, negJTr, algorithms::SolverType::LU);
        } catch (...) {
            break;
        }

        Vector<T> p_new = p;
        p_new += delta;
        Vector<T> r_new = residuals(p_new);
        T cost_new = r_new.dot(r_new);

        T actual = cost - cost_new;
        T predicted = lambda * delta.dot(delta) - delta.dot(JTr);

        if (predicted > T(0) && actual > T(0)) {
            p = p_new;
            r = r_new;

            T relChange = actual / (cost + std::numeric_limits<T>::min());
            cost = cost_new;
            lambda *= options.lambdaDown;

            if (relChange < options.ftol) {
                converged = true;
                break;
            }

            T stepNorm = delta.dot(delta);
            T paramNorm = p.dot(p) + std::numeric_limits<T>::min();
            if (stepNorm / paramNorm < options.xtol * options.xtol) {
                converged = true;
                break;
            }
        } else {
            lambda *= options.lambdaUp;
        }
    }

    LeastSquaresFitResult<T> result;
    result.parameters = p;
    result.residuals = r;
    result.chiSquared = cost;
    result.iterations = iter;
    result.success = converged;
    result.dataPoints = m;
    result.numParameters = n;

    std::size_t dof = m - n;
    result.reducedChiSquared = cost / static_cast<T>(dof);

    Matrix<T> J_final = jacobian(p);
    Matrix<T> JTJ_final = J_final.transpose() * J_final;
    try {
        result.covariance = algorithms::lu_inverse(JTJ_final);
        T s2 = result.reducedChiSquared;
        for (std::size_t i = 0; i < n; ++i)
            for (std::size_t j = 0; j < n; ++j)
                result.covariance(i, j) *= s2;

        result.standardErrors = Vector<T>(n);
        for (std::size_t i = 0; i < n; ++i)
            result.standardErrors[i] = std::sqrt(std::abs(result.covariance(i, i)));
    } catch (...) {
        result.covariance = Matrix<T>(n, n, T(0));
        result.standardErrors = Vector<T>(n, T(0));
    }

    return result;
}

template<concepts::OrderedField T>
LeastSquaresFitResult<T> curveFit(
    const std::function<T(T, const Vector<T>&)>& model,
    const std::vector<T>& xdata,
    const std::vector<T>& ydata,
    const Vector<T>& p0,
    const LeastSquaresOptions<T>& options) {

    if (xdata.size() != ydata.size())
        throw std::invalid_argument("curveFit: xdata and ydata must have the same size");
    if (xdata.empty())
        throw std::invalid_argument("curveFit: data must not be empty");

    const std::size_t m = xdata.size();
    const std::size_t n = p0.size();

    auto res = [&](const Vector<T>& params) -> Vector<T> {
        Vector<T> r(m);
        for (std::size_t i = 0; i < m; ++i)
            r[i] = model(xdata[i], params) - ydata[i];
        return r;
    };

    auto jac = [&](const Vector<T>& params) -> Matrix<T> {
        Matrix<T> J(m, n);
        const T h = std::sqrt(std::numeric_limits<T>::epsilon());
        for (std::size_t j = 0; j < n; ++j) {
            Vector<T> pp = params, pm = params;
            T step = h * (std::abs(params[j]) + T(1));
            pp[j] += step;
            pm[j] -= step;
            for (std::size_t i = 0; i < m; ++i)
                J(i, j) = (model(xdata[i], pp) - model(xdata[i], pm)) / (T(2) * step);
        }
        return J;
    };

    return leastSquaresFit<T>(res, jac, p0, options);
}

} // namespace sangi

#endif // SANGI_OPTIMIZATION_ND_IMPL_HPP