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

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

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

template<typename T>
bool check_almost_equal(T a, T b, T tol)
{
    return std::abs(a - b) < tol;
}

template<typename T, typename V>
bool check_almost_equalV(const V & v1, const V & v2, T tol)
{
    bool ok = true;
    for(size_t i = 0 ; i < v1.size() ; i++)
        if(fabs(v1[i] - v2[i]) > tol)
        {
            ok = false;
            break;
        }
    return ok;
}

// ---------------------------------------------------------------------------------------------------------------------------------------------------

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);
}

// ---------------------------------------------------------------------------------------------------------------------------------------------------

/// Test function : length of vector
template<typename T, typename V>
T fctNto1(const V & x)
{
    T res = T(0);
    for(int i = 0 ; i < x.size() ; i++)
        res += x[i]*x[i];
    return sqrt(res);
}

/// Test function gradient : length of vector
template<typename T, typename V>
V dfctNto1(const V & x)
{
    T length = fctNto1<T>(x);
    V res = x;
    for(int i = 0 ; i < x.size() ; i++)
        res[i] = x[i]/length;
    return res;
}

// ---------------------------------------------------------------------------------------------------------------------------------------------------

constexpr int BIG_GLOBAL_N_STATIC = 10;
constexpr int BIG_GLOBAL_N_DYNAMIC = 100;

CREATE_GRAD_FUNCTION_OBJECT_1_1(fct1, Grad_fct1);
CREATE_GRAD_FUNCTION_OBJECT_N_1_S(fctNto1, Grad_fctNto1S, BIG_GLOBAL_N_STATIC);
CREATE_GRAD_FUNCTION_OBJECT_N_1_D(fctNto1, Grad_fctNto1D, BIG_GLOBAL_N_DYNAMIC);

int main()
{
    cout.precision(16);
    using S = double;
    constexpr S tol = 1e-12;

    { PRINT_STR("\nGradFunc1\n");
        S x = 0.1, fx, gradfx;
        auto grad_fct1 = GradFunc1(fct1<DualS<S,1>>, x);
        grad_fct1.get_f_grad(x, fx, gradfx);
        PRINT_VAR(x);
        PRINT_VAR(fx);
        PRINT_VAR(gradfx);
        PRINT_VAR(dfct1(x));
        assert(check_almost_equal(gradfx, dfct1(x), tol));
    }

	{ PRINT_STR("\nCREATE_GRAD_FUNCTION_OBJECT_1_1\n");
        S x = 0.1, fx, gradfx;
        auto grad_fct1 = Grad_fct1();
        grad_fct1.get_f_grad(x, fx, gradfx);
        PRINT_VAR(x);
        PRINT_VAR(fx);
        PRINT_VAR(gradfx);
        PRINT_VAR(dfct1(x));
        assert(check_almost_equal(gradfx, dfct1(x), tol));
    }

	{ PRINT_STR("\nCREATE_GRAD_FUNCTION_OBJECT_N_1_S\n");
        constexpr int N = BIG_GLOBAL_N_STATIC;
        // using D = DualD<S,N>;
        using V = Eigen::Array<S,N,1>;
        V x;
        for(int i = 0 ; i < x.size() ; i++)
            x[i] = S(i);
        S fx = fctNto1<S>(x);
        V dfx = dfctNto1<S>(x);
        PRINT_VAR(fx);
        PRINT_VEC(dfx);

        Grad_fctNto1S<S> grad_fctNto1;
        S fx2; V gradfx2;
        grad_fctNto1.get_f_grad(x, fx2, gradfx2);
        PRINT_VAR(fx2);
        PRINT_VEC(gradfx2);
        PRINT_VEC(grad_fctNto1(x));

        for (size_t i = 0; i < N; i++)
            cout << dfx[i] << "\t" << gradfx2[i] << "\t" << (dfx[i] - gradfx2[i]) << endl;

        assert(check_almost_equal(fx, fx2, tol));
        assert(check_almost_equalV(dfx, gradfx2, tol));
    }

	//*
    { PRINT_STR("\nCREATE_GRAD_FUNCTION_OBJECT_N_1_D\n");
        constexpr int N = BIG_GLOBAL_N_DYNAMIC;
        // using D = DualD<S,N>;
        using V = Eigen::Array<S,-1,1>;
        V x(N);
        for(int i = 0 ; i < x.size() ; i++)
            x[i] = S(i);
        S fx = fctNto1<S>(x);
        V dfx = dfctNto1<S>(x);
        PRINT_VAR(fx);
        PRINT_VEC(dfx);

        Grad_fctNto1D<S> grad_fctNto1;
        S fx2; V gradfx2;
        grad_fctNto1.get_f_grad(x, fx2, gradfx2);
        PRINT_VAR(fx2);
        PRINT_VEC(gradfx2);
        PRINT_VEC(grad_fctNto1(x));

        for (size_t i = 0; i < N; i++)
            cout << dfx[i] << "\t" << gradfx2[i] << "\t" << (dfx[i] - gradfx2[i]) << endl;

        assert(check_almost_equal(fx, fx2, tol));
        assert(check_almost_equalV(dfx, gradfx2, tol));
    }//*/
}