#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "interval.hpp"

namespace ub = boost::numeric::ublas;

ub::vector<interval> f(ub::vector<interval> x)
{
	ub::vector<interval> y(2);

	y(0) = x(0) * x(0) + x(1) * x(1) - 1.;
	y(1) = x(0) - x(1);

	return y;
}

ub::matrix<interval> df(ub::vector<interval> x)
{
	ub::matrix<interval> y(2,2);

	y(0,0) = 2. * x(0);
	y(0,1) = 2. * x(1);
	y(1,0) = 1.;
	y(1,1) = -1.;

	return y;
}


bool invert(ub::matrix<double> a, ub::matrix<double>& b)
{
	int i, j, k, n;
	double tmp;
	int p_num;
	double p_max;

	n = a.size1();
	if (a.size2() != n) return false;

	b.resize(n,n);
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			if (i == j) b(i,j) = 1.;
			else b(i,j) = 0.;
		}
	}

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

		for (i=k; i<n; i++) {
			tmp = a(k,i);
			a(k,i) = a(p_num,i);
			a(p_num,i) = tmp;
		}
		for (i=0; i<n; i++) {
			tmp = b(k,i);
			b(k,i) = b(p_num,i);
			b(p_num,i) = tmp;
		}

		for (i=k+1; i<n; i++) {
			tmp = a(i,k) / a(k,k);
			for (j=k; j<n; j++) {
				a(i,j) -= a(k,j) * tmp;
			}
			for (j=0; j<n; j++) {
				b(i,j) -= b(k,j) * tmp;
			}
		}
	}

	for (i = n-1; i>=0; i--) {
		for (j = i+1; j<n; j++) {
			for (k=0; k<n; k++) {
				b(i,k) -= a(i,j) * b(j,k);
			}
		}
		for (k=0; k<n; k++) {
			b(i,k) /= a(i,i);
		}
	}

	return true;
}

int main()
{
	ub::vector<interval> I, fc, fi, c, K;
	ub::matrix<interval> fdc, fdi, M;
	ub::matrix<double> mfdc, R;
	int i, j;
	bool flag;

	std::cout.precision(17);

	I.resize(2);

	I(0) = interval(0.6,0.8);
	I(1) = interval(0.6,0.8);
	std::cout << "I: " << I << "\n";

	c.resize(2);
	for (i=0; i<2; i++) {
		c(i) = mid(I(i));
	}
	std::cout << "c: " << c << "\n";

	fc = f(c);
	std::cout << "fc: " << fc << "\n";

	fdc = df(c);
	std::cout << "fdc: " << fdc << "\n";

	mfdc.resize(2,2);
	for (i=0; i<2; i++) {
		for (j=0; j<2; j++) {
			mfdc(i,j) = mid(fdc(i,j));
		}
	}

	invert(mfdc, R);
	std::cout << "R: " << R << "\n";

	fdi = df(I);
	std::cout << "fdi: " << fdi << "\n";

	M.resize(2,2);
	for (i=0; i<2; i++) {
		for (j=0; j<2; j++) {
			if (i == j) M(i,j) = 1.;
			else M(i,j) = 0.;
		}
	}
	M -= prod(R, fdi);
	std::cout << "M: " << M << "\n";

	K = c - prod(R, fc) +  prod(M, I - c);
	std::cout << "K: " << K << "\n";

	flag = true;
	for (i=0; i<2; i++) {
		if (!proper_subset(K(i), I(i))) flag = false;
	}
	if (flag) {
		std::cout << "solution found!\n";
	}
}