// ---------------------------------------------------------------------
//
// Copyright (c) 2014 - 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/IBFEPatchRecoveryPostProcessor.h"
#include "ibamr/IBHierarchyIntegrator.h"
#include "ibamr/namespaces.h" // IWYU pragma: keep

#include "ibtk/IBTK_CHKERRQ.h"
#include "ibtk/IndexUtilities.h"
#include "ibtk/LEInteractor.h"
#include "ibtk/libmesh_utilities.h"

#include "SAMRAI_config.h"

#include "libmesh/boundary_info.h"
#include "libmesh/dense_vector.h"
#include "libmesh/dof_map.h"
#include "libmesh/equation_systems.h"
#include "libmesh/fe_base.h"
#include "libmesh/fe_interface.h"
#include "libmesh/mesh.h"
#include "libmesh/periodic_boundaries.h"
#include "libmesh/periodic_boundary.h"
#include "libmesh/petsc_vector.h"
#include "libmesh/quadrature.h"
#include "libmesh/string_to_enum.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <boost/multi_array.hpp>
IBTK_ENABLE_EXTRA_WARNINGS

IBTK_DISABLE_EXTRA_WARNINGS
#include "Eigen/Dense"
IBTK_ENABLE_EXTRA_WARNINGS

using namespace libMesh;

/////////////////////////////// NAMESPACE ////////////////////////////////////

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

namespace
{
unsigned int
num_polynomial_basis_fcns(const unsigned int dim, const unsigned int order)
{
    unsigned int num_basis_fcns = 0;
    unsigned int order_p_1 = order + 1;
    switch (dim)
    {
    case 1:
        num_basis_fcns = order_p_1;
        break;
    case 2:
        num_basis_fcns = order_p_1 * order_p_1 + order_p_1;
        num_basis_fcns /= 2;
        break;
    case 3:
        num_basis_fcns = order_p_1 * order_p_1 * order_p_1 + 3 * order_p_1 * order_p_1 + 2 * order_p_1;
        num_basis_fcns /= 6;
        break;
    default:
        TBOX_ERROR("only supports dim = 1, 2, or 3\n");
    }
    return num_basis_fcns;
} // num_polynomial_basis_fcns

void
evaluate_polynomial_basis_fcns(Eigen::VectorXd& P,
                               const libMesh::Point& x_center,
                               const libMesh::Point& x_eval,
                               const unsigned int dim,
                               const unsigned int order)
{
    TBOX_ASSERT(static_cast<unsigned int>(P.size()) == num_polynomial_basis_fcns(dim, order));

    // Compute powers of the components of x up to the specified order.
    libMesh::Point x = x_center - x_eval;
    boost::multi_array<double, 2> x_pow(boost::extents[dim][order + 1]);
    for (unsigned int d = 0; d < dim; ++d)
    {
        x_pow[d][0] = 1.0;
        for (unsigned int k = 1; k <= order; ++k)
        {
            x_pow[d][k] = x(d) * x_pow[d][k - 1];
        }
    }

    // Evaluate the complete polynomial basis functions of the specified order.
    static const unsigned int X_IDX = 0;
    static const unsigned int Y_IDX = 1;
    static const unsigned int Z_IDX = 2;
    switch (dim)
    {
    case 1:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow, ++k)
        {
            unsigned int x_exp = total_pow;
            P(k) = x_pow[X_IDX][x_exp];
        }
        break;
    case 2:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow)
        {
            for (unsigned int x_exp = 0; x_exp <= total_pow; ++x_exp, ++k)
            {
                unsigned int y_exp = total_pow - x_exp;
                P(k) = x_pow[X_IDX][x_exp] * x_pow[Y_IDX][y_exp];
            }
        }
        break;
    case 3:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow)
        {
            for (unsigned int x_exp = 0; x_exp <= total_pow; ++x_exp)
            {
                for (unsigned int y_exp = 0; y_exp <= total_pow - x_exp; ++y_exp, ++k)
                {
                    unsigned int z_exp = total_pow - (x_exp + y_exp);
                    P(k) = x_pow[X_IDX][x_exp] * x_pow[Y_IDX][y_exp] * x_pow[Z_IDX][z_exp];
                }
            }
        }
        break;
    default:
        TBOX_ERROR("only supports dim = 1, 2, or 3\n");
    }
    return;
} // evaluate_polynomial_basis_fcns
} // namespace

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

IBFEPatchRecoveryPostProcessor::IBFEPatchRecoveryPostProcessor(MeshBase* mesh, FEDataManager* fe_data_manager)
    : d_mesh(mesh),
      d_fe_data_manager(fe_data_manager),
      d_periodic_boundaries(nullptr),
      d_interp_order(INVALID_ORDER),
      d_quad_order(INVALID_ORDER)
{
    // Active local elements.
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();

    // Determine the number of quadrature/interpolation points in each element.
    //
    // We use full-order Gaussian quadrature rules (i.e. third-order Gauss
    // quadrature for first-order elements and fifth-order Gauss quadrature for
    // second-order elements) in all elements to avoid special treatment at
    // boundary nodes.
    bool first_order_elems = false;
    bool second_order_elems = false;
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const unsigned int dim = elem->dim();
        TBOX_ASSERT(dim == d_mesh->mesh_dimension());
        Order elem_order = elem->default_order();
        if (elem_order == FIRST) first_order_elems = true;
        if (elem_order == SECOND) second_order_elems = true;
        TBOX_ASSERT(elem_order == FIRST || elem_order == SECOND);
    }
    if (first_order_elems && second_order_elems)
    {
        TBOX_ERROR(
            "cannot have both first- and second-order elements in the same "
            "mesh.\n");
    }
    d_interp_order = first_order_elems ? FIRST : SECOND;
    d_quad_order = first_order_elems ? THIRD : FIFTH;
    return;
} // IBFEPatchRecoveryPostProcessor

IBFEPatchRecoveryPostProcessor::~IBFEPatchRecoveryPostProcessor()
{
    // intentionally blank
    return;
} // ~IBFEPatchRecoveryPostProcessor

void
IBFEPatchRecoveryPostProcessor::initializeFEData(const PeriodicBoundaries* const periodic_boundaries)
{
    d_periodic_boundaries = periodic_boundaries;

    const Parallel::Communicator& comm = d_mesh->comm();
    const int mpi_rank = comm.rank();
    const int mpi_size = comm.size();

    // Active local elements.
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();

    // Determine the element patches associated with each node N, which is
    // defined to be the collection of elements that contain node N.
    //
    // Unlike the standard Z-Z patch recovery algorithm, we use "tight" element
    // patches for non-vertex nodes.
    std::unique_ptr<PointLocatorBase> point_locator = PointLocatorBase::build(TREE, *d_mesh);
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        for (unsigned int n = 0; n < elem->n_nodes(); ++n)
        {
            // Only set up patches for local nodes.
            const Node* const node = elem->get_node_ptr(n);
            if (node->processor_id() != mpi_rank) continue;

            // Only set up patches once for each node.
            const dof_id_type node_id = node->id();
            if (d_local_elem_patches.find(node_id) != d_local_elem_patches.end()) continue;

            // Find the elements that touch this node.
            ElemPatch& elem_patch = d_local_elem_patches[node_id];
            std::set<const Elem*> elems;
            elem->find_point_neighbors(*node, elems);
            for (std::set<const Elem*>::const_iterator it = elems.begin(); it != elems.end(); ++it)
            {
                elem_patch.insert(boost::make_tuple(*it, CompositePeriodicMapping(), CompositePeriodicMapping()));
            }

            // Account for periodic boundaries.
            bool done = false || !d_periodic_boundaries;
            while (!done)
            {
                ElemPatch periodic_neighbors;
                for (ElemPatch::const_iterator it = elem_patch.begin(); it != elem_patch.end(); ++it)
                {
                    const Elem* const elem = it->get<0>();
                    const CompositePeriodicMapping& forward_mapping = it->get<1>();
                    const CompositePeriodicMapping& inverse_mapping = it->get<2>();
                    const libMesh::Point p = apply_composite_periodic_mapping(forward_mapping, *node);
                    TBOX_ASSERT(elem->contains_point(p));
                    for (unsigned int i = 0; i < elem->n_neighbors(); ++i)
                    {
                        if (!elem->neighbor_ptr(i))
                        {
                            const std::vector<boundary_id_type>& boundary_ids =
                                d_mesh->boundary_info->boundary_ids(elem, i);
                            for (std::vector<boundary_id_type>::const_iterator j = boundary_ids.begin();
                                 j != boundary_ids.end();
                                 ++j)
                            {
                                const boundary_id_type boundary_id = *j;
                                const PeriodicBoundaryBase* const periodic_boundary =
                                    d_periodic_boundaries->boundary(boundary_id);
                                if (periodic_boundary)
                                {
                                    const libMesh::Point periodic_image = periodic_boundary->get_corresponding_pos(p);
                                    const Elem* neighbor =
                                        d_periodic_boundaries->neighbor_ptr(boundary_id, *point_locator, elem, i);
                                    if (elem->level() < neighbor->level())
                                    {
                                        neighbor = neighbor->parent();
                                    }
                                    if (neighbor->contains_point(periodic_image))
                                    {
                                        std::set<const Elem*> elems;
                                        neighbor->find_point_neighbors(periodic_image, elems);
                                        for (std::set<const Elem*>::const_iterator k = elems.begin(); k != elems.end();
                                             ++k)
                                        {
                                            const Elem* const elem = *k;
                                            if (elem_patch.find(elem) == elem_patch.end() &&
                                                periodic_neighbors.find(elem) == periodic_neighbors.end())
                                            {
                                                CompositePeriodicMapping forward = forward_mapping;
                                                CompositePeriodicMapping inverse = inverse_mapping;
                                                forward.insert(
                                                    forward.end(),
                                                    periodic_boundary->clone(PeriodicBoundaryBase::FORWARD).release());
                                                inverse.insert(
                                                    inverse.begin(),
                                                    periodic_boundary->clone(PeriodicBoundaryBase::INVERSE).release());
                                                periodic_neighbors.insert(boost::make_tuple(elem, forward, inverse));
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                elem_patch.insert(periodic_neighbors.begin(), periodic_neighbors.end());
                periodic_neighbors.clear();
                done = periodic_neighbors.empty();
            }
        }
    }

    // Setup mappings used to fill global indexing data structures.
    //
    // We use full-order Gaussian quadrature rules (i.e. third-order Gauss
    // quadrature for first-order elements and fifth-order Gauss quadrature for
    // second-order elements) in all elements to avoid special treatment at
    // boundary nodes.
    d_n_qp_global = 0;
    d_n_qp_local = 0;
    d_qp_global_offset = 0;
    const unsigned int n_elem = d_mesh->n_elem();
    d_elem_n_qp.resize(n_elem, 0);
    d_elem_qp_global_offset.resize(n_elem, 0);
    d_elem_qp_local_offset.resize(n_elem, 0);
    std::unique_ptr<QBase> qrule;
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const unsigned int dim = elem->dim();
        bool reinit_qrule = false;
        if (!qrule.get() || qrule->get_dim() != dim || qrule->get_order() != d_quad_order)
        {
            qrule = QBase::build(QGAUSS, dim, d_quad_order);
            reinit_qrule = true;
        }
        else if (qrule->get_elem_type() != elem->type() || qrule->get_p_level() != elem->p_level())
        {
            reinit_qrule = true;
        }
        if (reinit_qrule) qrule->init(elem->type(), elem->p_level());
        unsigned int n_qp = qrule->n_points();
        const dof_id_type elem_id = elem->id();
        d_elem_n_qp[elem_id] = n_qp;
        d_elem_qp_local_offset[elem_id] = d_n_qp_local;
        d_n_qp_local += n_qp;
        d_elem_sigma[elem->id()].resize(n_qp);
        d_elem_pressure[elem->id()].resize(n_qp);
    }
    std::vector<int> n_qp_per_proc(mpi_size);
    n_qp_per_proc[mpi_rank] = d_n_qp_local;
    comm.sum(n_qp_per_proc);
    d_qp_global_offset = std::accumulate(n_qp_per_proc.begin(), n_qp_per_proc.begin() + mpi_rank, 0);
    d_n_qp_global = std::accumulate(n_qp_per_proc.begin() + mpi_rank, n_qp_per_proc.end(), d_qp_global_offset);
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        d_elem_qp_global_offset[elem_id] = d_elem_qp_local_offset[elem_id] + d_qp_global_offset;
    }
    comm.sum(d_elem_qp_global_offset);
    comm.sum(d_elem_qp_local_offset);

    // Set up element patch L2 projection matrices.
    unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::MatrixXd M(num_basis_fcns, num_basis_fcns);
    Eigen::VectorXd P(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    d_local_patch_proj_solver.resize(d_local_elem_patches.size());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::iterator it = d_local_elem_patches.begin(); it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        ElemPatch& elem_patch = it->second;
        M.setZero();
        for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
        {
            const Elem* const elem = el_it->get<0>();
            const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
            fe->reinit(elem);
            for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
            {
                evaluate_polynomial_basis_fcns(
                    P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                M += P * P.transpose();
            }
        }
        d_local_patch_proj_solver[k] = M.colPivHouseholderQr();
        if (!d_local_patch_proj_solver[k].isInvertible())
        {
            TBOX_ERROR(
                "IBFEPatchRecoveryPostProcessor could not construct L2 "
                "reconstruction for element patch associated with node "
                << node_id << "\n");
        }
    }
    return;
} // initializeFEData

System*
IBFEPatchRecoveryPostProcessor::initializeCauchyStressSystem()
{
    EquationSystems* equation_systems = d_fe_data_manager->getEquationSystems();
    System* sigma_system = &equation_systems->add_system<System>("CAUCHY_STRESS_RECOVERY_SYSTEM");
    for (unsigned int i = 0; i < NDIM; ++i)
    {
        for (unsigned int j = i; j < NDIM; ++j)
        {
            std::string var_name = "sigma_";
            var_name += (i == 0 ? 'x') : i == 1 ? 'y' : 'z');
            var_name += (j == 0 ? 'x') : j == 1 ? 'y' : 'z');

            sigma_system->add_variable(var_name), d_interp_order, LAGRANGE);
        }
    }
    return sigma_system;
} // initializeCauchyStressSystem

System*
IBFEPatchRecoveryPostProcessor::initializePressureSystem()
{
    EquationSystems* equation_systems = d_fe_data_manager->getEquationSystems();
    System* p_system = &equation_systems->add_system<System>("PRESSURE_RECOVERY_SYSTEM");
    p_system->add_variable("p", d_interp_order, LAGRANGE);
    return p_system;
} // initializePressureSystem

void
IBFEPatchRecoveryPostProcessor::registerCauchyStressValue(const Elem* const elem,
                                                          const QBase* const qrule,
                                                          const unsigned int qp,
                                                          const TensorValue<double>& sigma)
{
    const Parallel::Communicator& comm = d_mesh->comm();
    if (elem->processor_id() != comm.rank() || !elem->active())
    {
        TBOX_ERROR("must register stresses only for active local elements\n");
    }
    TBOX_ASSERT(elem->default_order() == d_interp_order);
    TBOX_ASSERT(qrule->type() == QGAUSS);
    TBOX_ASSERT(qrule->get_order() == d_quad_order);
    TBOX_ASSERT(qrule->get_elem_type() == elem->type());
    TBOX_ASSERT(qrule->get_p_level() == elem->p_level());
    TBOX_ASSERT(qp < d_elem_n_qp[elem->id()]);
    d_elem_sigma[elem->id()][qp] = sigma;
    return;
} // registerCauchyStressValue

void
IBFEPatchRecoveryPostProcessor::registerPressureValue(const Elem* const elem,
                                                      const QBase* const qrule,
                                                      const unsigned int qp,
                                                      const double p)
{
    const Parallel::Communicator& comm = d_mesh->comm();
    if (elem->processor_id() != comm.rank() || !elem->active())
    {
        TBOX_ERROR("must register pressures only for active local elements\n");
    }
    TBOX_ASSERT(elem->default_order() == d_interp_order);
    TBOX_ASSERT(qrule->type() == QGAUSS);
    TBOX_ASSERT(qrule->get_order() == d_quad_order);
    TBOX_ASSERT(qrule->get_elem_type() == elem->type());
    TBOX_ASSERT(qrule->get_p_level() == elem->p_level());
    TBOX_ASSERT(qp < d_elem_n_qp[elem->id()]);
    d_elem_pressure[elem->id()][qp] = p;
    return;
} // registerPressureValue

void
IBFEPatchRecoveryPostProcessor::reconstructCauchyStress(System& sigma_system)
{
    const unsigned int sigma_sys_num = sigma_system.number();
    NumericVector<double>& sigma_vec = *sigma_system.solution;

    // Communicate the stored values of the Cauchy stress.
    static const unsigned int NVARS = (NDIM * (NDIM + 1)) / 2;
    std::vector<double> sigma_vals(NVARS * d_n_qp_global, 0.0);
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        const int global_offset = d_elem_qp_global_offset[elem_id];
        for (unsigned int qp = 0; qp < d_elem_n_qp[elem_id]; ++qp)
        {
            const TensorValue<double>& stress = d_elem_sigma[elem_id][qp];
            for (unsigned int i = 0, k = 0; i < NDIM; ++i)
            {
                for (unsigned int j = i; j < NDIM; ++j, ++k)
                {
                    sigma_vals[NVARS * (global_offset + qp) + k] = stress(i, j);
                }
            }
        }
    }
    const Parallel::Communicator& comm = d_mesh->comm();
    comm.sum(sigma_vals);

    // Perform element patch L2 projections.
    const unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::VectorXd P(num_basis_fcns), a(num_basis_fcns), f(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::const_iterator it = d_local_elem_patches.begin();
         it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        const ElemPatch& elem_patch = it->second;
        Eigen::ColPivHouseholderQR<Eigen::MatrixXd>& patch_proj_solver = d_local_patch_proj_solver[k];
        for (unsigned int var = 0; var < NVARS; ++var)
        {
            // Solve for the coefficients of the reconstruction.
            f.setZero();
            for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
            {
                const Elem* const elem = el_it->get<0>();
                const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
                const dof_id_type elem_id = elem->id();
                const int global_offset = d_elem_qp_global_offset[elem_id];
                fe->reinit(elem);
                for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
                {
                    evaluate_polynomial_basis_fcns(
                        P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                    f += P * sigma_vals[NVARS * (global_offset + qp) + var];
                }
            }
            a = patch_proj_solver.solve(f);

            // Evaluate the reconstruction at the node.
            const int dof_index = node.dof_number(sigma_sys_num, var, 0);
            sigma_vec.set(dof_index, a(0));
        }
    }
    return;
} // reconstructCauchyStress

void
IBFEPatchRecoveryPostProcessor::reconstructPressure(System& p_system)
{
    const unsigned int p_sys_num = p_system.number();
    NumericVector<double>& p_vec = *p_system.solution;

    // Communicate the stored values of the Cauchy stress.
    std::vector<double> pressure_vals(d_n_qp_global, 0.0);
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        const int global_offset = d_elem_qp_global_offset[elem_id];
        for (unsigned int qp = 0; qp < d_elem_n_qp[elem_id]; ++qp)
        {
            pressure_vals[global_offset + qp] = d_elem_pressure[elem_id][qp];
        }
    }
    const Parallel::Communicator& comm = d_mesh->comm();
    comm.sum(pressure_vals);

    // Perform element patch L2 projections.
    const unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::VectorXd P(num_basis_fcns), a(num_basis_fcns), f(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::const_iterator it = d_local_elem_patches.begin();
         it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        const ElemPatch& elem_patch = it->second;
        Eigen::ColPivHouseholderQR<Eigen::MatrixXd>& patch_proj_solver = d_local_patch_proj_solver[k];

        // Solve for the coefficients of the reconstruction.
        f.setZero();
        for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
        {
            const Elem* const elem = el_it->get<0>();
            const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
            const dof_id_type elem_id = elem->id();
            const int global_offset = d_elem_qp_global_offset[elem_id];
            fe->reinit(elem);
            for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
            {
                evaluate_polynomial_basis_fcns(
                    P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                f += P * pressure_vals[global_offset + qp];
            }
        }
        a = patch_proj_solver.solve(f);

        // Evaluate the reconstruction at the node.
        const unsigned int var = 0;
        const int dof_index = node.dof_number(p_sys_num, var, 0);
        p_vec.set(dof_index, a(0));
    }
    return;
} // reconstructPressure

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

/////////////////////////////// NAMESPACE ////////////////////////////////////

} // namespace IBAMR

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


/*
 *
 * Copyright (c) Kresimir Fresl & Toon Knapen 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SPSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SPSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B with A symmetric matrix
    // stored in packed format
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * spsv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric matrix stored in packed
     * format and X and B are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^T,  if UPLO = 'U',
     *   A = L * D * L^T,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is symmetric and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks.
     * The factored form of A is then used to solve the system
     * of equations A * X = B.
     */

    namespace detail {

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 float* ap, int* ipiv,
                 float* b, int const ldb, int* info)
      {
        LAPACK_SSPSV (&uplo, &n, &nrhs, ap, ipiv, b, &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 double* ap, int* ipiv,
                 double* b, int const ldb, int* info)
      {
        LAPACK_DSPSV (&uplo, &n, &nrhs, ap, ipiv, b, &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* ap, int* ipiv,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CSPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* ap, int* ipiv,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZSPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmA, typename MatrB, typename IVec>
      inline
      int spsv (SymmA& a, IVec& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::matrix_structure,
          traits::symmetric_packed_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
#endif

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));

        char uplo = traits::matrix_uplo_tag (a);
        int info;
        spsv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int spsv (SymmA& a, IVec& i, MatrB& b) {
      assert (traits::matrix_size1 (a) == traits::vector_size (i));
      return detail::spsv (a, i, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int spsv (SymmA& a, MatrB& b) {
      // with 'internal' pivot vector

      int info = -101;
      traits::detail::array<int> i (traits::matrix_size1 (a));

      if (i.valid())
        info = detail::spsv (a, i, b);
      return info;
    }

    /*
     * sptrf() computes the factorization of a symmetric matrix A
     * in packed storage using the  Bunch-Kaufman diagonal pivoting
     * method. The form of the factorization is
     *    A = U * D * U^T  or  A = L * D * L^T
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is symmetric and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void sptrf (char const uplo, int const n,
                  float* ap, int* ipiv, int* info)
      {
        LAPACK_SSPTRF (&uplo, &n, ap, ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  double* ap, int* ipiv, int* info)
      {
        LAPACK_DSPTRF (&uplo, &n, ap, ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  traits::complex_f* ap, int* ipiv, int* info)
      {
        LAPACK_CSPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  traits::complex_d* ap, int* ipiv, int* info)
      {
        LAPACK_ZSPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

    }

    template <typename SymmA, typename IVec>
    inline
    int sptrf (SymmA& a, IVec& i) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::sptrf (uplo, n, traits::matrix_storage (a),
                     traits::vector_storage (i), &info);
      return info;
    }

    /*
     * sptrs() solves a system of linear equations A*X = B with
     * a symmetric matrix A in packed storage using the factorization
     *    A = U * D * U^T   or  A = L * D * L^T
     * computed by sptrf().
     */

    namespace detail {

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  float const* a, int const* ipiv,
                  float* b, int const ldb, int* info)
      {
        LAPACK_SSPTRS (&uplo, &n, &nrhs, a, ipiv, b, &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  double const* a, int const* ipiv,
                  double* b, int const ldb, int* info)
      {
        LAPACK_DSPTRS (&uplo, &n, &nrhs, a, ipiv, b, &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* a, int const* ipiv,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CSPTRS (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* a, int const* ipiv,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZSPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sptrs (SymmA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::sptrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
                     traits::vector_storage (i),
#else
                     traits::matrix_storage_const (a),
                     traits::vector_storage_const (i),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

    namespace detail {
      inline
      void sptri (char const uplo, int const n,
          float* ap, int* ipiv, float* work, int* info)
      {
        LAPACK_SSPTRI (&uplo, &n, ap, ipiv, work, info);
      }

      inline
      void sptri (char const uplo, int const n,
          double* ap, int* ipiv, double* work, int* info)
      {
        LAPACK_DSPTRI (&uplo, &n, ap, ipiv, work, info);
      }

      inline
      void sptri (char const uplo, int const n,
          traits::complex_f* ap, int* ipiv, traits::complex_f* work, int* info)
      {
        LAPACK_CSPTRI (&uplo, &n, traits::complex_ptr (ap),
            ipiv, traits::complex_ptr (work), info);
      }

      inline
      void sptri (char const uplo, int const n,
          traits::complex_d* ap, int* ipiv, traits::complex_d* work, int* info)
      {
        LAPACK_ZSPTRI (&uplo, &n, traits::complex_ptr (ap),
            ipiv, traits::complex_ptr (work), info);
      }
    } // namespace detail

    template <typename SymmA, typename IVec>
    inline
    int sptri (SymmA& a, IVec& ipiv)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (ipiv));

      char uplo = traits::matrix_uplo_tag (a);
      int info;

      typedef typename SymmA::value_type value_type;
      traits::detail::array<value_type> work(traits::matrix_size1(a));

      detail::sptri (uplo, n, traits::matrix_storage (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
          traits::vector_storage (ipiv),
#else
          traits::vector_storage_const (ipiv),
#endif
          traits::vector_storage (work),
          &info);
      return info;
    }

  } // namespace lapack

}}} // namespace boost::numeric::bindings

#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "LazyDensityOperatorFFT.h"

#include "FFT.tcc"
#include "VectorFromMatrixSliceIterator.h"

#include <boost/range/algorithm/for_each.hpp>

using namespace fft; using namespace linalg;

namespace {

void ffTransformCV(CVector& psi, Direction dir)
{
  struct Helper
  {
    static void _(CVector& psi, int i1, int i2, double norm)
    {
      dcomp temp(psi(i1));
      psi(i1)=norm*psi(i2);
      psi(i2)=norm*temp;
    }
  };

  int size=psi.size();

  if (size<2) return;

  transform(psi,dir);

  int halfnumber=psi.size()>>1;

  // NEEDS_WORK express the following with blitz
  for (int j=0; j<halfnumber; j++ ) Helper::_(psi,j,j+halfnumber,pow(size,-.5));
  for (int j=1; j<size      ; j+=2) psi(j)*=-1;

}

}

void quantumdata::ffTransform(CVector& psi, fft::Direction dir)
{
  ffTransformCV(psi,dir);
}

void quantumdata::ffTransform(CMatrix& rho, fft::Direction dir)
{
  using namespace blitzplusplus::vfmsi;

  for(auto& v : fullRange<Left >(rho)) ffTransformCV(v,dir);
  for(auto& v : fullRange<Right>(rho)) ffTransformCV(v,reverse(dir));

}


#include "TerrainOps.h"

#include <armadillo/armadillo>

namespace world {

void TerrainOps::fill(Terrain &terrain, double value) {
    terrain._array.fill(value);
}

void TerrainOps::applyOffset(Terrain &terrain, const arma::mat &offset) {
    if (offset.n_rows != terrain._array.n_rows ||
        offset.n_cols != terrain._array.n_cols) {
        throw std::runtime_error(
            "TerrainManipulator::applyOffset : bad matrix dimensions");
    }

    terrain._array += offset;
}

void TerrainOps::applyOffset(world::Terrain &terrain, double offset) {
    terrain._array += offset;
}

void TerrainOps::multiply(Terrain &terrain, const arma::mat &factor) {
    if (factor.n_rows != terrain._array.n_rows ||
        factor.n_cols != terrain._array.n_cols) {
        throw std::runtime_error(
            "TerrainManipulator::multiply : bad matrix dimensions");
    }

    terrain._array %= factor;
}

void TerrainOps::multiply(Terrain &terrain, double factor) {
    terrain._array *= factor;
}

void TerrainOps::copyNeighbours(Terrain &terrain, const TileCoordinates &coords,
                                const TerrainGrid &storage) {
    // TODO unit test this method
    int m = terrain.getResolution() - 1;

    // corners
    TerrainElement *neighbour;
    if (storage.tryGet(coords + vec2i{-1, -1}, &neighbour)) {
        terrain(0, 0) = neighbour->_terrain(m, m);
    }

    if (storage.tryGet(coords + vec2i{-1, 1}, &neighbour)) {
        terrain(0, m) = neighbour->_terrain(m, 0);
    }

    if (storage.tryGet(coords + vec2i{1, -1}, &neighbour)) {
        terrain(m, 0) = neighbour->_terrain(0, m);
    }

    if (storage.tryGet(coords + vec2i{1, 1}, &neighbour)) {
        terrain(m, m) = neighbour->_terrain(0, 0);
    }

    // sides
    if (storage.tryGet(coords + vec2i{-1, 0}, &neighbour)) {
        for (int i = 0; i <= m; ++i) {
            terrain(0, i) = neighbour->_terrain(m, i);
        }
    }

    if (storage.tryGet(coords + vec2i{1, 0}, &neighbour)) {
        for (int i = 0; i <= m; ++i) {
            terrain(m, i) = neighbour->_terrain(0, i);
        }
    }

    if (storage.tryGet(coords + vec2i{0, -1}, &neighbour)) {
        for (int i = 0; i <= m; ++i) {
            terrain(i, 0) = neighbour->_terrain(i, m);
        }
    }

    if (storage.tryGet(coords + vec2i{0, 1}, &neighbour)) {
        for (int i = 0; i <= m; ++i) {
            terrain(i, m) = neighbour->_terrain(i, 0);
        }
    }
}
} // namespace world


#ifndef BOOST_METAPARSE_GETTING_STARTED_8_1_HPP
#define BOOST_METAPARSE_GETTING_STARTED_8_1_HPP

// Automatically generated header file

// Definitions before section 8
#include "8.hpp"

// Definitions of section 8
#include <boost/mpl/divides.hpp>

template <class L, class R> struct eval_binary_op<L, '/', R> : boost::mpl::divides<L, R>::type {};

using divides_token = token<lit_c<'/'>>;

using mult_exp2 =
 foldl_start_with_parser<
   sequence<one_of<times_token, divides_token>, int_token>,
   int_token,
   boost::mpl::quote2<binary_op>
 >;

using exp_parser16 =
 build_parser<
   foldl_start_with_parser<
     sequence<one_of<plus_token, minus_token>, mult_exp2>,
     mult_exp2,
     boost::mpl::quote2<binary_op>
   >
 >;

// query:
//    exp_parser16::apply<BOOST_METAPARSE_STRING("8 / 4")>::type

#endif


#pragma once

#include <list>

#include <Eigen/Core>

#include "rcvio/macros.h"
#include "rcvio/input_buffer.hpp"

namespace rcvio
{
    class PreIntegrator
    {
    public:
        POINTER_TYPEDEFS(PreIntegrator);
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        PreIntegrator(const cv::FileStorage &fs_settings);

        void propagate(Eigen::VectorXd &xkk,
                       Eigen::MatrixXd &Pkk,
                       std::list<ImuData *> &imu_data);

    public:
        Eigen::VectorXd xk1k;
        Eigen::MatrixXd Pk1k;

    private:
        double gravity_;
        double small_angle_;

        double gyro_noise_sigma_;
        double gyro_rand_walk_sigma_;
        double accel_noise_sigma_;
        double accel_rand_walk_sigma_;

        Eigen::Matrix<double, 12, 12> imu_noise_matrix_;
    };
}


#include <boost/python.hpp>
#include <scitbx/array_family/tiny_types.h>
#include <scitbx/array_family/small.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/ref.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <scitbx/vec3.h>
#include <scitbx/mat3.h>
#include <cctype>
#include <complex>
#include <cmath>

namespace fractbx {
  namespace ext {

    // discover mandelbrot set

    static size_t iterations(std::complex<double> c)
    {
      std::complex<double> z = 0;
      size_t jmax = 2048;
      for (size_t j = 0; j < jmax; j++) {
        z = z * z + c;
        if (std::abs(z) > 2) {
          return j;
        }
      }
      return jmax;
    }

    static scitbx::af::versa< int, scitbx::af::c_grid<2> > make_flex(size_t n)
    {
      scitbx::af::c_grid<2> grid(n, n);
      scitbx::af::versa< int, scitbx::af::c_grid<2> > result(grid, 0);
      double scale = 4.0 / (n - 1);
      int off = (n - 1) / 2;
      for(int j = 0; j < n; j++) {
        for(int i = 0; i < n; i++) {
          std::complex<double> c(scale * (i - off), scale * (j - off));
          result(j, i) = iterations(c);
        }
      }
      return result;
    }

    void init_module()
    {
      using namespace boost::python;
      def("make_flex", make_flex, (arg("size")));
    }

  }
} // namespace fractbx::ext

BOOST_PYTHON_MODULE(fractbx_ext)
{
  fractbx::ext::init_module();
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_CONSTANT_DSL_REAL_HPP_INCLUDED
#define NT2_SDK_CONSTANT_DSL_REAL_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Turn some digits consatnt into DSL terminals
////////////////////////////////////////////////////////////////////////////////
#include <boost/proto/proto.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/category.hpp>

namespace nt2
{
  boost::proto::terminal< constant_<tag::pi_ > >::type          pi_         = {{}};
  boost::proto::terminal< constant_<tag::nan_> >::type          nan_        = {{}};
  boost::proto::terminal< constant_<tag::sqrt_2_o_2_> >::type   sqrt_2_o_2_ = {{}};
  boost::proto::terminal< constant_<tag::sqrt_2_> >::type       sqrt_2_     = {{}};
  boost::proto::terminal< constant_<tag::gold_> >::type         gold_       = {{}};
  boost::proto::terminal< constant_<tag::c_gold_> >::type       cgold_      = {{}};
  boost::proto::terminal< constant_<tag::m_half_> >::type       mhalf_      = {{}};
  boost::proto::terminal< constant_<tag::m_zero_> >::type       mzero_      = {{}};
  boost::proto::terminal< constant_<tag::half_> >::type         half_       = {{}};
  boost::proto::terminal< constant_<tag::third_> >::type        third_      = {{}};
  boost::proto::terminal< constant_<tag::quarter_> >::type      quarter_    = {{}};
}

#endif


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>

#include "matrix/comparable.hpp"
using namespace boost::hana;
using namespace cppcon;

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(equal(
        matrix(row(1, 2)),
        matrix(row(1, 2))
    ));
    BOOST_HANA_CONSTEXPR_CHECK(not_(equal(
        matrix(row(1, 2)),
        matrix(row(1, 5))
    )));

    BOOST_HANA_CONSTEXPR_CHECK(equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(1, 2),
               row(3, 4))
    ));
    BOOST_HANA_CONSTEXPR_CHECK(not_(equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(1, 2),
               row(0, 4))
    )));
    BOOST_HANA_CONSTEXPR_CHECK(not_(equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(0, 2),
               row(3, 4))
    )));

    BOOST_HANA_CONSTANT_CHECK(not_(equal(
        matrix(row(1),
               row(2)),
        matrix(row(3, 4),
               row(5, 6))
    )));
    BOOST_HANA_CONSTANT_CHECK(not_(equal(
        matrix(row(1),
               row(2)),
        matrix(row(3, 4))
    )));
}


/**
 * @file Vandermonde.h
 *
 */

#include "math/vandermonde/QuadSolver.h"
#include "math/vandermonde/FFTSolver.h"

#include "operation/field/setup.h"
//#include <boost/timer.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE( Vandermonde );

//******************************************************************************
BOOST_AUTO_TEST_CASE_TEMPLATE( vand_quad_simple, F, math::FieldTypes ) {
  math::FieldFixture<F> f; BOOST_CHECK( f ); // avoid unreferenced local variable warning

  BOOST_TEST_MESSAGE( "  Testing " << F::getName() );

  unsigned int size = 3;
  boost::scoped_array<F> entries(new F[size+1]);
  entries[0].template setTo<-1>();
  entries[1].template setTo<0>();
  entries[2].template setTo<1>();

  BOOST_CHECK(entries[0].template is<-1>());

  math::vandermonde::QuadSolver<F> qSolver(size, entries.get());

  BOOST_CHECK_EQUAL(qSolver.getMatrixSize(),size);

  BOOST_CHECK_EQUAL(qSolver.getEntry(0),F(-1));
  BOOST_CHECK_EQUAL(qSolver.getEntry(1),F(0));
  BOOST_CHECK_EQUAL(qSolver.getEntry(2),F(1));

  boost::scoped_array<F> values(new F[size]);

  values[0].template setTo<1>();
  values[1].template setTo<0>();
  values[2].template setTo<1>();

  boost::scoped_array<F> result(new F[size]);

  qSolver.solveTranspose(values.get(), result.get());

  BOOST_CHECK_EQUAL(result[0],F(1)/F(2));
  BOOST_CHECK_EQUAL(result[1],F(0));
  BOOST_CHECK_EQUAL(result[2],F(1)/F(2));

}

//******************************************************************************
BOOST_AUTO_TEST_CASE_TEMPLATE( vand_fft_simple, F, math::FieldTypes ) {
  math::FieldFixture<F> f; BOOST_CHECK( f ); // avoid unreferenced local variable warning

  BOOST_TEST_MESSAGE( "  Testing " << F::getName() );

  unsigned int size = 3;
  boost::scoped_array<F> entries(new F[size]);
  entries[0].template setTo<-1>();
  entries[1].template setTo<0>();
  entries[2].template setTo<1>();

  BOOST_CHECK(entries[0].template is<-1>());
  BOOST_CHECK(entries[1].template is<0>());
  BOOST_CHECK(entries[2].template is<1>());

  math::vandermonde::FFTSolver<F> fftSolver(size, entries.get());

  BOOST_CHECK_EQUAL(fftSolver.getMatrixSize(), size);

  BOOST_CHECK_EQUAL(fftSolver.getEntry(0), F(-1));
  BOOST_CHECK_EQUAL(fftSolver.getEntry(1), F(0));
  BOOST_CHECK_EQUAL(fftSolver.getEntry(2), F(1));

  boost::scoped_array<F> coeffs(new F[size]);
  coeffs[0] = F(1)/F(2);
  coeffs[1] = F(0);
  coeffs[2] = F(1)/F(2);

  boost::scoped_array<F> values(new F[size]);

  values[0].template setTo<1>();
  values[1].template setTo<0>();
  values[2].template setTo<1>();

  boost::scoped_array<F> result(new F[size]);

  fftSolver.evaluate(coeffs.get(),result.get());

  BOOST_CHECK_EQUAL(result[0], F(1));
  BOOST_CHECK_EQUAL(result[1], F(1)/F(2));
  BOOST_CHECK_EQUAL(result[2], F(1));

  fftSolver.solveTranspose(values.get(), result.get());

  BOOST_CHECK_EQUAL(result[0], F(1)/F(2));
  BOOST_CHECK_EQUAL(result[1], F(0));
  BOOST_CHECK_EQUAL(result[2], F(1)/F(2));

}

BOOST_AUTO_TEST_SUITE_END();

bool init_unit_test() {
  return true;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file robust_gaussian_filter_test.cpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "../typecast.hpp"
#include "gaussian_filter_test_suite.hpp"
#include <fl/util/meta.hpp>
#include <fl/filter/gaussian/gaussian_filter.hpp>
#include <fl/filter/gaussian/robust_gaussian_filter.hpp>
#include <fl/model/sensor/linear_cauchy_sensor.hpp>
#include <fl/model/sensor/body_tail_sensor.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>

using namespace fl;

template <
    int StateDimension,
    int InputDimension,
    int ObsrvDimension,
    int FilterIterations
>
struct RobutGaussianFilterTestConfiguration
{
    enum : signed int
    {
        StateDim = StateDimension,
        InputDim = InputDimension,
        ObsrvDim = ObsrvDimension,
        Iterations = FilterIterations
    };

    template <typename ModelFactory>
    struct FilterDefinition
    {
        typedef typename ModelFactory::LinearObservation::Obsrv Obsrv;
        typedef typename ModelFactory::LinearObservation::State State;

        typedef fl::LinearCauchySensor<Obsrv, State> CauchyModel;

        typedef fl::BodyTailSensor<
                    typename ModelFactory::LinearObservation,
                    CauchyModel
                > BodyTailSensor;

        typedef UnscentedQuadrature Quadrature;
//        typedef fl::SigmaPointQuadrature<
//                    fl::MonteCarloTransform<
//                        fl::ConstantPointCountPolicy<1000>>> Quadrature;

        typedef RobustGaussianFilter<
                    typename ModelFactory::LinearTransition,
                    BodyTailSensor,
                    Quadrature
                > Type;
    };

    template <typename ModelFactory>
    static typename FilterDefinition<ModelFactory>::Type
    create_filter(ModelFactory&& factory)
    {
        typedef FilterDefinition<ModelFactory> Definition;
        typedef typename Definition::Type Filter;
        typedef typename Definition::CauchyModel CauchyModel;
        typedef typename Definition::BodyTailSensor BodyTailSensor;

        auto body_model = factory.create_sensor();
        auto tail_model = CauchyModel();
        tail_model.noise_covariance(tail_model.noise_covariance() * 10.);

        return Filter(
            factory.create_linear_state_model(),
            BodyTailSensor(body_model, tail_model, 0.1),
            typename Definition::Quadrature());
    }
};

typedef ::testing::Types<
            StaticTest<RobutGaussianFilterTestConfiguration<1, 1, 1, 30>>
        > TestTypes;

INSTANTIATE_TYPED_TEST_CASE_P(RobustGaussianFilterTest,
                              GaussianFilterTest,
                              TestTypes);


#pragma once

#include <armadillo>

class Pipeline;

/*!
 * \brief Discretizer is an abstract class, the base class the implementation of
 * the discretization of the two different sets of governing equations; internal
 * energy and enthalpy.
 *
 * The governing equations are discretized using a implicit backward difference
 * method, as detailed in
 *  - Gas composition tracking in transient pipeline flow, Chaczykowski et. al., Journal of Natural Gas Science and Engineering 55 (2018)
 *
 * This finite difference scheme discretizes the governing properties flow,
 * pressure and temperature onto a grid. The derivatives and values for each
 * grid section \f$I\f$ are approximated by different combination of the flow,
 * pressure and temperatures at the grid points, \f$y_i\f$ and \f$y_{i+1}\f$,
 * as follows
 *
 * \f[
 *     \frac{\partial y(I, t_{n+1})}{\partial t} = \frac{y_{i+1}^{n+1} + y_{i}^{n+1} - y_{i+1}^{n} - y_{i}^{n}}{2\Delta t}
 * \f]
 *
 * \f[
 *     \frac{\partial y(I, t_{n+1})}{\partial x} = \frac{y_{i+1}^{n+1} - y_{i}^{n+1}}{\Delta x}
 * \f]
 *
 * \f[
 *     y(I, t_{n+1}) = \frac{y_{i+1}^{n+1} + y_{i}^{n+1}}{2}
 * \f]
 *
 * This sets up a matrix equation \f$Ax = b\f$, where the matrix \f$A\f$
 * contains the coeffcients of each term \f$y_i\f$, the vector \f$x\f$ contain
 * the unknowns \f$\dot m_i\f$ (flow), \f$p_i\f$ (pressure) and \f$T_i\f$
 * (temperature), and the vector \f$b\f$ contain the constant/known terms
 * (from boundary conditions and other knowns).
 *
 * \see MatrixEquation
 */
class Discretizer
{
public:
    //! Have to declare virtual destructor to avoid compiler warnings.
    //! Only declared here, to avoid the inline compiler-generated default destructor.
    virtual ~Discretizer();

    /*!
     * \brief Construct from number of grid points and number of equations and
     * variables.
     *
     * Allocates the matrices Discretizer::m_term_i, Discretizer::m_term_ipp,
     * and the vector Discretizer::m_boundaryTerm.
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     */
    Discretizer(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables);

    /*!
     * \brief Pure virtual function, stencil for subclasses. This method
     * calculates in the coefficients of \f$y_i\f$ and \f$y_{i+1}\f$ and stores
     * them in Discretizer::m_term_i and Discretizer::m_term_ipp.
     * \param dt Time step
     * \param currentState Current pipeline state
     * \param newState New/guess pipeline state
     */
    virtual void discretize(
            const arma::uword dt,
            const Pipeline& currentState,
            const Pipeline& newState) = 0;

    const arma::cube& term_i() const { return m_term_i; } //!< Get coefficients of \f$y_i\f$
    const arma::cube& term_ipp() const { return m_term_ipp; } //!< Get coefficients of \f$y_{i+1}\f$
    const arma::mat& boundaryTerms() const { return m_boundaryTerm; } //!< Get constant terms

protected:
    // TODO: better description of how m_term_i and m_term_ipp are organized
    /*!
     * \brief The coefficients of \f$y_i\f$ in the discretized governing equations.
     *
     * This is an `arma::cube`, which is organized as follows.
     * `m_term_i(grid point, equation number, variable number)`,
     * where grid points are the usual grid points, equations are the
     * continuity (0), momentum (1) and energy equations (2), and the variables
     * are in order flow (0), pressure (1) and temperature(1).
     * So for example if you want the coefficient at grid point number 5, in the
     * energy equation, for flow, you want
     * `m_term_i(4, 2, 0)` (zero-indexed).
     */
    arma::cube m_term_i;

    /*!
     * \brief The coefficients of \f$y_i\f$ in the discretized governing equations.
     *
     * See Discretizer::m_term_i for a description of the organization of this.
     */
    arma::cube m_term_ipp;

    /*!
     * \brief The constant/known terms in the discretized governing equations.
     *
     * This is a matrix, organized as
     * `m_boundaryTerm(grid point, equation number)`.
     */
    arma::mat m_boundaryTerm;

    double m_gravity = 9.81; //!< Gravity
};


//  Copyright John Maddock 2006, 2007
//  Copyright Paul A. Bristow 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 <pch.hpp>

#ifdef _MSC_VER
#  pragma warning(disable : 4127) // conditional expression is constant
#  pragma warning(disable : 4512) // assignment operator could not be generated
#  pragma warning(disable : 4756) // overflow in constant arithmetic
// Constants are too big for float case, but this doesn't matter for test.
#endif

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "test_legendre_hooks.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the Hermite polynomials.
// There are two sets of tests, spot
// tests which compare our results with selected values computed
// using the online special function calculator at
// functions.wolfram.com, while the bulk of the accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif

   //
   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*",      // test data group
      "boost::math::hermite", 10, 5);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                  // test type(s)
      ".*",      // test data group
      "boost::math::hermite", 10, 5);  // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void do_test_hermite(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(unsigned, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::hermite<value_type>;
#else
   pg funcp = boost::math::hermite;
#endif

   typedef unsigned (*cast_t)(value_type);

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test hermite against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func_int1(funcp, 0, 1),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::hermite", test_name);

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

template <class T>
void test_hermite(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value a, input value b and erf(a, b):
   //
#  include "hermite.ipp"

   do_test_hermite(hermite, name, "Hermite Polynomials");
}

template <class T>
void test_spots(T, const char* t)
{
   std::cout << "Testing basic sanity checks for type " << t << std::endl;
   //
   // basic sanity checks, tolerance is 100 epsilon:
   // These spots were generated by MathCAD, precision is
   // 14-16 digits.
   //
   T tolerance = (std::max)(boost::math::tools::epsilon<T>() * 100, static_cast<T>(1e-14));
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(0, static_cast<T>(1)), static_cast<T>(1.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1)), static_cast<T>(2.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(2)), static_cast<T>(4.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(10)), static_cast<T>(20), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(100)), static_cast<T>(200), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1e6)), static_cast<T>(2e6), tolerance);
   if(std::numeric_limits<T>::max_exponent >= std::numeric_limits<double>::max_exponent)
   {
      BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1e307)), static_cast<T>(2e307), tolerance);
      BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(99, static_cast<T>(100)), static_cast<T>(4.967223743011310E+227L), tolerance);
   }
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(30)), static_cast<T>(5.896624628001300E+17L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(1000)), static_cast<T>(1.023976960161280E+33L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(10)), static_cast<T>(8.093278209760000E+12L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(-10)), static_cast<T>(8.093278209760000E+12L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-10)), static_cast<T>(-7.880000000000000E+3L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-1000)), static_cast<T>(-7.999988000000000E+9L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-1000000)), static_cast<T>(-7.999999999988000E+18L), tolerance);
}

int test_main(int, char* [])
{
   BOOST_MATH_CONTROL_FP;

   boost::math::hermite(51, 915.0);

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_spots(0.0F, "float");
#endif
   test_spots(0.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L, "long double");
   test_spots(boost::math::concepts::real_concept(0.1), "real_concept");
#endif

   expected_results();

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_hermite(0.1F, "float");
#endif
   test_hermite(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_hermite(0.1L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_hermite(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <boost/dynamic_bitset.hpp>
#include <boost/container/vector.hpp>
#include <boost/unordered_map.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/niederreiter_base2.hpp>
#include <./Timer.cpp>

typedef boost::dynamic_bitset<> Vetor;
typedef boost::container::vector<Vetor> Matriz;

/*
*
* Ht: Matriz de verificação de paridade original
*
* Ht_long: Referência de onde guardar o array em que cada elemento é uma linha de Ht
*
* Erros_1: Referência de onde guardar o
    array em que cada elemento é um erro de peso=1 (mas só a porção de informação desse erro)
*   exemplo: se há 5 bits na palavra-código e os 2 primeiros são de informação, então um elemento
*   de Erros_1 pode ser 3=0b11, significando erro em cada um dos 2 bits de informação
*
* n_linhas, n_colunas, n_informacao: Quantas linhas e colunas tem Ht, e quantos bits são de informação
*   na palavra código (assume-se que são correspondem às primeiras linhas de Ht)
*/
void __gera_auxiliares(
    const Matriz& Ht,
    unsigned long* Ht_long,
    unsigned long* Erros_1,
    int n_linhas,
    int n_colunas,
    int n_informacao
) {
    /**
    * Só necessários os ulongs correspondentes a cada síndrome
    */
    for (int i = 0; i < n_linhas; ++i)
    {
        Ht_long[i] = Ht[i].to_ulong();
    }

    /**
    * Porção de informação dos erros associados a cada síndrome
    * (por isso só importam os erros em bits de informação)
    */
    for (int i = 0; i < n_linhas; ++i)
    {
        // Erros depois de n_informacao linhas correspondem somente a erros de bit
        // de paridade, portanto aparecem como 0 já que só se coletam os erros de informação
        Erros_1[i] = i < n_informacao ? ((unsigned long)1) << (n_informacao - i - 1) : 0;
    }
}

/**
* input_csv: Espera-se formato de valores 0 ou 1 separados por vírgula e \n, sem espaços
*
* M: Matriz onde armazenar a leitura.
*   Restrição: linha/coluna pode ter, no máximo (inclusive), 64 bits (cabe num unsigned long)
*/
void carrega_matriz(
    std::ifstream& input_csv,
    Matriz& M
) {
    std::string numero_string;

    while (getline(input_csv, numero_string)) {

        int commaPos;

        while ((commaPos = numero_string.find(",")) != std::string::npos)
            numero_string.erase(commaPos, 1);

        Vetor numero (numero_string.length(), stoul(numero_string, 0, 2));

        // std::cout << numero << "\n";

        M.push_back(numero);

    }
}

int testa_carrega_matriz() {
    std::string Htcsv = "./dados/Ht.csv";
    std::ifstream input {Htcsv};

    // if(!input)
    //     std::error("could not open " + Htcsv);

    std::string numero_string;
    Matriz Ht;

    while (getline(input, numero_string)) {

        int commaPos;

        while ((commaPos = numero_string.find(",")) != std::string::npos)
            numero_string.erase(commaPos, 1);

        Vetor numero (numero_string.length(), stoul(numero_string, 0, 2));

        std::cout << numero << "\n";

        Ht.push_back(numero);

    }

    std::cout << "Finished loading. Now replicating:" << std::endl;

    std::for_each(Ht.begin(), Ht.end(), [](Vetor line) {
        std::cout << line << std::endl;
    });
}

/**
* Auxiliar para `popular_dict`
*
*/
void __popular_dict(
    boost::unordered_map<unsigned long, unsigned long>& dict,
    unsigned long* Ht_long,
    unsigned long* Erros_1,
    int n_linhas,
    int n_colunas,
    int n_informacao,
    unsigned long sindrome_parcial,
    unsigned long erro_parcial,
    int linha_inicio,
    int niveis_restantes
) {
    if (niveis_restantes == 1) {
        unsigned long sindrome, erro;
        for (int i = linha_inicio; i < n_linhas; ++i)
        {
            sindrome = sindrome_parcial ^ Ht_long[i];
            erro = erro_parcial ^ Erros_1[i];
            if (dict.find(sindrome) == dict.end())
                dict[sindrome] = erro;
        }
        return;
    }

    for (int i = linha_inicio; i < n_linhas - (niveis_restantes - 1) /*iterar todas as síndromes*/; ++i)
    {
        __popular_dict(
            dict,
            Ht_long,
            Erros_1,
            n_linhas,
            n_colunas,
            n_informacao,
            sindrome_parcial ^ Ht_long[i],
            erro_parcial ^ Erros_1[i],
            i+1,
            niveis_restantes-1
        );
    }

}

/**
*
* dict: Onde guardar o mapa "síndrome->erro"
*
* Ht: Matriz de verificação de paridade (transposta). Assume-se que suas últimas linhas
* correspondem aos bits de informação (identidade em cima, outras linhas embaixo)
*
* n_linhas, n_colunas, n_informacao: Quantas linhas e colunas tem Ht, e quantos bits são de informação
*   na palavra código (assume-se que são correspondem às primeiras linhas de Ht)
*
* peso_maximo: O peso dos maiores erros de informação a serem catalogados em `dict`
*
*/
void popular_dict(
    boost::unordered_map<unsigned long, unsigned long>& dict,
    const Matriz& Ht,
    int n_linhas,
    int n_colunas,
    int n_informacao,
    int peso_maximo
) {

    unsigned long *Ht_long = new unsigned long[n_linhas];
    unsigned long *Erros_1 = new unsigned long[n_linhas];
    __gera_auxiliares(
        Ht,
        Ht_long,
        Erros_1,
        n_linhas,
        n_colunas,
        n_informacao
    );

    for (int i = 1; i <= peso_maximo; ++i)
    {
        __popular_dict(
        dict,
        Ht_long,
        Erros_1,
        n_linhas,
        n_colunas,
        n_informacao,
        0,
        0,
        0,
        i
    );
    }

}

void testa_popular_dict() {
    std::ifstream Htcsv("dados/Ht.csv");
    Matriz Ht;
    boost::unordered_map<unsigned long, unsigned long> dict;

    carrega_matriz(Htcsv, Ht);
    int n_linhas = Ht.size();
    int n_colunas =  Ht[0].size();
    int n_informacao = n_linhas - n_colunas;

    popular_dict(
        dict,
        Ht,
        n_linhas,
        n_colunas,
        n_informacao,
        3
    );

    assert(dict.find(0) == dict.end());
    assert(dict.at(1099511595008) == 34359738368); // 1ª linha de Ht -> erro só no 1º bit de info
    assert(dict.at(1064615018496) == 17179869184); // 2ª linha de Ht -> erro só no 2º bit de info
    assert(dict.at(34896576512) == 51539607552); // 1ª+2ª linhas de Ht -> erros nos 2 MSB de info
    assert(dict.at(1052568944640) == 55834574848); // 1ª+2ª+4ª linhas de Ht -> erros nos 1º,2º,4º bits de info
    assert(dict.at(549755813888) == 0); // erro composto só pela 37ª linha de Ht não aparece nos bits de info
    assert(dict.at(481038172167) == 1); // mas se erro for de 36ª+37ª linhas de Ht -> erro no último bit de info
                                         // (e ignora o bit de paridade)
}

/**
* Retorna nova matriz em que cada linha é uma coluna da matriz original.
*/
Matriz& transposta(
    const Matriz& M
) {
    int antes_colunas = M[0].size();
    int antes_linhas = M.size();
    Matriz* resultado = new Matriz(antes_colunas);

    for (int i = 0; i < antes_colunas; ++i)
    {
        (*resultado)[i] = Vetor (antes_linhas);
        // Se não for Vetor&, boost copia implicitamente
        Vetor& linha = (*resultado)[i];
        for (int j = 0; j < antes_linhas; ++j)
        {
            linha[antes_linhas - j - 1] = M[j][antes_colunas - i - 1];
        };
    }
    return *resultado;
}

void testa_transposta() {
    Matriz m = Matriz(3);
    /*
    * m == [1 1; 0 1; 0 0]
    */
    m[0] = Vetor(2, 3);
    m[1] = Vetor(2, 1);
    m[2] = Vetor(2, 0);

    Matriz mt = transposta(m);

    assert(mt[0] == Vetor(3, 4));
    assert(mt[1] == Vetor(3, 6));
}

/**
* Calcula M.v
*/
Vetor& mult(const Matriz& M, const Vetor& v) {
    int n_linhas = M.size(), n_colunas = v.size();
    Vetor* resultado = new Vetor(n_linhas);

    for (int i = 0; i < n_linhas; ++i)
    {
        (*resultado)[n_linhas - i - 1] =  (M[i] & v).count() & 1;
    }

    return *resultado;
}

/**
* Calcula M.N
*/
Matriz& mult(const Matriz& M, const Matriz& N) {
    int n_linhas = M.size();
    int n_colunas = N[0].size();
    Matriz pre_resultado = Matriz(n_colunas);

    Matriz Nt = transposta(N);

    for (int i = 0; i < n_colunas; ++i)
    {
        pre_resultado[i] = mult(M, Nt[i]);
    }

    Matriz& resultado = transposta(pre_resultado);

    return resultado;
}

void testa_mult() {
    // M == [1 0 1; 0 0 1]
    Matriz M = Matriz(2);
    M[0] = Vetor(3, 5);
    M[1] = Vetor(3, 1);

    // v == [0 1 1]
    Vetor v = Vetor(3, 3);

    // resultado deve ser [1 1]
    assert(mult(M, v) == Vetor(2, 3));

    // [1 1; 0 0; 1 1]
    Matriz N = Matriz(3);
    N[0] = Vetor(2, 3);
    N[1] = Vetor(2, 0);
    N[2] = Vetor(2, 3);

    assert(mult(M, N).size() == 2);
    assert(mult(M, N)[0] == Vetor(2, 0));
    assert(mult(M, N)[1] == Vetor(2, 7));
}

/**
* Modifica `Transmitido`, com chance `p` de inverter cada bit
* de cada palavra código.
*
* Transmitido: Assume-se que cada linha é uma palavra-código
*/
void canal(Matriz& Transmitido, double p) {
    int count=0;
    int n_linhas = Transmitido.size();
    int n_colunas = Transmitido[0].size();
    Vetor* linha;
    boost::random::niederreiter_base2 gen(4);
    boost::random::uniform_01<double> random;
    for (int i = 0; i < n_linhas; ++i)
    {
        linha = &Transmitido[i];
        for (int j = 0; j < n_colunas; ++j)
        {
            if (random(gen) < p) {
                (*linha)[j].flip();
                count++;
            }
        }
    }
    // std::cout << "p ≃ " << ((double) count) / (n_linhas*n_colunas) << std::endl;
}

/**
* amostras_informacao: Lista de palavras de informacao a serem enviadas
*   Espera-se uma palavra por linha, elementos separados por vírgulas (sem espaço)
*
* Ht_csv: Matriz de verificação de paridade (transposta). Mesmo formato de `amostras_informacao_csv`
*
* Gt_csv: Matriz de geração do código (transposta). Mesmo formato de `amostras_informacao_csv`
*
* p: Lista de chances de o canal BSC inverter um bit transmitido
*
* peso_maximo_memorizado: Caso se encontre síndrome com peso maior que isto, ela não será corrigida
*
* Retorna lista de chances de erro de bit (uma para cada valor de p)
*/
boost::container::vector<double> desempenho(
    std::ifstream& amostras_informacao_csv,
    std::ifstream& Ht_csv,
    std::ifstream& Gt_csv,
    const boost::container::vector<double>& p,
    int peso_maximo_memorizado
) {
    boost::container::vector<double>* resultado = new boost::container::vector<double>(p.size());

    Matriz Ht;
    carrega_matriz(Ht_csv, Ht);

    /**
    * Exemplo de chave e valor:
    * - chave: 0b1001 , valor: 0b101 significa uma síndrome [1, 0, 0, 1] com erro associado [1, 0, 1, ...]
    *   em que as reticências indicam a parte do erro concernente aos bits de paridade (não importam)
    */
    boost::unordered_map<unsigned long, unsigned long> dict;

    int n_linhas = Ht.size();
    int n_colunas = Ht[0].size();
    int n_informacao = n_linhas - n_colunas;

    popular_dict(
        dict,
        Ht,
        n_linhas,
        n_colunas,
        n_informacao,
        peso_maximo_memorizado
    );

    Matriz Info;
    carrega_matriz(amostras_informacao_csv, Info);

    int n_amostras = Info.size();

    Matriz Gt;
    carrega_matriz(Gt_csv, Gt);
    Matriz G = transposta(Gt);

    for (int i_p = 0; i_p < p.size(); ++i_p)
    {
        Matriz Transmitido = mult(Info, G);

        canal(Transmitido, p[i_p]);

        Matriz Sindromes = mult(Transmitido, Ht);

        Matriz Transmitido_informacao = Matriz(n_amostras);
        for (int i = 0; i < n_amostras; ++i)
        {
            Transmitido_informacao[i] = Vetor(n_informacao);
            for (int j = 0; j < n_informacao; ++j)
            {
                Transmitido_informacao[i][n_informacao - j - 1] = Transmitido[i][n_linhas - j - 1];
            }
            // assert(Transmitido_informacao.size() == Transmitido.size());
            // assert(Transmitido_informacao[0].size() == Info[0].size());
            // assert(Transmitido_informacao[i] == Info[i]);
        }

        int n_erros = 0;
        int incr=0;
        unsigned long sindrome;
        Vetor correcao_nula = Vetor(n_informacao, 0);
        Vetor correcao;
        for (int i = 0; i < n_amostras; ++i)
        {
            sindrome = Sindromes[i].to_ulong();
            if (dict.find(sindrome) == dict.end())
                correcao = correcao_nula;
            else
                correcao = Vetor(n_informacao, dict.at(sindrome));
            incr = (Transmitido_informacao[i] ^ correcao ^ Info[i]).count();
            // if (incr != 0) {
            //     std::cout << correcao << ": " << Transmitido_informacao[i] << " versus " << Info[i] << std::endl;
            //     std::cout << "diferença " << (Transmitido_informacao[i] ^ Info[i]) << std::endl;
            //     std::cout << "Originais: " << Transmitido[i] << " versus " << mult(Gt, Info[i]) << std::endl;
            //     std::cout << "diferença: " << (Transmitido[i] ^ mult(Gt, Info[i])) << std::endl;
            //     std::cout << "síndrome: " << Sindromes[i] << std::endl;
            //     std::cout << std::endl;
            // }

            n_erros += incr;
        }

        (*resultado)[i_p] = n_erros / ((double) n_amostras * n_informacao);
        std::cout << "parcial(" << i_p << "): " << (*resultado)[i_p] << std::endl;
    }
    return *resultado;
}

int main(int argc, char** argv) {

    int arg_peso_maximo = std::stoi(argv[1]);
    std::string arg_resultados = argv[2];

    std::ifstream Htcsv ("dados/Ht.csv");
    std::ifstream amostrasInput("dados/amostra-informacao.csv");
    std::ifstream GtInput("dados/Gt.csv");
    std::ofstream Resultados(arg_resultados);
    std::ofstream P("dados/lista-de-p.csv");

    boost::container::vector<double> p = boost::container::vector<double>(0);
    double p0 = 0.5;
    while(p0 > 1 /((double) 1000000)) {
        p.push_back(p0);
        p0 *= 0.5;
    }

    boost::container::vector<double> des = desempenho(amostrasInput, Htcsv, GtInput, p, arg_peso_maximo);
    Resultados << des[0];
    P << p[0];
    for (int i = 1; i < des.size(); ++i)
    {
        Resultados << ",";
        Resultados << des[i];
        P << ",";
        P << p[i];
    }
    Resultados << std::endl;
    P << std::endl;

    return 0;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/collective.hpp>
#include <rokko/utility/xyz_hamiltonian_mpi.hpp>
#include <boost/tuple/tuple.hpp>
#include <fstream>
#include <iostream>

typedef rokko::matrix_col_major matrix_major;

int main(int argc, char *argv[]) {
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
  MPI_Comm comm = MPI_COMM_WORLD;
  std::string solver_name(rokko::parallel_dense_solver::default_solver());
  std::string lattice_file("xyz.dat");
  if (argc >= 2) solver_name = argv[1];
  if (argc >= 3) lattice_file = argv[2];

  rokko::grid g(comm);
  int myrank = g.get_myrank();

  std::cout.precision(5);

  std::ifstream ifs(lattice_file.c_str());
  if (!ifs) {
    std::cout << "can't open file" << std::endl;
    exit(1);
  }
  int num_sites, num_bonds;
  std::vector<std::pair<int, int> > lattice;
  std::vector<boost::tuple<double, double, double> > coupling;
  ifs >> num_sites >> num_bonds;
  int dim = 1 << num_sites;
  for (int i = 0; i < num_bonds; ++i) {
    int j, k;
    ifs >> j >> k;
    lattice.push_back(std::make_pair(j, k));
  }
  for (int i = 0; i < num_bonds; ++i) {
    double jx, jy, jz;
    ifs >> jx >> jy >> jz;
    coupling.push_back(boost::make_tuple(jx, jy, jz));
  }

  rokko::parallel_dense_solver solver(solver_name);
  solver.initialize(argc, argv);
  if (myrank == 0)
    std::cout << "Eigenvalue decomposition of XYZ model" << std::endl
              << "num_procs = " << g.get_nprocs() << std::endl
              #ifdef _OPENMP
              << "num_threads per process = " << omp_get_max_threads() << std::endl
              #endif
              << "solver = " << solver_name << std::endl
              << "lattice file = " << lattice_file << std::endl
              << "number of sites = " << num_sites << std::endl
              << "number of bonds = " << num_bonds << std::endl
              << "dimension = " << dim << std::endl;

  rokko::distributed_matrix<double, matrix_major> mat(dim, dim, g, solver);
  rokko::xyz_hamiltonian::generate(num_sites, lattice, coupling, mat);
  rokko::localized_matrix<double, matrix_major> mat_loc(dim, dim);
  rokko::gather(mat, mat_loc, 0);

  rokko::localized_vector<double> eigval(dim);
  rokko::distributed_matrix<double, matrix_major> eigvec(dim, dim, g, solver);
  try {
    solver.diagonalize(mat, eigval, eigvec);
  }
  catch (const char *e) {
    if (myrank == 0) std::cout << "Exception : " << e << std::endl;
    MPI_Abort(MPI_COMM_WORLD, 22);
  }

  rokko::localized_matrix<double, matrix_major> eigvec_loc(dim, dim);
  rokko::gather(eigvec, eigvec_loc, 0);
  if (myrank == 0) {
    std::cout << "smallest eigenvalues:";
    for (int i = 0; i < std::min(dim, 10); ++i) std::cout << ' ' << eigval(i);
    std::cout << std::endl;
    std::cout << "residual of the smallest eigenvalue/vector: |x A x - lambda| = "
              << std::abs(eigvec_loc.col(0).transpose() * mat_loc * eigvec_loc.col(0) - eigval(0))
              << std::endl;
  }

  solver.finalize();
  MPI_Finalize();
}


#ifndef CAFFE_UTIL_PREDICTION_H_
#define CAFFE_UTIL_PREDICTION_H_

#include <vector>
#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>
#include "caffe/blob.hpp"

namespace caffe {

	template <typename Dtype>
	using Slice = Eigen::Matrix<Dtype,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>;
	template <typename Dtype>
	using Grid = std::vector<Slice<Dtype> >;

	template <typename Dtype>
	Grid<Dtype> rotate_voxels_prediction(const Grid<Dtype> &vox,
										 const Eigen::Matrix<Dtype,4,4> &model1,
										 const Eigen::Matrix<Dtype,4,4> &view1,
										 const Eigen::Matrix<Dtype,4,4> &model2,
										 const Eigen::Matrix<Dtype,4,4> &view2,
										 const Eigen::Matrix<Dtype,4,4> &proj);

	template <typename Dtype>
	Dtype rotated_proba_value(const Grid<Dtype> &vox,
							  const Eigen::Matrix<Dtype,4,4> &model1,
							  const Eigen::Matrix<Dtype,4,4> &view1,
							  const Eigen::Matrix<Dtype,4,4> &model2,
							  const Eigen::Matrix<Dtype,4,4> &view2,
							  const Eigen::Matrix<Dtype,4,4> &proj,
							  Dtype z,Dtype x, Dtype y);

	template <typename Dtype>
	Dtype z_to_depth(int z, int size, const Eigen::Matrix<Dtype,4,4> &proj);
	template <typename Dtype>
	int depth_to_z(Dtype depth, int size, const Eigen::Matrix<Dtype,4,4> &proj);

	template <typename Dtype>
	Eigen::Matrix<Dtype,3,1> rotate_coords(Dtype x, Dtype y, Dtype z,
										   const Eigen::Matrix<Dtype,4,4> &model1,
										   const Eigen::Matrix<Dtype,4,4> &view1,
										   const Eigen::Matrix<Dtype,4,4> &model2,
										   const Eigen::Matrix<Dtype,4,4> &view2,
										   const Eigen::Matrix<Dtype,4,4> &proj);
	template <typename Dtype>
	void rotate_blobs(const Blob<Dtype> * pred,
					  const Dtype* model1,
					  const Dtype* view_mat1,
					  const Dtype* model2,
					  const Dtype* view_mat2,
					  const Dtype* proj_mat,
					  Dtype * output) ;

		template <typename Dtype>
		Grid<Dtype> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols);
	template <typename Dtype>
	int CV_type();

}

#endif


/*
 [auto_generated]
 libs/numeric/odeint/test/integrate.cpp

 [begin_description]
 This file tests the integrate function and its variants.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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)
 */

#define BOOST_TEST_MODULE odeint_integrate_functions

#include <vector>
#include <cmath>
#include <iostream>

#include <boost/numeric/odeint/config.hpp>

#include <boost/array.hpp>
#include <boost/ref.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

// nearly everything from odeint is used in these tests
#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/integrate/integrate_times.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/stepper/euler.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double value_type;
typedef std::vector< value_type > state_type;

void lorenz( const state_type &x , state_type &dxdt , const value_type t )
{
    //const value_type sigma( 10.0 );
    const value_type R( 28.0 );
    const value_type b( value_type( 8.0 ) / value_type( 3.0 ) );

    // first component trivial
    dxdt[0] = 1.0; //sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

struct push_back_time
{
    std::vector< double >& m_times;

    state_type& m_x;

    push_back_time( std::vector< double > &times , state_type &x )
    :  m_times( times ) , m_x( x ) { }

    void operator()( const state_type &x , double t )
    {
        m_times.push_back( t );
        boost::numeric::odeint::copy( x , m_x );
    }
};

template< class Stepper >
struct perform_integrate_const_test
{
    void operator()( const value_type t_end , const value_type dt )
    {
        std::cout << "Testing integrate_const with " << typeid( Stepper ).name() << std::endl;

        state_type x( 3 , 10.0 ) , x_end( 3 );

        std::vector< value_type > times;

        integrate_const( Stepper() , lorenz , x , 0.0 , t_end ,
                                        dt , push_back_time( times , x_end ) );

        // int steps = times.size()-1;

        //std::cout << t_end << " (" << dt << "), " << steps << " , " << times.size() << " , " << 10.0+dt*steps << "=" << x_end[0] << std::endl;

        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , static_cast<int>(floor(t_end/dt))+1 );

        for( size_t i=0 ; i<times.size() ; ++i )
        {
            //std::cout << i << " , " << times[i] << " , " << static_cast< value_type >(i)*dt << std::endl;
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_SMALL( times[i] - static_cast< value_type >(i)*dt , (i+1) * 2E-16 );
        }

        // check first, trivial, component
        BOOST_CHECK_SMALL( (10.0 + times[times.size()-1]) - x_end[0] , 1E-6 ); // precision of steppers: 1E-6
        //BOOST_CHECK_EQUAL( x[1] , x_end[1] );
        //BOOST_CHECK_EQUAL( x[2] , x_end[2] );
    }
};

template< class Stepper >
struct perform_integrate_adaptive_test
{
    void operator()( const value_type t_end = 10.0 , const value_type dt = 0.03 )
    {
        std::cout << "Testing integrate_adaptive with " << typeid( Stepper ).name() << std::endl;

        state_type x( 3 , 10.0 ) , x_end( 3 );

        std::vector< value_type > times;

        size_t steps = integrate_adaptive( Stepper() , lorenz , x , 0.0 , t_end ,
                                        dt , push_back_time( times , x_end ) );

//        std::cout << t_end << " , " << steps << " , " << times.size() << " , " << 10.0+dt*steps << "=" << x_end[0] << std::endl;

        BOOST_CHECK_EQUAL( times.size() , steps+1 );

        BOOST_CHECK_SMALL( times[0] - 0.0 , 2E-16 );
        BOOST_CHECK_SMALL( times[times.size()-1] - t_end , times.size() * 2E-16 );

        // check first, trivial, component
        BOOST_CHECK_SMALL( (10.0 + t_end) - x_end[0] , 1E-6 ); // precision of steppers: 1E-6
//        BOOST_CHECK_EQUAL( x[1] , x_end[1] );
//        BOOST_CHECK_EQUAL( x[2] , x_end[2] );
    }
};

template< class Stepper >
struct perform_integrate_times_test
{
    void operator()( const int n = 10 , const int dn=1 , const value_type dt = 0.03 )
    {
        std::cout << "Testing integrate_times with " << typeid( Stepper ).name() << std::endl;

        state_type x( 3 ) , x_end( 3 );
        x[0] = x[1] = x[2] = 10.0;

        std::vector< double > times;

        std::vector< double > obs_times( abs(n) );
        for( int i=0 ; boost::numeric::odeint::detail::less_with_sign( static_cast<double>(i) ,
                       static_cast<double>(obs_times.size()) ,
                       dt ) ; i+=dn )
        {
            obs_times[i] = i;
        }
        // simple stepper
        integrate_times( Stepper() , lorenz , x , obs_times.begin() , obs_times.end() ,
                    dt , push_back_time( times , x_end ) );

        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , abs(n) );

        for( size_t i=0 ; i<times.size() ; ++i )
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );

        // check first, trivial, component
        BOOST_CHECK_SMALL( (10.0 + 1.0*times[times.size()-1]) - x_end[0] , 1E-6 ); // precision of steppers: 1E-6
//        BOOST_CHECK_EQUAL( x[1] , x_end[1] );
//        BOOST_CHECK_EQUAL( x[2] , x_end[2] );
    }
};

template< class Stepper >
struct perform_integrate_n_steps_test
{
    void operator()( const int n = 200 , const value_type dt = 0.01 )
    {
        std::cout << "Testing integrate_n_steps with " << typeid( Stepper ).name() << std::endl;

        state_type x( 3 ) , x_end( 3 );
        x[0] = x[1] = x[2] = 10.0;

        std::vector< double > times;

        // simple stepper
        value_type end_time = integrate_n_steps( Stepper() , lorenz , x , 0.0 , dt , n , push_back_time( times , x_end ) );

        BOOST_CHECK_SMALL( end_time - n*dt , 2E-16 );
        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , n+1 );

        for( size_t i=0 ; i<times.size() ; ++i )
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_SMALL( times[i] - static_cast< value_type >(i)*dt , 2E-16 );

        // check first, trivial, component
        BOOST_CHECK_SMALL( (10.0 + end_time) - x_end[0] , 1E-6 ); // precision of steppers: 1E-6
//        BOOST_CHECK_EQUAL( x[1] , x_end[1] );
//        BOOST_CHECK_EQUAL( x[2] , x_end[2] );

    }
};

class stepper_methods : public mpl::vector<
    euler< state_type > ,
    modified_midpoint< state_type > ,
    runge_kutta4< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type > ,
    controlled_runge_kutta< runge_kutta_cash_karp54< state_type > > ,
    controlled_runge_kutta< runge_kutta_dopri5< state_type > > ,
    controlled_runge_kutta< runge_kutta_fehlberg78< state_type > > ,
    bulirsch_stoer< state_type > ,
    dense_output_runge_kutta< controlled_runge_kutta< runge_kutta_dopri5< state_type > > >
    //bulirsch_stoer_dense_out< state_type >
> { };

BOOST_AUTO_TEST_SUITE( integrate_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_const_test_case , Stepper, stepper_methods )
{
    perform_integrate_const_test< Stepper > tester;
    tester( 1.005 , 0.01 );
    tester( 1.0 , 0.01 );
    tester( 1.1 , 0.01 );
    tester( -1.005 , -0.01 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_adaptive_test_case , Stepper, stepper_methods )
{
    perform_integrate_adaptive_test< Stepper > tester;
    tester( 1.005 , 0.01 );
    tester( 1.0 , 0.01 );
    tester( 1.1 , 0.01 );
    tester( -1.005 , -0.01 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_times_test_case , Stepper, stepper_methods )
{
    perform_integrate_times_test< Stepper > tester;
    tester();
    //tester( -10 , -0.01 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_n_steps_test_case , Stepper, stepper_methods )
{
    perform_integrate_n_steps_test< Stepper > tester;
    tester();
    tester( 200 , 0.01 );
    tester( 200 , 0.01 );
    tester( 200 , 0.01 );
    tester( 200 , -0.01 );
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// 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)

#include "print_values.hpp"

#include <mpllibs/metamonad/lambda_c.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/metafunction.hpp>
#include <mpllibs/metamonad/lazy_metafunction.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/lazy_protect_args.hpp>
#include <mpllibs/metamonad/if_.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/apply.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/equal_to.hpp>

#include <iostream>

using boost::mpl::int_;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::list_c;
using boost::mpl::insert_range;
using boost::mpl::end;
using boost::mpl::equal_to;

using mpllibs::metamonad::lambda_c;
using mpllibs::metamonad::lazy;
using mpllibs::metamonad::lazy_protect_args;
using mpllibs::metamonad::if_;
using mpllibs::metamonad::tmp_value;
using mpllibs::metamonad::apply;

using namespace mpllibs::metamonad::name;

/*
 * The typeclass
 */
template <class Tag>
struct addable;
// Requires:
//   struct add { template <class, class> struct apply; };

MPLLIBS_METAFUNCTION(add, (Tag)(A)(B))
((apply<typename addable<Tag>::add, A, B>));

MPLLIBS_METAFUNCTION(double_, (Tag)(X))
((apply<typename addable<Tag>::double_, X>));

template <class Tag>
struct addable_defaults
{
  typedef lambda_c<n, add<Tag, n, n> > double_;
};

/*
 * Integers
 */
typedef int_<0>::tag int_tag;

template <>
struct addable<int_tag> : addable_defaults<int_tag>
{
  struct add : tmp_value<add>
  {
    MPLLIBS_LAZY_METAFUNCTION(apply, (A)(B)) ((int_<A::value + B::value>));
  };
};

/*
 * Lists
 */
typedef list_c<int>::tag list_tag;

template <>
struct addable<list_tag> : addable_defaults<list_tag>
{
  typedef
    lambda_c<a, b,
      lazy<
        insert_range<
          lazy_protect_args<a>,
          end<lazy_protect_args<a> >,
          lazy_protect_args<b>
        >
      >
    >
    add;
};

/*
 * Generic function
 */
MPLLIBS_LAZY_METAFUNCTION(mult, (T)(N))
((
  if_<
    equal_to<int_<1>, N>,
    T,
    add<typename T::tag, mult<T, minus<N, int_<1> > >, T>
  >
));

int main()
{
  using std::cout;
  using std::endl;

  cout
    << "add<int_tag, 11, 2> = " << add<int_tag, int_<11>, int_<2> >::type::value
    << endl;

  cout
    << "double_<int_tag, 3> = " << double_<int_tag, int_<3> >::type::value
    << endl;

  cout
    << "mult<3, 4> = " << mult<int_<3>, int_<4> >::type::value
    << endl;

  cout << "add<list_tag, [1, 2], [3, 4]> = ";
  print_values<add<list_tag, list_c<int, 1, 2>, list_c<int, 3, 4> > >();
  cout << endl;

  cout << "double_<list_tag, [1, 2]> = ";
  print_values<double_<list_tag, list_c<int, 1, 2> > >();
  cout << endl;

  cout << "mult<[1, 2], 4> = ";
  print_values<mult<list_c<int, 1, 2>, int_<4> > >();
  cout << endl;
}