#include <iostream> 
#include <cmath> 
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ub = boost::numeric::ublas;

/* 
 * Gaussian Elimination:
 * solution of ax=b is put into b, a is destructed.
 */

bool gauss(ub::matrix<double>& a, ub::vector<double>& b)
{
	int n = a.size1();
	int i, j, k;
	int p_num;
	double p_max;
	double tmp;

	/* forward elimination */
	for (k = 0; k < n; k++) {

		/* search pivot */
		p_max = 0.;
		p_num = -1;
		for (i = k; i < n; i++) {
			tmp = fabs(a(i,k));
			if (tmp > p_max) {
				p_max = tmp;
				p_num = i;
			}
		}

		/* a is singular */
		if (p_num == -1) return false;

		/* swap k-th row and p_num-th row */
		for (j = k; j < n; j++) {
			tmp = a(p_num,j);
			a(p_num,j) = a(k,j);
			a(k,j) = tmp;
		}
		tmp = b(p_num);
		b(p_num) = b(k);
		b(k)= tmp;

		for (i = k+1; i < n ; i++) {
			/* subtract (a(i,k)/a(k,k) times k-th row) from i-th row */
			tmp = a(i,k)/a(k,k);
			for (j = k+1; j < n; j++) {
				a(i,j) -= tmp * a(k,j);
			}
			b(i) -= tmp * b(k);
		}

	} /* end of loop k */

	/* backward substitution */
	for (i = n-1; i >= 0; i--){
		for (j = i+1; j < n; j++) { 
			b(i) -= b(j) * a(i,j);
		}
		b(i) /= a(i,i);
	} /* end of loop i */

	return true;
}

int main()
{
	int i, j, n;

	std::cout.precision(17);

	ub::matrix<double> a(2,2), a0(2,2);
	ub::vector<double> b(2), b0(2);

	a(0,0) = 0.780;
	a(0,1) = 0.563;
	a(1,0) = 0.913;
	a(1,1) = 0.659;

	b(0) = 0.217;
	b(1) = 0.254;

	a0 = a; b0 = b;
	gauss(a0, b0);

	std::cout << b0 << "\n";

	b0(0) = 0.999;
	b0(1) = -1.001;

	std::cout << prod(a, b0) - b << "\n";
	
	b0(0) = 0.341;
	b0(1) = -0.087;
	
	std::cout << prod(a, b0) - b << "\n";
}