#include "test_Physics.hpp"

inline constexpr size_t operator "" _zu(unsigned long long int i) {
    return (size_t) i;
}

class MasterTriangle : public ::testing::Test {
public:
    virtual void SetUp() {
        node.push_back(XY{0.0, 0.0});
        node.push_back(XY{1.0, 0.0});
        node.push_back(XY{0.0, 1.0});

        tri.push_back(Triangle<1>(0, 1_zu, 2_zu, 0_zu));

        reg.push_back(Region<2>(std::vector<size_t>{0}));

        bnd.push_back(Boundary<2>(std::vector<size_t>{0, 1}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{0, 2}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{1, 2}));

        femesh = FiniteElementMesh<2, 1>(node, tri, reg, bnd);
    }

    std::vector<XY> node;
    std::vector<Triangle<1>> tri;
    std::vector<Region<2>> reg;
    std::vector<Boundary<2>> bnd;

    FiniteElementMesh<2, 1> femesh;
};

TEST_F(MasterTriangle, mass_matrix) {
    auto mat = femesh.basis<2>();
    auto det = femesh.determinant<2>();

    Eigen::SparseMatrix<double> M = (mat[0] * det(0) * mat[0].transpose()) * TriangleQuadratureRule<2>::w[0]
                                    + (mat[1] * det(1) * mat[1].transpose()) * TriangleQuadratureRule<2>::w[1]
                                    + (mat[2] * det(2) * mat[2].transpose()) * TriangleQuadratureRule<2>::w[2];

    EXPECT_NEAR(M.coeffRef(0, 0), 1.0 / 12.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(1, 0), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(2, 0), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(0, 1), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(1, 1), 1.0 / 12.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(2, 1), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(0, 2), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(1, 2), 1.0 / 24.0, FLT_EPSILON);
    EXPECT_NEAR(M.coeffRef(2, 2), 1.0 / 12.0, FLT_EPSILON);
}

TEST_F(MasterTriangle, stiffness_matrix) {
    auto df = femesh.derivative<1>();
    auto det = femesh.determinant<1>();

    Eigen::SparseMatrix<double> K = (df.dx(0) * det(0) * df.dx(0).transpose() + df.dy(0) * det(0) * df.dy(0).transpose()) * TriangleQuadratureRule<1>::w[0];

    EXPECT_NEAR(K.coeffRef(0, 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(1, 0), -1.0 / 2.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(2, 0), -1.0 / 2.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(0, 1), -1.0 / 2.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(1, 1), 1.0 / 2.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(2, 1), 0.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(0, 2), -1.0 / 2.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(1, 2), 0.0, FLT_EPSILON);
    EXPECT_NEAR(K.coeffRef(2, 2), 1.0 / 2.0, FLT_EPSILON);
}

TEST_F(MasterTriangle, magnetostatic) {
    //TODO: This isn't really testing anything...
    static constexpr size_t Q = 1;

    Magnetostatic<2, 1, Q, Variable::A> ms(femesh);
    ms.time(1.0);

    auto ff = [](double t) { return 1.0 * t; };

    ms.add_current_density(ff, std::vector<size_t>{0});

    ms.build_matrices();

    auto J = ms.init_unknown_matrix();
    auto v = ms.init_unknown_vector();
    auto r = ms.init_unknown_vector();
    auto f = ms.init_unknown_vector();
    auto Fx = ms.init_element_vector();
    auto Fy = ms.init_element_vector();
    auto dFxdx = ms.init_element_matrix();
    auto dFydy = ms.init_element_matrix();
    auto dFxdy = ms.init_element_matrix();

    ms.calculate_forcing(f);
    ms.linearize(J, v, r, f, Fx, Fy, dFxdx, dFydy, dFxdy);

    //auto ff = [](double t) { return 1.0 * t; };

    //HomogeneousForcing<2,1,Q> hf{ff, std::vector<size_t>{0}, ms.basis(), ms.weights(), ms.domain()};

    std::cout << f << std::endl;
}

class MasterTriangleRotationReflection : public ::testing::Test {
public:
    virtual void SetUp() {
        node.push_back(XY{0.0, 0.0});
        node.push_back(XY{1.0, 0.0});
        node.push_back(XY{0.0, 1.0});
        node.push_back(XY{-1.0, 0.0});
        node.push_back(XY{0.0, -1.0});

        tri.push_back(Triangle<1>(0, 1_zu, 2_zu, 0_zu)); // Master
        tri.push_back(Triangle<1>(1, 2_zu, 3_zu, 0_zu)); // 90 degree
        tri.push_back(Triangle<1>(2, 3_zu, 4_zu, 0_zu)); // 180 degree
        tri.push_back(Triangle<1>(3, 4_zu, 1_zu, 0_zu)); // 270 degree
        tri.push_back(Triangle<1>(4, 3_zu, 2_zu, 0_zu)); // y-axis reflection
        tri.push_back(Triangle<1>(5, 1_zu, 4_zu, 0_zu)); // x-axis reflection
        tri.push_back(Triangle<1>(6, 4_zu, 3_zu, 0_zu)); // x=y reflection

        reg.push_back(Region<2>(std::vector<size_t>{0, 1, 2, 3})); // Triangles 4,5,6 are duplicates

        femesh = FiniteElementMesh<2, 1>(node, tri, reg, bnd);
    }

    std::vector<XY> node;
    std::vector<Triangle<1>> tri;
    std::vector<Region<2>> reg;
    std::vector<Boundary<2>> bnd;


    FiniteElementMesh<2, 1> femesh;
};

TEST_F(MasterTriangleRotationReflection, jacobian_1) {
    auto J = tri[0].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), 1.0, FLT_EPSILON);

    J = tri[1].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), 0.0, FLT_EPSILON);

    J = tri[2].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), -1.0, FLT_EPSILON);

    J = tri[3].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), 1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), 0.0, FLT_EPSILON);

    J = tri[4].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), 1.0, FLT_EPSILON);

    J = tri[5].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), -1.0, FLT_EPSILON);

    J = tri[6].jacobian<1>(node);

    EXPECT_NEAR(J(0, 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(J(0, 1), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(J(1, 1), 0.0, FLT_EPSILON);
}

TEST_F(MasterTriangleRotationReflection, basis_1) {
    auto mat = femesh.basis<1>();

    for (size_t i = 0; i != tri.size(); ++i) {
        EXPECT_NEAR(mat(0, tri[i].node(0), i), 1.0 / 3.0, FLT_EPSILON);
        EXPECT_NEAR(mat(0, tri[i].node(1), i), 1.0 / 3.0, FLT_EPSILON);
        EXPECT_NEAR(mat(0, tri[i].node(2), i), 1.0 / 3.0, FLT_EPSILON);
    }
}

TEST_F(MasterTriangleRotationReflection, derivative_1) {
    auto df = femesh.derivative<1>();

    EXPECT_NEAR(df.dx(0, tri[0].node(0), 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(df.dy(0, tri[0].node(0), 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(df.dx(0, tri[0].node(1), 0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(df.dy(0, tri[0].node(1), 0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(df.dx(0, tri[0].node(2), 0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(df.dy(0, tri[0].node(2), 0), -1.0, FLT_EPSILON);
}

TEST_F(MasterTriangleRotationReflection, basis_2) {
    auto mat = femesh.basis<2>();

    for (size_t i = 0; i != tri.size(); ++i) {
        EXPECT_NEAR(mat(0, tri[i].node(0), i), 2.0 / 3.0, FLT_EPSILON);
        EXPECT_NEAR(mat(0, tri[i].node(1), i), 1.0 / 6.0, FLT_EPSILON);
        EXPECT_NEAR(mat(0, tri[i].node(1), i), 1.0 / 6.0, FLT_EPSILON);

        EXPECT_NEAR(mat(1, tri[i].node(0), i), 1.0 / 6.0, FLT_EPSILON);
        EXPECT_NEAR(mat(1, tri[i].node(1), i), 2.0 / 3.0, FLT_EPSILON);
        EXPECT_NEAR(mat(1, tri[i].node(2), i), 1.0 / 6.0, FLT_EPSILON);

        EXPECT_NEAR(mat(2, tri[i].node(0), i), 1.0 / 6.0, FLT_EPSILON);
        EXPECT_NEAR(mat(2, tri[i].node(1), i), 1.0 / 6.0, FLT_EPSILON);
        EXPECT_NEAR(mat(2, tri[i].node(2), i), 2.0 / 3.0, FLT_EPSILON);
    }
}

TEST_F(MasterTriangleRotationReflection, derivative_2) {
    auto df = femesh.derivative<2>();

    for (size_t q = 0; q != 3; ++q) {
        EXPECT_NEAR(df.dx(q, tri[0].node(0), 0), 1.0, FLT_EPSILON);
        EXPECT_NEAR(df.dy(q, tri[0].node(0), 0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(df.dx(q, tri[0].node(1), 0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(df.dy(q, tri[0].node(1), 0), 1.0, FLT_EPSILON);
        EXPECT_NEAR(df.dx(q, tri[0].node(2), 0), -1.0, FLT_EPSILON);
        EXPECT_NEAR(df.dy(q, tri[0].node(2), 0), -1.0, FLT_EPSILON);
    }
}

class SimpleSquare : public ::testing::Test {
public:
    virtual void SetUp() {
        node.push_back(XY{0.0, 0.0});
        node.push_back(XY{1.0, 0.0});
        node.push_back(XY{0.0, 1.0});
        node.push_back(XY{1.0, 1.0});

        tri.push_back(Triangle<1>(0, 0_zu, 1_zu, 2_zu));
        tri.push_back(Triangle<1>(1, 1_zu, 3_zu, 2_zu));

        reg.push_back(Region<2>(std::vector<size_t>{0}));
        reg.push_back(Region<2>(std::vector<size_t>{1}));

        bnd.push_back(Boundary<2>(std::vector<size_t>{0, 1}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{0, 2}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{1, 2}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{1, 3}));
        bnd.push_back(Boundary<2>(std::vector<size_t>{2, 3}));

        femesh = FiniteElementMesh<2, 1>(node, tri, reg, bnd);

        vx_m << 1.0, 0.0, 1.0, 0.0;
        vx_p << 0.0, 1.0, 0.0, 1.0;
        vy_m << 1.0, 1.0, 0.0, 0.0;
        vy_p << 0.0, 0.0, 1.0, 1.0;
    }

    std::vector<XY> node;
    std::vector<Triangle<1>> tri;
    std::vector<Region<2>> reg;
    std::vector<Boundary<2>> bnd;

    Eigen::Vector4d vx_m;
    Eigen::Vector4d vx_p;
    Eigen::Vector4d vy_m;
    Eigen::Vector4d vy_p;

    FiniteElementMesh<2, 1> femesh;
};

TEST_F(SimpleSquare, derivative_1_temp) {
    auto df = femesh.derivative<1>();

    Eigen::Vector2d tmp;

    tmp = df.dx(0).transpose() * vx_p;
    EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);

    tmp = df.dx(0).transpose() * vx_m;
    EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);

    tmp = df.dx(0).transpose() * vy_m;
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

    tmp = df.dx(0).transpose() * vy_m;
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

    tmp = df.dy(0).transpose() * vy_p;
    EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);

    tmp = df.dy(0).transpose() * vy_m;
    EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);

    tmp = df.dy(0).transpose() * vx_m;
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

    tmp = df.dy(0).transpose() * vx_m;
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
    EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
}

TEST_F(SimpleSquare, derivative_2_temp) {
    auto df = femesh.derivative<2>();

    Eigen::Vector2d tmp;

    for (size_t i = 0; i != 3; ++i) {
        tmp = df.dx(i).transpose() * vx_p;
        EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);

        tmp = df.dx(i).transpose() * vx_m;
        EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);

        tmp = df.dx(i).transpose() * vy_m;
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

        tmp = df.dx(i).transpose() * vy_m;
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

        tmp = df.dy(i).transpose() * vy_p;
        EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 1.0, FLT_EPSILON);

        tmp = df.dy(i).transpose() * vy_m;
        EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), -1.0, FLT_EPSILON);

        tmp = df.dy(i).transpose() * vx_m;
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);

        tmp = df.dy(i).transpose() * vx_m;
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
        EXPECT_NEAR(tmp(0), 0.0, FLT_EPSILON);
    }
}

TEST_F(SimpleSquare, magnetostatic_forcing) {
    // TODO: Write Physics::add_condition() (templated?)
    // TODO: BoundaryCondition, ForcingCondition, MotionCondition?
    // TODO: ?then template parameters of conditions can be hidden?
    // TODO: Hierarchy of conditions: Those altering RHS only (forcing, some bcs), those altering matrices in a fixed way (some bcs), those altering the mesh (motion, some bcs)
    //      TODO: Applied in reverse order, Motion->BoundaryCondition->Forcing,
    //      TODO: Or, TimeVarying->TimeInvariant->Forcing?

    Magnetostatic<2, 1, 2, Variable::A> ms{femesh};
    ms.time(1.0);
    ms.build_matrices();

    auto ff = [](double t) { return 1.0 * t; };
    auto f = ms.init_unknown_vector();

    ms.add_current_density(ff, std::vector<size_t>{0});
    ms.calculate_forcing(f);
    EXPECT_NEAR(f(0), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(1), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(2), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(3), 0.0 / 6.0, FLT_EPSILON);
    ms.remove_current_density(0);

    ms.add_current_density(ff, std::vector<size_t>{1});
    ms.calculate_forcing(f);
    EXPECT_NEAR(f(0), 0.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(1), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(2), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(3), 1.0 / 6.0, FLT_EPSILON);
    ms.remove_current_density(0);

    ms.add_current_density(ff, std::vector<size_t>{0, 1});
    ms.calculate_forcing(f);
    EXPECT_NEAR(f(0), 1.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(1), 2.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(2), 2.0 / 6.0, FLT_EPSILON);
    EXPECT_NEAR(f(3), 1.0 / 6.0, FLT_EPSILON);
    ms.remove_current_density(0);

    /*
    auto J = ms.init_matrix();
    auto v = ms.init_vector();
    auto r = ms.init_vector();
    v.setZero();

    ms.linearize(J,v,r,f);

    //Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    //solver.compute(J);
    //if(solver.info()!=Eigen::Success) {
    //    std::cout << "Failed" << std::endl;
    //} else {
    //    std::cout << "Success" << std::endl;
    //}

    Eigen::SparseMatrix<double> J3 = J.block(1,1,3,3);
    Eigen::SparseMatrix<double> J2 = J.block(1,0,3,1);
    Eigen::SparseMatrix<double> J1 = J.block(0,1,1,3);
    Eigen::SparseMatrix<double> J0 = J.block(0,0,1,1);

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    Eigen::SparseMatrix<double> Jb = J3 - (J2 * (J0 \ J1));

    std::cout << J3 << std::endl;
    std::cout << J2 << std::endl;
    std::cout << J1 << std::endl;
    std::cout << J0 << std::endl;
    std::cout << Jb << std::endl;

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Lower|Eigen::Upper> cg;
    cg.compute(J);
    if(cg.info()!=Eigen::Success) {
        std::cout << "Failed" << std::endl;
    } else {
        std::cout << "Success" << std::endl;
    }
    v -= cg.solve(r);

    std::cout << J << std::endl;
    std::cout << v << std::endl;
    std::cout << r << std::endl;
    std::cout << f << std::endl;
    std::cout << J * v - r << std::endl;


    ms.linearize(J,v,r,f);
    std::cout << J << std::endl;
    std::cout << v << std::endl;
    std::cout << r << std::endl;
    std::cout << f << std::endl;

    std::cout << ms.derivatives().dx(0).transpose() * v << std::endl;
    std::cout << ms.derivatives().dy(0).transpose() * v << std::endl;
    */
}

class TwoRegionHex : public ::testing::Test {
public:
    virtual void SetUp() {
        node.push_back(XY{0.0, 0.0});

        double r{1.0};
        for (size_t i = 0; i != 6; ++i) {
            double a = (i * M_PI) / 3.0;
            node.push_back(XY{r * std::cos(a), r * std::sin(a)});
        }

        r = 2.0;
        for (size_t i = 0; i != 6; ++i) {
            double a = (i * M_PI) / 3.0 + M_PI / 6.0;
            node.push_back(XY{r * std::cos(a), r * std::sin(a)});
        }

        tri.push_back(Triangle<1>(0, 0_zu, 1_zu, 2_zu));
        tri.push_back(Triangle<1>(1, 0_zu, 2_zu, 3_zu));
        tri.push_back(Triangle<1>(2, 0_zu, 3_zu, 4_zu));
        tri.push_back(Triangle<1>(3, 0_zu, 4_zu, 5_zu));
        tri.push_back(Triangle<1>(4, 0_zu, 5_zu, 6_zu));
        tri.push_back(Triangle<1>(5, 0_zu, 6_zu, 1_zu));

        reg.push_back(Region<2>(std::vector<size_t>{0, 1, 2, 3, 4, 5}));

        bnd.push_back(Boundary<2>(std::vector<size_t>{1, 2, 3, 4, 5, 6}));

        tri.push_back(Triangle<1>(6, 1_zu, 7_zu, 2_zu));
        tri.push_back(Triangle<1>(7, 2_zu, 7_zu, 8_zu));

        tri.push_back(Triangle<1>(8, 2_zu, 8_zu, 3_zu));
        tri.push_back(Triangle<1>(9, 3_zu, 8_zu, 9_zu));

        tri.push_back(Triangle<1>(10, 3_zu, 9_zu, 4_zu));
        tri.push_back(Triangle<1>(11, 4_zu, 9_zu, 10_zu));

        tri.push_back(Triangle<1>(12, 4_zu, 10_zu, 5_zu));
        tri.push_back(Triangle<1>(13, 5_zu, 10_zu, 11_zu));

        tri.push_back(Triangle<1>(14, 5_zu, 11_zu, 6_zu));
        tri.push_back(Triangle<1>(15, 6_zu, 11_zu, 12_zu));

        tri.push_back(Triangle<1>(16, 6_zu, 12_zu, 1_zu));
        tri.push_back(Triangle<1>(17, 1_zu, 12_zu, 7_zu));

        reg.push_back(Region<2>(std::vector<size_t>{6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}));

        bnd.push_back(Boundary<2>(std::vector<size_t>{7, 8, 9, 10, 11, 12}));

        femesh = FiniteElementMesh<2, 1>(node, tri, reg, bnd);
    }

    std::vector<XY> node;
    std::vector<Triangle<1>> tri;
    std::vector<Region<2>> reg;
    std::vector<Boundary<2>> bnd;

    FiniteElementMesh<2, 1> femesh;
};

TEST_F(TwoRegionHex, magnetostatic_forcing) {
    Magnetostatic<2, 1, 1, Variable::A> ms{femesh};
    ms.time(0.0);

    // Test without boundary conditions
    ms.build_matrices();

    auto ff = [](double t) { return 1.0; };
    auto f = ms.init_unknown_vector();

    ms.add_current_density(ff, std::vector<size_t>{0});
    ms.add_magnetic_insulation(std::vector<size_t>{1});
    ms.calculate_forcing(f);

    double nodal_current{sqrt(3.0) / 4.0 / 3.0};
    EXPECT_NEAR(f(0), nodal_current * 6.0, FLT_EPSILON);
    for (size_t i = 1; i != 7; ++i) {
        EXPECT_NEAR(f(i), nodal_current * 2.0, FLT_EPSILON);
    }

    auto J = ms.init_unknown_matrix();
    auto v = ms.init_unknown_vector();
    auto r = ms.init_unknown_vector();
    auto Fx = ms.init_element_vector();
    auto Fy = ms.init_element_vector();
    auto dFxdx = ms.init_element_matrix();
    auto dFydy = ms.init_element_matrix();
    auto dFxdy = ms.init_element_matrix();

    v.setZero();

    ms.linearize(J, v, r, f, Fx, Fy, dFxdx, dFydy, dFxdy);

    //std::cout << J << std::endl;
    //std::cout << r << std::endl;

    // Test with boundary conditions
    ms.apply_conditions();

    J = ms.init_unknown_matrix();
    v = ms.init_unknown_vector();
    r = ms.init_unknown_vector();
    f = ms.init_unknown_vector();
    Fx = ms.init_element_vector();
    Fy = ms.init_element_vector();
    dFxdx = ms.init_element_matrix();
    dFydy = ms.init_element_matrix();
    dFxdy = ms.init_element_matrix();

    v.setZero();

    ms.calculate_forcing(f);
    EXPECT_NEAR(f(0), nodal_current * 6.0, FLT_EPSILON);
    for (size_t i = 1; i != 7; ++i) {
        EXPECT_NEAR(f(i), nodal_current * 2.0, FLT_EPSILON);
    }

    ms.linearize(J, v, r, f, Fx, Fy, dFxdx, dFydy, dFxdy);
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;
    ldlt.compute(J);
    ASSERT_EQ(ldlt.info(), Eigen::Success);
    v -= ldlt.solve(r);

    // Verify equation is solved
    ms.linearize(J, v, r, f, Fx, Fy, dFxdx, dFydy, dFxdy);
    for (size_t i = 0; i != r.size(); ++i) {
        EXPECT_NEAR(r(i), 0.0, FLT_EPSILON);
    }

    // Fundamental solution is 0.25*r^2 + C in forced region (0)
    EXPECT_NEAR(v(0), v(1) + 0.25, FLT_EPSILON);
    for (size_t i = 1; i != 7; ++i) {
        EXPECT_NEAR(v(1), v(i), FLT_EPSILON);
    }

    // Test flux-density values
    // Values are exact in forced region (0)
    // Values are approximated in unforced region (1)
    Eigen::VectorXd Bx = ms.derivatives().dy(0).transpose() * v;
    Eigen::VectorXd By = -ms.derivatives().dx(0).transpose() * v;

    Eigen::VectorXd Bmag(By.size());
    Eigen::VectorXd Bang(By.size());

    for (size_t i = 0; i != By.size(); ++i) {
        Bmag(i) = hypot(Bx(i), By(i));
        Bang(i) = atan2(By(i), Bx(i)) * 180.0 / M_PI;
    }

    double x_q = (1.0 + cos(M_PI / 3.0)) / 3.0;
    double y_q = sin(M_PI / 3.0) / 3.0;
    double r_q = hypot(x_q, y_q);
    double B_q = r_q / 2.0;

    for (size_t i = 0; i != 6; ++i) {
        EXPECT_NEAR(Bmag(i), B_q, FLT_EPSILON);

        double ang = 120.0 + 60.0 * i;
        ang -= 360.0 * (ang > 180.0);

        EXPECT_NEAR(Bang(i), ang, FLT_EPSILON);
    }

    x_q = (1.0 + cos(M_PI / 3.0)) / 2.0;
    y_q = sin(M_PI / 3.0) / 2.0;
    r_q = 2.0 - hypot(x_q, y_q);
    B_q = v(1) / r_q;

    for (size_t i = 6; i < 18; i += 2) {
        EXPECT_NEAR(Bmag(i), B_q, FLT_EPSILON);

        double ang = 120.0 + 60.0 * (i - 6) / 2.0;
        ang -= 360.0 * (ang > 180.0);

        EXPECT_NEAR(Bang(i), ang, FLT_EPSILON);
    }

    x_q = (2.0 * cos(M_PI / 6.0) + 2.0 * cos(M_PI / 6.0 * 3.0)) / 2.0;
    y_q = (2.0 * sin(M_PI / 6.0) + 2.0 * sin(M_PI / 6.0 * 3.0)) / 2.0;
    r_q = hypot(x_q, y_q) - 1.0;
    B_q = v(1) / r_q;

    for (size_t i = 7; i < 18; i += 2) {
        EXPECT_NEAR(Bmag(i), B_q, FLT_EPSILON);

        double ang = 120.0 + 60.0 * (i - 6) / 2.0;
        ang -= 360.0 * (ang > 180.0);

        EXPECT_NEAR(Bang(i), ang, FLT_EPSILON);
    }
}