// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2019 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibamr/AdvDiffHierarchyIntegrator.h"
#include "ibamr/INSVCStaggeredHierarchyIntegrator.h"
#include "ibamr/WaveDampingFunctions.h"
#include "ibamr/WaveUtilities.h"
#include "ibamr/app_namespaces.h" // IWYU pragma: keep

#include "ibtk/HierarchyGhostCellInterpolation.h"
#include "ibtk/HierarchyMathOps.h"

#include "BasePatchHierarchy.h"
#include "BasePatchLevel.h"
#include "Box.h"
#include "CartesianGridGeometry.h"
#include "CartesianPatchGeometry.h"
#include "CellData.h"
#include "CellIndex.h"
#include "CellVariable.h"
#include "HierarchyCellDataOpsReal.h"
#include "Index.h"
#include "IntVector.h"
#include "MultiblockDataTranslator.h"
#include "Patch.h"
#include "PatchHierarchy.h"
#include "PatchLevel.h"
#include "SideData.h"
#include "SideGeometry.h"
#include "SideIndex.h"
#include "Variable.h"
#include "VariableContext.h"
#include "VariableDatabase.h"
#include "tbox/PIO.h"
#include "tbox/Pointer.h"
#include "tbox/Utilities.h"

#include <boost/math/tools/roots.hpp>

#include <cmath>
#include <limits>
#include <ostream>
#include <string>
#include <utility>
#include <vector>

namespace SAMRAI
{
namespace solv
{
template <int DIM>
class RobinBcCoefStrategy;
} // namespace solv
} // namespace SAMRAI

namespace IBAMR
{
/////////////////////////////// STATIC ///////////////////////////////////////

namespace
{
/*!
 * A struct holding the required information used by the variable alpha damping
 * method
 */
struct MassConservationFunctor
{
    std::pair<double, double> operator()(const double& alpha);
    double d_dt;
    SAMRAI::tbox::Pointer<SAMRAI::hier::BasePatchHierarchy<NDIM> > d_patch_hierarchy;
    int d_u_idx;
    int d_phi_new_idx;
    int d_phi_current_idx;
    int d_I_idx;
    int d_dI_idx;
    WaveDampingData* d_ptr_wave_damper;
    static double s_newton_guess, s_newton_min, s_newton_max;
};

double MassConservationFunctor::s_newton_guess = 0.0;
double MassConservationFunctor::s_newton_min = 0.0;
double MassConservationFunctor::s_newton_max = 3.0;

// Functor returning the value and 1st derviative.
std::pair<double, double>
MassConservationFunctor::operator()(const double& alpha)
{
    // Zone and wave parameters
    const double x_zone_start = d_ptr_wave_damper->d_x_zone_start;
    const double x_zone_end = d_ptr_wave_damper->d_x_zone_end;
    const double depth = d_ptr_wave_damper->d_depth;

    // Compute the integrad
    const int coarsest_ln = 0;
    const int finest_ln = d_patch_hierarchy->getFinestLevelNumber();
    static const int dir = NDIM == 2 ? 1 : 2;
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = d_patch_hierarchy->getPatchLevel(ln);
        Pointer<CartesianGridGeometry<NDIM> > grid_geom = level->getGridGeometry();
        const double* const grid_X_lower = grid_geom->getXLower();
        const double* const grid_X_upper = grid_geom->getXUpper();
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_dx = patch_geom->getDx();
            const double* const patch_x_lower = patch_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            const IntVector<NDIM>& patch_lower = patch_box.lower();

            Pointer<SideData<NDIM, double> > u_data = patch->getPatchData(d_u_idx);
            Pointer<CellData<NDIM, double> > phi_new_data = patch->getPatchData(d_phi_new_idx);
            Pointer<CellData<NDIM, double> > phi_current_data = patch->getPatchData(d_phi_current_idx);
            Pointer<CellData<NDIM, double> > I_data = patch->getPatchData(d_I_idx);
            Pointer<CellData<NDIM, double> > dI_data = patch->getPatchData(d_dI_idx);

            for (Box<NDIM>::Iterator it(patch_box); it; it++)
            {
                SAMRAI::hier::Index<NDIM> i = it();
                CellIndex<NDIM> ci(it());
                CellIndex<NDIM> cl = ci;
                CellIndex<NDIM> cu = ci;
                cl(0) -= 1;
                cu(0) += 1;
                const SideIndex<NDIM> sl(ci, /*axis*/ 0, SideIndex<NDIM>::Lower);
                const SideIndex<NDIM> su(ci, /*axis*/ 0, SideIndex<NDIM>::Upper);

                // Note: assumes (0,0) in bottom left corner, and water phase denoted by
                // negative level set
                const double phi_new = (*phi_new_data)(ci);
                const double phi_current = (*phi_current_data)(ci);
                const double phi_new_sc_lower = 0.5 * (phi_new + (*phi_new_data)(cl));
                const double phi_new_sc_upper = 0.5 * (phi_new + (*phi_new_data)(cu));
                const double u_sc_lower = (*u_data)(sl);
                const double u_sc_upper = (*u_data)(su);

                const auto x_posn = patch_x_lower[0] + patch_dx[0] * (static_cast<double>(i(0) - patch_lower(0)) + 0.5);

                const auto z_posn =
                    patch_x_lower[dir] + patch_dx[dir] * (static_cast<double>(i(dir) - patch_lower(dir)) + 0.5);

                // Indicator for absorption region
                const bool within_damping = (x_zone_start <= x_posn && x_posn <= x_zone_end);

                if (!within_damping)
                {
                    (*I_data)(ci) = 0.0;
                    (*dI_data)(ci) = 0.0;
                }
                else
                {
                    // 1 - Heaviside
                    double theta_new_sc_lower, theta_new_sc_upper;
                    double vol_cell = 1.0;
                    for (int d = 0; d < NDIM; ++d) vol_cell *= patch_dx[d];
                    auto beta = 1.0 * std::pow(vol_cell, 1.0 / static_cast<double>(NDIM));
                    if (phi_new_sc_lower < -beta)
                    {
                        theta_new_sc_lower = 1.0;
                    }
                    else if (std::abs(phi_new_sc_lower) <= beta)
                    {
                        theta_new_sc_lower = 1.0 - (0.5 + 0.5 * phi_new_sc_lower / beta +
                                                    1.0 / (2.0 * M_PI) * std::sin(M_PI * phi_new_sc_lower / beta));
                    }
                    else
                    {
                        theta_new_sc_lower = 0.0;
                    }
                    if (phi_new_sc_upper < -beta)
                    {
                        theta_new_sc_upper = 1.0;
                    }
                    else if (std::abs(phi_new_sc_upper) <= beta)
                    {
                        theta_new_sc_upper = 1.0 - (0.5 + 0.5 * phi_new_sc_upper / beta +
                                                    1.0 / (2.0 * M_PI) * std::sin(M_PI * phi_new_sc_upper / beta));
                    }
                    else
                    {
                        theta_new_sc_upper = 0.0;
                    }

                    // NDIM - 1 area
                    double area = 1.0;
                    for (int d = 1; d < NDIM; ++d) area *= (grid_X_upper[d] - grid_X_lower[d]);

                    const double fac = 5;
                    const double xtilde = (x_posn - x_zone_start) / (x_zone_end - x_zone_start);
                    const double xalph = std::pow(1.0 - xtilde, alpha);
                    const double gamma = 1.0 - exp(-fac * xalph);
                    const double eta_new = -phi_new + (z_posn - depth);
                    const double eta_current = -phi_current + (z_posn - depth);
                    const double dtu_dx =
                        (theta_new_sc_upper * u_sc_upper - theta_new_sc_lower * u_sc_lower) / patch_dx[0];
                    const double dgamma_dalpha = fac * exp(-fac * xalph) * xalph * log(1.0 - xtilde);
                    (*I_data)(ci) = d_dt * gamma * dtu_dx + (gamma * eta_new - eta_current) / area;
                    (*dI_data)(ci) = dgamma_dalpha * (d_dt * dtu_dx + eta_new / area);
                }
            }
        }
    }
    HierarchyCellDataOpsReal<NDIM, double> hier_cc_data_ops(d_patch_hierarchy, coarsest_ln, finest_ln);
    HierarchyMathOps hier_math_ops("HierarchyMathOps", d_patch_hierarchy);
    hier_math_ops.setPatchHierarchy(d_patch_hierarchy);
    hier_math_ops.resetLevels(coarsest_ln, finest_ln);
    const int wgt_cc_idx = hier_math_ops.getCellWeightPatchDescriptorIndex();
    const double val = hier_cc_data_ops.integral(d_I_idx, wgt_cc_idx);
    const double dval = hier_cc_data_ops.integral(d_dI_idx, wgt_cc_idx);
    TBOX_ASSERT(!std::isnan(val));
    TBOX_ASSERT(!std::isnan(dval));
    return std::make_pair(val, dval);
} // MassConservationFunctor::operator()

} // namespace

/////////////////////////////// PUBLIC ///////////////////////////////////////

namespace WaveDampingFunctions
{
void
callRelaxationZoneCallbackFunction(double /*current_time*/,
                                   double /*new_time*/,
                                   bool /*skip_synchronize_new_state_data*/,
                                   int /*num_cycles*/,
                                   void* ctx)
{
    auto ptr_wave_damper = static_cast<WaveDampingData*>(ctx);
    const double x_zone_start = ptr_wave_damper->d_x_zone_start;
    const double x_zone_end = ptr_wave_damper->d_x_zone_end;
    const double depth = ptr_wave_damper->d_depth;
    const double alpha = ptr_wave_damper->d_alpha;
    const double sign_gas = ptr_wave_damper->d_sign_gas_phase;

    Pointer<PatchHierarchy<NDIM> > patch_hierarchy = ptr_wave_damper->d_ins_hier_integrator->getPatchHierarchy();
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    int u_new_idx = var_db->mapVariableAndContextToIndex(ptr_wave_damper->d_ins_hier_integrator->getVelocityVariable(),
                                                         ptr_wave_damper->d_ins_hier_integrator->getNewContext());

    Pointer<CellVariable<NDIM, double> > phi_cc_var = ptr_wave_damper->d_phi_var;
    if (!phi_cc_var)
        TBOX_ERROR(
            "WaveDampingStrategy::callRelaxationZoneCallbackFunction: Level "
            "set variable must be cell centered!");
    int phi_new_idx =
        var_db->mapVariableAndContextToIndex(phi_cc_var, ptr_wave_damper->d_adv_diff_hier_integrator->getNewContext());
    static const int dir = NDIM == 2 ? 1 : 2;

    const int coarsest_ln = 0;
    const int finest_ln = patch_hierarchy->getFinestLevelNumber();

    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_dx = patch_geom->getDx();
            const double* const patch_x_lower = patch_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            const IntVector<NDIM>& patch_lower = patch_box.lower();

            Pointer<SideData<NDIM, double> > u_data = patch->getPatchData(u_new_idx);

            for (int axis = 0; axis < NDIM; ++axis)
            {
                for (Box<NDIM>::Iterator it(SideGeometry<NDIM>::toSideBox(patch_box, axis)); it; it++)
                {
                    SAMRAI::hier::Index<NDIM> i = it();
                    SideIndex<NDIM> i_side(i, axis, SideIndex<NDIM>::Lower);
                    double x_posn = patch_x_lower[0] + patch_dx[0] * (static_cast<double>(i(0) - patch_lower(0)));
                    const double shift_x = (axis == 0 ? 0.0 : 0.5);
                    x_posn += patch_dx[0] * shift_x;

                    if (x_posn >= x_zone_start && x_posn <= x_zone_end)
                    {
                        const double xtilde = (x_posn - x_zone_start) / (x_zone_end - x_zone_start);
                        const double gamma = 1.0 - std::expm1(std::pow(xtilde, alpha)) / std::expm1(1.0);
                        const double target = 0.0;

                        if (axis == 0 || axis == dir)
                        {
                            (*u_data)(i_side, 0) = gamma * (*u_data)(i_side, 0) + (1.0 - gamma) * target;
                        }
                    }
                }
            }
        }
    }

    // Modify the level set in the generation zone.
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_dx = patch_geom->getDx();
            const double* const patch_x_lower = patch_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            const IntVector<NDIM>& patch_lower = patch_box.lower();

            Pointer<CellData<NDIM, double> > phi_data = patch->getPatchData(phi_new_idx);
            for (Box<NDIM>::Iterator it(patch_box); it; it++)
            {
                SAMRAI::hier::Index<NDIM> i = it();

                const auto x_posn = patch_x_lower[0] + patch_dx[0] * (static_cast<double>(i(0) - patch_lower(0)) + 0.5);

                const auto dir_posn =
                    patch_x_lower[dir] + patch_dx[dir] * (static_cast<double>(i(dir) - patch_lower(dir)) + 0.5);

                const double dir_surface = sign_gas * (dir_posn - depth);

                if (x_posn >= x_zone_start && x_posn <= x_zone_end)
                {
                    const double xtilde = (x_posn - x_zone_start) / (x_zone_end - x_zone_start);
                    const double gamma = 1.0 - std::expm1(std::pow(xtilde, alpha)) / std::expm1(1.0);
                    const double target = dir_surface;

                    (*phi_data)(i, 0) = gamma * (*phi_data)(i, 0) + (1.0 - gamma) * target;
                }
            }
        }
    }

    return;
} // callRelaxationZoneCallbackFunction

void
callConservedWaveAbsorbingCallbackFunction(double current_time,
                                           double new_time,
                                           bool /*skip_synchronize_new_state_data*/,
                                           int /*num_cycles*/,
                                           void* ctx)
{
    // Reference: Hu Zhe, et al., "Numerical Wave Tank Based on A Conserved
    // Wave-Absorbing Method"
    auto ptr_wave_damper = static_cast<WaveDampingData*>(ctx);
    const double x_zone_start = ptr_wave_damper->d_x_zone_start;
    const double x_zone_end = ptr_wave_damper->d_x_zone_end;
    const double depth = ptr_wave_damper->d_depth;
    const double sign_gas = ptr_wave_damper->d_sign_gas_phase;

    // Initialize the required variables
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const std::string var_name = "callConservedWaveAbsorbingCallbackFunction::I_var";
    IntVector<NDIM> no_ghosts = 0;
    Pointer<CellVariable<NDIM, double> > I_var = var_db->getVariable(var_name);
    if (!I_var) I_var = new CellVariable<NDIM, double>(var_name);
    const int I_idx =
        var_db->registerVariableAndContext(I_var, var_db->getContext(var_name + "::INTEGRAND"), no_ghosts);
    const int dI_idx =
        var_db->registerVariableAndContext(I_var, var_db->getContext(var_name + "::DERIVATIVE"), no_ghosts);
#if !defined(NDEBUG)
    TBOX_ASSERT(I_idx >= 0);
    TBOX_ASSERT(dI_idx >= 0);
#endif

    // Allocate patch data
    Pointer<PatchHierarchy<NDIM> > patch_hierarchy = ptr_wave_damper->d_ins_hier_integrator->getPatchHierarchy();
    const int coarsest_ln = 0;
    const int finest_ln = patch_hierarchy->getFinestLevelNumber();
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        patch_hierarchy->getPatchLevel(ln)->allocatePatchData(I_idx, new_time);
        patch_hierarchy->getPatchLevel(ln)->allocatePatchData(dI_idx, new_time);
    }

    // Get the required patch data
    int u_new_idx = var_db->mapVariableAndContextToIndex(ptr_wave_damper->d_ins_hier_integrator->getVelocityVariable(),
                                                         ptr_wave_damper->d_ins_hier_integrator->getNewContext());
    Pointer<CellVariable<NDIM, double> > phi_cc_var = ptr_wave_damper->d_phi_var;
    if (!phi_cc_var)
        TBOX_ERROR(
            "WaveDampingStrategy::"
            "callConservedWaveAbsorbingCallbackFunction: Level set variable "
            "must be cell "
            "centered!");
    int phi_current_idx = var_db->mapVariableAndContextToIndex(
        phi_cc_var, ptr_wave_damper->d_adv_diff_hier_integrator->getCurrentContext());
    int phi_new_idx =
        var_db->mapVariableAndContextToIndex(phi_cc_var, ptr_wave_damper->d_adv_diff_hier_integrator->getNewContext());

    // Allocate scratch data
    IntVector<NDIM> cell_ghosts = 1;
    const int phi_scratch_idx = var_db->registerVariableAndContext(
        phi_cc_var, var_db->getContext("callConservedWaveAbsorbingCallbackFunction::phi_cc_var::SCRATCH"), cell_ghosts);
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->allocatePatchData(phi_scratch_idx, new_time);
    }

    // Fill ghost cells for level set
    RobinBcCoefStrategy<NDIM>* phi_bc_coef =
        ptr_wave_damper->d_adv_diff_hier_integrator->getPhysicalBcCoefs(phi_cc_var).front();
    using InterpolationTransactionComponent = HierarchyGhostCellInterpolation::InterpolationTransactionComponent;
    InterpolationTransactionComponent phi_transaction(phi_scratch_idx,
                                                      phi_new_idx,
                                                      "CONSERVATIVE_LINEAR_REFINE",
                                                      false,
                                                      "CONSERVATIVE_COARSEN",
                                                      "LINEAR",
                                                      false,
                                                      phi_bc_coef);
    Pointer<HierarchyGhostCellInterpolation> hier_bdry_fill = new HierarchyGhostCellInterpolation();
    hier_bdry_fill->initializeOperatorState(phi_transaction, patch_hierarchy);
    hier_bdry_fill->fillData(new_time);

    // Create the functor and set its options
    MassConservationFunctor mass_eval_functor;
    mass_eval_functor.d_dt = new_time - current_time;
    mass_eval_functor.d_patch_hierarchy = patch_hierarchy;
    mass_eval_functor.d_u_idx = u_new_idx;
    mass_eval_functor.d_phi_new_idx = phi_scratch_idx;
    mass_eval_functor.d_phi_current_idx = phi_current_idx;
    mass_eval_functor.d_I_idx = I_idx;
    mass_eval_functor.d_dI_idx = dI_idx;
    mass_eval_functor.d_ptr_wave_damper = ptr_wave_damper;

    // Set up Newton itertion
    const double guess = MassConservationFunctor::s_newton_guess;
    const double min = MassConservationFunctor::s_newton_min;
    const double max = MassConservationFunctor::s_newton_max;

    // Accuracy doubles at each step, so stop when just over half of the
    // digits are correct, and rely on that step to polish off the remainder:
    auto get_digits = static_cast<int>(std::numeric_limits<double>::digits * 0.6);
    const boost::uintmax_t maxit = 50;
    boost::uintmax_t it = maxit;

    double alpha = boost::math::tools::newton_raphson_iterate(mass_eval_functor, guess, min, max, get_digits, it);
    if (alpha > max - 0.1 || alpha < 0.0) alpha = 0.0;
    plog << "alpha relaxation dynamic = " << alpha << std::endl;

    // Carry out the damping
    static const int dir = NDIM == 2 ? 1 : 2;
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_dx = patch_geom->getDx();
            const double* const patch_x_lower = patch_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            const IntVector<NDIM>& patch_lower = patch_box.lower();

            Pointer<SideData<NDIM, double> > u_data = patch->getPatchData(u_new_idx);

            for (int axis = 0; axis < NDIM; ++axis)
            {
                for (Box<NDIM>::Iterator it(SideGeometry<NDIM>::toSideBox(patch_box, axis)); it; it++)
                {
                    SAMRAI::hier::Index<NDIM> i = it();
                    SideIndex<NDIM> i_side(i, axis, SideIndex<NDIM>::Lower);
                    double x_posn = patch_x_lower[0] + patch_dx[0] * (static_cast<double>(i(0) - patch_lower(0)));
                    const double shift_x = (axis == 0 ? 0.0 : 0.5);
                    x_posn += patch_dx[0] * shift_x;
                    if (x_posn >= x_zone_start && x_posn <= x_zone_end)
                    {
                        double xtilde = (x_posn - x_zone_start) / (x_zone_end - x_zone_start);
                        double gamma = 1.0 - exp(-5.0 * std::pow(1.0 - xtilde, alpha));
                        const double target = 0.0;

                        if (axis == 0 || axis == dir)
                        {
                            (*u_data)(i_side, 0) = gamma * (*u_data)(i_side, 0) + (1.0 - gamma) * target;
                        }
                    }
                }
            }
        }
    }

    // Modify the level set in the generation zone.
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_dx = patch_geom->getDx();
            const double* const patch_x_lower = patch_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            const IntVector<NDIM>& patch_lower = patch_box.lower();

            Pointer<CellData<NDIM, double> > phi_data = patch->getPatchData(phi_new_idx);
            for (Box<NDIM>::Iterator it(patch_box); it; it++)
            {
                SAMRAI::hier::Index<NDIM> i = it();

                const auto x_posn = patch_x_lower[0] + patch_dx[0] * (static_cast<double>(i(0) - patch_lower(0)) + 0.5);

                const auto dir_posn =
                    patch_x_lower[dir] + patch_dx[dir] * (static_cast<double>(i(dir) - patch_lower(dir)) + 0.5);
                const double dir_surface = sign_gas * (dir_posn - depth);

                if (x_posn >= x_zone_start && x_posn <= x_zone_end)
                {
                    double xtilde = (x_posn - x_zone_start) / (x_zone_end - x_zone_start);
                    double gamma = 1.0 - exp(-5.0 * std::pow(1.0 - xtilde, alpha));
                    double target = dir_surface;

                    (*phi_data)(i, 0) = gamma * (*phi_data)(i, 0) + (1.0 - gamma) * target;
                }
            }
        }
    }

    // Deallocate patch data and remove indices
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        patch_hierarchy->getPatchLevel(ln)->deallocatePatchData(I_idx);
        patch_hierarchy->getPatchLevel(ln)->deallocatePatchData(dI_idx);
        patch_hierarchy->getPatchLevel(ln)->deallocatePatchData(phi_scratch_idx);
    }
    var_db->removePatchDataIndex(I_idx);
    var_db->removePatchDataIndex(dI_idx);
    var_db->removePatchDataIndex(phi_scratch_idx);

    return;

} // callConservedWaveAbsorbingCallbackFunction

} // namespace WaveDampingFunctions

} // namespace IBAMR


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief kitchen_sink.cpp

\details
More extensive quantity tests.

Output:
@verbatim

//[kitchen_sink_output_1
S1 :    2
X1 :    2
X2 :    (4/3)
U1 :    N
U2 :    J
Q1 :    1 N
Q2 :    2 J
//]

//[kitchen_sink_output_2
U1*S1 : 2 N
S1*U1 : 2 N
U1/S1 : 0.5 N
S1/U1 : 2 m^-1 kg^-1 s^2
//]

//[kitchen_sink_output_3
U1+U1 : N
U1-U1 : N
U1*U1 : m^2 kg^2 s^-4
U1/U1 : dimensionless
U1*U2 : m^3 kg^2 s^-4
U1/U2 : m^-1
U1^X  : m^2 kg^2 s^-4
X1vU1 : m^(1/2) kg^(1/2) s^-1
U1^X2 : m^(4/3) kg^(4/3) s^(-8/3)
X2vU1 : m^(3/4) kg^(3/4) s^(-3/2)
//]

//[kitchen_sink_output_4
Q1*S1 : 2 N
S1*Q1 : 2 N
Q1/S1 : 0.5 N
S1/Q1 : 2 m^-1 kg^-1 s^2
//]

//[kitchen_sink_output_5
U1*Q1 : 1 m^2 kg^2 s^-4
Q1*U1 : 1 m^2 kg^2 s^-4
U1/Q1 : 1 dimensionless
Q1/U1 : 1 dimensionless
//]

//[kitchen_sink_output_6
+Q1   : 1 N
-Q1   : -1 N
Q1+Q1 : 2 N
Q1-Q1 : 0 N
Q1*Q1 : 1 m^2 kg^2 s^-4
Q1/Q1 : 1 dimensionless
Q1*Q2 : 2 m^3 kg^2 s^-4
Q1/Q2 : 0.5 m^-1
Q1^X1 : 1 m^2 kg^2 s^-4
X1vQ1 : 1 m^(1/2) kg^(1/2) s^-1
Q1^X2 : 1 m^(4/3) kg^(4/3) s^(-8/3)
X2vQ1 : 1 m^(3/4) kg^(3/4) s^(-3/2)
//]

//[kitchen_sink_output_7
l1 == l2    false
l1 != l2    true
l1 <= l2    true
l1 < l2     true
l1 >= l2    false
l1 > l2     false
//]

dimless = 1

//[kitchen_sink_output_8
v1 = 2 m s^-1
//]

//[kitchen_sink_output_9
F  = 1 N
dx = 1 m
E  = 1 J
//]

//[kitchen_sink_output_10
r = 5e-07 m
P = 101325 Pa
V = 5.23599e-19 m^3
T = 310 K
n = 2.05835e-17 mol
R = 8.314472 m^2 kg s^-2 K^-1 mol^-1 (rel. unc. = 1.8e-06)
//]

//[kitchen_sink_output_11
theta            = 0.375 rd
sin(theta)       = 0.366273 dimensionless
asin(sin(theta)) = 0.375 rd
//]

//[kitchen_sink_output_12
V   = (12.5,0) V
I   = (3,4) A
Z   = (1.5,-2) Ohm
I*Z = (12.5,0) V
//]

//[kitchen_sink_output_13
x+y-w         = 0.48(+/-0.632772) m
w*x           = 9.04(+/-0.904885) m^2
x/y           = 0.666667(+/-0.149071) dimensionless
//]

//[kitchen_sink_output_14
w*y^2/(u*x)^2 = 10.17(+/-3.52328) m^-1
w/(u*x)^(1/2) = 3.19612(+/-0.160431) dimensionless
//]

//[kitchen_sink_output_15
I*w   = m^2 kg s^-1 rad^-1
I*w/L = dimensionless
I*w^2 = J
//]

//[kitchen_sink_output_16
1 F
1 kat
1 S
1 C
1 V
1 J
1 N
1 Hz
1 lx
1 H
1 lm
1 Wb
1 T
1 W
1 Pa
1 Ohm
//]

//[kitchen_sink_output_18
1 farad
1 katal
1 siemen
1 coulomb
1 volt
1 joule
1 newton
1 hertz
1 lux
1 henry
1 lumen
1 weber
1 tesla
1 watt
1 pascal
1 ohm
//]

@endverbatim
**/

#include <cmath>
#include <complex>
#include <iostream>

#include <boost/typeof/std/complex.hpp>

#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/codata/physico-chemical_constants.hpp>
#include <boost/units/systems/si/io.hpp>

#include "measurement.hpp"

namespace boost {

namespace units {

//[kitchen_sink_function_snippet_3
/// the physical definition of work - computed for an arbitrary unit system
template<class System,class Y>
constexpr
quantity<unit<energy_dimension,System>,Y>
work(quantity<unit<force_dimension,System>,Y> F,
     quantity<unit<length_dimension,System>,Y> dx)
{
    return F*dx;
}
//]

//[kitchen_sink_function_snippet_4
/// the ideal gas law in si units
template<class Y>
constexpr
quantity<si::amount,Y>
idealGasLaw(const quantity<si::pressure,Y>& P,
            const quantity<si::volume,Y>& V,
            const quantity<si::temperature,Y>& T)
{
    using namespace boost::units::si;

    using namespace constants::codata;
    return (P*V/(R*T));
}
//]

} // namespace units

} // namespace boost

int main()
{
    using namespace boost::units;
    using namespace boost::units::si;

    {
    //[kitchen_sink_snippet_1
    /// scalar
    const double    s1 = 2;

    const long                  x1 = 2;
    const static_rational<4,3>  x2;

    /// define some units
    force       u1 = newton;
    energy      u2 = joule;

    /// define some quantities
    quantity<force>      q1(1.0*u1);
    quantity<energy>     q2(2.0*u2);
    //]

    /// check scalar, unit, and quantity io
    std::cout << "S1 :    " << s1 << std::endl
              << "X1 :    " << x1 << std::endl
              << "X2 :    " << x2 << std::endl
              << "U1 :    " << u1 << std::endl
              << "U2 :    " << u2 << std::endl
              << "Q1 :    " << q1 << std::endl
              << "Q2 :    " << q2 << std::endl
              << std::endl;

    /// check scalar-unit algebra
    std::cout //<< "U1+S1 : " << u1+s1 << std::endl    // illegal
              //<< "S1+U1 : " << s1+u1 << std::endl    // illegal
              //<< "U1-S1 : " << u1-s1 << std::endl    // illegal
              //<< "S1-U1 : " << s1-u1 << std::endl    // illegal
              << "U1*S1 : " << u1*s1 << std::endl
              << "S1*U1 : " << s1*u1 << std::endl
              << "U1/S1 : " << u1/s1 << std::endl
              << "S1/U1 : " << s1/u1 << std::endl
              << std::endl;

    /// check unit-unit algebra
    std::cout << "U1+U1 : " << u1+u1 << std::endl
              << "U1-U1 : " << u1-u1 << std::endl
              << "U1*U1 : " << u1*u1 << std::endl
              << "U1/U1 : " << u1/u1 << std::endl
              //<< "U1+U2 : " << u1+u2 << std::endl     // illegal
              //<< "U1-U2 : " << u1-u2 << std::endl     // illegal
              << "U1*U2 : " << u1*u2 << std::endl
              << "U1/U2 : " << u1/u2 << std::endl
              << "U1^X  : " << pow<2>(u1) << std::endl
              << "X1vU1 : " << root<2>(u1) << std::endl
              << "U1^X2 : " << pow<static_rational<4,3> >(u1) << std::endl
              << "X2vU1 : " << root<static_rational<4,3> >(u1) << std::endl
              << std::endl;

    /// check scalar-quantity algebra
    std::cout //<< "Q1+S1 : " << q1+s1 << std::endl    // illegal
              //<< "S1+Q1 : " << s1+q1 << std::endl    // illegal
              //<< "Q1-S1 : " << q1-s1 << std::endl    // illegal
              //<< "S1-Q1 : " << s1-q1 << std::endl    // illegal
              << "Q1*S1 : " << q1*s1 << std::endl
              << "S1*Q1 : " << s1*q1 << std::endl
              << "Q1/S1 : " << q1/s1 << std::endl
              << "S1/Q1 : " << s1/q1 << std::endl
              << std::endl;

    /// check unit-quantity algebra
    std::cout //<< "U1+Q1 : " << u1+q1 << std::endl    // illegal
              //<< "Q1+U1 : " << q1+u1 << std::endl    // illegal
              //<< "U1-Q1 : " << u1-q1 << std::endl    // illegal
              //<< "Q1-U1 : " << q1-u1 << std::endl    // illegal
              << "U1*Q1 : " << u1*q1 << std::endl
              << "Q1*U1 : " << q1*u1 << std::endl
              << "U1/Q1 : " << u1/q1 << std::endl
              << "Q1/U1 : " << q1/u1 << std::endl
              << std::endl;

    /// check quantity-quantity algebra
    std::cout << "+Q1   : " << +q1 << std::endl
              << "-Q1   : " << -q1 << std::endl
              << "Q1+Q1 : " << q1+q1 << std::endl
              << "Q1-Q1 : " << q1-q1 << std::endl
              << "Q1*Q1 : " << q1*q1 << std::endl
              << "Q1/Q1 : " << q1/q1 << std::endl
              //<< "Q1+Q2 : " << q1+q2 << std::endl    // illegal
              //<< "Q1-Q2 : " << q1-q2 << std::endl    // illegal
              << "Q1*Q2 : " << q1*q2 << std::endl
              << "Q1/Q2 : " << q1/q2 << std::endl
              << "Q1^X1 : " << pow<2>(q1) << std::endl
              << "X1vQ1 : " << root<2>(q1) << std::endl
              << "Q1^X2 : " << pow<static_rational<4,3> >(q1) << std::endl
              << "X2vQ1 : " << root<static_rational<4,3> >(q1) << std::endl
              << std::endl;

    //[kitchen_sink_snippet_2
    /// check comparison tests
    quantity<length>    l1(1.0*meter),
                        l2(2.0*meters);
    //]

    std::cout << std::boolalpha
              << "l1 == l2" << "\t" << (l1 == l2) << std::endl
              << "l1 != l2" << "\t" << (l1 != l2) << std::endl
              << "l1 <= l2" << "\t" << (l1 <= l2) << std::endl
              << "l1 < l2 " << "\t" << (l1  < l2) << std::endl
              << "l1 >= l2" << "\t" << (l1 >= l2) << std::endl
              << "l1 > l2 " << "\t" << (l1  > l2) << std::endl
              << std::endl;

    //[kitchen_sink_snippet_3
    /// check implicit unit conversion from dimensionless to value_type
    const double    dimless = (q1/q1);
    //]

    std::cout << "dimless = " << dimless << std::endl
              << std::endl;

    quantity<velocity>  v1 = 2.0*meters/second;

    std::cout << "v1 = " << v1 << std::endl
              << std::endl;

    //[kitchen_sink_snippet_4
    /// test calcuation of work
    quantity<force>       F(1.0*newton);
    quantity<length>      dx(1.0*meter);
    quantity<energy>      E(work(F,dx));
    //]

    std::cout << "F  = " << F << std::endl
              << "dx = " << dx << std::endl
              << "E  = " << E << std::endl
              << std::endl;

    {
    //[kitchen_sink_snippet_5
    /// test ideal gas law
    quantity<temperature>   T = (273.+37.)*kelvin;
    quantity<pressure>      P = 1.01325e5*pascals;
    quantity<length>        r = 0.5e-6*meters;
    quantity<volume>        V = (4.0/3.0)*3.141592*pow<3>(r);
    quantity<amount>        n(idealGasLaw(P,V,T));
    //]

    std::cout << "r = " << r << std::endl
              << "P = " << P << std::endl
              << "V = " << V << std::endl
              << "T = " << T << std::endl
              << "n = " << n << std::endl
              #if BOOST_UNITS_HAS_TYPEOF
              << "R = " << constants::codata::R << std::endl
              #else
              << "no typeof" << std::endl
              #endif // BOOST_UNITS_HAS_TYPEOF
              << std::endl;
    }

    //[kitchen_sink_snippet_6
    /// test trig stuff
    quantity<plane_angle>           theta = 0.375*radians;
    quantity<dimensionless>         sin_theta = sin(theta);
    quantity<plane_angle>           thetap = asin(sin_theta);
    //]

    std::cout << "theta            = " << theta << std::endl
              << "sin(theta)       = " << sin_theta << std::endl
              << "asin(sin(theta)) = " << thetap << std::endl
              << std::endl;

    /// test implicit conversion of dimensionless to value
    double  tmp = sin_theta;

    tmp = sin_theta;

    /// test implicit conversion from value to dimensionless
    quantity<dimensionless>     tmpp = tmp;

    tmpp = tmp;

    /// check complex quantities
    typedef std::complex<double>    complex_type;

    //[kitchen_sink_snippet_7
    quantity<electric_potential,complex_type> v = complex_type(12.5,0.0)*volts;
    quantity<current,complex_type>            i = complex_type(3.0,4.0)*amperes;
    quantity<resistance,complex_type>         z = complex_type(1.5,-2.0)*ohms;
    //]

    std::cout << "V   = " << v << std::endl
              << "I   = " << i << std::endl
              << "Z   = " << z << std::endl
              << "I*Z = " << i*z << std::endl
              << std::endl;

    /// check quantities using user-defined type encapsulating error propagation

    //[kitchen_sink_snippet_8
    quantity<length,measurement<double> >
        u(measurement<double>(1.0,0.0)*meters),
        w(measurement<double>(4.52,0.02)*meters),
        x(measurement<double>(2.0,0.2)*meters),
        y(measurement<double>(3.0,0.6)*meters);
    //]

    std::cout << "x+y-w         = " << x+y-w << std::endl
              << "w*x           = " << w*x << std::endl
              << "x/y           = " << x/y << std::endl
              << "w*y^2/(u*x)^2 = " << w*y*y/pow<2>(u*x) << std::endl
              << "w/(u*x)^(1/2) = " << w/pow< static_rational<1,2> >(u*x)
              << std::endl << std::endl;
    }

    /// check moment of inertia/angular momentum/rotational energy

    //[kitchen_sink_snippet_9
    std::cout << symbol_format
              << "I*w   = " << moment_of_inertia()*angular_velocity() << std::endl
              << "I*w/L = " << moment_of_inertia()*angular_velocity()/angular_momentum() << std::endl
              << "I*w^2 = " << moment_of_inertia()*pow<2>(angular_velocity()) << std::endl
              << std::endl;
    //]

    //[kitchen_sink_snippet_10
//    std::cout << typename_format
//              << quantity<capacitance>(1.0*farad) << std::endl
//              << quantity<catalytic_activity>(1.0*katal) << std::endl
//              << quantity<conductance>(1.0*siemen) << std::endl
//              << quantity<electric_charge>(1.0*coulomb) << std::endl
//              << quantity<electric_potential>(1.0*volt) << std::endl
//              << quantity<energy>(1.0*joule) << std::endl
//              << quantity<force>(1.0*newton) << std::endl
//              << quantity<frequency>(1.0*hertz) << std::endl
//              << quantity<illuminance>(1.0*lux) << std::endl
//              << quantity<inductance>(1.0*henry) << std::endl
//              << quantity<luminous_flux>(1.0*lumen) << std::endl
//              << quantity<magnetic_flux>(1.0*weber) << std::endl
//              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
//              << quantity<power>(1.0*watt) << std::endl
//              << quantity<pressure>(1.0*pascals) << std::endl
//              << quantity<resistance>(1.0*ohm) << std::endl
//              << std::endl;
    //]

    //[kitchen_sink_snippet_11
//    std::cout << raw_format
//              << quantity<capacitance>(1.0*farad) << std::endl
//              << quantity<catalytic_activity>(1.0*katal) << std::endl
//              << quantity<conductance>(1.0*siemen) << std::endl
//              << quantity<electric_charge>(1.0*coulomb) << std::endl
//              << quantity<electric_potential>(1.0*volt) << std::endl
//              << quantity<energy>(1.0*joule) << std::endl
//              << quantity<force>(1.0*newton) << std::endl
//              << quantity<frequency>(1.0*hertz) << std::endl
//              << quantity<illuminance>(1.0*lux) << std::endl
//              << quantity<inductance>(1.0*henry) << std::endl
//              << quantity<luminous_flux>(1.0*lumen) << std::endl
//              << quantity<magnetic_flux>(1.0*weber) << std::endl
//              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
//              << quantity<power>(1.0*watt) << std::endl
//              << quantity<pressure>(1.0*pascals) << std::endl
//              << quantity<resistance>(1.0*ohm) << std::endl
//              << std::endl;
    //]

    //[kitchen_sink_snippet_12
    std::cout << symbol_format
              << quantity<capacitance>(1.0*farad) << std::endl
              << quantity<catalytic_activity>(1.0*katal) << std::endl
              << quantity<conductance>(1.0*siemen) << std::endl
              << quantity<electric_charge>(1.0*coulomb) << std::endl
              << quantity<electric_potential>(1.0*volt) << std::endl
              << quantity<energy>(1.0*joule) << std::endl
              << quantity<force>(1.0*newton) << std::endl
              << quantity<frequency>(1.0*hertz) << std::endl
              << quantity<illuminance>(1.0*lux) << std::endl
              << quantity<inductance>(1.0*henry) << std::endl
              << quantity<luminous_flux>(1.0*lumen) << std::endl
              << quantity<magnetic_flux>(1.0*weber) << std::endl
              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
              << quantity<power>(1.0*watt) << std::endl
              << quantity<pressure>(1.0*pascals) << std::endl
              << quantity<resistance>(1.0*ohm) << std::endl
              << std::endl;
    //]

    //[kitchen_sink_snippet_13
    std::cout << name_format
              << quantity<capacitance>(1.0*farad) << std::endl
              << quantity<catalytic_activity>(1.0*katal) << std::endl
              << quantity<conductance>(1.0*siemen) << std::endl
              << quantity<electric_charge>(1.0*coulomb) << std::endl
              << quantity<electric_potential>(1.0*volt) << std::endl
              << quantity<energy>(1.0*joule) << std::endl
              << quantity<force>(1.0*newton) << std::endl
              << quantity<frequency>(1.0*hertz) << std::endl
              << quantity<illuminance>(1.0*lux) << std::endl
              << quantity<inductance>(1.0*henry) << std::endl
              << quantity<luminous_flux>(1.0*lumen) << std::endl
              << quantity<magnetic_flux>(1.0*weber) << std::endl
              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
              << quantity<power>(1.0*watt) << std::endl
              << quantity<pressure>(1.0*pascals) << std::endl
              << quantity<resistance>(1.0*ohm) << std::endl
              << std::endl;
    //]

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  #ifndef _MSC_VER
  #warning deprecated
#endif
/*
Matrix3d m = Matrix3d::Zero();
m.part<Eigen::StrictlyUpperTriangular>().setOnes();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "And let us now compute m*m.adjoint() in a very optimized way" << endl
     << "taking advantage of the symmetry." << endl;
Matrix3d n;
n.part<Eigen::SelfAdjoint>() = (m*m.adjoint()).lazy();
cout << "The result is:" << endl << n << endl;
*/
  return 0;
}


//	=====================
//	standard C++ headers
//	=====================

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <iomanip>

//	=====================
//	OpenMPI header
//	=====================

#ifndef __CLASSMC_MPI__
#define __CLASSMC_MPI__
	#include "mpi.h"
#endif

//	=====================
//	ClassEngine headers
//	=====================

#include "class.h"
#include "Engine.hh"
#include "ClassEngine.hh"

//	========================================
//	Model, Data, ParamNames and Likelihoods
//	========================================

#include "CosmoTheory.hpp"
#include "DataList.hpp"
#include "ParamList.hpp"
#include "Likelihoods.hpp"

//	================================
//	Armadillo C++ linear algebra lib
//	================================
#include <armadillo>


/*! \file 2d_y_grid.cpp
  \brief 2D plot with Y axis grids.
  \details Shows an example of adding a Y horizontal grid to a 2D plot.
  \author Jacob Voytko and Paul A. Bristow
  \date 2007
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2009

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/svg_plot/svg_2d_plot.hpp>
#include <vector>

using std::map;
using namespace boost::svg;

int main()
{
  map<double, double> map1;
  map<double, double> map2;

  // This is random data used purely for example.
  map1[1.] = 3.2;
  map1[2.] = 5.4;
  map1[7.3] = 9.1;

  map2[3.1] = 6.1;
  map2[5.4] = 7.;

  svg_2d_plot my_plot;

  my_plot.title("Race Times")
         .legend_on(true)
         .x_range(-1, 11)
         .background_border_color(black)
         .plot_window_on(true);

    my_plot.y_major_grid_on(true)
       .y_minor_grid_on(true)
       .y_major_grid_color(blue)
       .y_minor_grid_color(lightgray);

    my_plot.plot(map1, "Series 1").stroke_color(blue).fill_color(red);
    my_plot.plot(map2, "Series 2").stroke_color(orange); // Fill white by default.

    my_plot.write("./2d_y_grid.svg");
  return 0;
} // int main()


/*
The MIT License

Copyright (c) 2019 Mert Turanli

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <string>
#include <cstring>
#include <iostream>
#include <cmath>

#include <cassert>

#include "hnn.h"

double HNN::dt = 0.1;
double HNN::l = 0.1;

#include <Eigen/Dense>

using Eigen::Matrix2d;
using Eigen::Array22d;
using Eigen::Matrix;
using Eigen::ArrayXXd;
using Eigen::MatrixXd;

using namespace std;

HNN::HNN(double hf_alpha, double hf_beta) {

	pose[0] = 0;
	pose[1] = 0;
	pose[2] = 0;
	pose_1[0] = 0;
	pose_1[1] = 0;
	pose_1[2] = 0;

	velocity[0] = 0;
	velocity[1] = 0;
	velocity[2] = 0;
	velocity_1[0] = 0;
	velocity_1[1] = 0;
	velocity_1[2] = 0;

	xt[0] = 0;
	xt[1] = 0;
	xt_1[0] = 0;
	xt_1[1] = 0;

	// estimation vector
	Ki[0] = 0;
	Ki[1] = 0;
	Ki_est[0] = 2.0;
	Ki_est[1] = 2.0;

	//
	this->AA = MatrixXd::Zero(2, 2);
	this->x1 = 0;
	this->x2 = 0;
	this->yy = MatrixXd::Zero(2, 1);

	this->WW = MatrixXd::Zero(2, 2);
	this->bb = MatrixXd::Zero(2, 1);

	this->dpitdt = MatrixXd::Zero(2, 1);
	this->dpitdt_1 = MatrixXd::Zero(2, 1);
	this->pit = MatrixXd::Zero(2, 1);
	this->pit_1 = MatrixXd::Zero(2, 1);
	this->pit_1(0, 0) = 1e-4; // hf_beta * atanh(1.0 / hf_alpha / Ki_est[0]); // 1e-2
	this->pit_1(1, 0) = 1e-4; // hf_beta * atanh(1.0 / hf_alpha / Ki_est[1]);

	this->si = MatrixXd::Zero(2, 1);
	this->theta_est = MatrixXd::Zero(2, 1);

	//
	this->estErr = 0;

	//

	this->hf_alpha = hf_alpha;
	this->hf_beta = hf_beta;

	//
	firstLoop = true;

}

HNN::~HNN() { }

void HNN::set(const double* pose, const double* velocity, const double* xt) {
	this->pose[0] = pose[0];
	this->pose[1] = pose[1];
	this->pose[2] = pose[2];

	this->velocity[0] = velocity[0];
	this->velocity[1] = velocity[1];
	this->velocity[2] = velocity[2];

	this->xt[0] = xt[0];
	this->xt[1] = xt[1];

	if (firstLoop == true) {
		this->pose_1[0] = pose[0];
		this->pose_1[1] = pose[1];
		this->pose_1[2] = pose[2];
		this->pose_2[0] = pose[0];
		this->pose_2[1] = pose[1];
		this->pose_2[2] = pose[2];

		this->velocity_1[0] = velocity[0];
		this->velocity_1[1] = velocity[1];
		this->velocity_1[2] = velocity[2];

		this->xt_1[0] = xt[0];
		this->xt_1[1] = xt[1];
		this->xt_2[0] = xt[0];
		this->xt_2[1] = xt[1];
	}
}

void HNN::setRealVector(const double* Ki) {
	this->Ki[0] = Ki[0];
	this->Ki[1] = Ki[1];
}

void HNN::estimate() {

	// derivatives
	double dx[3] = {0};

	if (firstLoop == false) {
		dx[0] = velocity[0];
		dx[1] = velocity[1];
		dx[2] = velocity[2];

		// find linear and angular velocities

		double theta = pose_1[2]; //

		Matrix<double, 2, 2> A = MatrixXd::Zero(2, 2);
		A(0, 0) = cos(theta);
		A(0, 1) = -sin(theta);
		A(1, 0) = sin(theta);
		A(1, 1) = cos(theta);

		double angle = atan2(dx[1], dx[0]) - theta;
		angle = fmod(angle, 2.0*M_PI);
		if (angle < 0) {
			angle += 2.0*M_PI;
		}

		if (angle > M_PI) {
			angle -= 2.0*M_PI;
		}
		else if (angle < -M_PI) {
			angle += 2.0*M_PI;
		}
		double angleDegrees = fabs(angle / M_PI * 180.0);
		double direction = 0;
		if (angleDegrees > 90.0) {
			direction = -1;
		}
		else {
			direction = 1;
		}

		Matrix<double, 2, 1> M_velocities = MatrixXd::Zero(2, 1);
		M_velocities(0, 0) = direction * sqrt(dx[0]*dx[0] + dx[1]*dx[1]);
		M_velocities(1, 0) = dx[2] * l;

		M_dx = A * M_velocities;

		std::cout << "velocity = (" << velocity[0] << ", " << velocity[1] << ", " << velocity[2] << ")" << std::endl;
		std::cout << "(v_calc, w_calc) = (" << direction * sqrt(dx[0]*dx[0] + dx[1]*dx[1]) << ", " << dx[2] << ")" << std::endl;

	}
	else {
		M_dx(0, 0) = 0;
		M_dx(1, 0) = 0;

	}

	/* AA = [  -dx(1), 0;
	            0, -dx(2) ];
	    x1 = Probot_1(1, i);
	    x2 = Probot_1(2, i);
	    yy = [  x1 - xt_1(1, i); x2 - xt_1(2, i)  ];*/

	AA(0, 0) = -M_dx(0, 0);
	AA(1, 1) = -M_dx(1, 0);

	x1 = pose_1[0];
	x2 = pose_1[1];

	yy(0, 0) = x1 - xt_1[0];
	yy(1, 0) = x2 - xt_1[1];

	WW = AA.transpose() * AA;
	bb = -AA.transpose() * yy;

	/* dpitdt(:, i) = -(WW * si(:, i) + bb);
	    pit(:, i) = pit_1(:, i) + (dpitdt(:, i) + dpitdt_1(:, i)) / 2.0 * dt;
	    */
	dpitdt = -(WW * si + bb);
	// integrate
	pit(0, 0) = pit_1(0, 0) + (dpitdt(0, 0) + dpitdt_1(0, 0)) / 2.0 * dt;
	pit(1, 0) = pit_1(1, 0) + (dpitdt(1, 0) + dpitdt_1(1, 0)) / 2.0 * dt;

	// si(:, i) = hf_alpha * tanh(pit(:, i) / hf_beta);
	si(0, 0) = hf_alpha * tanh(pit(0, 0) / hf_beta);
	si(1, 0) = hf_alpha * tanh(pit(1, 0) / hf_beta);

	theta_est = si;

	// assign Ki_est
	Ki_est[0] = 1.0 / theta_est(0, 0);
	Ki_est[1] = 1.0 / theta_est(1, 0);

	// output
	double estErr = sqrt((Ki_est[0] - Ki[0]) * (Ki_est[0] - Ki[0]) +
			     (Ki_est[1] - Ki[1]) * (Ki_est[1] - Ki[1]));

	this->estErr = estErr;

	cout << "Ki_est = [" << Ki_est[0] << ", " << Ki_est[1] << "]" << endl;

	pit_1 = pit;
	dpitdt_1 = dpitdt;

	//
    	pose_1[0] = pose[0];
	pose_1[1] = pose[1];
	pose_1[2] = pose[2];

	xt_1[0] = xt[0];
	xt_1[1] = xt[1];

	velocity_1[0] = velocity[0];
	velocity_1[1] = velocity[1];
	velocity_1[2] = velocity[2];

	//
	firstLoop = false;

	return;
}

void HNN::getEstimationVector(double* Ki) {
	Ki[0] = this->Ki_est[0];
	Ki[1] = this->Ki_est[1];

	return;
}

double HNN::getEstimationError() {
	return this->estErr;
}


////////////////////////////////////////////////////////////////////////////////
#include <polyfem/RBFWithQuadraticLagrange.hpp>
#include <polyfem/RBFWithQuadratic.hpp>
#include <polyfem/AssemblerUtils.hpp>
#include <polyfem/Types.hpp>
#include <polyfem/MatrixUtils.hpp>
#include <polyfem/Logger.hpp>

#include <igl/Timer.h>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <array>
////////////////////////////////////////////////////////////////////////////////

using namespace polyfem;

namespace {

// Harmonic kernel
double kernel(const bool is_volume, const double r) {
	if (r < 1e-8) { return 0; }

	if (is_volume) {
		return 1/r;
	} else {
		return log(r);
	}
}

double kernel_prime(const bool is_volume, const double r) {
	if (r < 1e-8) { return 0; }

	if(is_volume) {
		return -1/(r*r);
	} else {
		return 1/r;
	}
}

// Biharmonic kernel (2d only)
// double kernel(const bool is_volume, const double r) {
// 	assert(!is_volume);
// 	if (r < 1e-8) { return 0; }

// 	return r * r * (log(r)-1);
// }

// double kernel_prime(const bool is_volume, const double r) {
// 	assert(!is_volume);
// 	if (r < 1e-8) { return 0; }

// 	return r * ( 2 * log(r) - 1);
// }

} // anonymous namespace

////////////////////////////////////////////////////////////////////////////////

RBFWithQuadraticLagrange::RBFWithQuadraticLagrange(
		const std::string &assembler_name,
		const Eigen::MatrixXd &centers,
		const Eigen::MatrixXd &collocation_points,
		const Eigen::MatrixXd &local_basis_integral,
		const Quadrature &quadr,
		Eigen::MatrixXd &rhs,
		bool with_constraints)
	: centers_(centers)
{
	// centers_.resize(0, centers.cols());
	compute_weights(assembler_name, collocation_points, local_basis_integral, quadr, rhs, with_constraints);
}

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

void RBFWithQuadraticLagrange::basis(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	Eigen::MatrixXd tmp;
	bases_values(samples, tmp);
	val = tmp.col(local_index);
}

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

void RBFWithQuadraticLagrange::grad(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	Eigen::MatrixXd tmp;
	const int dim = centers_.cols();
	val.resize(samples.rows(), dim);
	for (int d = 0; d < dim; ++d) {
		bases_grads(d, samples, tmp);
		val.col(d) = tmp.col(local_index);
	}
}

////////////////////////////////////////////////////////////////////////////////

void RBFWithQuadraticLagrange::bases_values(const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	// Multiply by the weights
	val = A * weights_;
}

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

void RBFWithQuadraticLagrange::bases_grads(const int axis, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	const int num_kernels = centers_.rows();
	const int dim = (is_volume() ? 3 : 2);

	// Compute ∇xA
	Eigen::MatrixXd A_prime(samples.rows(), num_kernels + 1 + dim + dim*(dim+1)/2);
	A_prime.setZero();

	for (int j = 0; j < num_kernels; ++j) {
		A_prime.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x)
			{ return kernel_prime(is_volume(), x) / x; });
		A_prime.col(j) = (samples.col(axis).array() - centers_(j, axis)) * A_prime.col(j).array();
	}
	// Linear terms
	A_prime.middleCols(num_kernels + 1 + axis, 1).setOnes();
	// Mixed terms
	if (dim == 2) {
		A_prime.col(num_kernels + 1 + dim) = samples.col(1 - axis);
	} else {
		A_prime.col(num_kernels + 1 + dim + axis) = samples.col((axis+1)%dim);
		A_prime.col(num_kernels + 1 + dim + (axis+2)%dim) = samples.col((axis+2)%dim);
	}
	// Quadratic terms
	A_prime.rightCols(dim).col(axis) = 2.0 * samples.col(axis);

	// Apply weights
	val = A_prime * weights_;
}

////////////////////////////////////////////////////////////////////////////////

void RBFWithQuadraticLagrange::compute_kernels_matrix(const Eigen::MatrixXd &samples, Eigen::MatrixXd &A) const {
	// Compute A
	const int num_kernels = centers_.rows();
	const int dim = (is_volume() ? 3 : 2);

	A.resize(samples.rows(), num_kernels + 1 + dim + dim*(dim+1)/2);
	for (int j = 0; j < num_kernels; ++j) {
		A.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x)
			{ return kernel(is_volume(), x); });
	}
	A.col(num_kernels).setOnes(); // constant term
	A.middleCols(num_kernels + 1, dim) = samples; // linear terms
	if (dim == 2) {
		A.middleCols(num_kernels + dim + 1, 1) = samples.rowwise().prod(); // mixed terms
	} else if (dim == 3) {
		A.middleCols(num_kernels + dim + 1, 3) = samples;
		A.middleCols(num_kernels + dim + 1 + 0, 1).array() *= samples.col(1).array();
		A.middleCols(num_kernels + dim + 1 + 1, 1).array() *= samples.col(2).array();
		A.middleCols(num_kernels + dim + 1 + 2, 1).array() *= samples.col(0).array();
	}
	A.rightCols(dim) = samples.array().square(); // quadratic terms
}

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

void RBFWithQuadraticLagrange::compute_constraints_matrix_2d_old(
	const int num_bases, const Quadrature &quadr, Eigen::MatrixXd &C) const
{
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();
	assert(dim == 2);

	// K_cst = ∫φj
	// K_lin = ∫∇x(φj), ∫∇y(φj)
	// K_mix = ∫y·∇x(φj), ∫x·∇y(φj)
	// K_sqr = ∫x·∇x(φj), ∫y·∇y(φj)
	Eigen::VectorXd K_cst = Eigen::VectorXd::Zero(num_kernels);
	Eigen::MatrixXd K_lin = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_mix = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_sqr = Eigen::MatrixXd::Zero(num_kernels, dim);
	for (int j = 0; j < num_kernels; ++j) {
		// ∫∇x(φj)(p) = Σ_q (xq - xk) * 1/r * h'(r) * wq
		// - xq is the x coordinate of the q-th quadrature point
		// - wq is the q-th quadrature weight
		// - r is the distance from pq to the kernel center
		// - h is the RBF kernel (scalar function)
		for (int q = 0; q < quadr.points.rows(); ++q) {
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();
			const RowVectorNd gradPhi = p * kernel_prime(is_volume(), r) / r * quadr.weights(q);
			K_cst(j) += kernel(is_volume(), r) * quadr.weights(q);
			K_lin.row(j) += gradPhi;
			K_mix(j,0) += quadr.points(q,1)*gradPhi(0);
			K_mix(j,1) += quadr.points(q,0)*gradPhi(1);
			K_sqr.row(j) += (quadr.points.row(q).array() * gradPhi.array()).matrix();
		}
	}

	// I_lin = ∫x, ∫y
	// I_mix = ∫xy
	// I_sqr = ∫x², ∫y²
	Eigen::RowVectorXd I_lin = (quadr.points.array().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_mix = (quadr.points.rowwise().prod().array() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_sqr = (quadr.points.array().square().colwise() * quadr.weights.array()).colwise().sum();
	double volume = quadr.weights.sum();

	//TODO
	// std::cout << I_lin << std::endl;
	// std::cout << I_mix << std::endl;
	// std::cout << I_sqr << std::endl;

	// Compute M
	Eigen::Matrix<double, 5, 5> M;
	M <<    volume,          0,          I_lin(1), 2*I_lin(0),          0,
	             0,     volume,          I_lin(0),          0, 2*I_lin(1),
	      I_lin(1),   I_lin(0), I_sqr(0)+I_sqr(1), 2*I_mix(0), 2*I_mix(0),
	    4*I_lin(0), 2*I_lin(1),        4*I_mix(0), 6*I_sqr(0), 2*I_sqr(1),
	    2*I_lin(0), 4*I_lin(1),        4*I_mix(0), 2*I_sqr(0), 6*I_sqr(1);
	Eigen::FullPivLU<Eigen::Matrix<double, 5, 5>> lu(M);

	show_matrix_stats(M);

	// Compute L
	C.resize(5, num_kernels + 1 + dim + dim*(dim+1)/2);
	C.setZero();
	C.block(0, 0, dim, num_kernels) = K_lin.transpose();
	C.block(dim, 0, 1, num_kernels) = K_mix.transpose().colwise().sum();
	C.block(dim + 1, 0, dim, num_kernels) = 2.0 * (K_sqr.colwise() + K_cst).transpose();
	C.block(dim + 1, num_kernels, dim, 1).setConstant(2.0 * volume);
	C.bottomRightCorner(5, 5) = M;
	// std::cout << L.bottomRightCorner(10, 10) << std::endl;
}

void RBFWithQuadraticLagrange::compute_constraints_matrix_2d(const std::string &assembler_name,
	const int num_bases, const Quadrature &quadr, Eigen::MatrixXd &C) const
{
	const int num_kernels = centers_.rows();
	const int space_dim = centers_.cols();
	const auto &assembler = AssemblerUtils::instance();
	const int assembler_dim = assembler.is_tensor(assembler_name) ? 2 : 1;
	assert(space_dim == 2);

	std::array<Eigen::MatrixXd, 5> strong;

	// ass_val = [q_10, q_01, q_11, q_20, q_02, psi_0, ..., psi_k]
	ElementAssemblyValues ass_val;
	ass_val.has_parameterization = false;
	ass_val.basis_values.resize(5 + num_kernels);

	//evaluating monomial and grad of monomials at quad points
	RBFWithQuadratic::setup_monomials_vals_2d(0, quadr.points, ass_val);
	RBFWithQuadratic::setup_monomials_strong_2d(assembler_dim, assembler_name, quadr.points, quadr.weights.array(), strong);

	//evaluating psi and grad psi at quadr points
	for (int j = 0; j < num_kernels; ++j) {
		ass_val.basis_values[5 + j].val  = Eigen::MatrixXd(quadr.points.rows(), 1);
		ass_val.basis_values[5 + j].grad = Eigen::MatrixXd(quadr.points.rows(), quadr.points.cols());

		for (int q = 0; q < quadr.points.rows(); ++q) {
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();

			ass_val.basis_values[5 + j].val(q) = kernel(is_volume(), r);
			ass_val.basis_values[5 + j].grad.row(q) = p * kernel_prime(is_volume(), r) / r;
		}
	}

	for(size_t i = 5; i < ass_val.basis_values.size(); ++i)
	{
		ass_val.basis_values[i].grad_t_m = ass_val.basis_values[i].grad;
	}

	// Compute C
	C.resize(RBFWithQuadratic::index_mapping(assembler_dim-1, assembler_dim-1, 4, assembler_dim)+1, num_kernels + 1 + 5);
	C.setZero();

	for(int d = 0; d < 5; ++d)
	{
		//first num_kernels bases
		for(int i = 0; i < num_kernels; ++i)
		{
			const auto tmp = assembler.local_assemble(assembler_name, ass_val, d, 5 + i, quadr.weights);
			for(int alpha = 0; alpha < assembler_dim; ++alpha)
			{
				for(int beta = 0; beta < assembler_dim; ++beta)
				{
					const int loc_index = alpha*assembler_dim + beta;
					C(RBFWithQuadratic::index_mapping(alpha, beta, d, assembler_dim), i) =  tmp(loc_index) + (strong[d].row(loc_index).transpose().array() * ass_val.basis_values[5+i].val.array()).sum();
				}
			}
		}

		//second the q_i
		for(int i = 0; i < 5; ++i)
		{
			const auto tmp = assembler.local_assemble(assembler_name, ass_val, d, i, quadr.weights);
			for(int alpha = 0; alpha < assembler_dim; ++alpha)
			{
				for(int beta = 0; beta < assembler_dim; ++beta)
				{
					const int loc_index = alpha*assembler_dim + beta;
					C(RBFWithQuadratic::index_mapping(alpha, beta, d, assembler_dim), num_kernels + i + 1) =  tmp(loc_index) + (strong[d].row(loc_index).transpose().array() * ass_val.basis_values[i].val.array()).sum();
				}
			}
		}

		//finally the constant
		for(int alpha = 0; alpha < assembler_dim; ++alpha)
		{
			for(int beta = 0; beta < assembler_dim; ++beta)
			{
				// std::cout<<alpha <<" "<< beta <<" "<< d <<" "<< assembler_dim<< " -> r = "<< RBFWithQuadratic::index_mapping(alpha, beta, d, assembler_dim)<<std::endl;
				const int loc_index = alpha*assembler_dim + beta;
				C(RBFWithQuadratic::index_mapping(alpha, beta, d, assembler_dim), num_kernels) = strong[d].row(loc_index).sum();
			}
		}
	}

	// {
	// 	std::ofstream file;
	// 	file.open("C.txt");
	// 	file << C;
	// 	file.close();
	// }

	// Eigen::MatrixXd Cold;
	// compute_constraints_matrix_2d_old(num_bases, quadr,Cold);
	// {
	// 	std::ofstream file;
	// 	file.open("Cold.txt");
	// 	file << Cold;
	// 	file.close();
	// }
}

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

void RBFWithQuadraticLagrange::compute_constraints_matrix_3d(
	const int num_bases, const Quadrature &quadr, Eigen::MatrixXd &C) const
{
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();
	assert(dim == 3);

	// K_cst = ∫φj
	// K_lin = ∫∇x(φj), ∫∇y(φj), ∫∇z(φj)
	// K_mix = ∫(y·∇x(φj)+x·∇y(φj)), ∫(z·∇y(φj)+y·∇z(φj)), ∫(x·∇z(φj)+z·∇x(φj))
	// K_sqr = ∫x·∇x(φj), ∫y·∇y(φj), ∫z·∇z(φj)
	Eigen::VectorXd K_cst = Eigen::VectorXd::Zero(num_kernels);
	Eigen::MatrixXd K_lin = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_mix = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_sqr = Eigen::MatrixXd::Zero(num_kernels, dim);
	for (int j = 0; j < num_kernels; ++j) {
		// ∫∇x(φj)(p) = Σ_q (xq - xk) * 1/r * h'(r) * wq
		// - xq is the x coordinate of the q-th quadrature point
		// - wq is the q-th quadrature weight
		// - r is the distance from pq to the kernel center
		// - h is the RBF kernel (scalar function)
		for (int q = 0; q < quadr.points.rows(); ++q) {
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();
			const RowVectorNd gradPhi = p * kernel_prime(is_volume(), r) / r * quadr.weights(q);
			K_cst(j) += kernel(is_volume(), r) * quadr.weights(q);
			K_lin.row(j) += gradPhi;
			for (int d = 0; d < dim; ++d) {
				K_mix(j,d) += quadr.points(q,(d+1)%dim)*gradPhi(d) + quadr.points(q,d)*gradPhi((d+1)%dim);
			}
			K_sqr.row(j) += (quadr.points.row(q).array() * gradPhi.array()).matrix();
		}
	}

	// I_lin = ∫x, ∫y, ∫z
	// I_sqr = ∫x², ∫y², ∫z²
	// I_mix = ∫xy, ∫yz, ∫zx
	Eigen::RowVectorXd I_lin = (quadr.points.array().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_sqr = (quadr.points.array().square().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_mix(3);
	I_mix(0) = (quadr.points.col(0).array() * quadr.points.col(1).array() * quadr.weights.array()).sum();
	I_mix(1) = (quadr.points.col(1).array() * quadr.points.col(2).array() * quadr.weights.array()).sum();
	I_mix(2) = (quadr.points.col(2).array() * quadr.points.col(0).array() * quadr.weights.array()).sum();
	double volume = quadr.weights.sum();

	// std::cout << I_lin << std::endl;
	// std::cout << I_mix << std::endl;
	// std::cout << I_sqr << std::endl;

	// Compute M
	Eigen::Matrix<double, 9, 9> M;
	M <<    volume,          0,          0,          I_lin(1),                 0,          I_lin(2), 2*I_lin(0),          0,          0,
	             0,     volume,          0,          I_lin(0),          I_lin(2),                 0,          0, 2*I_lin(1),          0,
	             0,          0,     volume,                 0,          I_lin(1),          I_lin(0),          0,          0, 2*I_lin(2),
	      I_lin(1),   I_lin(0),          0, I_sqr(0)+I_sqr(1),          I_mix(2),          I_mix(1), 2*I_mix(0), 2*I_mix(0),          0,
	             0,   I_lin(2),   I_lin(1),          I_mix(2), I_sqr(1)+I_sqr(2),          I_mix(0),          0, 2*I_mix(1), 2*I_mix(1),
	      I_lin(2),          0,   I_lin(0),          I_mix(1),          I_mix(0), I_sqr(2)+I_sqr(0), 2*I_mix(2),          0, 2*I_mix(2),
	    2*I_lin(0),          0,          0,        2*I_mix(0),                 0,        2*I_mix(2), 4*I_sqr(0),          0,          0,
	             0, 2*I_lin(1),          0,        2*I_mix(0),        2*I_mix(1),                 0,          0, 4*I_sqr(1),          0,
	             0,          0, 2*I_lin(2),                 0,        2*I_mix(1),        2*I_mix(2),          0,          0, 4*I_sqr(2);
	Eigen::Matrix<double, 1, 9> M_rhs;
	M_rhs.segment<3>(0) = I_lin;
	M_rhs.segment<3>(3) = I_mix;
	M_rhs.segment<3>(6) = I_sqr;
	// M_rhs << I_lin, I_mix, I_sqr;
	M.bottomRows(dim).rowwise() += 2.0 * M_rhs;
	//TODO
	// assert(false);

	show_matrix_stats(M);

	// Compute L
	C.resize(9, num_kernels + 1 + dim + dim*(dim+1)/2);
	C.setZero();
	C.block(0, 0, dim, num_kernels) = K_lin.transpose();
	C.block(dim, 0, dim, num_kernels) = K_mix.transpose();
	C.block(dim + dim, 0, dim, num_kernels) = 2.0 * (K_sqr.colwise() + K_cst).transpose();
	C.block(dim + dim, num_kernels, dim, 1).setConstant(2.0 * volume);
	C.bottomRightCorner(9, 9) = M;
	// std::cout << C.bottomRightCorner(9, 12) << std::endl;
}

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

void RBFWithQuadraticLagrange::compute_weights(const std::string &assembler_name, const Eigen::MatrixXd &samples,
	const Eigen::MatrixXd &local_basis_integral, const Quadrature &quadr,
	Eigen::MatrixXd &b, bool with_constraints)
{
	logger().trace("#kernel centers: {}", centers_.rows());
	logger().trace("#collocation points: {}", samples.rows());
	logger().trace("#quadrature points: {}", quadr.weights.size());
	logger().trace("#non-vanishing bases: {}", b.cols());
	logger().trace("#constraints: {}", b.cols());

	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	if (!with_constraints) {
		// Solve the system
		const int num_kernels = centers_.rows();
		logger().trace("-- Solving system of size {}x{}", num_kernels, num_kernels);
		weights_ = (A.transpose() * A).ldlt().solve(A.transpose() * b);
		logger().trace("-- Solved!");

		return;
	}

	const int num_bases = b.cols();

    const Eigen::MatrixXd At = A.transpose();

	// Compute C
	Eigen::MatrixXd C;
	if (is_volume()) {
		compute_constraints_matrix_3d(num_bases, quadr, C);
	} else {
		compute_constraints_matrix_2d(assembler_name, num_bases, quadr, C);
	}

	const int dim = centers_.cols();
	assert(centers_.rows() + 1 + dim + dim*(dim+1)/2 > C.rows());
	logger().trace("#constraints: {}", C.rows());

	// Compute rhs = [ A^T b; d ]
	assert(local_basis_integral.cols() == C.rows());
	assert(local_basis_integral.rows() == b.cols());
	assert(A.rows() == b.rows());
	Eigen::MatrixXd rhs(A.cols() + local_basis_integral.cols(), b.cols());
	rhs.topRows(A.cols()) = At * b;
	rhs.bottomRows(local_basis_integral.cols()) = local_basis_integral.transpose();

	// Compute M = [ A^T A, C^T; C, 0]
	assert(C.cols() == A.cols());
	assert(A.rows() == b.rows());
	Eigen::MatrixXd M(A.cols() + C.rows(), A.cols() + C.rows());
	M.topLeftCorner(A.cols(), A.cols()) = At * A;
	M.topRightCorner(A.cols(), C.rows()) = C.transpose();
	M.bottomLeftCorner(C.rows(), A.cols()) = C;
	M.bottomRightCorner(C.rows(), C.rows()).setZero();

	// std::cout << M.bottomRightCorner(10, 10) << std::endl;

	// Solve the system
	logger().trace("-- Solving system of size {}x{}", M.rows(), M.cols());
	auto ldlt = M.ldlt();
	if (ldlt.info() == Eigen::NumericalIssue) {
		logger().error("-- WARNING: Numerical issues when solving the harmonic least square.");
	}
	const auto tmp = ldlt.solve(rhs);
	weights_ = tmp.topRows(A.cols());
	logger().trace("-- Solved!");
    logger().trace("-- Mean residual: {}", (A * weights_ - b).array().abs().colwise().maxCoeff().mean());
    logger().trace("-- Max constraints error: {}", (C * weights_ - local_basis_integral.transpose()).array().abs().maxCoeff());

 //    {
	// 	std::ofstream file;
	// 	file.open("M.txt");
	// 	file << M;
	// 	file.close();
	// }

	// {
	// 	std::ofstream file;
	// 	file.open("C.txt");
	// 	file << C;
	// 	file.close();
	// }

	// {
	// 	std::ofstream file;
	// 	file.open("b.txt");
	// 	file << local_basis_integral.transpose();
	// 	file.close();
	// }

 //    {
	// 	std::ofstream file;
	// 	file.open("rhs.txt");
	// 	file << rhs;
	// 	file.close();
	// }

	// std::cout << M.bottomRightCorner(10, 10) << std::endl;

#if 0
	// Compute rhs = [ A^T b; d ]
	assert(local_basis_integral.cols() == C.rows());
	assert(local_basis_integral.rows() == b.cols());
	assert(A.rows() == b.rows());
	Eigen::MatrixXd rhs(A.cols() + local_basis_integral.cols(), b.cols());
	rhs.topRows(A.cols()) = At * b;
	rhs.bottomRows(local_basis_integral.cols()) = local_basis_integral.transpose();

	// Compute M = [ A^T A, C^T; C, 0]
	assert(C.cols() == A.cols());
	assert(A.rows() == b.rows());
	Eigen::MatrixXd M(A.cols() + C.rows(), A.cols() + C.rows());
	M.topLeftCorner(A.cols(), A.cols()) = At * A;
	M.topRightCorner(A.cols(), C.rows()) = C.transpose();
	M.bottomLeftCorner(C.rows(), A.cols()) = C;
	M.bottomRightCorner(C.rows(), C.rows()).setZero();

	// std::cout << M.bottomRightCorner(10, 10) << std::endl;

	// Solve the system
	logger().trace("-- Solving system of size {}x{}", M.rows(), M.cols());
	auto ldlt = M.ldlt();
	if (ldlt.info() == Eigen::NumericalIssue) {
		logger().error("-- WARNING: Numerical issues when solving the harmonic least square.");
	}
	weights_ = ldlt.solve(rhs).topRows(A.cols());
	logger().trace("-- Solved!");
    logger().trace("-- Mean residual: {}", (A * weights_ - b).array().abs().colwise().maxCoeff().mean());

	Eigen::MatrixXd MM, x, dx, val;
	basis(0, quadr.points, val);
	grad(0, quadr.points, MM);
	int dim = (is_volume() ? 3 : 2);
	for (int d = 0; d < dim; ++d) {
		// basis(0, quadr.points, x);
		// auto asd = quadr.points;
		// asd.col(d).array() += 1e-7;
		// basis(0, asd, dx);
		// std::cout << (dx - x) / 1e-7 - MM.col(d) << std::endl;
		std::cout << (MM.col(d).array() * quadr.weights.array()).sum() - local_basis_integral(0, d) << std::endl;
		std::cout << ((
				MM.col((d+1)%dim).array() * quadr.points.col(d).array()
				+ MM.col(d).array() * quadr.points.col((d+1)%dim).array()
			) * quadr.weights.array()).sum() - local_basis_integral(0, (dim == 2 ? 2 : (dim+d) )) << std::endl;
		std::cout << 2.0 * (
				(quadr.points.col(d).array() * MM.col(d).array()
				+ val.array())
			* quadr.weights.array()
			).sum() - local_basis_integral(0, (dim == 2 ? (3 + d) : (dim+dim+d))) << std::endl;
	}
#endif
}


/*
  The ziggurat method for RNOR and REXP
  Combine the code below with the main program in which you want
  normal or exponential variates.   Then use of RNOR in any expression
  will provide a standard normal variate with mean zero, variance 1,
  while use of REXP in any expression will provide an exponential variate
  with density exp(-x),x>0.
  Before using RNOR or REXP in your main, insert a command such as
  zigset(86947731 );
  with your own choice of seed value>0, rather than 86947731.
  (If you do not invoke zigset(...) you will get all zeros for RNOR and REXP.)
  For details of the method, see Marsaglia and Tsang, "The ziggurat method
  for generating random variables", Journ. Statistical Software.

  Original functions converted to a c++ object and static variables removed
  // mavarjok
*/

#ifndef ZIGGURAT_HH
#define ZIGGURAT_HH

#include <cmath>
#include <ctime>

// Visual studio doesn't have stdint.h varjokal 24.3.2010
#ifdef _MSC_VER
#include <boost/cstdint.hpp>
using namespace boost;
#else
#include <stdint.h>
#endif

/** Namespace for Ziggurat N(0,1) random number generator. */
namespace ziggurat {

  class Rnd {

  public:

    Rnd()
    {
      jsr=123456789;
      zigset(time(NULL));
    }

    Rnd(uint32_t s)
    {
      jsr=123456789;
      zigset(s);
    }

    /* Generates a random sample from N(0,1) distribution */
    float rnor()
    {
      hz=shr3();
      iz=hz&127;
      return ((fabs((float)((hz)<int(kn[iz])))) ? hz*wn[iz] : nfix());
    }
    /* Generates a random sample from the exponential distribution */
    float rexp()
    {
      jz=shr3();
      iz=jz&255;
      return ((jz <ke[iz]) ? jz*we[iz] : efix());
    }

  private:

    uint32_t jz,jsr;
    int32_t hz;
    uint32_t iz, kn[128], ke[256];
    float wn[128],fn[128], we[256],fe[256];
    float x, y;
    float r;     /* The start of the right tail */

    /* Generates variates from the residue when rejection in RNOR occurs */
    float nfix(void);

    /* Generates variates from the residue when rejection in REXP occurs */
    float efix(void);

    /* Sets the seed and creates the tables */
    void zigset(uint32_t jsrseed);

    inline uint32_t shr3()
    {
      jz=jsr;
      jsr^=(jsr<<13);
      jsr^=(jsr>>17);
      jsr^=(jsr<<5);
      return jz+jsr;
    }

    inline double uni()
    {
      return .5 + (signed) shr3()*.2328306e-9;
    }

    inline uint32_t iuni()
    {
      return shr3();
    }
  };

  /** Global generator instance for normal use. */
  extern Rnd rnd;

};

#endif /* ZIGGURAT_HH */


#define BOOST_TEST_MODULE DynMultimodalModelTestSuite
#define BOOST_TEST_DYN_LINK

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <hops/Model/DynMultimodalModel.hpp>
#include "hops/Model/MultivariateGaussianModel.hpp"

BOOST_AUTO_TEST_SUITE(DynMultiModelTestSuite)
    template<typename Matrix, typename Vector>
    class ModelMock {
    public:
        using MatrixType = Matrix;
        using VectorType = Vector;

        typename MatrixType::Scalar fx;
        ModelMock (typename MatrixType::Scalar fx) : fx(fx) {
            // do nothing
        }

        double computeNegativeLogLikelihood(const VectorType &) const {
            return -std::log(fx);
        }

        [[nodiscard]] MatrixType computeExpectedFisherInformation(const VectorType &x) const {
            return Eigen::MatrixXd::Ones(x.rows(), x.rows());
        }

        [[nodiscard]] VectorType computeLogLikelihoodGradient(const VectorType &x) const {
            return x;
        }
    };

    BOOST_AUTO_TEST_CASE(CalculateNegativeLogLikelihood) {
        std::vector<double> componentLikelihoods({1, 2, 3, 4, 5});
        std::vector<double> weights({5, 4, 3, 2, 1});

        double expectedNegativeLogLikelihood = 0;
        std::vector<ModelMock<Eigen::MatrixXd, Eigen::VectorXd>> modelComponents;

        for (size_t i = 0; i < componentLikelihoods.size(); ++i) {
            modelComponents.push_back(ModelMock<Eigen::MatrixXd, Eigen::VectorXd>(componentLikelihoods[i]));
            expectedNegativeLogLikelihood += weights[i] * componentLikelihoods[i];
        }

        expectedNegativeLogLikelihood = -std::log(expectedNegativeLogLikelihood);

        auto multimodalModel = hops::DynMultimodalModel<ModelMock<Eigen::MatrixXd, Eigen::VectorXd>>(modelComponents, weights);
        auto evaluationPoint = Eigen::VectorXd::Zero(2);

        double actualNegativeLogLikelihood = multimodalModel.computeNegativeLogLikelihood(evaluationPoint);
        BOOST_CHECK_CLOSE(actualNegativeLogLikelihood, expectedNegativeLogLikelihood, 0.001);
    }

    BOOST_AUTO_TEST_CASE(CalculateNegativeLogLikelihoodForGaussianMixture) {
        unsigned d = 1;
        Eigen::VectorXd mean1 = Eigen::VectorXd::Ones(d);
        Eigen::VectorXd mean2 = -2 * Eigen::VectorXd::Ones(d);
        Eigen::MatrixXd covariance = 0.1 * Eigen::MatrixXd::Identity(d, d);

        hops::MultivariateGaussianModel model1(mean1, covariance);
        hops::MultivariateGaussianModel model2(mean2, covariance);

        hops::DynMultimodalModel multimodalModel = hops::DynMultimodalModel<hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>>({model1, model2}, {1, 1});

        Eigen::VectorXd evaluationPoint = Eigen::VectorXd::Zero(d);
        BOOST_CHECK_CLOSE(multimodalModel.computeNegativeLogLikelihood(evaluationPoint), 4.767645680805376, 0.001);

        evaluationPoint = 0.5 * Eigen::VectorXd::Ones(d);
        BOOST_CHECK_CLOSE(multimodalModel.computeNegativeLogLikelihood(evaluationPoint), 1.017645986707556, 0.001);
    }

    BOOST_AUTO_TEST_CASE(CalculateLogLikelihoodGradient) {
        unsigned d = 1;
        Eigen::VectorXd expectedLogLikelihoodGradient = 0.166667 * Eigen::VectorXd::Ones(d);

        hops::MultivariateGaussianModel model1 = hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>(Eigen::VectorXd::Zero(d), Eigen::MatrixXd::Identity(d, d));
        hops::MultivariateGaussianModel model2 = hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>(Eigen::VectorXd::Ones(d), Eigen::MatrixXd::Identity(d, d));

        Eigen::VectorXd evaluationPoint = 0.5 * Eigen::VectorXd::Ones(d);

        hops::DynMultimodalModel multimodalModel = hops::DynMultimodalModel<hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>>({model1, model2}, {1, 2});

        Eigen::VectorXd actualLogLikelihoodGradient = multimodalModel.computeLogLikelihoodGradient(evaluationPoint);

        BOOST_CHECK_SMALL((actualLogLikelihoodGradient - expectedLogLikelihoodGradient).squaredNorm(), 1.e-7);
    }

    BOOST_AUTO_TEST_CASE(CalculateLogLikelihoodGradientTrivial) {
        unsigned d = 1;
        Eigen::VectorXd expectedLogLikelihoodGradient = Eigen::VectorXd::Zero(d);

        hops::MultivariateGaussianModel model = hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>(Eigen::VectorXd::Zero(d), Eigen::MatrixXd::Identity(d, d));
        Eigen::VectorXd evaluationPoint = Eigen::VectorXd::Zero(d);

        hops::DynMultimodalModel multimodalModel = hops::DynMultimodalModel<hops::MultivariateGaussianModel<Eigen::MatrixXd, Eigen::VectorXd>>({model, model}, {1, 2});

        Eigen::VectorXd actualLogLikelihoodGradient = multimodalModel.computeLogLikelihoodGradient(evaluationPoint);

        BOOST_CHECK_SMALL((actualLogLikelihoodGradient - expectedLogLikelihoodGradient).squaredNorm(), 1.e-7);
    }
BOOST_AUTO_TEST_SUITE_END()


//  Copyright John Maddock 2007.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

//[policy_eg_1

#include <boost/math/special_functions/gamma.hpp>
//
// Define the policy to use:
//
using namespace boost::math::policies;
typedef policy<
   domain_error<errno_on_error>,
   pole_error<errno_on_error>,
   overflow_error<errno_on_error>,
   evaluation_error<errno_on_error>
> c_policy;
//
// Now use the policy when calling tgamma:
//
int main()
{
   errno = 0;
   std::cout << "Result of tgamma(30000) is: "
      << boost::math::tgamma(30000, c_policy()) << std::endl;
   std::cout << "errno = " << errno << std::endl;
   std::cout << "Result of tgamma(-10) is: "
      << boost::math::tgamma(-10, c_policy()) << std::endl;
   std::cout << "errno = " << errno << std::endl;
}

//]


#define BOOST_TEST_MODULE test_tir

#include "Leg/FixedLeg.h"
#include <Leg/ZeroCurve/ZeroCouponCurve.h>
#include <boost/test/unit_test.hpp>
#include <Tir/Tir.h>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_SUITE(tir_test_suite)

/*
    BOOST_AUTO_TEST_CASE(actual_count) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double notional = 100;
        const double rate = 5.0/100;
        std::vector<boost::gregorian::date> referenceDates {};
        referenceDates.push_back(boost::gregorian::from_string("2016-04-01"));
        referenceDates.push_back(boost::gregorian::from_string("2016-10-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-04-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-10-02"));
        referenceDates.push_back(boost::gregorian::from_string("2018-04-02"));

        Actual_360 actualCalc = Actual_360();
        ZeroCouponCurve zeroCouponCurve {};
        Tir tir = Tir(notional, rate, referenceDates, actualCalc);

        //Tir tir = Tir();

        double expected_values = 6.76;

        double calculated_values = tir.Get_Tir(6.76, 0.2);

        BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST_MESSAGE(" - Diff " << calculated_values - expected_values);
        BOOST_TEST(expected_values == calculated_values, boost::test_tools::tolerance(1e-15));
    }*/

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/interleave_second.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL( "unary interleave_second based shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
 {
    bs::pack<T,4> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>() };
    bs::pack<T,4> b{ T(13), bs::Valmin<T>(), T(37), bs::Valmax<T>() };
    bs::pack<T,4> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 2, 2, 3, 3>(a))  , (bs::interleave_second(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 2, 2, 3, 3>(a,b)), (bs::interleave_second(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 2,-1, 3>(a))  , (bs::interleave_second(z,a)) );

    STF_ALL_EQUAL( (bs::shuffle< 6, 6, 7, 7>(a,b)), (bs::interleave_second(b,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 6,-1, 7>(a,b)), (bs::interleave_second(z,b)) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>(), T(37), bs::Valmax<T>(), T(55), T(99) };
    bs::pack<T,8> b{ T(13), bs::Valmax<T>(), T(42), bs::Valmax<T>(), T(76), bs::Valmin<T>(), T(42), T(69) };
    bs::pack<T,8> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 4,4, 5,5, 6,6, 7,7>(a))  , (bs::interleave_second(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 4,4, 5,5, 6,6, 7,7>(a,b)), (bs::interleave_second(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1,4,-1,5,-1,6,-1,7>(a))  , (bs::interleave_second(z,a)) );

    STF_ALL_EQUAL( (bs::shuffle<12,12,13,13,14,14,15,15>(a,b)), (bs::interleave_second(b,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1,12,-1,13,-1,14,-1,15>(a,b)), (bs::interleave_second(z,b)) );
  }
}

STF_CASE_TPL( "binary interleave_second based shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
 {
    bs::pack<T,4> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>() };
    bs::pack<T,4> b{ T(13), bs::Valmin<T>(), T(37), bs::Valmax<T>() };
    bs::pack<T,4> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 2, 6, 3, 7>(a,b)), (bs::interleave_second(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 6,-1, 7>(a,b)), (bs::interleave_second(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 2,-1, 3,-1>(a,b)), (bs::interleave_second(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle< 6, 2, 7, 3>(a,b)), (bs::interleave_second(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 2,-1, 3>(a,b)), (bs::interleave_second(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 6,-1, 7,-1>(a,b)), (bs::interleave_second(b,z)) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>(), T(37), bs::Valmax<T>(), T(55), T(99) };
    bs::pack<T,8> b{ T(13), bs::Valmax<T>(), T(42), bs::Valmax<T>(), T(76), bs::Valmin<T>(), T(42), T(69) };
    bs::pack<T,8> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 4,12, 5,13, 6,14, 7,15>(a,b)), (bs::interleave_second(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1,12,-1,13,-1,14,-1,15>(a,b)), (bs::interleave_second(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 4,-1, 5,-1, 6,-1, 7,-1>(a,b)), (bs::interleave_second(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle<12, 4,13, 5,14, 6,15, 7>(a,b)), (bs::interleave_second(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 4,-1, 5,-1, 6,-1, 7>(a,b)), (bs::interleave_second(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle<12,-1,13,-1,14,-1,15,-1>(a,b)), (bs::interleave_second(b,z)) );
  }
}


// Author: Jean-Gabriel Young <info@jgyoung.ca>
// Simplicial Configuration Model MCMC sampler
// Reference: https://doi.org/10.1103/PhysRevE.96.032312
// arXiv link:  https://arxiv.org/abs/1705.10298
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// STL
#include <cstdlib>   // EXIT_FAILURE, EXIT_SUCCESS
#include <iostream>
#include <fstream> // handle facet_list text file
#include <cmath>   // pow, exp
#include <chrono>  // high_resolution_clock
#include <vector>
#include <random>  // mt19937
#include <algorithm>  // max
// Boost
#include <boost/program_options.hpp>
#include <boost/math/special_functions/binomial.hpp>
// Program headers
#include "types.h"
#include "scm/scm.h"
#include "io_functions.h"

namespace po = boost::program_options;

int main(int argc, char const *argv[])
{
  /* ~~~~~ Program options ~~~~~~~*/
  std::string facet_list_path;
  unsigned int burn_in;
  unsigned int sampling_steps;
  unsigned int sampling_frequency;
  unsigned int seed = 0;
  unsigned int L_max = 0;
  float prop_param = 1;
  po::options_description description("Options");
  description.add_options()
  ("burn_in,b", po::value<unsigned int>(&burn_in),
      "Burn-in time. Defaults to M log M, where M is the sum of degrees.")
  ("sampling_steps,t", po::value<unsigned int>(&sampling_steps),
      "Number of sampling steps.")
  ("sampling_frequency,f", po::value<unsigned int>(&sampling_frequency),
      "Number of step between each sample. Defaults to M log M, where M is the sum of degrees.")
  ("seed,d", po::value<unsigned int>(&seed),
      "Seed of the pseudo random number generator (Mersenne-twister 19937). Seed with time if not specified.")
  ("l_max,l", po::value<unsigned int>(&L_max),
      "Manually set L_max. The correctness of the sampler is not guaranteed if L_max < 2 max s. Defaults to 10% of the sum of facet sizes. ")
  ("exp_prop", "Use exponential proposal distribution.")
  ("pl_prop", "Use power law proposal distribution.")
  ("unif_prop", "Use uniform proposal distribution [default].")
  ("prop_param", po::value<float>(&prop_param),
      "Parameter of the proposal distribution (only works for the exponential and power law proposal distributions).")
  ("cleansed_input,c", "Assume that the input is already cleansed, i.e., that nodes are labeled with 0 indexed contiguous integers and that no facet is included in another.")
  ("verbose,v", "Output log messages.")
  ("help,h", "Produce this help message.")
  ;
  po::options_description hidden;
  hidden.add_options()
  ("facet_list_path", po::value<std::string>(&facet_list_path),
      "Path to facet list.")
  ;
  po::positional_options_description p;
  p.add("facet_list_path", -1);
  po::options_description all_options;
  all_options.add(description);
  all_options.add(hidden);
  po::variables_map var_map;
  po::store(po::command_line_parser(argc, argv).
          options(all_options).
          positional(p).
          run(),
          var_map);
  po::notify(var_map);
  if (var_map.count("help") || argc == 1)
  {
      std::cout << "Usage:\n"
                << "  "+std::string(argv[0])+" [--option_1=VAL] ... [--option_n=VAL] path-to-facet-list\n";
      std::cout << description;
      return EXIT_SUCCESS;
  }
  if (!var_map.count("facet_list_path"))
  {
      std::cerr << "No facet list given.\n";
      return EXIT_FAILURE;
  }
  if (!var_map.count("seed")) {
      seed = (unsigned int) std::chrono::high_resolution_clock::now().time_since_epoch().count();
  }

  /* ~~~~~ Load max. facets ~~~~~~~*/
  if (var_map.count("verbose")) std::clog << "Loading facet file.\n";
  adj_list_t maximal_facets;
  vmap_t id_to_vertex;
  std::ifstream file(facet_list_path.c_str());
  if (!file.is_open()) return EXIT_FAILURE;
  unsigned int largest_facet = read_facet_list(maximal_facets, file, var_map.count("cleansed_input") != 0, id_to_vertex);
  file.close();

  /* ~~~~~ Sampling ~~~~~~~*/
  scm_t K(maximal_facets);
  std::mt19937 engine(seed);
  // prepare proposal distribution
  if (!var_map.count("l_max"))
  {
    L_max = std::min(std::max((unsigned int) 0.1 * K.M(), 2 * largest_facet), K.M());
  }
  if (L_max < 2 * largest_facet && var_map.count("l_max"))
  {
    std::clog << "Warning: Manually set L_max does not guarantee connectivity. ("<< L_max << " < " << 2 * largest_facet << ")\n";
  }
  std::vector<double> weights(L_max + 1, 0);
  if (var_map.count("exp_prop"))
  {
    for (unsigned int l = 2; l <= L_max; ++l) weights[l] = exp(l * prop_param);
  }
  else if (var_map.count("pl_prop"))
  {
    for (unsigned int l = 2; l <= L_max; ++l) weights[l] = pow(l, -prop_param);
  }
  else
  { // uniform (default)
    for (unsigned int l = 2; l <= L_max; ++l) weights[l] = 1;
  }
  std::discrete_distribution<> rand_int(weights.begin(), weights.end());

  if (!var_map.count("sampling_frequency"))
  {
    sampling_frequency = (unsigned int) K.M() * std::log(K.M());
  }
  if (!var_map.count("burn_in"))
  {
    burn_in = (unsigned int) K.M() * std::log(K.M());
  }
  // finally ready to output params (need initialized proposal for that)
  if (var_map.count("verbose"))
  {
    std::clog << "Parameters:\n";
    std::clog << "\tfacet_list_path: " << facet_list_path << "\n";
    std::clog << "\tburn_in: " << burn_in << "\n";
    std::clog << "\tsampling_steps: " << sampling_steps << "\n";
    std::clog << "\tsampling_frequency: " << sampling_frequency << "\n";
    std::clog << "\tseed: " << seed << "\n";
    std::clog << "\tL_max: " << L_max << "\n";
    std::clog << "\tproposal_distribution: ";
    if (var_map.count("exp_prop")) {std::clog << "exponential\n";}
    else if (var_map.count("pl_prop")) {std::clog << "power law\n";}
    else {std::clog << "uniform\n";}
    std::clog << "\tprop_param: " << prop_param << "\n";
    std::clog << "\tcleansed_input: ";
    if (var_map.count("cleansed_input")) {std::clog << "yes\n";}
    else {std::clog << " no\n";}
  }
  // Burn-in
  if (var_map.count("verbose")) std::clog << "Burn-in in progress\n";
  for (unsigned int t = 0; t < burn_in;)
  {
    unsigned int l = rand_int(engine);
    auto moves = K.random_rewire(l, engine);
    if (K.do_moves(moves)) ++t;
  }
  // Sample
  if (var_map.count("verbose")) std::clog << "Starting sampling\n";
  unsigned int accepted = 0;
  for (unsigned int t = 1; t < sampling_steps * sampling_frequency + 1; ++t)
  {
    unsigned int l = rand_int(engine);
    auto moves = K.random_rewire(l, engine);
    if (K.do_moves(moves))
    {
      ++accepted;
    }
    if (t % sampling_frequency == 0)
    {
      output_K(K, std::cout, id_to_vertex);
    }
  }
  float acceptance_ratio = float(accepted) / float(sampling_steps * sampling_frequency);
  if (var_map.count("verbose"))
  {
    std::clog << "# acceptance_ratio=" << acceptance_ratio << "\n";
    std::clog << "Done.\n";
  }

  return EXIT_SUCCESS;
}


/**
 * @file tests/union_find_test.cpp
 * @author Bill March (march@gatech.edu)
 *
 * Unit tests for the Union-Find data structure.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/methods/emst/union_find.hpp>

#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::emst;

BOOST_AUTO_TEST_SUITE(UnionFindTest);

BOOST_AUTO_TEST_CASE(TestFind)
{
  static const size_t testSize = 10;
  UnionFind testUnionFind(testSize);

  for (size_t i = 0; i < testSize; i++)
    BOOST_REQUIRE(testUnionFind.Find(i) == i);

  testUnionFind.Union(0, 1);
  testUnionFind.Union(1, 2);

  BOOST_REQUIRE(testUnionFind.Find(2) == testUnionFind.Find(0));
}

BOOST_AUTO_TEST_CASE(TestUnion)
{
  static const size_t testSize = 10;
  UnionFind testUnionFind(testSize);

  testUnionFind.Union(0, 1);
  testUnionFind.Union(2, 3);
  testUnionFind.Union(0, 2);
  testUnionFind.Union(5, 0);
  testUnionFind.Union(0, 6);

  BOOST_REQUIRE(testUnionFind.Find(0) == testUnionFind.Find(1));
  BOOST_REQUIRE(testUnionFind.Find(2) == testUnionFind.Find(3));
  BOOST_REQUIRE(testUnionFind.Find(1) == testUnionFind.Find(5));
  BOOST_REQUIRE(testUnionFind.Find(6) == testUnionFind.Find(3));
}

BOOST_AUTO_TEST_SUITE_END();