#include "AutomaticDifferentiationVector.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
	{
		DualVector<double, 1> x = 1.54;
		
		PRINT_DUAL(x);
		x.diff(0);
		PRINT_DUAL(x);

		DualVector<double, 1> 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
	{
		DualVector<double,3> x(2.), y(3.), z(-1.5);
		x.diff(0);
		y.diff(1);
		z.diff(2);
		DualVector<double,3> 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));
	}

	return 0;
}