#include <iostream>

#include <unistd.h>

#include "test.h"
#include "gmsh_io.h"
#include "sgr.hpp"

using namespace sgr;

static void
make_geometry(int order, double mesh_h)
{
    gm::add("test");

    std::vector<std::pair<int,int>> objects;
    objects.push_back(
        std::make_pair(3, gmo::addBox(0.0, 0.0, 0.0, 1.0, 1.0, 1.0) )
        );
    objects.push_back(
        std::make_pair(3, gmo::addSphere(0.5, 0.5, 0.5, SPHERE_RADIUS) )
        );

    std::vector<std::pair<int, int>> tools;
    gmsh::vectorpair odt;
    std::vector<gmsh::vectorpair> odtm;
    gmo::fragment(objects, tools, odt, odtm);

    gmo::synchronize();

    gvp_t vp;
    gm::getEntities(vp);
    gmm::setSize(vp, mesh_h);
}

int
test_basics(int geometric_order, int approximation_order)
{
    make_geometry(0, 0.1);

    model mod(geometric_order, approximation_order);

    auto idx = geometric_order - 1;

    std::cout << cyanfg << "Testing geometric order " << geometric_order;
    std::cout << ", approximation order = " << approximation_order << nofg;
    std::cout << std::endl;

    double vol_e0 = (4.0/3.0)*M_PI*SPHERE_RADIUS*SPHERE_RADIUS*SPHERE_RADIUS;
    double vol_e1 = 1.0 - vol_e0;

    /*****************************************/
    std::cout << yellowfg << "Volumes by reference quadrature points and determinants";
    std::cout << nofg << std::endl;
    std::vector<double> t1_vals { 0.07, 0.00145, 0.00024, 1.83e-5 };
    COMPARE_VALUES_RELATIVE("Entity 0 vol r+d", mod.entity_at(0).measure(), vol_e0, t1_vals[idx]);
    std::vector<double> t2_vals { 0.0024, 5.02e-5, 8.21e-06, 6.35e-7 };
    COMPARE_VALUES_RELATIVE("Entity 1 vol r+d", mod.entity_at(1).measure(), vol_e1, t2_vals[idx]);

    /*****************************************/
    std::cout << yellowfg << "Volumes by physical quadrature points" << nofg << std::endl;
    double vol_e0_pqp = 0.0;
    auto& e0 = mod.entity_at(0);
    for (const auto& e : e0)
    {
        const auto& pqps = e.integration_points();
        for (const auto& qp : pqps)
            vol_e0_pqp += qp.weight(); 
    }
    COMPARE_VALUES_RELATIVE("Entity 0 vol pqp", vol_e0_pqp, vol_e0, t1_vals[idx]);

    double vol_e1_pqp = 0.0;
    auto& e1 = mod.entity_at(1);
    for (const auto& e : e1)
    {
        const auto& pqps = e.integration_points();
        for (const auto& qp : pqps)
            vol_e1_pqp += qp.weight(); 
    }
    COMPARE_VALUES_RELATIVE("Entity 1 vol pqp", vol_e1_pqp, vol_e1, t2_vals[idx]);

    /*****************************************/
    std::cout << yellowfg << "Volumes by jacobians" << nofg << std::endl;
    double vol_e0_j = 0.0;
    for (const auto& e : e0)
    {
        const auto& js = e.jacobians();
        auto& re = e0.refelem(e);
        auto rqps = re.integration_points();
        auto dets = e.determinants();
        for (size_t i = 0; i < js.size(); i++)
        {
            double dj = js[i].determinant();
            double dg = dets[i];
            auto relerr = dj/dg - 1;
            assert( std::abs(relerr) < 3e-14 );
            vol_e0_j += dj*rqps[i].weight();
        }
    }
    COMPARE_VALUES_RELATIVE("Entity 0 vol j", vol_e0_j, vol_e0, t1_vals[idx]);

    double vol_e1_j = 0.0;
    for (const auto& e : e1)
    {
        const auto& js = e.jacobians();
        auto& re = e1.refelem(e);
        auto rqps = re.integration_points();
        auto dets = e.determinants();
        for (size_t i = 0; i < js.size(); i++)
        {
            double dj = js[i].determinant();
            double dg = dets[i];
            auto relerr = dj/dg - 1;
            assert( std::abs(relerr) < 3e-14 );
            vol_e1_j += dj*rqps[i].weight();
        }
    }
    COMPARE_VALUES_RELATIVE("Entity 1 vol j", vol_e1_j, vol_e1, t2_vals[idx]);

    /*****************************************/
    std::cout << yellowfg << "Volumes by integrating f(x) = 1 over the domain";
    std::cout << nofg << std::endl;
    double vol_e0_mass = 0.0;
    auto f = [](const point_3d& pt) -> double { return 1; };
    vecxd f_e0 = e0.project(f);
    for (size_t iT = 0; iT < e0.num_elements(); iT++)
    {
        auto& pe = e0.elem(iT);
        auto& re = e0.refelem(pe);
        auto num_bf = re.num_basis_functions();
        matxd mass = e0.mass_matrix(iT);
        vecxd f_pe = f_e0.segment(num_bf*iT, num_bf);
        vol_e0_mass += f_pe.transpose() * mass * f_pe;
    }
    COMPARE_VALUES_RELATIVE("Entity 0 vol mass", vol_e0_mass, vol_e0, t1_vals[idx]);

    double vol_e1_mass = 0.0;
    vecxd f_e1 = e1.project(f);
    for (size_t iT = 0; iT < e1.num_elements(); iT++)
    {
        auto& pe = e1.elem(iT);
        auto& re = e1.refelem(pe);
        auto num_bf = re.num_basis_functions();
        matxd mass = e1.mass_matrix(iT);
        vecxd f_pe = f_e1.segment(num_bf*iT, num_bf);
        vol_e1_mass += f_pe.transpose() * mass * f_pe;
    }
    COMPARE_VALUES_RELATIVE("Entity 1 vol mass", vol_e1_mass, vol_e1, t2_vals[idx]);

    return 0;
}