#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <cfloat>
#include <cmath>

double* allocateVector(int n)
{
    double* temp = (double*) malloc (n * sizeof (double));
    return temp;
}

double** allocateMatrix(int l, int c)
{
    double** temp = (double**) malloc (l * sizeof (double *));
    for (int i = 0; i < l; i++)
        temp[i] = allocateVector(c);
    return temp;
}

void freeVector(double*& v)
{
    free(v);
    v = NULL;
}

void freeMatrix(double**& M, int l)
{
    for (int i = 0; i < l; i++)
        freeVector(M[i]);
    free(M);
    M = NULL;
}

void printMatrix (const char* msg, double** M, int l, int c)
{
    printf("\n");

    int msglen = strlen(msg);
    for (int i = 0 ; i < l; i++)
    {
        if (i == l/2)
            printf(msg);
        else
            for (int k = 0; k < msglen; k++)
                printf (" ");
        for (int j = 0; j < c; j++)
            printf("%+.3f\t", M[i][j]);
        printf("\n");
    }
    printf("\n");
}

void printVector (const char* msg, double* A, int m)
{
    printf("\n");

    int msglen = strlen(msg);
    for (int i = 0 ; i < m; i++)
    {
        if (i == m/2)
            printf(msg);
        else
            for (int k = 0; k < msglen; k++)
                printf (" ");
        printf("%+.3f\n", A[i]);
    }
    printf("\n");
}

// C = A - B
void substractMatrix (int l, int c, double** C, double** A, double** B)
{
    for (int i = 0 ; i < l; i++)
        for (int j = 0; j < c; j++)
            C[i][j] = A[i][j] - B[i][j];
}

void substractVector (double* C, double* A, double* B, int m)
{
    for (int i = 0 ; i < m; i++)
            C[i] = A[i] - B[i];
}

double matrix_norm_1 (double** M, int l, int c)
{
    double max = -DBL_MAX;
    for (int j = 0; j < c; j++)
    {
        double sum = 0.0;
        for (int i = 0; i < l; i++)
            sum += abs (M[i][j]);
        if (max < sum)
            max = sum;
    }
    return max;
}

double matrix_norm_inf (double** M, int l, int c)
{
    double max = -DBL_MAX;
    for (int i = 0; i < l; i++)
    {
        double sum = 0.0;
        for (int j = 0; j < c; j++)
            sum += abs (M[i][j]);
        if (max < sum)
            max = sum;
    }
    return max;
}

// A = B
void vec_copy (double* A, double* B, int m)
{
    for (int i = 0; i < m; i++)
        A[i] = B[i];
}

double vec_norm_1 (double* A, int m)
{
    double sum = 0.0;
    for (int i = 0; i < m; i++)
        sum += abs(A[i]);
    return sum;
}

double vec_norm_inf (double* A, int m)
{
    double max = -DBL_MAX;
    for (int i = 0; i < m; i++)
        if (max < abs(A[i]))
            max = abs(A[i]);
    return max;
}

int main()
{
    double eps = 1e-10;

    int m = 4;

    double** I = allocateMatrix(m,m);
    double** A = allocateMatrix(m,m);
    double** B = allocateMatrix(m,m);
    double*  b = allocateVector(m);
    double*  x_n = allocateVector(m);
    double*  x_n1 = allocateVector(m);
    double*  x_diff = allocateVector(m);

    //Initializare I
    for (int i = 0; i < m; i++)
        for (int j = 0; j < m; j++)
            if (i == j)
                I[i][j] = 1;
            else
                I[i][j] = 0;

    // Initializare A
    A[0][0] =  0.9; A[0][1] =  0.1; A[0][2] =  0.2; A[0][3] =  0.3;
    A[1][0] = -0.2; A[1][1] =  0.8; A[1][2] =  0.1; A[1][3] =  0.4;
    A[2][0] =  0.1; A[2][1] = -0.3; A[2][2] =  0.7; A[2][3] = -0.1;
    A[3][0] = -0.3; A[3][1] = -0.2; A[3][2] = -0.1; A[3][3] =  0.9;

    // Initializare b
    b[0] = 3.6;     b[1] = 2.0;     b[2] = 2.4;     b[3] = 1.5;

    //Initializare x;
    x_n [0] = x_n [1] = x_n [2] = x_n [3] = 0.0;
    x_n1[0] = x_n1[1] = x_n1[2] = x_n1[3] = 0.0;

    // Initializare B
    substractMatrix (m, m, B, I, A);

    printMatrix("B =  ",B, m, m);

    const bool use_1_norm = true;

    double q = 2.0;
    if (use_1_norm)
    {
        q = matrix_norm_1(B, m, m);
        printf("||B||1   = %f\n", q);
    }
    else
    {
        q = matrix_norm_inf(B, m, m);
        printf("||B||inf = %f\n", q);
    }

    bool cont = true;
    double p = 0;

    if (q < 1.0)
    {
        printf("\nMetoda lui Jacobi se poate aplica\n");
        do
        {
            // Copiem x-ul anterior
            vec_copy(x_n, x_n1, m);

            // Calculam iteratia propriu-zisa
            for (int i = 0; i < m; i++)
            {
                double sum = 0.0;
                for (int j = 0; j < m; j++)
                    sum += B[i][j] * x_n[j];
                x_n1[i] = sum + b[i];
            }
            cont = true;
            substractVector(x_diff, x_n1, x_n, m);
            if (use_1_norm)
                p = vec_norm_1 (x_diff, m);
            else
                p = vec_norm_inf (x_diff, m);

        printf("Norma diferentei intre x(n+1) si x(n) este %.11f\n", p);
        }
        while (q/(1-q)*p > eps);
    }

    printf("\nRezultat final:");
    printVector("x =  ", x_n1, m);

    freeMatrix(I, m);
    freeMatrix(A, m);
    freeMatrix(B, m);
    freeVector(b);
    freeVector(x_n);
    freeVector(x_n1);
    freeVector(x_diff);

    system("pause");
    return 0;
}