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




// Cria Matriz MxN
 double** mat_cria(int m, int n)
 {
  double** A;
  int i;
  A = (double**)malloc(m*sizeof(double*));
  for(i=0; i < m; i++)
  {
   A[i] = (double*)malloc(n*sizeof(double));
  }
  return A;
 }




// Libera o espaço alocado pela matriz
 void mat_libera(int m, double** A)
 {
  int i;
  for(i=0; i < m; i++)
  {
   free(A[i]);
  }
  free(A);
 }
 // Transposta da matriz
 void mat_transposta(int m, int n, double** A, double** T)
 {
  int i, j;
  for(i=0; i < m; i++)
  {
   for(j=0;j < n; j++)
   {
    T[j][i] = A[i][j];
   }
  }
 }
 // Multiplicação de Vetor x Matriz
 void mat_multv(int m, int n, double** A, double* v, double* w)
 {
 /* matriz MxN . vetor Nx1 -> vetor Mx1*/
  int i, j;
  for(i = 0; i < m; i++)
   w[i] = 0;
  for(i = 0; i < m; i++)
  {
   for(j = 0; j < n; j++)
   {
    w[i] += A[i][j] * v[j];
   }
  }
 }
 // Multiplicação de Matrizes
 void mat_multm(int m, int n, int q, double** A, double** B, double** C)
 {
  int i, j, k, soma = 0;
  for(i = 0; i < m; i ++)
  {
   for(j = 0; j < q; j++)
   {
    C[i][j] = 0;
   }
  }
  for(i = 0; i < m; i++)
  {
   for(j = 0; j < q; j++)
   {
    for(k = 0; k < n; k++)
    {
     soma+= A[i][k] * B[k][j];
    }
    C[i][j] = soma;
    soma = 0;
   }
  }
 }

void printMatriz(int m, int n, double**A)
{
	int i,j;
	for(i = 0; i < m; i++){
		for(j = 0; j < n; j++){
			printf("%.2f\t",A[i][j]);
		}
		printf("\n");
	}
}

void printVetor(int n, double * A)
{
	int i;
	for(i = 0; i < n; i++)
	{
		printf(" %g ", A[i]);
	}
	printf("\n");
}

void swap(double* v1, double*v2)
 {
     double temp = *v1;
     *v1 = *v2;
     *v2 = temp;
 }

void Cholesky (int n, double** A)
{
	int t = 0, t2 = 0, j = 0, k = 0, i = 0;
	double** R = mat_cria(n, n);

	for (k = 0; k < n; k++)
	{
		double** u = mat_cria(n - (k+1), 1);
        double** uT = mat_cria(1, n - (k+1));
        double** uuT = mat_cria(n - (k+1), n - (k+1));

		R[k][k] = sqrt(A[k][k]);

        for (i = 0; i < k; i++)
        {
			A[k][i] = 0;
		}

		for (i = k+1; i < n; i++)
		{
			uT[0][t] = (1/R[k][k])*A[k][i];
			t++;
		}

		mat_transposta(1, n - (k+1), uT, u);
		mat_multm(n - (k+1), 1, n - (k+1), u, uT, uuT);
		t = 0;

		for (i = k+1; i < n; i++)
		{
			for (j = k+1; j < n; j++)
			{
			    printf("A no nível %d - %d: %f\n", i, j, A[i][j]);
				A[i][j] = A[i][j] - uuT[t2][t];
				t++;
			}
			t = 0;
			t2++;
		}
		t2 = 0;
	}
}

void ConjugateGradient (int n, double** A, double* b, double* x)
{
    int i = 0, j = 0, k = 0;
	double num = 0, den = 0, alpha = 0, beta = 0;
	double *Ax = (double*)malloc(n*sizeof(double));
	double *Ad = (double*)malloc(n*sizeof(double));
	double *dk = (double*)malloc(n*sizeof(double));
	double *rk = (double*)malloc(n*sizeof(double));

	mat_multv(n,n, A, x, Ax);

	for(i = 0; i < n; i++)
	{
		dk[i] = b[i] - Ax[i];
		rk[i] = dk[i];
	}

	while(k != n)
	{
		mat_multv(n, n, A, dk, Ad);

		for(i = 0; i < n; i++)
		{
		    num += rk[i]*rk[i];
			den +=dk[i]*Ad[i];
		}

    	alpha = num/den;

		den = num;
		num = 0;

		for(i = 0; i < n; i++)
		{
			x[i] = x[i] + alpha*dk[i];
			rk[j] = rk[j] - alpha*Ad[i];
			num += rk[j]*rk[j];
		}

		beta = num/den;

		for(i = 0; i < n; i++){
			dk[i] = rk[i] + beta*dk[i];
			if (rk[i] == 0) j++;
	    }

	    if (j == n) break;
	    k++;
	}
}

int main (void)
{
	int i, j = 0;
	double** A1 = mat_cria(3, 3);
    double** A2 = mat_cria(3, 3);
	double* x1 = (double*)malloc(3*sizeof(double));
	double* x2 = (double*)malloc(3*sizeof(double));
    double* b1 = (double*)malloc(3*sizeof(double));
	double* b2 = (double*)malloc(3*sizeof(double));

	A1[0][0] = 1;	A1[0][1] = -1;	A1[0][2] = 0;
	A1[1][0] = -1;	A1[1][1] = 2;	A1[1][2] = 1;
	A1[2][0] = 0;	A1[2][1] = 1;	A1[2][2] = 2;

	for(i = 0; i < 3; i++){
		for(j = 0; j < 3; j++){
			A2[i][j] = A1[i][j];
		}
	}

	A2[2][2] = 5;

	b1[0] = 0;	b1[1] = 2;	b1[2] = 3;

	b2[0] = 3;	b2[1] = -3;	b2[2] = 4;

    ConjugateGradient (3, A1, b1, x1);
    ConjugateGradient (3, A2, b2, x2);
	Cholesky(3, A1);
	Cholesky(3, A2);

	printMatriz(3, 3, A1);
    printMatriz(3, 3, A2);
    printVetor(3, x1);
    printVetor(3, x2);
	return 0;
}