#include <vector>
#include <cmath>
#include <limits>
#include <algorithm>
#include "powell.h"

using Vec = std::vector<double>;

Vec operator+(const Vec& a, const Vec& b) {
    Vec r(a.size());
    for (size_t i = 0; i < a.size(); ++i) r[i] = a[i] + b[i];
    return r;
}

Vec operator-(const Vec& a, const Vec& b) {
    Vec r(a.size());
    for (size_t i = 0; i < a.size(); ++i) r[i] = a[i] - b[i];
    return r;
}

Vec operator*(double s, const Vec& a) {
    Vec r(a.size());
    for (size_t i = 0; i < a.size(); ++i) r[i] = s * a[i];
    return r;
}

double norm(const Vec& a) {
    double s = 0;
    for (double v : a) s += v * v;
    return std::sqrt(s);
}

double line_search(
    const Vec& x,
    const Vec& dir,
    function<double(Vec)> obj,
    double a,
    double b
) {
    const double gr = 0.666666;
    double c = b - gr * (b - a);
    double d = a + gr * (b - a);

    auto f = [&](double t) {
        return obj(x + t * dir);
    };

    for (int i = 0; i < 50; ++i) {
        if (f(c) < f(d))
            b = d;
        else
            a = c;
        c = b - gr * (b - a);
        d = a + gr * (b - a);
    }
    return 0.5 * (a + b);
}

Vec powell(
    Vec x,
    function<double(Vec)> obj,
    double tol,
    int max_iter
) {
    const int n = x.size();
    std::vector<Vec> dirs(n, Vec(n, 0.0));
    for (int i = 0; i < n; ++i)
        dirs[i][i] = 1.0;

    for (int iter = 0; iter < max_iter; ++iter) {
        Vec x_start = x;
        double f_start = obj(x);

        int best_dir = 0;
        double best_drop = 0.0;

        for (int i = 0; i < n; ++i) {
            double t = line_search(x, dirs[i], obj);
            Vec x_new = x + t * dirs[i];
            double f_new = obj(x_new);

            double drop = f_start - f_new;
            if (drop > best_drop) {
                best_drop = drop;
                best_dir = i;
            }

            x = x_new;
            f_start = f_new;
        }

        Vec new_dir = x - x_start;
        if (norm(new_dir) < tol)
            break;

        double t = line_search(x, new_dir, obj);
        x = x + t * new_dir;

        dirs[best_dir] = new_dir;

        if (norm(x - x_start) < tol)
            break;
    }
    return x;
}