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

// Konstruktor
CMyMatrix::CMyMatrix(int z, int s) {
    zeilen = z;
    spalten = s;
    matrix.resize(zeilen);
    for (int i = 0; i < zeilen; ++i)
        matrix[i].resize(spalten);
}

double CMyMatrix::getZeilen() {
    return zeilen;
}

double CMyMatrix::getSpalten() {
    return spalten;
}

void CMyMatrix::setWert(double wert, int zeile, int spalte) {
    matrix[zeile][spalte] = wert;
}

double CMyMatrix::getWert(int zeile, int spalte) {
    return matrix[zeile][spalte];
}

double& CMyMatrix::operator()(int zeile, int spalte) {
    return matrix[zeile][spalte];
}

CMyMatrix CMyMatrix::invers() {
    if (this->getZeilen() != 2 || this->getSpalten() != 2)
        throw std::runtime_error("Keine 2x2 Matrix");

    double a = this->getWert(0, 0);
    double b = this->getWert(1, 0);
    double c = this->getWert(0, 1);
    double d = this->getWert(1, 1);

    double determinate = a * d - b * c;

    if (determinate == 0)
        throw std::runtime_error("detA == 0");

    double kehrwertDeterminante = 1 / determinate;
    CMyMatrix inverse(2,2);

    inverse.setWert(kehrwertDeterminante * d, 0, 0);
    inverse.setWert(kehrwertDeterminante * -c, 0, 1);
    inverse.setWert(kehrwertDeterminante * -b, 1, 0);
    inverse.setWert(kehrwertDeterminante * a, 1, 1);

    return inverse;
}

// Matrix-Vektor Multiplikation
CMyVektor operator*(CMyMatrix A, CMyVektor x) {
    // M Spalten == V Zeilen
    if (A.getSpalten() != x.getDimension())
        throw std::runtime_error("Matrix(m x n) * Vektor(i) n != i");

    CMyVektor ergebnis(A.getZeilen());

    for (int i = 0; i < A.getZeilen(); i++) {                                            // Zeile
        for (int j = 0; j < A.getSpalten(); j++) // Spalte
            ergebnis[i] += A(i, j) * x[j];       // Aufsummierung
    }
    return ergebnis;
}

std::ostream& operator<<(std::ostream& os, CMyMatrix x) {

    for (int i = 0; i < x.getZeilen(); ++i) {
        os << "\t\t\t(";
        for (int j = 0; j < x.getSpalten(); ++j)
            j == x.getSpalten() - 1 ? os << std::fixed << std::setprecision(10) << x(i, j) : os << std::fixed << std::setprecision(10) << x(i, j) << "|";

        os << ")\n";
    }
    return os;
}

CMyMatrix jacobi(CMyVektor x, CMyVektor(*f)(CMyVektor)) {
    double const h = pow(10, -4);
    CMyVektor f_x = f(x);
    CMyVektor tmp{ x };

    CMyMatrix jacobiMatrix(f_x.getDimension(), x.getDimension());

    for (int i = 0; i < f_x.getDimension(); ++i) { // Zeile bzw. Teilfunktion f_i
        for (int j = 0; j < x.getDimension(); ++j) { // Spalte bzw. Variable x_j
            tmp[j] += h;
            jacobiMatrix(i, j) = (f(tmp)[i] - f_x[i]) / h;
            tmp[j] -= h;
        }
    }
    return jacobiMatrix;
};

void newtonverfahren(CMyVektor x, CMyVektor(*f)(CMyVektor)) {
    int schritt = 0;
    CMyVektor f_x = f(x);
    CMyMatrix jacobi_f_x = jacobi(x, f);
    CMyMatrix jacobiInvers = jacobi_f_x.invers();

    double funktionLaenge = f_x.length();

    CMyVektor dx = -1 * (jacobiInvers * f_x);

    while (schritt != 50 && funktionLaenge >= 1e-5) {
        std::cout << "Schritt " << schritt << ":"
        << "\n\t            𝐱 = " << x
        << "\n\t         𝑓(𝐱) = " << f_x << "\n"
        << "\n\t        ∂𝑓(𝐱) = \n " << jacobi_f_x
        << "\n\t      ∂𝑓(𝐱)⁻¹ = \n" << jacobiInvers
        << "\n\t           Δ𝐱 = " << dx
        << "\n\t     ||𝑓(𝐱)|| = " << funktionLaenge << std::endl << std::endl;

        schritt++;
        x = x + dx;
        f_x = f(x);
        jacobi_f_x = jacobi(x, f);
        jacobiInvers = jacobi_f_x.invers();
        dx = -1 * (jacobiInvers * f_x);
        funktionLaenge = f_x.length();

        if (schritt == 50) {
            std::cout << "Ende wegen Schrittanzahl = 50 bei";
            std::cout << "\n\t             x = " << x
                << "\n\t          𝑓(𝐱) = " << f_x
                << "\n\t      ||𝑓(𝐱)|| = " << funktionLaenge << std::endl;
        }
        else if (funktionLaenge < 1e-5) {
            std::cout << "Ende wegen ||𝑓(𝐱)||<1e-5 bei";
            std::cout << "\n\t             x = " << x
                << "\n\t          𝑓(𝐱) = " << f_x
                << "\n\t      ||𝑓(𝐱)|| = " << funktionLaenge << std::endl;
        }
    }
};