#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <math.h>
#include <time.h>

int malloced;

float *fvector(int nr)
{

	float *v;

	if ((v = (float *) calloc((unsigned) nr, sizeof(float))) == NULL)
		printf("Error: out of memory.\n");

	malloced += (nr * sizeof(float));

	return v;

}

void free_fvector(float *v, int nr)
{

	free(v);
	malloced -= (nr * sizeof(float));
	
}

float **fmatrix(int nr, int nc)
{
  
	int i;
	float **m;
  
	if ((m = (float**) calloc((unsigned) (nr + 1), sizeof(float*))) == NULL)
    {	
		printf("Error: out of memory.\n");
		return 0;
    }
  
	for(i=0;i<=nr;i++)
    {
     
		if ((m[i] = (float*) calloc((unsigned) (nc + 1), sizeof(float)))==NULL)
		{	
			printf("Error: out of memory.\n");
			return 0;
		}
	}

	malloced += ((nr + 1) * (nc + 1) * sizeof(float));

	return m;

}

void free_fmatrix(float **m, int nr, int nc)
{
  
	int i;

	for (i=nr;i>=0;i--) 
		free((void*) (m[i]+0));
  
	free((char*) (m));

	malloced -= ((nr + 1) * (nc + 1) * sizeof(float));

}

//Solve matrix [M] x {x} = {b}

void SolveMatrixGaussSiedel(float **M, float *x, float *b, int nbcols, int nbrows, double tol, int maxiter)
{

	int i, j, n;

	double beta = 1.65;
	
	double norm1, norm2;
	double res;
	double sum;
	
	double xnew, xold;

	n = 0;

	do
	{

		n++;

		norm1 = 0;
		norm2 = 0;

		for (i = 0; i < nbrows; i++)
		{

			sum = 0.0;

			for (j = 0; j < nbcols; j++)
			{

				if (i != j)
					sum += M[i][j] * x[j];
			}

			xold = x[i];
			xnew = (b[i] - sum) / M[i][i];
		
			// SOR
			x[i] = (float) (xold + beta * (xnew - xold));

			norm1 += pow(xnew - xold, 2);
			norm2 += pow(xnew, 2);

		}

		norm1 = sqrt(norm1);
		norm2 = sqrt(norm2);

		if (norm2 < 1E-30)
			res = norm1;
		else
			res = norm1 / norm2;

	} while (n < maxiter && res > tol);

	printf("interation: %d, residual: %e\n", n, res);

}

int main(int argc, char *argv[])
{

	float **M;			// Matrix 
	float *x;			// Variable
	float *b;			// Source

	printf("\n");
	printf("*****************************************\n");
	printf("*                                       *\n");
	printf("*    GAUSS-SIEDEL MATRIX SOLVER         *\n");
	printf("*                                       *\n");
	printf("*****************************************\n");
	printf("\n");

	M = fmatrix(5, 5);
	x = fvector(5);
	b = fvector(5);

	M[0][0] = 6;
	M[0][1] = 1;
	M[0][2] = 2;
	M[0][3] = 0;
	M[0][4] = 1;

	M[1][0] = 2;
	M[1][1] = 8;
	M[1][2] = 1;
	M[1][3] = 2;
	M[1][4] = 2;

	M[2][0] = 1;
	M[2][1] = -2;
	M[2][2] = 8;
	M[2][3] = 1;
	M[2][4] = 0;

	M[3][0] = -1;
	M[3][1] = 0;
	M[3][2] = 0;
	M[3][3] = 9;
	M[3][4] = 2;

	M[4][0] = 1;
	M[4][1] = 1;
	M[4][2] = 0;
	M[4][3] = -1;
	M[4][4] = 7;

	b[0] = 10;
	b[1] = 15;
	b[2] = 8;
	b[3] = 10;
	b[4] = 8;

	//   x0 x1 x2 x3 x4

	// [ 6  1  2  0  1 =  10 ]
	// [ 2  8  1  2  2 =  15 ]
	// [ 1 -2  8  1  0 =   8 ]
	// [ 0  0 -1  9  2 =  10 ]
	// [ 1  1  0 -1  7 =   8 ]

	SolveMatrixGaussSiedel(M, x, b, 5, 5, 1E-6, 60);

	printf("\nSolution:\n");

	printf("x[0]: %f\n", x[0]);
	printf("x[1]: %f\n", x[1]);
	printf("x[2]: %f\n", x[2]);
 	printf("x[3]: %f\n", x[3]);
	printf("x[4]: %f\n", x[4]);
 
	return 0;
}