#ifndef lint
static char *sccsid = "@(#)matrix.c	1.1 (Steve Hill) 3/9/90";
#endif

/* MATRIX.C
 *
 * Package of functions for dealing with matrices
 *
 */

#include <stdio.h>

#include "basetype.h"
#include "error.h"
#include "malloc.h"
#include "print.h"
#include "matrix.h"


/* MatrixZero
 *
 * Set all entries in a matrix to zero.
 */

matrix_t *
MatrixZero(m)
matrix_t	*m;
{
	int	r, c;

	for (r = 0; r < m->rows; r++)
		for (c = 0; c < m->rows; c++)
			m->matrix[r][c] = REAL_ZERO;

	return(m);
}


/* MatrixMalloc
 *
 * Private function to allocate a matrix structure.
 */

static
char *
MatrixMalloc(size)
int	size;
{
	char	*p;

	p = malloc((unsigned) size);

	if (p == NULL)
		FatalError(__FILE__, __LINE__, "Matrix: out of memory");

	return(p);
}


/* Matrix
 *
 * Create and initialise a matrix.  The matrix will be rows by
 * cols, and will be initialised with the one dimensional real
 * array init.  If this array is NULL, the all entries will
 * be set to zero.
 */

matrix_t *
Matrix(rows, cols, init)
int	rows, cols;
real_t	*init;
{
	/* Create a matrix of the specified size */

	int		r, c;
	matrix_t	*m;

	m = (matrix_t *) MatrixMalloc(sizeof(matrix_t));

	m->rows = rows;
	m->cols = cols;

	m->matrix = (real_t **) MatrixMalloc(rows * sizeof(real_t *));

	for (r = 0; r < rows; r++)
		m->matrix[r] = (real_t *) MatrixMalloc(cols * sizeof(real_t));

	if (init == NULL)
		MatrixZero(m);
	else
	{
		for (r = 0; r < rows; r++)
			for (c = 0; c < cols; c++)
				m->matrix[r][c] = init[r*rows+c];
	}
	
	return(m);
}


/* MatrixLet
 *
 * Assign m2 to m1.
 */

matrix_t *
MatrixLet(m1, m2)
matrix_t	*m1, *m2;
{
	int	r, c;

	if ((m1->rows != m2->rows) || (m1->cols != m2->cols))
		FatalError(__FILE__, __LINE__,
                           "MatrixLet: matrices incompatible sizes");
	
	for (r = 0; r < m1->rows; r ++)
		for (c = 0; c < m2->cols; c++)
			m1->matrix[r][c] = m2->matrix[r][c];

	return(m1);
}


/* MatrixCopy
 *
 * Duplicate a matrix, and return a pointer to the new
 * one.
 */

matrix_t *
MatrixCopy(m1)
matrix_t	*m1;
{
	matrix_t	*m;

	if (m1 == MatrixNull)
		return(MatrixNull);

	m = Matrix(m1->rows, m1->cols, (real_t *) NULL);
	MatrixLet(m, m1);
	return(m);
}


/* MatrixSet
 * 
 * Set the entry in row r column c of the matrix m to v
 */

matrix_t *
MatrixSet(m, r, c, v)
matrix_t	*m;
int		r, c;
real_t		v;
{
	if (r < 0 || c < 0 || r >= m->rows || c >= m->cols)
		FatalError(__FILE__, __LINE__, "MatrixSet: element out of range");

	m->matrix[r][c] = v;

	return(m);
}


/* MatrixGet
 *
 * Retrieve the entry at row r column c in matrix m, and return it.
 */

real_t
MatrixGet(m, r, c)
matrix_t	*m;
int		r, c;
{
	
	if (r < 0 || c < 0 || r >= m->rows || c >= m->cols)
		FatalError(__FILE__, __LINE__, "MatrixGet: element out of range");

	return(m->matrix[r][c]);
}


/* MatrixId
 *
 * Create and return pointer to the identity matrix order r
 */

matrix_t *
MatrixId(r)
int	r;
{
	matrix_t	*m;

	m = Matrix(r, r, (real_t *) NULL);

	for (r = 0; r < m->rows; r++)
		MatrixSet(m, r, r, REAL_ONE);
	
	return(m);
}


/* MatrixAdd
 *
 * Add matrix m2 to matrix m3, and return the result in m1 if
 * supplied.  If m1 is NULL allocate new result matrix, and return
 * pointer to this.
 */

matrix_t *
MatrixAdd(m1, m2, m3)
matrix_t	*m1, *m2, *m3;
{
	int	r, c;

	if ((m2->rows != m3->rows) || (m2->cols != m3->cols))
		FatalError(__FILE__, __LINE__,
                           "MatrixAdd: operand matrices incompatible sizes");

	if (m1 == NULL)
		m1 = Matrix(m2->rows, m2->cols, (real_t *) NULL);
	else
	if ((m1->rows != m2->rows) || (m1->cols != m2->cols))
		FatalError(__FILE__, __LINE__,
                           "MatrixAdd: result matrix wrong size");

	for (r = 0; r < m1->rows; r++)
	for (c = 0; c < m1->cols; c++)
		m1->matrix[r][c] = m2->matrix[r][c] + m3->matrix[r][c];

	return(m1);
}


/* MatrixSub
 *
 * Subtract matrix m3 from matrix m2.  Place result in m1 if supplied.
 * Otherwise place result in new matrix and return a pointer to the
 * result.
 */

matrix_t *
MatrixSub(m1, m2, m3)
matrix_t	*m1, *m2, *m3;
{
	int	r, c;

	if ((m2->rows != m3->rows) || (m2->cols != m3->cols))
		FatalError(__FILE__, __LINE__,
                           "MatrixSub: operand matrices incompatible sizes");

	if (m1 == NULL)
		m1 = Matrix(m2->rows, m2->cols, (real_t *) NULL);
	else
		if ((m1->rows != m2->rows) || (m1->cols != m2->cols))
			FatalError(__FILE__, __LINE__,
                                   "MatrixSub: result matrix wrong size");

	for (r = 0; r < m1->rows; r++)
	for (c = 0; c < m1->cols; c++)
		m1->matrix[r][c] = m2->matrix[r][c] - m3->matrix[r][c];

	return(m1);
}


/* MatrixMul
 *
 * Multiply matrix m2 by matrix m3.  If m1 is not null return result
 * in it, otherwise allocate new result matrix, and return pointer to
 * it.
 */

matrix_t *
MatrixMul(m1, m2, m3)
matrix_t	*m1, *m2, *m3;
{
	int		r, c, k;
	matrix_t	*mr;

	if (m2->cols != m3->rows)
	{
		FatalError(__FILE__, __LINE__,
                           "MatrixMul: operand matrices incompatible sizes");
	}

	if (m1 != NULL)
		if  ((m1->rows != m2->rows) || (m1->cols != m3->cols))
			FatalError(__FILE__, __LINE__,
                                   "MatrixMul: result matrix is wrong size");

	mr = Matrix(m2->rows, m3->cols, (real_t *) NULL);

	for (r = 0; r < mr->rows; r++)
	for (c = 0; c < mr->cols; c++)
	{
		real_t	t;

		t = REAL_ZERO;

		for (k = 0; k < m2->cols; k++)
			t += m2->matrix[r][k] * m3->matrix[k][c];

		mr->matrix[r][c] = t;
	}

	if (m1 == NULL)
		return(mr);
	else
	{
		MatrixLet(m1, mr);
		MatrixFree(mr);
		return(m1);
	}
}


/* MatrixScale
 *
 * Scale a matrix by a real factor s.  Return result in m1 unless NULL
 * in which case allocate new matrix for result and return pointer to this.
 */

matrix_t *
MatrixScale(m1, m2, s)
matrix_t	*m1, *m2;
real_t		s;
{
	int	r, c;

	if (m1 == NULL)
		m1 = Matrix(m2->rows, m2->cols, (real_t *) NULL);

	for (r = 0; r < m2->rows; r++)
	for (c = 0; c < m2->cols; c++)
		m1->matrix[r][c] = m2->matrix[r][c] * s;

	return(m1);
}


/* MatrixTranspose
 *
 * Transpose the matrix m2.  Return transposed matrix in m1.  If m1
 * is NULL allocate new matrix for result and return pointer to it.
 */

matrix_t *
MatrixTranspose(m1, m2)
matrix_t	*m1, *m2;
{
	int	r, c;

	if (m1 == NULL)
		m1 = Matrix(m2->cols, m2->rows, (real_t *) NULL);
	else
		if (m1->rows != m2->cols || m1->cols != m2->rows)
			FatalError(__FILE__, __LINE__,
                                   "MatrixTranspose: result matrix wrong");




	for (r = 0; r < m2->rows; r++)
	for (c = 0; c < m2->cols; c++)
		m1->matrix[c][r] = m2->matrix[r][c];

	return(m1);
}


/* PrintMatrix
 *
 * Print a matrix indented to a file.
 */

void
PrintMatrix(file, indent, m)
FILE		*file;
int		indent;
matrix_t	*m;
{
	int	r, c;

	if (m == MatrixNull)
	{
		fprintf(file, "Null Matrix\n");
		return;
	}

	for (r = 0; r < m->rows; r++)
	{
		PrintIndent(file, indent, "| ");
		for (c = 0; c < m->cols; c++)
		{
			real_t	n;

			n = m->matrix[r][c];
			if (n >= REAL_ZERO)
				fprintf(file, " %.2e ", m->matrix[r][c]);
			else
				fprintf(file, "%.2e ", m->matrix[r][c]);
		}
		fputs(" |\n", file);
	}
}


/* MatrixPrint
 *
 * Print a matrix to a file.
 */

void
MatrixPrint(FilePtr, m)
FILE		*FilePtr;
matrix_t	*m;
{
	PrintMatrix(FilePtr, 0, m);
}


/* MatrixFree
 *
 * Free a matrix structure, and all sub-components.
 */

void
MatrixFree(m)
matrix_t	*m;
{
	int	r;

	if (m == MatrixNull)
		return;

	for (r = 0; r < m->rows; r++)
		(void) free((char *) m->matrix[r]);

	(void) free((char *) m->matrix);
	(void) free((char *) m);
}