#include <iostream>
#include <kv/strobomap.hpp>
#include <kv/kraw-approx.hpp>

namespace ub = boost::numeric::ublas;

typedef kv::interval<double> itv;

struct Func {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		T x0, dx, lamb;
		ub::vector<T> y(3);
		static T mu = kv::constants<T>::str("0.8");

		x0 = x(0);
		dx = x(1);
		lamb = x(2);

		y(0) = dx;
		y(1) = (2 * mu * sin(t) * cos(t) * dx + (t - lamb) * x0) / (1 - mu * pow(sin(t), 2));
		y(2) = 0;

		return y;
	}
};

template <class F, class TT>
struct Make_Eq {
	F f;
	TT b0, b1, d0;

	Make_Eq(F f, TT b0, TT b1, TT d0): f(f), b0(b0), b1(b1), d0(d0) {}

	template <class T> T operator() (const T& x) {
		ub::vector<T> in(3), out;
		in(0) = b0;
		in(1) = d0;
		in(2) = x;
		out = f(in);
		return out(0) - b1;
	}
};

int main()
{
	itv ix;
	ub::vector<itv> ix2;
	itv end;
	int r;
	itv p;

	p = kv::constants<itv>::pi();

	std::cout.precision(17);

	Func f;

	kv::StroboMap<Func,double> g(f, (itv)0., p);

	Make_Eq<kv::StroboMap<Func,double>, itv> h(g, (itv)0, (itv)0, (itv)1);

	r = kv::krawczyk_approx(h, 2.1224, ix, 5, 0);
	if (r == false) return 1;

	ix2.resize(3);
	ix2(0) = 0.;
	ix2(1) = 1.;
	ix2(2) = ix;
	end = p;
	kv::odelong_maffine(f, ix2, itv(0.), end, kv::ode_param<double>(), kv::ode_callback_dense_print<double>((itv)0., (itv)pow(2., -7)));
	std::cout << "t: " << end << "\n";
	std::cout << ix2 << "\n";
}