#include "AutomaticDifferentiation.hpp"

#include <iostream>
#include <iomanip>
using std::cout;
using std::endl;
using std::setw;
#define PRINT_VAR(x) std::cout << #x << "\t= " << std::setprecision(16) << (x) << std::endl
#define PRINT_DUAL(x) std::cout << #x << "\t= " << std::fixed << std::setprecision(4) << std::setw(10) << (x).a << ", " << std::setw(10) << (x).b << std::endl

#define TEST_EQ_TOL 1e-9
// #define TEST_FUNCTION_ON_DUAL_DOUBLE(fct, x) assert(fct(Dual<double>(x)).a == fct(x))
// #define TEST_FUNCTION_ON_DUAL_DOUBLE(fct, x) assert(abs((fct(Dual<double>((x))).a) - (fct((x)))) <= TEST_EQ_TOL)
#define TEST_EQ_DOUBLE(a, b) assert(std::abs((a) - (b)) <= TEST_EQ_TOL)

template<typename Scalar>
Scalar f1(const Scalar & x)
{
	return Scalar(5.)*x*x*x + Scalar(3.)*x*x - Scalar(2.)*x + Scalar(4.);
}

template<typename Scalar>
Scalar df1(const Scalar & x)
{
	return Scalar(15.)*x*x + Scalar(6.)*x - Scalar(2.);
}

template<typename Scalar>
Scalar f3(const Scalar & x, const Scalar & y, const Scalar & z)
{
	return sqrt(z*z+y*y+x*x);
}

template<typename Scalar>
Scalar df3x(const Scalar & x, const Scalar & y, const Scalar & z)
{
	return x/sqrt(z*z+y*y+x*x);
}

template<typename Scalar>
Scalar df3y(const Scalar & x, const Scalar & y, const Scalar & z)
{
	return y/sqrt(z*z+y*y+x*x);
}

template<typename Scalar>
Scalar df3z(const Scalar & x, const Scalar & y, const Scalar & z)
{
	return z/sqrt(z*z+y*y+x*x);
}

int main()
{
	// 1 var function
	{
		Dual<double> x = 1.54;
		
		PRINT_DUAL(x);
		x.diff(0);
		PRINT_DUAL(x);

		Dual<double> y = f1(x);

		PRINT_VAR(x);
		PRINT_DUAL(y);
		PRINT_VAR(y.b);
		PRINT_VAR(df1(x.a));

		TEST_EQ_DOUBLE(df1(x.a), y.b[0]);
	}

	// 3 var function
	{
		Dual<double> x(2.), y(3.), z(-1.5);
		x.diff(0, 3);
		y.diff(1, 3);
		z.diff(2, 3);
		Dual<double> res = f3(x, y, z);

		PRINT_DUAL(res);
		PRINT_VAR(res.b[0]);
		PRINT_VAR(res.b[1]);
		PRINT_VAR(res.b[2]);
		PRINT_VAR(df3x(x.a, y.a, z.a));
		PRINT_VAR(df3y(x.a, y.a, z.a));
		PRINT_VAR(df3z(x.a, y.a, z.a));

		TEST_EQ_DOUBLE(res.b[0], df3x(x.a, y.a, z.a));
		TEST_EQ_DOUBLE(res.b[1], df3y(x.a, y.a, z.a));
		TEST_EQ_DOUBLE(res.b[2], df3z(x.a, y.a, z.a));
	}

	// test d() diff and D
	{
		Dual<double> x;// by default, size(b)=1
		assert(x.b.size() == 1);
		// x.d(3);// assertion thrown
		x.d(0);// good
		assert(x.d(0) == x.b[0]);

		// assignment through d()
		x.d(0) = 1.;// good
		assert(x.d(0) == 1.);

		// b resizing through d
		x.diff(2, 3);// good
		assert(x.b[0] == 1.);
		assert(x.b[1] == 0.);
		assert(x.b[2] == 1.);
		assert(x.b[2] == 1.);

		assert(Dual<double>::D(0).b.size() == 1);
		assert(Dual<double>::D(0).b[0] == 1.);

		assert(Dual<double>::D(0, 3).b[0] == 1.);
		assert(Dual<double>::D(0, 3).b[1] == 0.);
		assert(Dual<double>::D(0, 3).b[2] == 0.);

		assert(Dual<double>::D(1, 3).b[0] == 0.);
		assert(Dual<double>::D(1, 3).b[1] == 1.);
		assert(Dual<double>::D(1, 3).b[2] == 0.);

		assert(Dual<double>::D(2, 3).b[0] == 0.);
		assert(Dual<double>::D(2, 3).b[1] == 0.);
		assert(Dual<double>::D(2, 3).b[2] == 1.);
	}

	return 0;
}