/* brian's bunch o' hozerish matrix functions */
/* Mon Mar 16 03:10:02 PST 1998 */

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

#include "matfuncs.h"

void
m_dealloc(mat *m)
{
	unsigned long i = 0;

	for (i = 0; i < m->rows; i++) {
		/* printf("Freeing m->e[%lu] == 0x%lx...\n",i,m->e[i]); */
		free(m->e[i]);
		/* printf("Freed m->e[%lu].\n",i); */
		m->e[i] = NULL;
	}
	/* printf("Freeing m->e == 0x%lx...\n",m->e); */
	free(m->e);
	/* printf("Freed m->e.\n"); */
	m->e = NULL;
	/* printf("Freeing m == 0x%lx...\n",m); */
	free(m);
	/* printf("Freed m.\n",m); */
}

mat *
m_alloc(unsigned long rows, unsigned long cols)
{
	mat *temp;
	unsigned long i, j;

	if (rows <= 0 || cols <= 0) {
		fprintf(stderr,
			"it is unuseful to consider matrices of dimension [%lux%lu]\n",
			rows,cols);
		return NULL;
	}
	temp = (mat *) malloc(sizeof(mat));
	if (!temp) return NULL;
	temp->e = (double **) malloc(rows * sizeof(double *));
	if (!temp->e) {
		free(temp);
		return NULL;
	}
	for (i = 0; i < rows; i++) {
		temp->e[i] = (double *) malloc(cols * sizeof(double));
		if (!temp->e[i]) {
			for (j = 0; j < i; j++) {
				free(temp->e[j]);
			}
			free(temp->e);
			free(temp);
			return NULL;
		}
	}
	temp->rows = rows;
	temp->cols = cols;
	return temp;
}

mat *
m_read_from_stream(FILE *fp)
{
	mat *temp = NULL;
	unsigned long i = 0, j = 0;
	int rc;
	
	fscanf(fp, "%lu", &i);
	fscanf(fp, "%lu", &j);
	temp = m_alloc(i,j);
	if (!temp) return NULL;

	for (i = 0; i < temp->rows; i++) {
		for (j = 0; j < temp->cols; j++) {
			rc = fscanf(fp, "%lf", &temp->e[i][j]);
			if (rc == EOF) {
				fprintf(stderr,
					"unexpected end of file encountered at [%lu,%lu]\n",i,j);
				m_dealloc(temp);
				return NULL;
			} else if (rc == 0) {
				fprintf(stderr,
					"unexpected match fail encountered at [%lu,%lu]\n",i,j);
				m_dealloc(temp);
				return NULL;
			} else if (ferror(fp)) {
				fprintf(stderr,
					"stream error (%s) at [%lu,%lu]\n",strerror(errno),i,j);
				m_dealloc(temp);
				return NULL;
			}
		}
	}
	return temp;
}

mat *
m_read_from_file(char *filename)
{
	FILE *fp;
	mat *temp;

	fp = fopen(filename,"r");
	if (!fp) {
		perror(filename);
		return NULL;
	}
	temp = m_read_from_stream(fp);
	fclose(fp);
	return temp;
}

mat *
m_read_argument(char *filename)
{
	if (strcmp(filename,"-")==0) {
		return m_read_from_stream(stdin);
	} else {
		return m_read_from_file(filename);
	}
}

void
m_row_exch(mat *a, unsigned long row_from, unsigned long row_to)
{
	double *temp = NULL;

	if (a->rows < row_from || a->rows < row_to) {
		fprintf(stderr,"cannot exch rows [%lu,%lu] in [%lux%lu] matrix\n",
			row_from,row_to,a->rows,a->cols);
		return;
	}
	temp = a->e[row_to];
	a->e[row_to] = a->e[row_from];
	a->e[row_from] = temp;
}

void
m_fprint(FILE *fp, mat *a)
{
	unsigned long i, j;

	fprintf(fp,"%lu %lu\n",a->rows,a->cols);
	for (i = 0; i < a->rows; i++) {
		for (j = 0; j < a->cols; j++) {
			fprintf(fp,"%f ",a->e[i][j]);
		}
		fprintf(fp,"\n");
	}
}

void
m_print(mat *a)
{
	m_fprint(stdout, a);
}

mat *
m_copy(mat *a)
{
	mat *temp;
	unsigned long i;

	temp = m_alloc(a->rows, a->cols);
	if (!temp) {
		return NULL;
	}
	for (i = 0; i < a->rows; i++) {
		memcpy(temp->e[i],a->e[i],a->cols * sizeof(double));
	}
	return temp;
}

mat *
m_identity(unsigned long rows, unsigned long cols)
{
	unsigned long i, j;
	mat *temp;

	temp = m_alloc(rows,cols);
	if (!temp)
		return NULL;

	for (i = 0; i < temp->rows; i++) {
		for (j = 0; j < temp->cols; j++) {
			temp->e[i][j] = ((i==j) ? 1.0 : 0.0);
		}
	}

	return temp;
}

void
m_gausselim(mat *a, mat **u, mat **l)
{
	mat *U, *L;
	double c;
	unsigned long from, to, col;

	*u = NULL;
	*l = NULL;
	U = m_copy(a); if (!U) return;
	L = m_identity(a->rows, a->cols); if (!L) return;

	for (col = 0; col < a->cols; col++)
		for (from = col; from < a->rows; from++)
			for (to = from + 1; to < a->rows; to++)
			{
				if (U->e[to][col] == 0)
					continue;
				if (U->e[from][col] == 0) {
					/* printf("I will not be a ho for INFINITY\n"); */
					break;
				}
				/* printf("Comparing U[%u][%u]=%.4f and U[%u][%u]=%.4f\n", */
					/* from,col,U->e[from][col],to,col,U->e[to][col]); */
				c = U->e[to][col]/U->e[from][col];
				/* printf("subtracting %.3f * row %u from row %u [col %u]\n", */
					/* c,from,to,col); */
				m_addrow(U, -c, from, to);
				L->e[to][from] = c; /* save multiplier */
			}

	*u = U;
	*l = L;
}

/*
 2 3 4 5 -> 2  3  4  5 -> 2  3    4  5   -> 2  3    4  5
 2 2 2 2 -> 0 -1 -2 -3 -> 0 -1   -2 -3   -> 0 -1   -2 -3
 1 1 1 2 -> 1  1  1  2 -> 0 -0.5 -1 -0.5 -> 0 -0.5 -1 -0.5
 4 3 3 2 -> 4  3  3  2 -> 4  3    3  2   -> 0  0   -1 -3
add -(2/2) * row 1 to row 2
add -(1/2) * row 1 to row 3
add -(4/2) * row 1 to row 4
 2  3    4  5   ->  2  3  4  5
 0 -1   -2 -3   ->  0 -1 -2 -3
 0 -0.5 -1 -0.5 ->  0  0  0  0
 0  0   -1 -3   ->  0  0 -1 -3
add -(-0.5/-1) * row 2 to row 3
*/

void
m_rowmult(mat *a, unsigned long row, double c)
{
	unsigned long j;

	if (a->rows < row || row < 0) {
		fprintf(stderr,"cannot mult %f by row [%lu] in [%lux%lu] matrix\n",
			c,row,a->rows,a->cols);
		return;
	}
	for (j = 0; j < a->rows; j++) {
		a->e[row][j] *= c;
	}
}

void
m_addrow(mat *a, double c, unsigned long row_from, unsigned long row_to)
{
	unsigned long j;

	if (row_from < 0 || row_from > a->rows || row_to < 0 || row_to > a->rows) {
		fprintf(stderr,"cannot add rows [%f*%lu+%lu] in [%lux%lu] matrix\n",
			c,row_from,row_to,a->rows,a->cols);
		return;
	}
	for (j = 0; j < a->cols; j++) {
		a->e[row_to][j] += c * a->e[row_from][j];
	}
}

void 
m_mult(mat **product, mat *a, mat *b)
{
	mat *p = NULL;
	unsigned long i,j,k;

	*product = NULL;
	if (b->rows != a->cols) {
		fprintf(stderr,"cannot multiply [%lux%lu] x [%lux%lu] matrices\n",
			a->rows,a->cols,b->rows,b->cols);
		return;
	}
	p = m_alloc(a->rows, b->cols);
	if (!p) return;
	for (i = 0; i < a->rows; i++)
		for (j = 0; j < b->cols; j++) {
			p->e[i][j] = 0;
			for (k = 0; k < b->rows; k++)
				p->e[i][j] += a->e[i][k] * b->e[k][j];
		}
	*product = p;
}

void 
m_smult(mat **product, double c, mat *a)
{
	mat *p = NULL;
	unsigned long i,j;

	*product = NULL;
	p = m_copy(a);
	if (!p) return;
	for (i = 0; i < p->rows; i++)
		for (j = 0; j < p->cols; j++)
			p->e[i][j] *= c;
	*product = p;
}

void
m_addmat(mat **c, mat *a, mat *b)
{
	mat *result;
	unsigned long i,j;

	*c = NULL;
	if (!(a->rows == b->rows && a->cols == b->cols)) {
		fprintf(stderr,"cannot add [%lux%lu] + [%lux%lu] matrices\n",
			a->rows,a->cols,b->rows,b->cols);
		return;
	}
	result = m_alloc(a->rows, a->cols);
	if (!result)
		return;
	for (i = 0; i < a->rows; i++) {
		for (j = 0; j < a->cols; j++) {
			result->e[i][j] = a->e[i][j] + b->e[i][j];
		}
	}
	*c = result;
}