/**********************************************************************
 
	Copyright (C) 2003 Hirohisa MORI <joshua@nichibun.ac.jp>
 
	This program is free software; you can redistribute it 
	and/or modify it under the terms of the GLOBALBASE 
	Library General Public License (G-LGPL) as published by 

	http://www.globalbase.org/
 
	This program is distributed in the hope that it will be 
	useful, but WITHOUT ANY WARRANTY; without even the 
	implied warranty of MERCHANTABILITY or FITNESS FOR A 
	PARTICULAR PURPOSE.

**********************************************************************/



#include	"equation.h"

/* m[tate-index][yoko-index] */


void
print_matrix(double m[E_DIM][E_DIM])
{
int i,j;
	printf("---\n");
	for ( i = 0 ; i < E_DIM ; i ++ ) {
		printf("(");
		for ( j = 0 ; j < E_DIM ; j ++ )
			printf("%lf ",m[i][j]);
		printf(")\n");
	}
	printf("---\n");
}

void
print_vector(double v[E_DIM])
{
int i;
	printf("(");
	for ( i = 0 ; i < E_DIM ; i++ )
		printf("%lf ",v[i]);
	printf(")\n");
}

int
_equation_a(
	double m[E_DIM][E_DIM],
	double v[E_DIM],
	int ofs)
{
int i,j;
int max;
double a,b;
	a = 0;
	max = -1;
	for ( i = ofs ; i < E_DIM ; i ++ ) {
		if ( m[i][ofs] < 0 )
			b = - m[i][ofs];
		else	b = m[i][ofs];
		if ( b > a ) {
			max = i;
			a = b;
		}
	}
	if ( a == 0 )
		return -1;
	if ( max != ofs ) {
		for ( i = 0 ; i < E_DIM ; i ++ ) {
			a = m[ofs][i];
			m[ofs][i] = m[max][i];
			m[max][i] = a;
		}

		a = v[ofs];
		v[ofs] = v[max];
		v[max] = a;

	}
	for ( i = ofs + 1 ; i < E_DIM ; i ++ ) {
		if ( m[i][ofs] == 0 )
			continue;
		a = m[ofs][ofs]/m[i][ofs];
		for ( j = ofs ; j < E_DIM ; j ++ )
			m[i][j] = a*m[i][j] - m[ofs][j];
		m[i][ofs] = 0;

		v[i] = a*v[i] - v[ofs];
	}
	return 0;
}

int
equation(
	 double result[E_DIM],
	 double m[E_DIM][E_DIM],
	 double v[E_DIM])
{
int i,j;
double r;

	for ( i = 0 ; i < E_DIM ; i ++ ) {
		if ( _equation_a(m,v,i) < 0 )
			return -1;
	}
	for ( i = E_DIM-1 ; i >= 0 ; i -- ) {
		r = 0;
		for ( j = i + 1 ; j < E_DIM ; j ++ )
			r += m[i][j]*result[j];
		result[i] = (v[i] - r)/m[i][i];
	}
	return 0;
}

void
test_equation()
{
double m[E_DIM][E_DIM] = {
{1, 2, 3},
{3, 6, 4},
{4, 6, 2}
};

double res[E_DIM] = { 1, 2, 3 };
double v[E_DIM];
double a;
double result[E_DIM];

int i,j;
	for ( i = 0 ; i < E_DIM ; i ++ ) {
		a = 0;
		for ( j = 0 ; j < E_DIM ; j ++ ) {
			a += m[i][j]*res[j];
		}
		v[i] = a;
	}
	printf("v = (%lf %lf %lf)\n",v[0],v[1],v[2]);
	equation(result,m,v);
	printf("result = (%lf %lf %lf)\n",result[0],result[1],result[2]);
}
