#include"opt_alg.h"

solution golden(double a, double b, double epsilon, int Nmax, matrix P)
{
    // złoty podział
    double alfa = (sqrt(5.0) - 1) / 2;
    solution A, B, C, D;
    A.x = a;
    B.x = b;
    C.x = B.x - alfa*(B.x - A.x);
    C.fit_fun(P);
    D.x = A.x + alfa*(B.x - A.x);
    D.fit_fun(P);
    while (true)
    {
        if (C.y < D.y)
        {
            B = D;
            D = C;
            C.x = B.x - alfa*(B.x - A.x);
            C.fit_fun(P);
        }
        else
        {
            A = C;
            C = D;
            D.x = A.x + alfa*(B.x - A.x);
            D.fit_fun(P);
        }
        if (B.x - A.x < epsilon || solution::f_calls > Nmax)
        {
            A.x = (A.x + B.x) / 2;
            A.fit_fun(P);
            return A;
        }
    }
}

solution najszybszy_spadek(matrix x0, double epsilon, int Nmax, double krok)
{
    int *n = get_size(x0);
    solution X, Xt;
    X.x = x0;
    matrix d(n[0], 1);
    matrix P(n[0], 2);
    solution h;
    double b;
    while (true)
    {
        // najpierw liczenie gradientu
        X.grad();
        d = -X.g;


        if (krok == 0.0) {
            // zmiennokrokowa
            P = set_col(P, X.x, 0);
            P = set_col(P, d, 1);
            b = policz_b(X.x, d);
            h = golden(0, b, epsilon, Nmax, P);
        }
        else {
            // stałokrokowa
            // h = 0.005
            //h = 0.012;
            h = krok;
        }

        Xt.x = X.x + h.x *d;
        if (norm(X.x - Xt.x) < epsilon || solution::f_calls > Nmax)
        {
            Xt.fit_fun();
            return Xt;
        }
        X = Xt;
    }
}

solution gradienty_sprzezone(matrix x0, double epsilon, int Nmax, double krok)
{
    int *n = get_size(x0);
    solution X, Xt;
    X.x = x0;
    matrix d(n[0], 1);
    matrix P(n[0], 2);
    solution h;
    double b, beta;
    // najpierw liczenie gradientu
    X.grad();
    d = -X.g;
    while (true)
    {
        if (krok == 0.0) {
            // zmiennokrokowa
            P = set_col(P, X.x, 0);
            P = set_col(P, d, 1);
            b = policz_b(X.x, d);
            h = golden(0, b, epsilon, Nmax, P);
        }
        else {
            // stałokrokowa
            // h = 0.005
            // h = 0.012;
            h = krok;
        }

        Xt.x = X.x + h.x *d;
        if (norm(X.x - Xt.x) < epsilon || solution::f_calls > Nmax)
        {
            Xt.fit_fun();
            return Xt;
        }
        Xt.grad();
        beta = pow(norm(Xt.g), 2) / pow(norm(X.g), 2);
        d = -Xt.g + beta*d;
        X = Xt;
    }
}

solution metoda_Newtona(matrix x0, double epsilon, int Nmax, double krok)
{
    int *n = get_size(x0);
    solution X, Xt;
    X.x = x0;
    matrix d(n[0], 1);
    matrix P(n[0], 2);
    solution h;
    double b;
    while (true)
    {
        // najpierw liczenie gradientu
        X.grad();
        X.hess();
        d = -inv(X.H) * X.g;

        if (krok == 0.0) {
            // zmiennokrokowa
            P = set_col(P, X.x, 0);
            P = set_col(P, d, 1);
            b = policz_b(X.x, d);
            h = golden(0, b, epsilon, Nmax, P);
        }
        else {
            // stałokrokowa
            // h = 0.005
            // h = 0.012
            h = krok;
        }

        Xt.x = X.x + h.x *d;
        if (norm(X.x - Xt.x) < epsilon || solution::f_calls > Nmax)
        {
            Xt.fit_fun();
            return Xt;
        }
        X = Xt;
    }
}

double policz_b(matrix x, matrix d)
{
    int *n = get_size(x);
    double b = 1e9; // duża liczba na początku
    double bi; // zawsze dodatnie
    for (int i = 0; i < n[0]; ++i)
    {
        if (d(i) == 0)
            bi = 1e9;
        else if (d(i) > 0)
            bi = (10 - x(i)) / d(i);
        else
            bi = (-10 - x(i)) / d(i);
        if (b > bi)
            b = bi;
    }
    // b na pewno jest w obszarze dopuszczalnym
    return b;
}