#include <iostream>
#include <Eigen/Dense>

#include <utils.hpp>
#include <AutomaticDifferentiation.hpp>

using std::cout;
using std::endl;
using namespace Eigen;

template<typename T>
T fct1(const T & x)
{
    return cos(x) + sqrt(x) + cbrt(sin(x)) + exp(-x*x)/log(x) + atanh(5*x) - 2*csc(x);
}

template<typename T>
T dfct1(const T & x)
{
    return -2*x*exp(-pow(x, 2))/log(x) - sin(x) + 2*cot(x)*csc(x) + (1.0/3.0)*cos(x)/pow(sin(x), 2.0/3.0) + 5/(-25*pow(x, 2) + 1) - exp(-pow(x, 2))/(x*pow(log(x), 2)) + (1.0/2.0)/sqrt(x);
}

template<typename T>
struct Fct1
{
    T operator()(const T & x) { return fct1(x); }
};

template<typename T>
T fct2(const T & x, const T & y, const T & z)
{
    T two(2);
    return 2*sqrt(x)*sqrt(y)/log(sin(z)) + sqrt(y) + exp(-pow(x, two) - pow(y, two) - pow(z, two))/(log(x)*log(y)*log(z)) + cbrt(sin(z))*tan(y) + cos(x) + 2*atanh(5*x)*csc(y);
}
template<typename T>
T dfct2dx(const T & x, const T & y, const T & z)
{
    T two(2);
    return -2*x*exp(-pow(x, two) - pow(y, two) - pow(z, two))/(log(x)*log(y)*log(z)) - sin(x) + 10*csc(y)/(-25*pow(x, two) + 1) - exp(-pow(x, two) - pow(y, two) - pow(z, two))/(x*pow(log(x), two)*log(y)*log(z)) + sqrt(y)/(sqrt(x)*log(sin(z)));
}
template<typename T>
T dfct2dy(const T & x, const T & y, const T & z)
{
    T two(2);
    return sqrt(x)/(sqrt(y)*log(sin(z))) - 2*y*exp(-pow(x, two) - pow(y, two) - pow(z, two))/(log(x)*log(y)*log(z)) + (pow(tan(y), two) + 1)*cbrt(sin(z)) - 2*cot(y)*atanh(5*x)*csc(y) - exp(-pow(x, two) - pow(y, two) - pow(z, two))/(y*log(x)*pow(log(y), two)*log(z)) + (1.0/2.0)/sqrt(y);
}
template<typename T>
T dfct2dz(const T & x, const T & y, const T & z)
{
    T two(2);
    return -2*sqrt(x)*sqrt(y)*cos(z)/(pow(log(sin(z)), two)*sin(z)) - 2*z*exp(-pow(x, two) - pow(y, two) - pow(z, two))/(log(x)*log(y)*log(z)) + (1.0/3.0)*cos(z)*tan(y)/pow(sin(z), 2.0/3.0) - exp(-pow(x, two) - pow(y, two) - pow(z, two))/(z*log(x)*log(y)*pow(log(z), two));
}

void test_derivatives();
int main()
{
	{// C++14 version
		{
			PRINT_STR("\nStatic\n");
			{
				DualS<double, 3> a;
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualS<double, 3> a(3.14);
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualS<double, 3> a(3.14, {1.,2.,3.});
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualS<double, 3> a(3.14, {1.,2.,3.}), b(2.71, {-1., 3., 4.});
				auto c = a + b;
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
				PRINT_VAR(b.a);
				PRINT_VAR(b.b.transpose());
				PRINT_VAR(c.a);
				PRINT_VAR(c.b.transpose());
			}
			{
				double af = 3.14, bf = 2.71;
				DualS<double, 2> a(af, {1.,0.}), b(bf, {0., 1.});
				PRINT_VAR((a+b).a);
				PRINT_VAR((a+b).b.transpose());
				PRINT_VAR((a-b).a);
				PRINT_VAR((a-b).b.transpose());
				PRINT_VAR((a*b).a);
				PRINT_VAR((a*b).b.transpose());
				PRINT_VAR((a/b).a);
				PRINT_VAR((a/b).b.transpose());
				PRINT_VAR(((a+b)*(a+b)).a);
				PRINT_VAR(((a+b)*(a+b)).b.transpose());
				PRINT_VAR((sqrt(a)*sqrt(b)).b.transpose());
				PRINT_VAR((1.0/2.0)*sqrt(bf)/sqrt(af));
				PRINT_VAR((1.0/2.0)*sqrt(af)/sqrt(bf));
			}
		}
		{
			PRINT_STR("\nDynamic\n");
			{
				DualD<double, 3> a;
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualD<double, 3> a(3.14);
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualD<double, 3>::VectorT ab(3); ab << 1.,2.,3.;
				DualD<double, 3> a(3.14, ab);
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
			}
			{
				DualD<double, 3>::VectorT ab(3); ab << 1.,2.,3.;
				DualD<double, 3>::VectorT bb(3); bb << -1.,3.,4.;
				DualD<double, 3> a(3.14, ab), b(2.71, bb);
				auto c = a + b;
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
				PRINT_VAR(b.a);
				PRINT_VAR(b.b.transpose());
				PRINT_VAR(c.a);
				PRINT_VAR(c.b.transpose());
			}
			{
				double af = 3.14, bf = 2.71;
				DualD<double, 3>::VectorT ab(3); ab << 1.,2.,3.;
				DualD<double, 3>::VectorT bb(3); bb << -1.,3.,4.;
				DualD<double, 3> a(af, ab), b(bf, bb);
				auto c = (1 + 3*a) + b*2 + 1/b + (3 - a);
				PRINT_VAR(a.a);
				PRINT_VAR(a.b.transpose());
				PRINT_VAR(b.a);
				PRINT_VAR(b.b.transpose());
				PRINT_VAR(c.a);
				PRINT_VAR(c.b.transpose());
				assert(c.a == ((1 + 3*af) + bf*2 + 1/bf + (3 - af)));
			}
			{
				double af = 3.14, bf = 2.71;
				DualD<double, 2> a(af), b(bf); a.diff(0); a.diff(1);
				PRINT_VAR((a+b).a);
				PRINT_VAR((a+b).b.transpose());
				PRINT_VAR((a-b).a);
				PRINT_VAR((a-b).b.transpose());
				PRINT_VAR((a*b).a);
				PRINT_VAR((a*b).b.transpose());
				PRINT_VAR((a/b).a);
				PRINT_VAR((a/b).b.transpose());
				PRINT_VAR(((a+b)*(a+b)).a);
				PRINT_VAR(((a+b)*(a+b)).b.transpose());
				PRINT_VAR((sqrt(a)*sqrt(b)).b.transpose());
				PRINT_VAR((1.0/2.0)*sqrt(bf)/sqrt(af));
				PRINT_VAR((1.0/2.0)*sqrt(af)/sqrt(bf));
			}
		}

		#if 0
		{// should throw an compile-time assertion
			Dual<double, 0> a;
		}
		#endif
		#if 0
		{
			DualD<double, 3> ad;
			DualS<double, 3> as;
			ad.b.resize(5);
			as.b.resize(5);// should throw an compile-time error
		}
		#endif
	}

	test_derivatives();

	return 0;
}

#define PRINT_F_DF(Term) cout << #Term << "\t: " << (Term).x() << "\t" << (Term).d(0) << "\n"

void test_derivatives()
{
	cout.precision(16);

	using S = double;
	constexpr S tol = 1e-12;

	{
		PRINT_STR("--------------------------------");
		DualS<S,1> x(.1); x.diff(0);
		PRINT_VAR(x);
		PRINT_F_DF(cos(x));
		PRINT_F_DF(sin(x));
		PRINT_F_DF(tan(x));
		PRINT_F_DF(sec(x));
		PRINT_F_DF(csc(x));
		PRINT_F_DF(cot(x));

		PRINT_F_DF(sqrt(x));
		PRINT_F_DF(cbrt(x));
		PRINT_F_DF(exp(x));
		PRINT_F_DF(2*exp(x));
		PRINT_F_DF(exp(2*x));
	}

	#if 1
	{// univariate
		PRINT_STR("--------------------------------");
		using D = DualS<S,1>;
		S xf = 0.1;
		D x(xf); x.diff(0);

		PRINT_VAR(fct1(x));
		PRINT_VAR(fct1(xf));

		PRINT_VAR(fct1(x).d(0));
		PRINT_VAR(dfct1(xf));

		assert(fabs(fct1(x).x()  -  fct1(xf)) < tol);
		assert(fabs(fct1(x).d(0) - dfct1(xf)) < tol);
	}
	#endif

	#if 1
	{
		using D = DualS<S,3>;
	    S xf = 0.11, yf = .12, zf = .13;
	    D x(xf); x.diff(0);
	    D y(yf); y.diff(1);
	    D z(zf); z.diff(2);
	    D fxyz = fct2(x, y, z);
	    assert(x.a == xf);
	    assert(y.a == yf);
	    assert(z.a == zf);
	    PRINT_VAR(fxyz);
	    PRINT_VAR(fxyz.d(0));
	    PRINT_VAR(fxyz.d(1));
	    PRINT_VAR(fxyz.d(2));

	    PRINT_VAR(dfct2dx(xf, yf, zf));
	    PRINT_VAR(dfct2dy(xf, yf, zf));
	    PRINT_VAR(dfct2dz(xf, yf, zf));

	    // CHECK(fabs(fxyz.d(0) - dfct2dx(xf, yf, zf)) < tol);
	    // CHECK(fabs(fxyz.d(1) - dfct2dy(xf, yf, zf)) < tol);
	    // CHECK(fabs(fxyz.d(2) - dfct2dz(xf, yf, zf)) < tol);
	}
	#endif

}