#include <iostream>    // notwendig zur Ausgabe
#include <vector>
#include "hdnum.hh"    // hdnum header

namespace hdnum {

  template<typename REAL>
  class SparseMatrix {

    struct MatrixEntry {
      int i;
      int j;
      REAL value;
    };

  public:

    void AddEntry (int i, int j, REAL value) {
      assert(i >= 0);
      assert(j >= 0);
      if (value != .0)
        entries.push_back(MatrixEntry{.i=i, .j=j, .value=value});
    }

    template<typename V>
    void mv (Vector<V>& y, const Vector<V>& x) {

      zero(y);

      for (MatrixEntry& matrix_entry : entries) {
        assert(y.size() > matrix_entry.i);
        assert(x.size() > matrix_entry.j);
        y[matrix_entry.i] += matrix_entry.value * x[matrix_entry.j];
      }
    }

  //private:
    std::vector<MatrixEntry> entries;
  };

}

// generic iterative updater function
typedef void iterativeUpdater(hdnum::DenseMatrix<double>& A,
                              hdnum::SparseMatrix<double>& A_s,
                              int N,
                              hdnum::Vector<double>& x_tmp,
                              hdnum::Vector<double>& b,
                              hdnum::Vector<double>& d,
                              hdnum::Vector<double>& x);

// solve iterative based on given method
void solveIterative(hdnum::DenseMatrix<double>& A,
                    hdnum::SparseMatrix<double>& A_s,
                    hdnum::Vector <double>& x,
                    hdnum::Vector<double>& b,
                    iterativeUpdater update) {
    int N = A.rowsize();
    // defekt vektor
    hdnum::Vector<double> d(N);
    // temporary variable
    hdnum::Vector<double> x_tmp(N);
    A_s.mv(x_tmp, x);
    d = b - x_tmp;
    double initialDefekt = d.two_norm();
    while (d.two_norm() > initialDefekt * 10e-4) {
        // run one iteration
        update(A, A_s, N, x_tmp, b, d, x);
    }
}

// richardson: W = 1/omega I => W^-1 = omega I
inline void richardson(hdnum::DenseMatrix<double>& A,
                       hdnum::SparseMatrix<double>& A_s,
                       int N,
                       hdnum::Vector<double>& x_tmp,
                       hdnum::Vector<double>& b,
                       hdnum::Vector<double>& d,
                       hdnum::Vector<double>& x) {
    A_s.mv(x_tmp, x);
    d = b - x_tmp;
    // x(k+1) = x(k) + omega * d(k)
    // hier omega = -0.01
    for (int i=0; i<N; i++) {
        x_tmp[i] = -0.5*d[i];
    }
    x += x_tmp;
}

// jacobi: W = D
inline void jacobi(hdnum::DenseMatrix<double>& A,
                   hdnum::SparseMatrix<double>& A_s,
                   int N,
                   hdnum::Vector<double>& x_tmp,
                   hdnum::Vector<double>& b,
                   hdnum::Vector<double>& d,
                   hdnum::Vector<double>& x) {
    A_s.mv(x_tmp, x);
    d = b - x_tmp;
    // jacobi iteration, teile durch Diagonal Elemente
    // x(k+1) = x(k) + D^-1 d(k)
    for (int i=0; i<N; i++) {
        x_tmp[i] = d[i]/A[i][i];
    }
    x += x_tmp;
}

// gauss seidel: W = L + D
inline void gaussSeidel(hdnum::DenseMatrix<double>& A,
                        hdnum::SparseMatrix<double>& A_s,
                        int N,
                        hdnum::Vector<double>& x_tmp,
                        hdnum::Vector<double>& b,
                        hdnum::Vector<double>& d,
                        hdnum::Vector <double>& x) {
    A_s.mv(x_tmp, x);
    d = b - x_tmp;
    x_tmp = d;
    // x(k+1) = x(k) + v(k)
    // W*v(k) = d(k)
    // W untere Dreicksmatrix, loese W*v(k) = d(k) durch Vorwaertseinsetzen
    for (int i = 0; i<N; i++) {
        x_tmp[i] = d[i];
        for (int j=i-1; 0<=j && j<i; j++) { // Elemente abseits der Hauptdiagonale von A sind 0
            x_tmp[i] -= x_tmp[j] * A[i][j];
        }
        x_tmp[i] /= A[i][i];
    }
    x += x_tmp;
}

void testApproximation(hdnum::DenseMatrix<double>& A,
                       hdnum::SparseMatrix<double>& A_s,
                       hdnum::Vector<double>& x,
                       hdnum::Vector<double>& b,
                       iterativeUpdater updater) {
    int N = A.rowsize();
    hdnum::Vector<double> y(N);
    // teste iteration mit gegebenem verfahren
    solveIterative(A,A_s,x,b,updater);
    // vergleiche mit LU Zerleger
    hdnum::linsolve(A,y,b);
    std::cout << "Relativer Fehler: " << (y - x).two_norm() / y.two_norm() << std::endl;
}

int main () {
    for(int n=4; n<=9; n++) {
        int N = pow(2,n);

        // Testmatrix aufsetzen
        hdnum::DenseMatrix<double> A(N,N,.0);
        hdnum::SparseMatrix<double> A_s;
        for (typename hdnum::DenseMatrix<double>::size_type i=0; i<A.rowsize(); ++i) {
            if (i > 0) {
                A[i][i-1] = 1.0;
                A_s.AddEntry(i, i-1, 1.0);
            }
            if (i + 1 < A.colsize()) {
                A[i][i+1] = 1.0;
                A_s.AddEntry(i, i+1, 1.0);
            }
            A[i][i] -= 2.0;
            A_s.AddEntry(i, i, -2.0);
        }

        // Rechte Seite und Lösungsvektor
        hdnum::Vector<double> x(N, 0.0);
        hdnum::Vector<double> b(N, 1.0);

        // Lösen Sie nun A*x=b iterativ

        // Pretty-printing fuer Vektoren
        x.scientific(false);
        x.width(15);

        // testApproximation(A,A_s,x,b,richardson);

        hdnum::Timer myTimer;
        solveIterative(A,A_s,x,b,gaussSeidel);
        //hdnum::linsolve(A,x,b);
        std::cout << N << " " << myTimer.elapsed() << std::endl;
    }
}