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

#include "xutil.h"
#include "matrix.h"

void matrix_zero(matrix_t *matrix) {
	xmemset(matrix->data, sizeof(int64_t) * matrix->m * matrix->n);
}

matrix_t *matrix_create(uint32_t m, uint32_t n) {
	int64_t size = m*n*sizeof(int64_t);
	matrix_t *matrix = xmalloc(sizeof(matrix_t));

	matrix->data = xmalloc(size);
	matrix->m = m;
	matrix->n = n;

	matrix_zero(matrix);

	return matrix;
}

void matrix_identity(matrix_t *matrix) {
	uint32_t i, j;
	uint32_t n = matrix->n;

	if (matrix->n != matrix->m) {
		xerror("Cannot create identity matrix, wrong dimensions!", __LINE__, __FILE__);
	}

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			if (i != j) {
				MAT(matrix, i, j) = 0;
			} else {
				MAT(matrix, i, j) = 1;
			}
		}
	}
}

void matrix_destroy (matrix_t *matrix) {
	if (matrix != NULL) {
		if (matrix->data != NULL) {
			free(matrix->data);
		}
		free(matrix);
	}
}

void matrix_print(matrix_t *matrix) {
	uint32_t i, j;
	uint32_t n = matrix->n;
	
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			printf("% 12"PRId64, MAT(matrix, i, j));
		}

		printf("\n");
	}
}

void matrix_expand(matrix_t *matrix) {
	uint32_t i, j;
	uint32_t n = matrix->n,
			 m = matrix->m;

	int64_t *TEMP = xmalloc( (n+1)*(n+1) * sizeof(int64_t));
	int64_t *data = matrix->data;

	for (i = 0; i < n +1; i++) {
		for (j = 0; j < m + 1; j++) {

			if (i != n) {
				if (j != m) {
					TEMP[i*(n+1) + j] = data[i*n + j];
				} else {
					TEMP[i*(n+1) + j] = 0;
				}
			} else {
				TEMP[i*(n+1) + j] = 0;
			}

		}
	}

	free(data);

	matrix->n += 1;
	matrix->m += 1;
	matrix->data = TEMP;
}

void matrix_remove(matrix_t *matrix, uint32_t i) {
	uint32_t k, l,
			 n = matrix->m;

	int64_t *TEMP;
	int64_t *a;

	// Copy B
	TEMP = xmalloc( (n-1)*(n-1)* sizeof(int64_t));
	a = matrix->data;

	uint32_t iinc = 0,
			 jinc = 0;

	for (k = 0; k < n; k++) {
		jinc = 0;
		if (k == i) {
			iinc = 1;
			continue;
		}

		for (l = 0; l < n; l++) {
			if (l != i) {
				TEMP[(k - iinc) * (n-1) + l - jinc] = a[k*n + l];
			} else {
				jinc = 1;
				continue;
			}
		}
	}

	matrix->data = TEMP;
	matrix->m = n - 1;
	matrix->n = n - 1;

	free(a);
}
