#include <catch2/catch.hpp>
#include <iostream>

#include "../utils.hpp"
#include "../AutomaticDifferentiation.hpp"

#define print(x) PRINT_VAR(x);
#define printstr(x) PRINT_STR(x);

template<typename T>
bool check_almost_equal(const std::vector<T> & v1, const std::vector<T> & 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;
}

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

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

/// Spherical coordinates [r, lat, lon] to cartesian [x, y, z] transformation.
template<typename T>
Eigen::Array<T,3,1> sph2cart(const Eigen::Array<T,3,1> & xsph)
{
    Eigen::Array<T,3,1> xcart;
    xcart[0] = xsph[0]*cos(xsph[1])*cos(xsph[2]);
    xcart[1] = xsph[0]*cos(xsph[1])*sin(xsph[2]);
    xcart[2] = xsph[0]*sin(xsph[1]);
    return xcart;
}

/// Gradient of spherical coordinates [r, lat, lon] to cartesian [x, y, z] transformation.
template<typename T>
Eigen::Matrix<T,3,3> sph2cartJ(const Eigen::Array<T,3,1> & xsph)
{
    // Jij = Dfi/dxj
    Eigen::Matrix<T,3,3> Jcart;
    Jcart <<                    cos(xsph[2])*cos(xsph[1]), -xsph[0]*sin(xsph[1])*cos(xsph[2]), -xsph[0]*sin(xsph[2])*cos(xsph[1]),
                                sin(xsph[2])*cos(xsph[1]), -xsph[0]*sin(xsph[2])*sin(xsph[1]),  xsph[0]*cos(xsph[2])*cos(xsph[1]),
                                             sin(xsph[1]),               xsph[0]*cos(xsph[1]),                               T(0);
    return Jcart;
}

// dxdr = cos(xsph[2])*cos(xsph[1]),
// dxth = -xsph[0]*sin(xsph[1])*cos(xsph[2]),
// dxdphi = -xsph[0]*sin(xsph[2])*cos(xsph[1])
//
// dydr = sin(xsph[2])*cos(xsph[1]),
// dydth = -xsph[0]*sin(xsph[2])*sin(xsph[1]),
// dydxsph[2] = xsph[0]*cos(xsph[2])*cos(xsph[1])
//
// dzdr = sin(xsph[1]),
// dzth = xsph[0]*cos(xsph[1]),
// dzphi = 0

TEST_CASE( "Dual numbers : Constructors", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 2;
    using D = DualS<S,N>;
    using V = D::VectorT;

    SECTION( "Default Constructor" ) {
        D a;
        REQUIRE(a.a == 0);
        REQUIRE(a.b.size() == N);
        for(int i = 0 ; i < N ; i++)
            REQUIRE(a.b[i] == 0);
    }

    SECTION( "Constructor with a" ) {
        S x = 3.14159;
        D a(x);
        REQUIRE(a.a == x);
        REQUIRE(a.b.size() == N);
        for(int i = 0 ; i < N ; i++)
            REQUIRE(a.b[i] == 0);
    }

    SECTION( "Constructor with a and b" ) {
        S x = 3.14159;
        V y = {1.5, 3.2};
        D a(x, y);
        REQUIRE(a.a == x);
        REQUIRE(a.b.size() == 2);
        REQUIRE(a.b[0] == y[0]);
        REQUIRE(a.b[1] == y[1]);
    }

    SECTION( "operator= Dual" ) {
        S x = 3.14159;
        V y = {1.5, 3.2};
        D a, b(x, y);
        a = b;
        REQUIRE(a.a == x);
        REQUIRE(a.b.size() == 2);
        REQUIRE(a.b[0] == y[0]);
        REQUIRE(a.b[1] == y[1]);
    }

    SECTION( "operator= Dual" ) {
        S x = 3.14159;
        D a;
        a = x;
        REQUIRE(a.a == x);
        REQUIRE(a.b.size() == N);
        for(int i = 0 ; i < N ; i++)
            REQUIRE(a.b[i] == 0);
    }
}

TEST_CASE( "Dual numbers : access parts", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 2;
    using D = DualS<S,N>;
    using V = D::VectorT;

    S x = 3.14159;
    V v = {1.5, 3.2};

    SECTION( "x()" ) {
        D a(x, v);
        REQUIRE(a.x() == x);
    }

    SECTION( "d()" ) {
        D a(x, v);
        REQUIRE(a.d(0) == v[0]);
        REQUIRE(a.d(1) == v[1]);
    }
}

TEST_CASE( "Dual numbers : D() and diff()", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    S x = 3.14159;

    SECTION( "D(0)" ) {
        D a = D::D(0);
        REQUIRE(a.a == 0);
        REQUIRE(a.b.size() == N);
        REQUIRE(a.d(0) == 1);
    }

    SECTION( "D(0, 3)" ) {
        D a = D::D(0);
        REQUIRE(a.a == 0);
        REQUIRE(a.b.size() == N);
        REQUIRE(a.d(0) == 1);
        REQUIRE(a.d(1) == 0);
        REQUIRE(a.d(2) == 0);
    }

    SECTION( "D(1, 3)" ) {
        D a = D::D(1);
        REQUIRE(a.a == 0);
        REQUIRE(a.b.size() == N);
        REQUIRE(a.d(0) == 0);
        REQUIRE(a.d(1) == 1);
        REQUIRE(a.d(2) == 0);
    }

    SECTION( "D(2, 3)" ) {
        D a = D::D(2);
        REQUIRE(a.b.size() == N);
        REQUIRE(a.d(0) == 0);
        REQUIRE(a.d(1) == 0);
        REQUIRE(a.d(2) == 1);
    }

    SECTION( "diff(0)" ) {
        D a(x);
        a.diff(0);
        REQUIRE(a.b.size() == N);
        REQUIRE(a.d(0) == 1);
        REQUIRE(a.d(1) == 0);
        REQUIRE(a.d(2) == 0);
    }

    SECTION( "diff(i,N)" ) {
        D a(x);
        for(int i = 0 ; i < N ; i++)
        {
            a.diff(i);
            REQUIRE(a.a == x);
            REQUIRE(a.b.size() == N);
            for(int j = 0 ; j < N ; j++)
                REQUIRE(a.d(j) == (j==i));
        }
    }
}

#define MAKE_TEST_SECTION_COMPARISON_OPERATOR(Op, A, B)\
SECTION( "operator"#Op ) {          \
    D a(A), b(B);                   \
    bool resD = a Op b;             \
    bool resS = A Op B;             \
    REQUIRE(resD == resS);          \
}

TEST_CASE( "Dual numbers : comparison operators", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(==, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(==, 1., 2.)

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(!=, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(!=, 1., 2.)

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<, 2., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<, 1., 2.)

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<=, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<=, 2., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(<=, 1., 2.)

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>, 2., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>, 1., 2.)

    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>=, 1., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>=, 2., 1.)
    MAKE_TEST_SECTION_COMPARISON_OPERATOR(>=, 1., 2.)
}

#define MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR(Op, A, B)\
SECTION( "operator"#Op ) {          \
    D a(A), b(B);                   \
    S resD = (a Op b).a;            \
    S resS = A Op B;                \
    REQUIRE(resD == resS);          \
}

TEST_CASE( "Dual numbers : basic binary operators", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR(+, 3.14, 2.71)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR(-, 3.14, 2.71)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR(*, 3.14, 2.71)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR(/, 3.14, 2.71)
}

#define MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_POST(Op, A, B)\
SECTION( "operator"#Op ) {          \
    D a = D(A) Op (B);              \
    S resS = A Op B;                \
    REQUIRE(a.a == resS);           \
}

TEST_CASE( "Dual numbers : basic binary operators with type mismatch POST", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_POST(+, 3.14, (int)2)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_POST(-, 3.14, (int)2)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_POST(*, 3.14, (int)2)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_POST(/, 3.14, (int)2)
}

#define MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_PRE(Op, A, B)\
SECTION( "operator"#Op ) {          \
    D a = (A) Op D(B);              \
    S resS = A Op B;                \
    REQUIRE(a.a == resS);           \
}

TEST_CASE( "Dual numbers : basic binary operators with type mismatch PRE", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_PRE(+, (int)2, 3.14)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_PRE(-, (int)2, 3.14)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_PRE(*, (int)2, 3.14)
    MAKE_TEST_SECTION_BASIC_BINARY_OPERATOR_TYPE_MISMATCH_PRE(/, (int)2, 3.14)
}

TEST_CASE( "Dual numbers : basic unary operators", "[dual]" )
{
    std::cout.precision(16);
    constexpr int N = 3;
    using D = DualS<S,N>;

    SECTION( "operator++" ) {
        S sa = 3.14;
        D a(sa);
        a++;
        sa++;
        REQUIRE(a.a == sa);
    }

    SECTION( "operator--" ) {
        S sa = 3.14;
        D a(sa);
        a--;
        sa--;
        REQUIRE(a.a == sa);
    }

    SECTION( "operator++" ) {
        S sa = 3.14;
        D a(sa);
        ++a;
        ++sa;
        REQUIRE(a.a == sa);
    }

    SECTION( "operator--" ) {
        S sa = 3.14;
        D a(sa);
        --a;
        --sa;
        REQUIRE(a.a == sa);
    }

    SECTION( "operator+" ) {
        S sa = 3.14;
        D a(sa);
        a = +a;
        sa = +sa;
        REQUIRE(a.a == sa);
    }

    SECTION( "operator-" ) {
        S sa = 3.14;
        D a(sa);
        a = -a;
        sa = -sa;
        REQUIRE(a.a == sa);
    }
}

TEST_CASE( "Basic numbers : extended math library", "[dual]" )
{
    S x = 0.500000000000000000000000000000;
    REQUIRE(fabs(sec(x) - 1.139493927324549016333321560523) < tol);
    REQUIRE(fabs(cot(x) - 1.830487721712451998357096272230) < tol);
    REQUIRE(fabs(csc(x) - 2.085829642933488159428634389769) < tol);
    REQUIRE(fabs(asec(3*x) - 0.841068670567930221082519892661) < tol);
    REQUIRE(fabs(acot(x) - 1.107148717794090408972351724515) < tol);
    REQUIRE(fabs(acsc(3*x) - 0.729727656226966336916461841611) < tol);
    REQUIRE(fabs(sech(x) - 0.886818883970073912337284127716) < tol);
    REQUIRE(fabs(coth(x) - 2.163953413738652908904214200447) < tol);
    REQUIRE(fabs(csch(x) - 1.919034751334943722511638952710) < tol);
    REQUIRE(fabs(asech(x) - 1.316957896924816795447554795828) < tol);
    REQUIRE(fabs(acoth(3*x) - 0.804718956217050140899971211184) < tol);
    REQUIRE(fabs(acsch(x) - 1.443635475178810301244425318146) < tol);
    REQUIRE(fabs(exp10(x) - 3.162277660168379522787063251599) < tol);
    REQUIRE(fabs(sign(x) - 1.000000000000000000000000000000) < tol);
    REQUIRE(fabs(sign(-x) - -1.000000000000000000000000000000) < tol);
    REQUIRE(fabs(heaviside(x) - 1.000000000000000000000000000000) < tol);
    REQUIRE(fabs(heaviside(0.) - 1.000000000000000000000000000000) < tol);
    REQUIRE(fabs(heaviside(-x) - 0.000000000000000000000000000000) < tol);
}

TEST_CASE( "Dual numbers : Elementary function derivatives", "[dual]" )
{
    S xf = 0.500000000000000000000000000000;
    constexpr int N = 3;
    using D = DualS<S,N>;

    D x(xf); x.diff(0);
    CHECK(fabs((cos(x)).d(0) - (-sin(xf))) < tol);
    CHECK(fabs((sin(x)).d(0) - (cos(xf))) < tol);
    CHECK(fabs((tan(x)).d(0) - (pow(tan(xf), 2) + 1)) < tol);
    CHECK(fabs((acos(x)).d(0) - (-1/sqrt(-pow(xf, 2) + 1))) < tol);
    CHECK(fabs((asin(x)).d(0) - (pow(-pow(xf, 2) + 1, -1.0/2.0))) < tol);
    CHECK(fabs((atan(x)).d(0) - (1.0/(pow(xf, 2) + 1))) < tol);
    CHECK(fabs((sec(x)).d(0) - (tan(xf)*sec(xf))) < tol);
    CHECK(fabs((cot(x)).d(0) - (-pow(cot(xf), 2) - 1)) < tol);
    CHECK(fabs((csc(x)).d(0) - (-cot(xf)*csc(xf))) < tol);
    CHECK(fabs((asec(3*x)).d(0) - ((1.0/3.0)/(pow(xf, 2)*sqrt(1 - 1.0/9.0/pow(xf, 2))))) < tol);
    CHECK(fabs((acot(x)).d(0) - (-1/(pow(xf, 2) + 1))) < tol);
    CHECK(fabs((acsc(3*x)).d(0) - (-1.0/3.0/(pow(xf, 2)*sqrt(1 - 1.0/9.0/pow(xf, 2))))) < tol);
    CHECK(fabs((cosh(x)).d(0) - (sinh(xf))) < tol);
    CHECK(fabs((sinh(x)).d(0) - (cosh(xf))) < tol);
    CHECK(fabs((atanh(x)).d(0) - (1.0/(-pow(xf, 2) + 1))) < tol);
    CHECK(fabs((asinh(x)).d(0) - (pow(pow(xf, 2) + 1, -1.0/2.0))) < tol);
    CHECK(fabs((acosh(3*x)).d(0) - (3/sqrt(9*pow(xf, 2) - 1))) < tol);
    CHECK(fabs((atanh(x)).d(0) - (1.0/(-pow(xf, 2) + 1))) < tol);
    CHECK(fabs((sech(x)).d(0) - (-tanh(xf)*sech(xf))) < tol);
    CHECK(fabs((coth(x)).d(0) - (-1/pow(sinh(xf), 2))) < tol);
    CHECK(fabs((csch(x)).d(0) - (-coth(xf)*csch(xf))) < tol);
    CHECK(fabs((asech(x)).d(0) - (-1/(xf*sqrt(-pow(xf, 2) + 1)))) < tol);
    CHECK(fabs((acoth(3*x)).d(0) - (3/(-9*pow(xf, 2) + 1))) < tol);
    CHECK(fabs((acsch(x)).d(0) - (-1/(pow(xf, 2)*sqrt(1 + pow(xf, -2))))) < tol);
    CHECK(fabs(exp(x).d(0) - (exp(xf))) < tol);
    CHECK(fabs(exp2(x).d(0) - (pow(2, xf)*M_LN2)) < tol);
    CHECK(fabs(exp10(x).d(0) - (pow(10, xf)*M_LN10)) < tol);
    CHECK(fabs((log(x)).d(0) - (1.0/xf)) < tol);
    CHECK(fabs((log2(x)).d(0) - (1/(xf*M_LN2))) < tol);
    CHECK(fabs((log10(x)).d(0) - (1/(xf*M_LN10))) < tol);
    CHECK(fabs((sqrt(x)).d(0) - ((1.0/2.0)/sqrt(xf))) < tol);
    CHECK(fabs(cbrt(x).d(0) - ((1.0/3.0)/pow(xf, 2.0/3.0))) < tol);
    CHECK(fabs((abs(x)).d(0) - ((((xf) > 0) - ((xf) < 0)))) < tol);
    CHECK(fabs((fabs(x)).d(0) - ((((xf) > 0) - ((xf) < 0)))) < tol);
    CHECK(fabs((pow(x, D(1.0/7.0))).d(0) - ((1.0/7.0)/pow(xf, 6.0/7.0))) < tol);
    CHECK(fabs((pow(x,(1.0/7.0))).d(0) - ((1.0/7.0)/pow(xf, 6.0/7.0))) < tol);
    CHECK(fabs((pow(D(1.0/7.0), x)).d(0) - (-pow(7, -xf)*log(7))) < tol);
    CHECK(fabs((pow(1.0/7.0, x)).d(0) - (-pow(7, -xf)*log(7))) < tol);
}

TEST_CASE( "Dual numbers : derivatives of compositions of functions ", "[dual]" )
{
    S xf = 0.500000000000000000000000000000;
    constexpr int N = 3;
    using D = DualS<S,N>;

    D x(xf); x.diff(0);
    CHECK(fabs((pow(x, pow(x, x))).d(0) - (pow(xf, pow(xf, xf))*(pow(xf, xf)*(log(xf) + 1)*log(xf) + pow(xf, xf)/xf))) < tol);
    CHECK(fabs((log(sin(x))).d(0) - (cos(xf)/sin(xf))) < tol);
    CHECK(fabs((pow(x, 2)/log(x)).d(0) - (2*xf/log(xf) - xf/pow(log(xf), 2))) < tol);
    CHECK(fabs((cbrt(sin(x))).d(0) - ((1.0/3.0)*cos(xf)/pow(sin(xf), 2.0/3.0))) < tol);
}

TEST_CASE( "Dual numbers : univariate function derivative", "[dual]" )
{
    S xf = 0.1;
    constexpr int N = 3;
    using D = DualS<S,N>;
    D x(xf); x.diff(0);
    CHECK(fabs(fct1(x).x()  -  fct1(xf)) < tol);
    CHECK(fabs(fct1(x).d(0) - dfct1(xf)) < tol);
}

TEST_CASE( "Dual numbers : univariate function derivative using GradFunc", "[dual]" )
{
    S xf = 0.1;
    auto gradfct1 = GradFunc1(fct1<DualS<S,1>>, xf);
    CHECK(fabs(gradfct1(xf) - dfct1(xf)) < tol);

    S fx, dfx;
    gradfct1.get_f_grad(xf, fx, dfx);
    CHECK(fabs(fx - fct1(xf)) < tol);
    CHECK(fabs(dfx - dfct1(xf)) < tol);
}

TEST_CASE( "Dual numbers : multivariate function derivative", "[dual]" )
{
    constexpr int N = 3;
    using D = DualS<S,N>;

    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);
    REQUIRE(x.a == xf);
    REQUIRE(y.a == yf);
    REQUIRE(z.a == zf);
    print(fxyz)
    print(fxyz.d(0))
    print(fxyz.d(1))
    print(fxyz.d(2))

    print(dfct2dx(xf, yf, zf))
    print(dfct2dy(xf, yf, zf))
    print(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);
}

TEST_CASE( "Dual numbers : Jacobian using Eigen arrays and Dual directly", "[dual]" )
{
    constexpr int N = 3;
    using D = DualS<S,N>;

    constexpr S deg = M_PI/180.;
    S r = 3, theta = 30*deg, phi = 120*deg;
    Eigen::Array<D,3,1> xsph(r, theta, phi);
    xsph[0].diff(0);
    xsph[1].diff(1);
    xsph[2].diff(2);
    Eigen::Array<D,3,1> xcart = sph2cart(xsph);
    print(xcart.transpose())
    print(xcart[0].b.transpose())
    print(xcart[1].b.transpose())
    print(xcart[2].b.transpose())
    auto Jxcart = sph2cartJ(Eigen::Array<S,3,1>(r, theta, phi));
    std::cout << "Jxcart =\n" << (Jxcart) << std::endl;

    for(int i = 0 ; i < 3 ; i++)
        for(int j = 0 ; j < 3 ; j++)
            CHECK(fabs(xcart[i].d(j) - Jxcart(i,j)) < tol);
}