/* -*- c -*-
 * Time-stamp: <2005-04-16 11:28:45 rsmith>
 * 
 * matrix/matrix.c
 * Copyright (C) 2002,2004,2005 R.F. Smith <rsmith@xs4all.nl>. 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

/* If this code fails to compile, check that you compiler supports
 * variable-length automatic arrays. If compilation fails with GCC, try
 * adding the -fno-unit-at-a-time switch to the compilation command. */

#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "matrix.h"

#ifndef NULL
#define NULL (void*)0
#endif

#ifndef RETURN_IF_FAIL
#define RETURN_IF_FAIL(test) if (!(test)) return
#endif

#ifndef RETURN_VAL_IF_FAIL
#define RETURN_VAL_IF_FAIL(test,val) if (!(test)) return (val)
#endif

#ifndef NDEBUG
/* __FUNCTION__ and varags macros are a GCC feature. */
#  ifdef __GNUC__
#    undef debug
#    define debug(a...) fprintf(stderr,"%s, line %i, %s(): ",\
__FILE__,__LINE__,__FUNCTION__); fprintf(stderr, ## a); fprintf(stderr, "\n")
#  else
#    undef debug
#    define debug(a) (void)0
#  endif                        /* __GNUC__ */
#else
#  undef debug
#  define debug(a) (void)0
#endif                          /* NDEBUG */

/* This enables you to see which files have been used in building a
 * binary. N.B, __attribute is a GCC extension, so we test for __GNUC__. */
#ifdef __GNUC__
static char *SCMID __attribute__ ((unused)) = 
"$Id: matrix.c 33 2005-04-16 21:47:48Z rsmith $";
#endif

/* If the product of the multiplication of two doubles is less than EPS,
 * the value is set to 0. */
#define EPS 1e-8

int mat_unity(int n, double a[n][n]);
int mat_cpy(int n, double a[n][n], double b[n][n]);
int mat_xform(int n, double m[n][n], double v[n], double mv[n]);
int mat_mul(int n, double a[n][n], double b[n][n], double ab[n][n]);
int mat_inv(int n, double m[n][n], double i[n][n]);
int mat_print(char *name, int n, double m[n][n]);

int mat_unity(int n, double a[n][n])
{
	int r, c;
	assert(n>0);
	RETURN_VAL_IF_FAIL(n>0,1);
	for (r=0; r<n; r++) {
		for (c=0; c<n; c++) {
			if (r==c) {
				a[r][c] = 1.0;
			} else {
				a[r][c] = 0.0;
			}
		}
	}
	return 0;
}

int mat_cpy(int n, double a[n][n], double b[n][n])
{
	int r, c;
	assert(n>0);
	RETURN_VAL_IF_FAIL(n>0,1);
	/* Make a copy of m */
	for (r=0; r<n; r++) {
		for (c=0; c<n; c++) {
			b[r][c] = a[r][c];
		}
	}
	return 0;
}

int mat_xform(int n, double m[n][n], double v[n], double mv[n])
{
	int r, c;
	double res[n];
	assert(n>0);
	RETURN_VAL_IF_FAIL(n>0,1);
	for (r=0; r<n; r++) {
		res[r] = 0.0;
		for (c=0; c<n; c++) {
			res[r] += m[r][c]*v[c];
		}
	}
	memcpy(mv, res, n*sizeof(double));
	return 0;
}

int mat_mul(int n, double a[n][n], double b[n][n], double ab[n][n])
{
	int r, c, t;
	double res[n][n];
	assert(n>0);
	RETURN_VAL_IF_FAIL(n>0,1);
	for (r=0; r<n; r++) {
		for (c=0; c<n; c++) {
			res[r][c] = 0.0;
			for (t=0; t<n; t++) {
				res[r][c] += a[r][t]*b[t][c];
			}
			if (fabs(res[r][c]) < EPS) {
				res[r][c] = 0.0;
			}
		}
	}
	memcpy(ab, res, n*n*sizeof(double));
	return 0;
}

int mat_inv(int n, double m[n][n], double i[n][n])
{
	int r, c, t;
	double m2[n][n], inv[n][n], factor;
	assert(n>0);
	RETURN_VAL_IF_FAIL(n>0,1);
	mat_cpy(n, m, m2);
	mat_unity(n, inv);
	/* Empty the lower half of the matrix */
	for (t=0; t<n-1; t++) {
		if (m2[t][t] == 0) {
			return 2;
		}
		for (r=t+1; r<n; r++) {
			factor=m2[r][t]/m2[t][t];
			for (c=0; c<n; c++) {
				inv[r][c] -= factor*inv[t][c];
				if (c<=t) {
					m2[r][c]=0;
				} else {
					m2[r][c] -= factor*m2[t][c];
				}
			}
		}
	}
	/* Now empty the rest. */
	for (t=n-1; t>0; t--) {
		for (r=t-1; r>=0; r--) {
			factor = m2[r][t]/m2[t][t];
			for (c=n-1; c>=0; c--) {
				if (c==t) {
					m2[r][c] = 0;
				}
				inv[r][c]-=factor*inv[t][c];
			}
		}
	}
	/* Correct the rows' magnitude */
	for (r=0; r<n; r++) {
		for (c=0; c<n; c++) {
			inv[r][c] /= m2[r][r];
		}
		m2[r][r] = 1.0;
	}
	mat_cpy(n, inv, i);
	return 0;
}

int mat_print(char *name, int n, double m[n][n])
{
	int r, c;
	assert(n>0);
	assert(name!=NULL);
	RETURN_VAL_IF_FAIL(n>0,1);
	RETURN_VAL_IF_FAIL(name!=NULL,2);
	printf("matrix %s =\n", name);
	for (r=0; r<n; r++) {
		for (c=0; c<n; c++) {
			if (c==0) {
				printf("|%g ", m[r][c]);
			} else if (c==n-1) {
				printf("%g|\n", m[r][c]);
			} else {
				printf("%g ", m[r][c]);
			}
		}
	}
	return 0;
}
/* EOF matrix.c */