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

// Allocate Matrix of Size rows(h) by cols(w) dynamically
double ** make_matrix(int h, int w) {
	int i;
	double **array2 = (double **)malloc(h * sizeof(double *));
	if (array2) {
		array2[0] = (double *)malloc(h * w * sizeof(double));
		if(array2[0]) {
			for(i = 1; i < h; i++)
				array2[i] = array2[0] + i * w;

			return array2;
		} else {
			free(array2);
		}
	}

	return NULL;
}

// Free memory from a matrix created with make_matrix
void free_matrix(double ** m) {
	if (m[0]) {
		free(m[0]);
		free(m);
	}
}

// Way to convert between row/col and linear index
int in2d(int row, int col, int n){
  return col + row * n;
}

void fscan_matrix(FILE * in, double **m, int h, int w) {
  int i, j;

  for(i = 0; i < h; ++i)
	for(j = 0; j < w; ++j)
	  fscanf(in, "%lf", &m[i][j]);

  return;
}

void scan_matrix(double **m, int h, int w){
  fscan_matrix(stdin, m, h, w);
  return;
}


double ** matrix_transpose(double ** m1, int h, int w) {
  int i, j;

  double ** mr = make_matrix(w, h);

  if (mr) {
	for(i = 0; i < h; ++i)
	  for(j = 0; j < w; ++j)
		mr[j][i] = m1[i][j];

	return mr;
  } else {
	return NULL;
  }
}

void fprint_matrix(FILE * out, double **m, int h, int w){
  int i, j;
  fprintf(out, "{\n");
  for(i = 0; i < h - 1; ++i) {
	fprintf(out, "  {");
	for(j = 0; j < w - 1; ++j)
	  fprintf(out, "%g, ", m[i][j]);
	fprintf(out, "%g},\n", m[i][j]);
  }
  fprintf(out, "  {");
  for(j = 0; j < w - 1; ++j)
	fprintf(out, "%g, ", m[i][j]);
  fprintf(out, "%g}\n}\n", m[i][j]);
  return;
}

void print_matrix(double **m, int h, int w){
  fprint_matrix(stdout, m, h, w);
  return;
}

double ** matrix_mult(double **m1, double **m2,
					int hm1, int wm1, int wm2){
  int i, j, k;
  double sum;

  double ** mr = make_matrix(hm1, wm2);

  if (mr) {
	  for(i = 0; i < hm1; ++i) {
		for(j = 0; j < wm2; ++j) {
		  sum = 0;
		  for(k = 0; k < wm1; ++k) {
			sum += m1[i][k] * m2[k][j];
		  }

		  mr[i][j] = sum;
		}
	  }
  }

  return mr;
}

void genp(double **m, double v[], int h, int w){
  int row, next_row, col;
  double factor;

  for(row = 0; row < (h - 1); ++row) {
    for(next_row = row + 1; next_row < h; ++next_row) {
      factor = m[next_row][row] / m[row][row];

      for(col = 0; col < w; ++col) {
        m[next_row][col] -= factor * m[row][col];
      }

      v[next_row] -= factor * v[row];
    }
  }
}

void back_substitute(double **m, double v[],
                     int h, int w){
  int row, next_row;

  for(row = h - 1; row >= 0; --row) {
    v[row] /= m[row][row];
    m[row][row] = 1;
    for(next_row = row - 1; next_row >= 0; --next_row) {
      v[next_row] -= v[row] * m[next_row][row];
      m[next_row][row] = 0;
    }
  }
}


void gepp(double **m, double v[], int h, int w){
  int row, next_row, col, max_row;
  double tmp, factor;

  for(row = 0; row < (h - 1); ++row) {

    // Find row with largest pivot.

    max_row = row;
    for(next_row = row + 1; next_row < h; ++next_row)
        if(m[next_row][row] > m[max_row][row])
            max_row = next_row;


    // Swap rows.

    if(max_row != row) {
        for(col = 0; col < w; ++col) {
            tmp = m[row][col];
            m[row][col] = m[max_row][col];
            m[max_row][col] = tmp;
        }
        tmp = v[row];
        v[row] = v[max_row];
        v[max_row] = tmp;
    }

    // Rest like Gaussian Elimination without Pivoting.
    for(next_row = row + 1; next_row < h; ++next_row) {
      factor = m[next_row][row] / m[row][row];

      for(col = 0; col < w; ++col) {
        m[next_row][col] -= factor * m[row][col];
      }

      v[next_row] -= factor * v[row];
    }

  }
  
  back_substitute(m, v, h, w);
}

void fprint_vector(FILE * out, double v[], int size){
  int i;

  fprintf(out, "{");
  for(i = 0; i < size - 1; i++)
    fprintf(out, "%g, ", v[i]);
  fprintf(out, "%g}\n", v[i]);

  return;
}

void print_vector(double v[], int size)
{
  fprint_vector(stdout, v, size);

  return;
}

int main() {
  //double m[4][3] = { {0, 1, 2}, {2, 3, 4}, {5, 6, 7}, {9, 1, 0}};
  int h = 4, w = 4;
  double v[4] = {3, 1, 0, 0};
  double ** m = make_matrix(h, w);
  scan_matrix(m, h, w);

  print_matrix(m, h, w);
  print_vector(v, 4);
  
  gepp(m, v, 4, 4);

  printf("Answer: ");
  print_vector(v, 4);

  free_matrix(m);

  return 0;
}