#include "test_Library_Integration.hpp"

TEST(Library_Integration_Circle, Uniform_Current_Density) {
    std::string save_dir = "./test/output/Integration/";

    // Create Sketch
    Sketch sk;

    auto v0 = sk.new_element<Vertex>(0.0,0.0);
    auto v1 = sk.new_element<Vertex>(1.0,0.0);
    auto v2 = sk.new_element<Vertex>(1.0 * std::cos(M_PI * 2.0 / 3.0), 1.0 * std::sin(M_PI * 2.0 / 3.0));
    auto v3 = sk.new_element<Vertex>(1.0 * std::cos(-M_PI * 2.0 / 3.0), 1.0 * std::sin(-M_PI * 2.0 / 3.0));

    auto c0 = sk.new_element<CircularArc>(v1,v2,v0,1.0);
    auto c1 = sk.new_element<CircularArc>(v2,v3,v0,1.0);
    auto c2 = sk.new_element<CircularArc>(v3,v1,v0,1.0);

    auto f0 = sk.new_element<Fixation>(v0);
    auto f1 = sk.new_element<Fixation>(v1);
    auto f2 = sk.new_element<Fixation>(v2);
    auto f3 = sk.new_element<Fixation>(v3);

    double_t tol = sk.solve();
    EXPECT_LE(tol, FLT_EPSILON);

    bool result = sk.build();
    EXPECT_TRUE(result);

    EXPECT_EQ(sk.size_contours(),1);

    sk.save_as<SaveMethod::Rasterize>(save_dir, std::string("circle_sketch"));

    // Create Refineable Mesh
    Mesh me{sk};
    me.create();

    me.MinimumElementSize = 0.01;
    me.MaximumElementSize = 0.1;
    me.MinimumElementQuality = 0.5;

    me.refine();

    me.save_as(save_dir, std::string("circle_mesh"));

    // Convert to FiniteElementMesh
    FiniteElementMesh<2, 1> fem{me};

    for (std::shared_ptr<Boundary<2>> const &b : fem.boundaries() ){
        double_t a0{-2*M_PI};
        for(size_t i : b->nodes()) {
            XY const &p = fem.node(i);

            // Test radius of boundary
            EXPECT_NEAR(1.0, std::hypot(p.x(), p.y()), FLT_EPSILON);

            // Test ordering of boundary nodes
            double_t a1{std::atan2(p.y(),p.x())};
            if (a1 < 0) {
                a1 += 2 * M_PI;
            }
            EXPECT_TRUE(a1 > a0);
            a0 = a1;
        }
    }

    // Create magnetostatic physics
    Magnetostatic<2, 1, 1, FieldVariable::A> msph{fem};

    msph.add_current_density([](double t) { return 1.0 / (2.0*M_PI*1e-7); }, {fem.region(0)});

    msph.add_magnetic_insulation({fem.boundary(0), fem.boundary(1), fem.boundary(2)});

    msph.build_matrices();

    msph.apply_conditions();

    // Initialize matrices
    auto J = msph.init_unknown_matrix();
    auto v = msph.init_unknown_vector();
    auto r = msph.init_unknown_vector();
    auto f = msph.init_unknown_vector();
    auto Fx = msph.init_element_array();
    auto Fy = msph.init_element_array();
    auto dFxdx = msph.init_element_array();
    auto dFydy = msph.init_element_array();
    auto dFxdy = msph.init_element_array();

    //
    // Set time
    msph.time(0.0);
    msph.calculate_forcing(f);

    // Linearize
    v.setZero();
    msph.linearize(J, v, r, f, Fx, Fy, dFxdx, dFydy, dFxdy);

    // Factor
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;
    ldlt.compute(J);
    ASSERT_EQ(ldlt.info(), Eigen::Success);

    // Solve
    v -= ldlt.solve(r);

    // Test
    msph.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);
    }

    // Test flux-density values
    Eigen::ArrayXd Bx = msph.derivatives().dy(0).transpose() * v;
    Eigen::ArrayXd By = -msph.derivatives().dx(0).transpose() * v;

    Eigen::ArrayXd Bmag(By.size());
    Eigen::ArrayXd 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;
    }

    std::vector<std::vector<XY>> qp = fem.quadrature_points<1>();

    for (size_t i = 0; i != qp.size(); ++i) {
        double_t r = std::hypot(qp[i][0].x(), qp[i][0].y());

        EXPECT_NEAR(Bmag(i), r, 0.1);

        double_t a = std::atan2(qp[i][0].y(), qp[i][0].x()) * 180 / M_PI + 90.0;

        if (a > 180.0) { a -= 360.0; }
        if (a - Bang(i) > 180.0)  { a -= 360.0; }
        if (a - Bang(i) < -180.0) { a += 360.0; }
        EXPECT_NEAR(Bang(i), a, 360 * 0.1);
    }
}