#include <stdlib.h>
#include <stdio.h>
#include "project.h"

matrix* create_matrix(int rows, int cols) {
	int i,j;
	matrix* mtx;
	double** array;

	array = (double**) malloc(rows * sizeof(double*));
	mtx = matrix_from_array(rows, cols, array);
	mtx->rows_filled = 0;

	return mtx;
}

matrix* matrix_from_array(int rows, int cols, double** array) {
	matrix* mtx;

	mtx = (matrix*) malloc(sizeof(matrix));
	mtx->rows = rows;
	mtx->cols = cols;
	mtx->array = array;
	mtx->rows_filled = rows;

	return mtx;
}

matrix* mtx_copy(matrix* mtx) {
	int i, j;
	matrix* new;
	new = (matrix*) malloc(sizeof(matrix));
	new->rows = mtx->rows;
	new->cols = mtx->cols;
	new->array = (double**) malloc(new->rows * sizeof(double*));
	for (i=0; i < new->rows; i++) {
		new->array[i] = (double*) malloc(new->cols * sizeof(double));
		for (j=0; j < new->cols; j++) {
			new->array[i][j] = mtx->array[i][j];
		}
	}
	return new;
}

int add(matrix* mtx, matrix* addend) {
	int i, j;
	if (mtx->rows != addend->rows || mtx->cols != addend->cols) {
		return 1;
	}
	for (i=0; i < mtx->rows; i++) {
		for (j=0; j < mtx->cols; j++) {
			mtx->array[i][j] += addend->array[i][j];
		}
	}
	return 0;
}

void mul_scalar(matrix* mtx, double c) {
	int i, j;
	for (i = 0; i < mtx->rows; i++) {
		for (j = 0; j < mtx->cols; j++) {
			mtx->array[i][j] *= c;
		}
	}
}

matrix* mul_matrix(matrix* mtx, matrix* mul) {
	matrix* new;
	int i, j, k;
	if (mtx->cols != mul->rows) {
		return NULL;
	}
	new = create_matrix(mtx->rows, mul->cols);
	for (i = 0; i < mtx->rows; i++) {
		for (j = 0; j < mul->cols; j++) {
			new->array[i][j] = 0;
			for (k = 1; k < mtx->cols; k++) {
				new->array[i][j] += mtx->array[i][k] * mul->array[k][j];
			}
		}
	}
	return new;
}

void inverse(matrix* mtx) {
	matrix* new;
	int i, j;
	new = create_matrix(mtx->rows, 2 * mtx->cols);
	for (i = 0; i < mtx->rows; i++) {
		for (j = 0; j < mtx->cols; j++) {
			new->array[i][j] = mtx->array[i][j];
			new->array[i][j + mtx->cols] = (i == j);
		}
	}
	rref(new);
	for (i = 0; i < mtx->rows; i++) {
		for (j = 0; j < mtx->cols; j++) {
			mtx->array[i][j] = mtx->array[i][j + mtx->cols];
		}
	}
}

int add_row(matrix* mtx, double* row) {
	if (mtx->rows_filled == mtx->rows) {
		return 0;
	}
	mtx->array[mtx->rows_filled] = row;
	mtx->rows_filled++;
	return 1;
}

void mtx_row_swap(matrix* mtx, int r1, int r2) {
	double* temp = mtx->array[r1];
	mtx->array[r1] = mtx->array[r2];
	mtx->array[r2] = temp;
}

void mtx_row_mul(matrix* mtx, int r, double mul) {
	int i;
	for (i=0; i < mtx->cols; i++) {
		mtx->array[r][i] *= mul;
	}
}

void mtx_row_mul_add(matrix* mtx, int r1, double mul, int r2) {
	int i;
	for (i=0; i < mtx->cols; i++) {
		mtx->array[r2][i] += mul * mtx->array[r1][i];
	}
}

int rref(matrix* mtx) {
	int r=0, c=0;
	int i, j, k;
	double a, b;
	while (r < mtx->rows && c < mtx->cols) {
		//mtx_print(mtx);
		//getc(stdin);
		if (mtx->array[r][c] == 0) {
			int swapped = 0;
			for (i=r+1; i < mtx->rows; i++) {
				if (mtx->array[i][c] != 0) {
					//printf("swapping rows %i and %i\n", r, i);
					mtx_row_swap(mtx, r, i);
					swapped = 1;
					break;
				}
			}
			if (!swapped) {
				c++;
				continue;
			}
		}
		//printf("(%i, %i) = %f\n", r, c, mtx->array[r][c]);
		mtx_row_mul(mtx, r, 1.0/(mtx->array[r][c]));
		for (i=r+1; i < mtx->rows; i++) {
			mtx_row_mul_add(mtx, r, -1 * mtx->array[i][c], i);
		}
		r++;
		c++;
	}
	//mtx_print(mtx);
	//printf("here\n");
	r = mtx->rows - 1;
	while (r >= 0) {
		int pivot = 0;
		for (c = 0; c < mtx->cols; c++) {
			if (mtx->array[r][c] != 0) {
				//printf("got pivot! (%i, %i) = %f\n", r, c, mtx->array[r][c]);
				pivot = 1;
				break;
			}
		}
		if (pivot) {
			for (i = 0; i < r; i++) {
				mtx_row_mul_add(mtx, r, -1 * mtx->array[i][c], i);
			}
		}
		r--;
	}
	return 0;
}

void destroy(matrix* mtx) {
	int i;
	for (i = 0; i < mtx->rows; i++) {
		free(mtx->array[i]);
	}
	free(mtx->array);
	free(mtx);
}

void mtx_print(matrix* mtx) {
	int i, j;
	for (i=0; i < mtx->rows; i++) {
		for (j=0; j < mtx->cols; j++) {
			printf("%f\t", mtx->array[i][j]);
		}
		printf("\n");
	}
}