#include "vector.hpp"
#include "matrix.hpp"
#include <stdexcept>


Matrix computeR(Matrix A) {
    Matrix R = A;
    for(int i=0; i<R.size()[0];i++) {
        R(i,i)=0.0;
    }
    return R;
}


Vector computeInvD(const Matrix& A) {
    Vector invD(A.size()[0]);
    for(int i=0; i<invD.size();i++) {
        if(A(i,i)==0.0) {
            throw std::runtime_error("A matrix contains a 0 on diagonal");
        }
        invD(i)=1/A(i,i);
    }
    return invD;
}


Vector Jacobi(const Vector& B, const Matrix& R, const Vector& InvD) {
    int size = B.size();
    Vector k(size);
    Vector k_1(size);
    int iter=1;
    double errorNorm;

    // D⁻¹b
    Vector firstPartial(size);
    for(int i=0; i<size; i++) {
        firstPartial(i)=InvD(i)*B(i);
    }
    // D⁻¹R
    Matrix secondPartial(size,size);
    for(int i=0; i<size; i++) {
        for(int j=0; j<size; j++) {
            secondPartial(i,j)=InvD(i)*R(i,j);
        } 
    }

    do {
        k=k_1;
        // finalizing
        Vector finalSecondPartial(size);
        for(int i=0; i<size; i++) {
            for(int j=0; j<size; j++) {
                finalSecondPartial(i)+=secondPartial(i,j)*k(j);
            } 
        }
        Vector error(size);
        for(int i=0; i<size; i++) {
            k_1(i)=firstPartial(i)-finalSecondPartial(i);
            error(i)=k(i)-k_1(i);
        }
        iter+=1;
        errorNorm=error.norm();
    } while (errorNorm>1e-6 && iter<100);

    std::cout << "Converge in "  << iter << " tentativi.\n";
    k_1.print();
    return k_1;
}

int main() {
    Matrix A(4,4);
    A(0,0)=4;
    A(0,1)=2;
    A(0,2)=0;
    A(0,3)=1;
    A(1,0)=3;
    A(1,1)=5;
    A(1,2)=2;
    A(1,3)=0;
    A(2,0)=1;
    A(2,1)=0;
    A(2,2)=3;
    A(2,3)=0;
    A(3,0)=3;
    A(3,1)=2;
    A(3,2)=2;
    A(3,3)=8;

    Vector B(4);
    B(0)=5;
    B(1)=0;
    B(2)=4;
    B(3)=2;

    Matrix R = computeR(A);
    
    Vector InvD = computeInvD(A);

    // x^(k+1) = D⁻¹b - D⁻¹R · x^(k)
    Jacobi(B, R, InvD);
    
    return 0;
}