#include "CMyVektor.h"
#include <math.h>
#include <iostream>

int CMyVektor::getDimension() {
    return dimension;
}

// Vektor Element bei index i
double& CMyVektor::operator[](int i) {
    return werte.at(i);
}

// Betrag des Vektors
double CMyVektor::length() {
    double l = 0.0;
    for (int i = 0; i < werte.size(); i++)
        l += pow(werte.at(i), 2);

    return(sqrt(l));
}

// Ausgabe im Vektorformat
std::ostream& operator<< (std::ostream& os, CMyVektor& vektor) {
    os << "(";
    for (int i = 0; i < vektor.getDimension(); i++) {
        os << vektor[i];
        if (i < vektor.getDimension() - 1) os << "|";
    }
    os << ")";

    return os;
}

/* Vektor Addition
 a1	+	b1	=	a1+b1
 a2	+	b2	=	a2+b2
 :		:		  :
 ai	+	bi	=	ai+bi
*/
CMyVektor operator+(CMyVektor a, CMyVektor b) {
    // Vektoren addierbar?
    if (a.getDimension() != b.getDimension())
        return a;

    CMyVektor sum(a.getDimension());

    for (int i = 0; i < a.getDimension(); i++)
        sum[i] = a[i] + b[i];

    return sum;
}

/* Skalare Multiplikation
 a1	*	s	=	v1
 a2	*	s	=	v2
 :		:		 :
 ai	*	s	=	vi
*/
CMyVektor operator*(double s, CMyVektor a) {
    CMyVektor vektor(a.getDimension());

    for (int i = 0; i < a.getDimension(); i++)
        vektor[i] = s * a[i];

    return vektor;
}

CMyVektor gradient(double f(CMyVektor x), CMyVektor x) {
    CMyVektor grad(x.getDimension());
    CMyVektor xh(x.getDimension()); 	// x mit "Wackelei" an x_i
    xh = x;

    double fx = f(x); 
    double h = 1e-8;

    // Berechnung der partiellen Ableitung nach x_i und Zusammenfassung in grad
    for (int i = 0; i < x.getDimension(); i++) {
        //  xh[0]	            xh[1]              ...  xh[n]  
        //  x_1+h,x_2,...,x_n   x_1,x_2+h,...,x_n       x_1,x_2,...,x_n+1
        xh[i] += h;

        // grad[i] = df/dx_i => numerische Ableitung nach dem jeweilige x_i
        grad[i] = (f(xh) - fx) / h;

        // Zurücksetzen der "Wackelei"
        xh[i] -= h;
    }
    return grad;
}

CMyVektor gradientenverfahren(double f(CMyVektor x), CMyVektor x, double lambda) {
    int schritt_zaehler = 0;
    CMyVektor gradfx(x.getDimension());
    CMyVektor x_neu(x.getDimension());
    gradfx = gradient(f, x);

    while (gradient(f, x).length() >= 1e-5 && schritt_zaehler < 25) {
        x_neu = x + lambda * gradfx;

        std::cout
            << "Schritt " << schritt_zaehler << ": " << std::endl
            << "               x = " << x << std::endl
            << "               λ = " << lambda << std::endl
            << "            f(x) = " << f(x) << std::endl
            << "       grad f(x) = " << gradfx << std::endl
            << "   ||grad f(x)|| = " << gradfx.length() << std::endl << std::endl
            << "           x_neu = " << x_neu << std::endl
            << "        f(x_neu) = " << f(x_neu) << std::endl << std::endl;

        // Halbierung
        if (f(x_neu) <= f(x)) {
            double lamda_test = lambda * 0.5;
            CMyVektor x_test = x + lamda_test * gradfx;

            std::cout
                << "     ↯ f(x) = " << f(x) << " ≥ f(x_neu) = " << f(x_neu) << std::endl << std::endl
                << "     ? Test mit halbierter Schrittweite (λ = " << lamda_test << "): " << std::endl
                << "          x_test = " << x_test << std::endl
                << "       f(x_test) = " << f(x_test) << std::endl << std::endl;

            if (f(x) < f(x_test))
                std::cout
                << "     ✓ f(x) = " << f(x) << " < f(x_test) = " << f(x_test) << std::endl
                << "     ! Übernehme Schrittweite." << std::endl << std::endl;

            // weiter Halbieren wenn f(x_test) <= f(x)
            while (f(x_test) <= f(x)) {
                std::cout
                    << "     ↯ f(x) = " << f(x) << " ≥ f(x_test) = " << f(x_test) << std::endl << std::endl;

                lamda_test *= 0.5;
                x_test = x + lamda_test * gradfx;

                std::cout
                    << "     ? Test mit halbierter Schrittweite (λ = " << lamda_test << "): " << std::endl
                    << "          x_test = " << x_test << std::endl
                    << "       f(x_test) = " << f(x_test) << std::endl << std::endl;

                // Ausgabe beim letzten Durchlauf der Schleife
                if (!(f(x_test) <= f(x)))
                    std::cout
                    << "     ✓ f(x) = " << f(x) << " < f(x_test) = " << f(x_test) << std::endl << std::endl
                    << "     ! Übernehme Schrittweite λ = " << lamda_test << std::endl << std::endl;
            }

            lambda = lamda_test;
            x_neu = x_test;
        }

        // Verdopplung
        else {
            double lamda_test = lambda * 2;
            CMyVektor x_test = x + lamda_test * gradfx;

            std::cout
                << "     ? Test mit doppelter Schrittweite (λ = " << lamda_test << "): " << std::endl
                << "          x_test = " << x_test << std::endl
                << "       f(x_test) = " << f(x_test) << std::endl << std::endl;

            if (f(x_test) > f(x_neu)) {
                x_neu = x_test;
                lambda = lamda_test;
                std::cout
                    << "     ! Übernehme verdoppelte Schrittweite λ = " << lamda_test << std::endl
                    << "     ✓ f(x_neu) = " << f(x_neu) << " < f(x_test) = " << f(x_test) << std::endl << std::endl;
            }
            else {
                std::cout
                    << "     ↯ f(x_neu) = " << f(x_neu) << " ≥ f(x_test) = " << f(x_test) << std::endl
                    << "     ! behalte aktuelle Schrittweite λ = " << lambda << std::endl << std::endl;
            }
        }
        std::cout << std::endl;

        x = x_neu;
        gradfx = gradient(f, x);
        schritt_zaehler++;
    }

    // Zusammenfassung des Endes 
    if (gradfx.length() < 1e-5)
        std::cout
        << "Ende wegen ||grad f(x)|| < 10^-5 bei" << std::endl;

    else
        std::cout
        << "Ende wegen 25. Schritt" << std::endl;

    std::cout
        << "               x = " << x << std::endl
        << "               λ = " << lambda << std::endl
        << "            f(x) = " << f(x) << std::endl
        << "       grad f(x) = " << gradfx << std::endl
        << "   ||grad f(x)|| = " << gradfx.length() << std::endl
        << "-----------------------------------------------------" << std::endl << std::endl;

    return x_neu;
}