// ---------------------------------------------------------------------
//
// Copyright (c) 2015 - 2022 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/CIBStrategy.h"
#include "ibamr/DirectMobilitySolver.h"
#include "ibamr/StokesSpecifications.h"
#include "ibamr/ibamr_enums.h"
#include "ibamr/ibamr_utilities.h"

#include "ibtk/IBTK_MPI.h"
#include "ibtk/PETScSAMRAIVectorReal.h"
#include "ibtk/ibtk_utilities.h"

#include "CartesianGridGeometry.h"
#include "IntVector.h"
#include "PatchHierarchy.h"
#include "PatchLevel.h"
#include "SAMRAIVectorReal.h"
#include "tbox/Database.h"
#include "tbox/MathUtilities.h"
#include "tbox/PIO.h"
#include "tbox/Pointer.h"
#include "tbox/Timer.h"
#include "tbox/TimerManager.h"
#include "tbox/Utilities.h"

#include "petscmat.h"
#include "petscvec.h"
#include "petscviewer.h"
#include "petscviewertypes.h"

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

#include <algorithm>
#include <cmath>
#include <map>
#include <ostream>
#include <string>
#include <utility>
#include <vector>

#include "ibamr/app_namespaces.h" // IWYU pragma: keep

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

namespace
{
// Timers.
static Timer* t_solve_system;
static Timer* t_solve_body_system;
static Timer* t_initialize_solver_state;
static Timer* t_deallocate_solver_state;
} // namespace

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

DirectMobilitySolver::DirectMobilitySolver(std::string object_name,
                                           Pointer<Database> input_db,
                                           Pointer<CIBStrategy> cib_strategy)
    : d_object_name(std::move(object_name)), d_cib_strategy(cib_strategy)
{
    // Get from input
    if (input_db) getFromInput(input_db);

    IBAMR_DO_ONCE(t_solve_system = TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::solveSystem()");
                  t_solve_body_system =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::solveBodySystem()");
                  t_initialize_solver_state =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::initializeSolverState()");
                  t_deallocate_solver_state =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::deallocateSolverState()"););

    return;
} // DirectMobilitySolver

DirectMobilitySolver::~DirectMobilitySolver()
{
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
        Mat& body_mobility_mat = d_petsc_mat_map[mat_name].second;
        MatDestroy(&mobility_mat);
        MatDestroy(&body_mobility_mat);
    }

    for (const auto& mat_pair : d_petsc_geometric_mat_map)
    {
        Mat& geometric_mat = d_petsc_geometric_mat_map[mat_pair.first];
        MatDestroy(&geometric_mat);
    }

    d_is_initialized = false;

    return;
} // ~DirectMobilitySolver

void
DirectMobilitySolver::registerMobilityMat(const std::string& mat_name,
                                          const unsigned prototype_struct_id,
                                          MobilityMatrixType mat_type,
                                          std::pair<MobilityMatrixInverseType, MobilityMatrixInverseType> inv_type,
                                          const int managing_proc,
                                          const std::string& filename,
                                          std::pair<double, double> scale)
{
    registerMobilityMat(mat_name,
                        std::vector<unsigned int>(1, prototype_struct_id),
                        mat_type,
                        inv_type,
                        managing_proc,
                        filename,
                        scale);

    return;
} // registerMobilityMat

void
DirectMobilitySolver::registerMobilityMat(const std::string& mat_name,
                                          const std::vector<unsigned>& prototype_struct_ids,
                                          MobilityMatrixType mat_type,
                                          std::pair<MobilityMatrixInverseType, MobilityMatrixInverseType> inv_type,
                                          const int managing_proc,
                                          const std::string& filename,
                                          std::pair<double, double> scale)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(!mat_name.empty());
    for (const auto& prototype_struct_id : prototype_struct_ids)
    {
        TBOX_ASSERT(prototype_struct_id < d_cib_strategy->getNumberOfRigidStructures());
    }
    TBOX_ASSERT(d_mat_map.find(mat_name) == d_mat_map.end());
    TBOX_ASSERT(mat_type != UNKNOWN_MOBILITY_MATRIX_TYPE);
    TBOX_ASSERT(inv_type.first != UNKNOWN_MOBILITY_MATRIX_INVERSE_TYPE);
    TBOX_ASSERT(inv_type.second != UNKNOWN_MOBILITY_MATRIX_INVERSE_TYPE);
#endif

    unsigned int num_nodes = 0;
    for (const auto& prototype_struct_id : prototype_struct_ids)
    {
        num_nodes += d_cib_strategy->getNumberOfNodes(prototype_struct_id);
    }

    // Fill-in various maps.
    d_mat_prototype_id_map[mat_name] = prototype_struct_ids;
    d_mat_proc_map[mat_name] = managing_proc;
    d_mat_nodes_map[mat_name] = num_nodes;
    d_mat_parts_map[mat_name] = static_cast<unsigned>(prototype_struct_ids.size());
    d_mat_type_map[mat_name] = mat_type;
    d_mat_inv_type_map[mat_name] = inv_type;
    d_mat_filename_map[mat_name] = filename;
    d_mat_scale_map[mat_name] = scale;
    d_mat_map[mat_name] = { {}, {} };
    d_geometric_mat_map[mat_name] = {};
    d_ipiv_map[mat_name] = { {}, {} };
    d_petsc_mat_map[mat_name] = { nullptr, nullptr };
    d_petsc_geometric_mat_map[mat_name] = nullptr;

    // Allocate the actual matrices.
    const int mobility_mat_size = num_nodes * NDIM;
    const int body_mobility_mat_size = d_mat_parts_map[mat_name] * s_max_free_dofs;
    const int rank = IBTK_MPI::getRank();

    if (rank == managing_proc)
    {
        d_mat_map[mat_name].first.resize(mobility_mat_size * mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          mobility_mat_size,
                          mobility_mat_size,
                          d_mat_map[mat_name].first.data(),
                          &d_petsc_mat_map[mat_name].first);

        d_mat_map[mat_name].second.resize(body_mobility_mat_size * body_mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          body_mobility_mat_size,
                          body_mobility_mat_size,
                          d_mat_map[mat_name].second.data(),
                          &d_petsc_mat_map[mat_name].second);

        d_geometric_mat_map[mat_name].resize(mobility_mat_size * body_mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          mobility_mat_size,
                          body_mobility_mat_size,
                          d_geometric_mat_map[mat_name].data(),
                          &d_petsc_geometric_mat_map[mat_name]);

        if (d_mat_inv_type_map[mat_name].first == LAPACK_LU)
        {
            d_ipiv_map[mat_name].first.resize(mobility_mat_size);
        }
        if (d_mat_inv_type_map[mat_name].second == LAPACK_LU)
        {
            d_ipiv_map[mat_name].second.resize(body_mobility_mat_size);
        }
    }

    return;
} // registerMobilityMat

void
DirectMobilitySolver::registerStructIDsWithMobilityMat(const std::string& mat_name,
                                                       const std::vector<std::vector<unsigned> >& struct_ids)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_map.find(mat_name) != d_mat_map.end());
    for (const auto& struct_id : struct_ids)
    {
        TBOX_ASSERT(struct_id.size() == d_mat_prototype_id_map[mat_name].size());
        unsigned num_nodes = 0;
        for (const auto& id : struct_id)
        {
            TBOX_ASSERT(id < d_cib_strategy->getNumberOfRigidStructures());

            num_nodes += d_cib_strategy->getNumberOfNodes(id);
        }
        TBOX_ASSERT(num_nodes == d_mat_nodes_map[mat_name]);
    }
#endif

    d_mat_actual_id_map[mat_name] = struct_ids;

    return;
} // registerStructIDsWithMobilityMat

void
DirectMobilitySolver::setStokesSpecifications(const StokesSpecifications& stokes_spec)
{
    d_rho = stokes_spec.getRho();
    d_mu = stokes_spec.getMu();

    return;
} // setStokesSpecifications

void
DirectMobilitySolver::setSolutionTime(const double solution_time)
{
    d_solution_time = solution_time;

    return;
} // setSolutionTime

void
DirectMobilitySolver::setTimeInterval(double current_time, double new_time)
{
    d_current_time = current_time;
    d_new_time = new_time;

    return;
} // setTimeInterval

bool
DirectMobilitySolver::solveSystem(Vec x, Vec b)
{
    IBAMR_TIMER_START(t_solve_system);

    // Initialize the solver, when necessary.
    const bool deallocate_after_solve = !d_is_initialized;
    if (deallocate_after_solve) initializeSolverState(x, b);

    const int rank = IBTK_MPI::getRank();
    static const int data_depth = NDIM;

    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mat = d_petsc_mat_map[mat_name].first;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].first;
        const std::vector<std::vector<unsigned> >& struct_ids = d_mat_actual_id_map[mat_name];
        const int managing_proc = d_mat_proc_map[mat_name];
        const int mat_size = d_mat_nodes_map[mat_name] * data_depth;
        const int num_structs = static_cast<int>(struct_ids.size());

        for (int k = 0; k < num_structs; ++k)
        {
            std::vector<double> rhs;
            if (rank == managing_proc) rhs.resize(mat_size);
            d_cib_strategy->copyVecToArray(b, rhs.data(), struct_ids[k], data_depth, managing_proc);
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ true,
                                            managing_proc,
                                            data_depth);
            }
            if (rank == managing_proc) computeSolution(mat, inv_type, d_ipiv_map[mat_name].first.data(), rhs.data());
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ false,
                                            managing_proc,
                                            data_depth);
            }
            d_cib_strategy->copyArrayToVec(x, rhs.data(), struct_ids[k], data_depth, managing_proc);
        }
    }

    IBAMR_TIMER_STOP(t_solve_system);

    return true;
} // solveSystem

bool
DirectMobilitySolver::solveBodySystem(Vec x, Vec b)
{
    IBAMR_TIMER_START(t_solve_body_system);

    // Initialize the solver, when necessary.
    const bool deallocate_after_solve = !d_is_initialized;
    if (deallocate_after_solve) initializeSolverState(x, b);

    const int rank = IBTK_MPI::getRank();
    static const int data_depth = s_max_free_dofs;

    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mat = d_petsc_mat_map[mat_name].second;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].second;
        const std::vector<std::vector<unsigned> >& struct_ids = d_mat_actual_id_map[mat_name];
        const int mat_size = d_mat_parts_map[mat_name] * data_depth;
        const int managing_proc = d_mat_proc_map[mat_name];
        const int num_structs = static_cast<int>(struct_ids.size());

        for (int k = 0; k < num_structs; ++k)
        {
            std::vector<double> rhs;
            if (rank == managing_proc) rhs.resize(mat_size);
            d_cib_strategy->copyFreeDOFsVecToArray(b, rhs.data(), struct_ids[k], managing_proc);
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ true,
                                            managing_proc,
                                            data_depth);
            }
            if (rank == managing_proc) computeSolution(mat, inv_type, d_ipiv_map[mat_name].second.data(), rhs.data());
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ false,
                                            managing_proc,
                                            data_depth);
            }
            d_cib_strategy->copyFreeDOFsArrayToVec(x, rhs.data(), struct_ids[k], managing_proc);
        }
    }

    IBAMR_TIMER_STOP(t_solve_body_system);

    return true;
} // solveBodySystem

void
DirectMobilitySolver::initializeSolverState(Vec x, Vec /*b*/)
{
    if (d_is_initialized) return;

    IBAMR_TIMER_START(t_initialize_solver_state);

    int rank = IBTK_MPI::getRank();
    auto managed_mats = static_cast<unsigned>(d_mat_map.size());

    static bool recreate_mobility_matrices = true;
    static std::vector<bool> read_files(managed_mats, false);
    bool initial_time = !d_recompute_mob_mat;

    if (recreate_mobility_matrices)
    {
        // Get grid-info
        Vec* vx;
        VecNestGetSubVecs(x, nullptr, &vx);
        Pointer<SAMRAIVectorReal<NDIM, double> > vx0;
        IBTK::PETScSAMRAIVectorReal::getSAMRAIVectorRead(vx[0], &vx0);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = vx0->getPatchHierarchy();
        const int finest_ln = patch_hierarchy->getFinestLevelNumber();
        IBTK::PETScSAMRAIVectorReal::restoreSAMRAIVectorRead(vx[0], &vx0);
        Pointer<PatchLevel<NDIM> > struct_patch_level = patch_hierarchy->getPatchLevel(finest_ln);
        const IntVector<NDIM>& ratio = struct_patch_level->getRatio();
        Pointer<CartesianGridGeometry<NDIM> > grid_geom = patch_hierarchy->getGridGeometry();
        const double* dx0 = grid_geom->getDx();
        const double* X_upper = grid_geom->getXUpper();
        const double* X_lower = grid_geom->getXLower();
        double domain_extents[NDIM], dx[NDIM];
        for (int d = 0; d < NDIM; ++d)
        {
            dx[d] = dx0[d] / ratio(d);
            domain_extents[d] = X_upper[d] - X_lower[d];
        }

        int file_counter = 0;
        for (auto it = d_petsc_mat_map.begin(); it != d_petsc_mat_map.end(); ++it, ++file_counter)
        {
            const std::string& mat_name = it->first;
            Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
            Mat& geometric_mat = d_petsc_geometric_mat_map[mat_name];
            const MobilityMatrixType& mat_type = d_mat_type_map[mat_name];
            const std::vector<unsigned>& struct_ids = d_mat_prototype_id_map[mat_name];
            const std::pair<double, double>& scale = d_mat_scale_map[mat_name];
            const int managing_proc = d_mat_proc_map[mat_name];

            if (mat_type == READ_FROM_FILE && !read_files[file_counter])
            {
                // Get the matrix from file.
                const std::string& filename = d_mat_filename_map[mat_name];
                if (rank == managing_proc)
                {
                    PetscViewer binary_viewer;
                    PetscViewerBinaryOpen(PETSC_COMM_SELF, filename.c_str(), FILE_MODE_READ, &binary_viewer);
                    MatLoad(mobility_mat, binary_viewer);
                    PetscViewerDestroy(&binary_viewer);
                }

                read_files[file_counter] = true;
            }
            else
            {
                d_cib_strategy->constructMobilityMatrix(mat_name,
                                                        mat_type,
                                                        mobility_mat,
                                                        struct_ids,
                                                        dx,
                                                        domain_extents,
                                                        initial_time,
                                                        d_rho,
                                                        d_mu,
                                                        scale,
                                                        d_f_periodic_corr,
                                                        managing_proc);
            }

            // Construct the geometric matrix that maps rigid body velocity to
            // nodal velocity.
            d_cib_strategy->constructGeometricMatrix(mat_name, geometric_mat, struct_ids, initial_time, managing_proc);
        }
        factorizeMobilityMatrix();
        constructBodyMobilityMatrix();
        factorizeBodyMobilityMatrix();
    }

    d_is_initialized = true;
    recreate_mobility_matrices = d_recompute_mob_mat;

    IBAMR_TIMER_STOP(t_initialize_solver_state);

    return;
} // initializeSolverState

void
DirectMobilitySolver::deallocateSolverState()
{
    d_is_initialized = false;

    return;
} // deallocateSolverState

const std::vector<unsigned>&
DirectMobilitySolver::getPrototypeStructIDs(const std::string& mat_name)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_prototype_id_map.find(mat_name) != d_mat_prototype_id_map.end());
#endif
    return d_mat_prototype_id_map[mat_name];

} // getPrototypeStructIDs

const std::vector<std::vector<unsigned> >&
DirectMobilitySolver::getStructIDs(const std::string& mat_name)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_actual_id_map.find(mat_name) != d_mat_actual_id_map.end());
#endif
    return d_mat_actual_id_map[mat_name];

} // getStructIDs

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

void
DirectMobilitySolver::getFromInput(Pointer<Database> input_db)
{
    Pointer<Database> comp_db;
    comp_db = input_db->isDatabase("LAPACK_SVD") ? input_db->getDatabase("LAPACK_SVD") : Pointer<Database>(nullptr);
    if (comp_db)
    {
        d_svd_replace_value = comp_db->getDouble("eigenvalue_replace_value");
        d_svd_eps = comp_db->getDouble("min_eigenvalue_threshold");
    }

    // Other parameters
    d_f_periodic_corr = input_db->getDoubleWithDefault("f_periodic_correction", d_f_periodic_corr);
    d_recompute_mob_mat = input_db->getBoolWithDefault("recompute_mob_mat_perstep", d_recompute_mob_mat);

    return;
} // getFromInput

void
DirectMobilitySolver::factorizeMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        Mat& mat = d_petsc_mat_map[mat_name].first;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].first;
        const int mat_size = d_mat_nodes_map[mat_name] * NDIM;
        double* mat_data = nullptr;
        MatDenseGetArray(mat, &mat_data);
        factorizeDenseMatrix(mat_data, mat_size, inv_type, d_ipiv_map[mat_name].first.data(), mat_name, "Mobility");
        MatDenseRestoreArray(mat, &mat_data);
    }
    return;

} // factorizeMobilityMatrix

void
DirectMobilitySolver::constructBodyMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        const int row_size = d_mat_nodes_map[mat_name] * NDIM;
        const int col_size = d_mat_parts_map[mat_name] * s_max_free_dofs;
        const MobilityMatrixInverseType& mobility_inv_type = d_mat_inv_type_map[mat_name].first;

        Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
        Mat& body_mob_mat = d_petsc_mat_map[mat_name].second;
        Mat& geometric_mat = d_petsc_geometric_mat_map[mat_name];

        // Allocate a temporary matrix that holds the Matrix-Matrix product.
        // Here we are multiplying inverse of mobility matrix with geometric matrix.
        std::vector<double> product_mat_data(row_size * col_size);
        Mat product_mat;
        MatCreateSeqDense(PETSC_COMM_SELF, row_size, col_size, product_mat_data.data(), &product_mat);
        MatCopy(geometric_mat, product_mat, SAME_NONZERO_PATTERN);

        for (int col = 0; col < col_size; ++col)
        {
            double* col_data;
            MatDenseGetArray(product_mat, &col_data);
            computeSolution(
                mobility_mat, mobility_inv_type, d_ipiv_map[mat_name].first.data(), &col_data[col * row_size]);
            MatDenseRestoreArray(product_mat, &col_data);
        }
        MatTransposeMatMult(geometric_mat, product_mat, MAT_REUSE_MATRIX, PETSC_DEFAULT, &body_mob_mat);

        MatDestroy(&product_mat);
    }

    return;
} // generateBodyFrictionMatrix

void
DirectMobilitySolver::factorizeBodyMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        Mat& mat = d_petsc_mat_map[mat_name].second;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].second;
        const int mat_size = d_mat_parts_map[mat_name] * s_max_free_dofs;

        double* mat_data = nullptr;
        MatDenseGetArray(mat, &mat_data);
        factorizeDenseMatrix(
            mat_data, mat_size, inv_type, d_ipiv_map[mat_name].second.data(), mat_name, "Body Mobility");
        MatDenseRestoreArray(mat, &mat_data);
    }
    return;

} // factorizeBodyMobilityMatrix

void
DirectMobilitySolver::factorizeDenseMatrix(double* mat_data,
                                           const int mat_size,
                                           const MobilityMatrixInverseType& inv_type,
                                           int* ipiv,
                                           const std::string& mat_name,
                                           const std::string& err_msg)
{
    using MatrixType = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    // Older versions of Eigen don't make Eigen::Index publicly available
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
    using IndexType = Eigen::Index;
#else
    using IndexType = typename MatrixType::Index;
#endif
    Eigen::Map<MatrixType> mat_view(mat_data, IndexType(mat_size), IndexType(mat_size));

    if (inv_type != LAPACK_LU)
    {
        // For compatibility with the old code, we copy the lower triangle into
        // the upper triangle, even if they aren't actually equal
        for (int i = 0; i < mat_size; ++i)
        {
            for (int j = i + 1; j < mat_size; ++j)
            {
                mat_view(i, j) = mat_view(j, i);
            }
        }
    }

    if (inv_type == LAPACK_CHOLESKY)
    {
        Eigen::LLT<MatrixType> cholesky_factorization(mat_view);
        mat_view = cholesky_factorization.matrixL();
    }
    else if (inv_type == LAPACK_LU)
    {
        Eigen::PartialPivLU<MatrixType> plu_factorization(mat_view);
        mat_view = plu_factorization.matrixLU();
        const auto& indices = plu_factorization.permutationP().indices();
        TBOX_ASSERT(indices.rows() * indices.cols() == mat_size);
        std::copy_n(indices.data(), mat_size, ipiv);
    }
    else if (inv_type == LAPACK_SVD)
    {
        // Use the symmetric eigenvalue decomposition as a stand-in for the SVD.
        // In particular, since A = V D V^T, where V is the matrix of eigenvectors
        // and D is the matrix of eigenvalues, we can factorize A as
        //
        //   A = V sqrt(D) sqrt(D) V^T
        //     = V sqrt(D) (V sqrt(D))^T
        //
        // and instead store A <- V sqrt(D)
        Eigen::SelfAdjointEigenSolver<MatrixType> eigensolver(mat_view);
        Eigen::Matrix<double, Eigen::Dynamic, 1> eigenvalues = eigensolver.eigenvalues();
        const MatrixType eigenvectors = eigensolver.eigenvectors();
        // Make negative eigenvalues to be equal to min eigen value from
        // input option
        int counter = 0, counter_zero = 0;
        for (int i = 0; i < mat_size; ++i)
        {
            if (eigenvalues[i] < d_svd_eps)
            {
                eigenvalues[i] = d_svd_replace_value;
                counter++;
            }
        }
        for (int i = 0; i < mat_size; ++i)
        {
            if (IBTK::abs_equal_eps(eigenvalues[i], 0.0))
            {
                counter_zero++;
            }

            for (int j = 0; j < mat_size; ++j)
            {
                if (IBTK::abs_equal_eps(eigenvalues[j], 0.0))
                {
                    mat_view(i, j) = 0.0;
                }
                else
                {
                    mat_view(i, j) = eigenvectors(i, j) / std::sqrt(eigenvalues[j]);
                }
            }
        }

        plog << "DirectMobilityMatrix::factorizeDenseMatrix(): For " << err_msg << " matrix: " << counter
             << " eigenvalues for dense matrix with handle " << mat_name
             << " have been changed. Number of zero eigenvalues placed are " << counter_zero << std::endl;
    }
    else
    {
        TBOX_ERROR("DirectMobilityMatrix::factorizeDenseMatrix(): Unsupported dense "
                   << "matrix inversion method called for " << err_msg << std::endl);
    }

    return;
} // factorizeDenseMatrix

void
DirectMobilitySolver::computeSolution(Mat& mat, const MobilityMatrixInverseType& inv_type, int* ipiv, double* rhs)
{
    // Get pointer to matrix.
    int mat_size = 0;
    double* mat_data = nullptr;
    MatGetSize(mat, &mat_size, nullptr);
    MatDenseGetArray(mat, &mat_data);
    using MatrixType = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    // Older versions of Eigen don't make Eigen::Index publicly available
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
    using IndexType = Eigen::Index;
#else
    using IndexType = typename MatrixType::Index;
#endif
    Eigen::Map<MatrixType> mat_view(mat_data, IndexType(mat_size), IndexType(mat_size));
    using VectorType = Eigen::Matrix<double, Eigen::Dynamic, 1>;
    Eigen::Map<VectorType> rhs_view(rhs, IndexType(mat_size), IndexType(1));

    if (inv_type == LAPACK_CHOLESKY)
    {
        rhs_view = mat_view.triangularView<Eigen::Lower>().solve(rhs_view);
        rhs_view = mat_view.triangularView<Eigen::Lower>().transpose().solve(rhs_view);
    }
    else if (inv_type == LAPACK_LU)
    {
        // A    = P L U
        // A^-1 = U^-1 L^-1 P^-1
        using IndicesType = Eigen::Matrix<int, Eigen::Dynamic, 1>;
        Eigen::Map<IndicesType> indices(ipiv, mat_size);
        PermutationWrapper<decltype(indices)> permutation(indices);
        rhs_view = permutation.transpose() * rhs_view;
        rhs_view = mat_view.triangularView<Eigen::UnitLower>().solve(rhs_view);
        rhs_view = mat_view.triangularView<Eigen::Upper>().solve(rhs_view);
    }
    else if (inv_type == LAPACK_SVD)
    {
        rhs_view = mat_view * mat_view.transpose() * rhs_view;
    }
    else
    {
        TBOX_ERROR("DirectMobilitySolver::computeSolution(). Inverse method not supported." << std::endl);
    }

    MatDenseRestoreArray(mat, &mat_data);

    return;
} // computeSolution

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

} // namespace IBAMR


#include <benchmark/benchmark.h>
#include <random>
#include <complex>
#include <boost/math/fft/bsl_backend.hpp>
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/math/constants/constants.hpp>

#include "fft_test_helpers.hpp"

std::default_random_engine gen;
std::uniform_real_distribution<double> distribution;

using Real = double;
std::vector<Real> random_vec(size_t N)
{
    std::vector<Real> V(N);
    for (auto& x : V)
      x = distribution(gen);
    return V;
}

void bench_bsl(benchmark::State& state)
{
    using fft_plan = boost::math::fft::bsl_rdft<Real>;
    auto A = random_vec(state.range(0));
    fft_plan P(A.size());
    for (auto _ : state)
    {
        P.real_to_halfcomplex(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_gsl(benchmark::State& state)
{
    using fft_plan = boost::math::fft::gsl_rdft<Real>;
    auto A = random_vec(state.range(0));
    fft_plan P(A.size());
    for (auto _ : state)
    {
        P.real_to_halfcomplex(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_fftw(benchmark::State& state)
{
    using fft_plan = boost::math::fft::fftw_rdft<Real>;
    auto A = random_vec(state.range(0));
    fft_plan P(A.size());
    for (auto _ : state)
    {
        P.real_to_halfcomplex(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}

//// powers of 2
BENCHMARK(bench_bsl)
    ->RangeMultiplier(4)
    ->Range(1 << 8, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_gsl)
    ->RangeMultiplier(4)
    ->Range(1 << 8, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_fftw)
    ->RangeMultiplier(4)
    ->Range(1 << 8, 1 << 20)
    ->Complexity(benchmark::oNLogN);

// powers of 10
//BENCHMARK(bench_bsl)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_gsl)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_fftw)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);

// primes
//BENCHMARK(bench_bsl)
//    ->Arg(109)
//    ->Arg(1009)
//    ->Arg(10009)
//    ->Arg(100003)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_gsl)
//    ->Arg(109)
//    ->Arg(1009)
//    ->Arg(10009)
//    ->Arg(100003)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_fftw)
//    ->Arg(109)
//    ->Arg(1009)
//    ->Arg(10009)
//    ->Arg(100003)
//    ->Complexity(benchmark::oNLogN);

BENCHMARK_MAIN();


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// GeometricExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Conctat: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file GeometricExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades geometric products of e3ga.

#ifndef E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__
#define E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Constants.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the geometric product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something else.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1 mv2 whose grade is 2
	template<typename T>
	void geometric_2_2_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(1)*mv2.coeff(2) + mv1.coeff(2)*mv2.coeff(1);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4>, 4> geometricFunctionsContainer =
	{{
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},geometric_2_2_2<T>,{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}}
	}};

}/// End of Namespace

#endif // E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  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)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP
#define BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/gamma.hpp>

  namespace boost { namespace math { namespace detail {

  template <class T, class Policy>
  inline T hypergeometric_0F1_bessel(const T& b, const T& z, const Policy& pol)
  {
    BOOST_MATH_STD_USING

    const bool is_z_nonpositive = z <= 0;

    const T sqrt_z = is_z_nonpositive ? T(sqrt(-z)) : T(sqrt(z));
    const T bessel_mult = is_z_nonpositive ?
      boost::math::cyl_bessel_j(b - 1, 2 * sqrt_z, pol) :
      boost::math::cyl_bessel_i(b - 1, 2 * sqrt_z, pol) ;

    if (b > boost::math::max_factorial<T>::value)
    {
       const T lsqrt_z = log(sqrt_z);
       const T lsqrt_z_pow_b = (b - 1) * lsqrt_z;
       T lg = (boost::math::lgamma(b, pol) - lsqrt_z_pow_b);
       lg = exp(lg);
       return lg * bessel_mult;
    }
    else
    {
       const T sqrt_z_pow_b = pow(sqrt_z, b - 1);
       return (boost::math::tgamma(b, pol) / sqrt_z_pow_b) * bessel_mult;
    }
  }

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP


#include <cmath>
#include <cstring>
#include <iostream>
#include <GL/glew.h>
#include <GLFW/glfw3.h>
#include <boost/format.hpp>
#include "../common/util.hpp"

using namespace std;
using namespace boost;

int main(int argc, char* argv[]) {
	if (argc < 2) {
		cout << format("Usage: %1% <star>") % argv[0]<< endl;
		return EXIT_FAILURE;
	}

	const int star = atoi(argv[1]);

	if (!glfwInit()) {
		throw runtime_error("Failed to initialize GLFW");
	}

	glfwWindowHint(GLFW_SAMPLES, 4);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
	glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

	GLFWwindow* window = glfwCreateWindow(600, 600, "Hexagram", NULL, NULL);
	if (!window) {
		throw runtime_error("Failed to open GLFW window");
	}
	glfwMakeContextCurrent(window);
	glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

	glewExperimental = GL_TRUE;
	if (glewInit() != GLEW_OK) {
		throw runtime_error("Failed to initialize GLEW");
	}

	GLuint programID = buildProgram("vertex.shader", "fragment.shader");
	glUseProgram(programID);
	glClearColor(25 / 255.0f, 25 / 255.0, 25 / 255.0, 0.0f);

	GLuint vertexArrayID;
	glGenVertexArrays(1, &vertexArrayID);
	glBindVertexArray(vertexArrayID);

	GLfloat vertexData[star * 12];

	// center vertex
	GLfloat centerVertex[2];
	centerVertex[0] = 0;
	centerVertex[1] = 0;

	for (int i = 0; i < star; i++) {
		vertexData[12 * i] = cos(2 * i * M_PI / star) / 2;
		vertexData[12 * i + 1] = sin(2 * i * M_PI / star) / 2;

		vertexData[12 * i + 2] = cos(2 * (i + 1) * M_PI / star) / 2;
		vertexData[12 * i + 3] = sin(2 * (i + 1) * M_PI / star) / 2;

		memcpy(vertexData + (12 * i + 4), centerVertex, sizeof(GLfloat) * 2);
		memcpy(vertexData + (12 * i + 6), vertexData + (12 * i), sizeof(GLfloat) * 4);

		vertexData[12 * i + 10] = cos((2 * i + 1) * M_PI / star);
		vertexData[12 * i + 11] = sin((2 * i + 1) * M_PI / star);
	}

	GLfloat colorData[star * 18];

	// center color
	GLfloat centerColor[3];
	centerColor[0] = float(rand()) / RAND_MAX;
	centerColor[1] = float(rand()) / RAND_MAX;
	centerColor[2] = float(rand()) / RAND_MAX;

	/*
	 * if: most case first
	 *
	 * first star => generate 3 random color
	 * other star => generate 2 random color, copy 1 color
	 * last  star => generate 1 random color, copy 2 color
	 *
	 */
	for (int i = 0; i < star; i++) {
		if (i != 0) {
			memcpy(colorData + (18 * i), colorData + (18 * i - 6), sizeof(GLfloat) * 3);
		} else {
			colorData[0] = float(rand()) / RAND_MAX;
			colorData[1] = float(rand()) / RAND_MAX;
			colorData[2] = float(rand()) / RAND_MAX;
		}

		if (i != (star - 1)) {
			colorData[18 * i + 3] = float(rand()) / RAND_MAX;
			colorData[18 * i + 4] = float(rand()) / RAND_MAX;
			colorData[18 * i + 5] = float(rand()) / RAND_MAX;
		} else {
			memcpy(colorData + (18 * i + 3), colorData, sizeof(GLfloat) * 3);
		}

		memcpy(colorData + (18 * i + 6), centerColor, sizeof(centerColor));
		memcpy(colorData + (18 * i + 9), colorData + (18 * i), sizeof(GLfloat) * 6);

		colorData[18 * i + 15] = float(rand()) / RAND_MAX;
		colorData[18 * i + 16] = float(rand()) / RAND_MAX;
		colorData[18 * i + 17] = float(rand()) / RAND_MAX;
	}

	GLuint vertexBuffer;
	glGenBuffers(1, &vertexBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
	glBufferData(GL_ARRAY_BUFFER, sizeof(vertexData), vertexData, GL_STATIC_DRAW);
	glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 0, (void*)0);
	glEnableVertexAttribArray(0);

	GLuint colorBuffer;
	glGenBuffers(1, &colorBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glBufferData(GL_ARRAY_BUFFER, sizeof(colorData), colorData, GL_STATIC_DRAW);
	glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);
	glEnableVertexAttribArray(1);

	do {
		glClear(GL_COLOR_BUFFER_BIT);
		glDrawArrays(GL_TRIANGLES, 0, star * 12);
		glfwSwapBuffers(window);
		glfwPollEvents();
	} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS && glfwWindowShouldClose(window) == 0);

	return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

//#define BOOST_GEOMETRY_DEBUG_WITH_MAPPER
//#define BOOST_GEOMETRY_DEBUG_ASSEMBLE
//#define BOOST_GEOMETRY_DEBUG_IDENTIFIER

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/buffer.hpp>
#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <test_buffer.hpp>

#include <boost/geometry/multi/multi.hpp> // TODO: more specific
#include <boost/geometry/multi/geometries/multi_geometries.hpp>
#include <boost/geometry/extensions/algorithms/buffer/multi_buffer_inserter.hpp>

static std::string const simplex = "MULTIPOINT((5 5),(7 7))";
static std::string const three = "MULTIPOINT((5 8),(9 8),(7 11))";

// Generates error (extra polygon on top of rest) at distance 14.0:
static std::string const multipoint_a = "MULTIPOINT((39 44),(38 37),(41 29),(15 33),(58 39))";

// Just one with holes at distance ~ 15
static std::string const multipoint_b = "MULTIPOINT((5 56),(98 67),(20 7),(58 60),(10 4),(75 68),(61 68),(75 62),(92 26),(74 6),(67 54),(20 43),(63 30),(45 7))";

template <typename P>
void test_all()
{
    //std::cout << typeid(bg::coordinate_type<P>::type).name() << std::endl;

    namespace buf = bg::strategy::buffer;
    typedef bg::model::polygon<P> polygon;
    typedef bg::model::multi_point<P> multi_point_type;

	double const pi = boost::geometry::math::pi<double>();

    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("simplex1", simplex, 2.0 * pi, 1.0, 1.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("simplex2", simplex, 22.8372, 2.0, 2.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("simplex3", simplex, 44.5692, 3.0, 3.0);

    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("three1", three, 3.0 * pi, 1.0, 1.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("three2", three, 36.7592, 2.0, 2.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("three19", three, 33.6914, 1.9, 1.9);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("three21", three, 39.6394, 2.1, 2.1);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("three3", three, 65.533, 3.0, 3.0);

    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("multipoint_a", multipoint_a, 2049.98, 14.0, 14.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("multipoint_b", multipoint_b, 7109.88, 15.0, 15.0);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("multipoint_b1", multipoint_b, 6911.89, 14.7, 14.7);
    test_one<multi_point_type, buf::join_miter, buf::end_round, polygon>("multipoint_b2", multipoint_b, 7174.79, 15.1, 15.1);
}

template
<
    typename GeometryOut,
    template<typename, typename> class JoinStrategy,
    template<typename, typename> class EndStrategy,
    typename Geometry
>
double test_growth(Geometry const& geometry, int n, int d, double distance)
{
    namespace bg = boost::geometry;

    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    typedef typename bg::point_type<Geometry>::type point_type;

    typedef typename bg::ring_type<GeometryOut>::type ring_type;

	typedef typename bg::tag<Geometry>::type tag;

    // extern int point_buffer_count;
    std::ostringstream complete;
    complete
        << "point" << "_"
        << "growth" << "_"
        << string_from_type<coordinate_type>::name()
        << "_" << "r"
        << "_" << n
        << "_" << d
         // << "_" << point_buffer_count
        ;

    //std::cout << complete.str() << std::endl;

    std::ostringstream filename;
    filename << "buffer_" << complete.str() << ".svg";

    std::ofstream svg(filename.str().c_str());

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
    bg::svg_mapper<point_type> mapper(svg, 500, 500);

    {
        bg::model::box<point_type> box;
        bg::envelope(geometry, box);

        bg::buffer(box, box, distance * 1.01);
        mapper.add(box);
    }
#endif

    JoinStrategy
        <
            point_type,
            typename bg::point_type<GeometryOut>::type
        > join_strategy;
    EndStrategy
        <
            point_type,
            typename bg::point_type<GeometryOut>::type
        > end_strategy;

    typedef bg::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy_type;
    distance_strategy_type distance_strategy(distance);

    std::vector<GeometryOut> buffered;

    bg::buffer_inserter<GeometryOut>(geometry, std::back_inserter(buffered),
                        distance_strategy,
                        join_strategy,
                        end_strategy
#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
                        , mapper
#endif
                                );

    typename bg::default_area_result<GeometryOut>::type area = 0;
    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        area += bg::area(polygon);
    }

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
    // Map input geometry in green
    mapper.map(geometry, "opacity:0.5;fill:rgb(0,128,0);stroke:rgb(0,128,0);stroke-width:10");

    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        mapper.map(polygon, "opacity:0.4;fill:rgb(255,255,128);stroke:rgb(0,0,0);stroke-width:3");
    }
#endif

    return area;
}

template <typename P>
void test_growth(int n, int distance_count)
{
    srand(int(time(NULL)));
    //std::cout << typeid(bg::coordinate_type<P>::type).name() << std::endl;
    boost::timer t;

    namespace buf = bg::strategy::buffer;
    typedef bg::model::polygon<P> polygon;
    typedef bg::model::multi_point<P> multi_point_type;

    multi_point_type multi_point;
    for (int i = 0; i < n; i++)
    {
        P point(rand() % 100, rand() % 100);
        multi_point.push_back(point);
    }

    std::cout << bg::wkt(multi_point) << std::endl;

    double previous_area = 0;
    double epsilon = 0.1;
    double distance = 15.0;
    for (int d = 0; d < distance_count; d++, distance += epsilon)
    {
        double area = test_growth<polygon, buf::join_miter, buf::end_round>(multi_point, n, d, distance);
        if (area < previous_area)
        {
            std::cout << "Error: " << area << " < " << previous_area << std::endl
                << " n=" << n << " distance=" << distance
                << bg::wkt(multi_point) << std::endl;
        }
        previous_area = area;
    }
    std::cout << "n=" << n << " time=" << t.elapsed() << std::endl;
}

int test_main(int, char* [])
{
    //std::cout << std::setprecision(6);
    //test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

#ifdef BOOST_GEOMETRY_BUFFER_TEST_GROWTH
    for (int i = 5; i <= 50; i++)
    {
        test_growth<bg::model::point<double, 2, bg::cs::cartesian> >(i, 20);
    }
#endif

    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE knapsack_verification_component_test

#include <chrono>

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>

#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/fields/mnt4/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt4/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>
#include <nil/crypto3/algebra/fields/mnt6/base_field.hpp>
#include <nil/crypto3/algebra/fields/mnt6/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt6.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/mnt6.hpp>
#include <nil/crypto3/algebra/curves/edwards.hpp>
// #include <nil/crypto3/algebra/fields/edwards/base_field.hpp>
// #include <nil/crypto3/algebra/fields/edwards/scalar_field.hpp>
// #include <nil/crypto3/algebra/fields/arithmetic_params/edwards.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/edwards.hpp>
// #include <nil/crypto3/algebra/curves/params/wnaf/edwards.hpp>

#include "knapsack.hpp"
#include "../verify_r1cs_scheme.hpp"

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::zk;

BOOST_AUTO_TEST_SUITE(knapsack_component_test_suite)

BOOST_AUTO_TEST_CASE(knapsack_component_test_bls12_381_case) {
    using curve_type = curves::bls12<381>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    std::cout << "Starting Knapsack component test for BLS12-381 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    components::blueprint<scalar_field_type> bp =
        test_knapsack_crh_with_bit_out_component<scalar_field_type>();

    BOOST_CHECK(verify_component<curve_type>(bp));

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Knapsack component test for BLS12-381 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(knapsack_component_test_mnt4_case) {
    using curve_type = curves::mnt4<298>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    std::cout << "Starting Knapsack component test for MNT4-298 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    components::blueprint<scalar_field_type> bp =
        test_knapsack_crh_with_bit_out_component<scalar_field_type>();

    BOOST_CHECK(verify_component<curve_type>(bp));

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Knapsack component test for MNT4-298 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(knapsack_component_test_mnt6_case) {
    using curve_type = curves::mnt6<298>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    std::cout << "Starting Knapsack component test for MNT6-298 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    components::blueprint<scalar_field_type> bp =
        test_knapsack_crh_with_bit_out_component<scalar_field_type>();

    BOOST_CHECK(verify_component<curve_type>(bp));

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Knapsack component test for MNT6-298 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(knapsack_component_test_edwards_183_case) {
    using curve_type = curves::edwards<183>;
    using scalar_field_type = typename curve_type::scalar_field_type;

    std::cout << "Starting Knapsack component test for Edwards-183 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    components::blueprint<scalar_field_type> bp =
        test_knapsack_crh_with_bit_out_component<scalar_field_type>();

    BOOST_CHECK(verify_component<curve_type>(bp));

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Knapsack component test for Edwards-183 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef Fitting_detail_EffectiveExposuresLinearRegression_hpp
#define Fitting_detail_EffectiveExposuresLinearRegression_hpp

/** @file EffectiveExposuresLinearRegression.hpp
  * @brief
  * @author C.D. Clark III
  * @date 07/07/17
  */

#include <Eigen/Dense>
#include "../../Utils/LinearRegression.hpp"

namespace libArrhenius {

/** @class ArrheniusFit<Real,EffectiveExposuresLinearRegression>
  * @brief Implements the "effective exposure" method from "Arrhenius Model Thermal Damage Coefficients for Birefringence Loss in Rabbit Myocardium" Pearse, Raghavan, and Thomsen (2003).
  * @author C.D. Clark III
  */
template<typename Real>
class ArrheniusFit<Real,EffectiveExposuresLinearRegression> : public ArrheniusFitBase<Real>
{
  public:
    ArrheniusFit() {};
    virtual ~ArrheniusFit() {};

    typedef typename ArrheniusFitBase<Real>::Return Return;

    Return
    exec() const
    {
      Return ret;
      ArrheniusIntegral<Real> integrator;
      std::vector<Real*> const &t = this->t;
      std::vector<Real*> const &T = this->T;
      std::vector<size_t> const &N = this->N;

      // construct effective exposure parameters for each profile
      Eigen::Matrix<Real,Eigen::Dynamic,1> logteff(N.size()),invTeff(N.size());

      // Get a range for Ea to evaluate A over
      // If the caller has specified a bound use it.
      // otherwise, calculate one
      Real Ea_lb = 1;
      Real Ea_ub = 2;

      if( this->minEa )
      {
        Ea_lb = this->minEa.get();
      }

      if( this->maxEa )
      {
        Ea_ub = this->maxEa.get();
      }
      else
      {

      // If Ea is too large, then the Arrhenius integral will
      // return zero. The point at which this happens depends on the
      // data type being used. Basically, higher-precision types can
      // evaluate the integral at larger Ea's before reaching zero.
      // So, we know Ea can't be larger than the smallest value that
      // gives zero for the integral. This gives us an initial upper bound on Ea.
      for( size_t i = 0; i < N.size(); i++ )
      {
        int prec = std::numeric_limits<Real>::digits - 3;
        eps_tolerance<Real> tol( prec );
        boost::uintmax_t maxit = 100;
        Real guess = 1e2; // a place to start
        Real factor = 2;  // multiplication factor to use when searching for upper bound
        auto Ea_ub_range = bracket_and_solve_root( [&](Real Ea){
            integrator.setEa(Ea);
            integrator.setA(1);
            return integrator(N[i],t[i],T[i]);}, guess, factor, false, tol, maxit );
        // use the smallest Ea for the upper bound.
        if( i == 0 || Ea_ub_range.first < Ea_ub )
          Ea_ub = Ea_ub_range.first;
      }
      }

      // compute a set of (Ea,log(A)) pairs
      for(size_t i = 0; i < N.size(); i++)
      {

        // We'll calculate (Ea,log(A)) pairs for every half decade
        int emin = static_cast<int>(log10(Ea_lb));
        int emax = static_cast<int>(log10(Ea_ub));
        Real de = 0.1;
        int num = 1+static_cast<int>((emax - emin) / de);

        Eigen::Matrix<Real,Eigen::Dynamic,1> Eas(num),logAs(num);
        for(int j = 0; j < num; ++j)
        {
          Eas[j] = pow(10,emin + de*j);
          integrator.setEa(Eas[j]);
          integrator.setA(1);
          logAs[j] = -log( integrator(N[i], t[i], T[i]) );
        }
        auto linreg = RUC::LinearRegression(Eas,logAs);
        // linreg[0] is 'b',
        // linreg[1] is 'm' for the fit
        logteff[i] = -linreg[0];
        invTeff[i] = linreg[1]*Constants::MKS::R;
      }

      // now perform linear regression with effective parameters
      auto linreg = RUC::LinearRegression( invTeff, logteff );

      ret.A = exp(-linreg[0]);
      ret.Ea = linreg[1]*Constants::MKS::R;

      return ret;

    }

  protected:
};

}

#endif // include protector


#include <iostream>
#include <stdio.h>

#include <Eigen/Dense>

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

using namespace boost::numeric;

using std::cout;
using std::endl;

#include "MotionModels.h"
#include "EKF.h"

namespace syllo {

     EKF::EKF()
     {
     }

     EKF::EKF(const Eigen::MatrixXf &F,
	      const Eigen::MatrixXf &B,
	      const Eigen::MatrixXf &H,
	      const Eigen::MatrixXf &Q,
	      const Eigen::MatrixXf &R,
	      double dt)
     {
	  setModel(F, B, H, Q, R, dt);
     }

     int EKF::setModel(const Eigen::MatrixXf &F,
		       const Eigen::MatrixXf &B,
		       const Eigen::MatrixXf &H,
		       const Eigen::MatrixXf &Q,
		       const Eigen::MatrixXf &R,
		       double dt)
     {
	  F_ = F;
	  B_ = B;
	  H_ = H;
          Q_ = Q;
	  R_ = R;
	  eye_ = Eigen::MatrixXf::Identity(F.rows(), F.cols());
	  dt_ = dt;

	  return 0;
     }

     int EKF::init(const Eigen::MatrixXf &x0,
		   const Eigen::MatrixXf &P0)
     {
	  x_ = x0;
	  P_ = P0;
	  return 0;
     }

     int EKF::predict(const Eigen::MatrixXf &u)
     {
          x_ = F_*x_ + B_*u;
          P_ = F_*P_*F_.transpose() + Q_;

	  //runge_kutta4< syllo::state_3d_type > stepper;
	  //syllo::state_3d_type x = {x_(0,0), x_(1,0), x_(2,0)};
	  //stepper.do_step(cart_model, x , 0 , dt_ );
          //
	  //x_ << x[0], x[1], x[2];
          //
	  //////x_ = F_*x_ + B_*u;
	  ////Eigen::MatrixXf xdot;
	  ////xdot.resize(3,1);
	  ////
	  ////xdot << u(0,0)   * cos(x_(2,0)),
	  ////        u(0,0)   * sin(x_(2,0)),
   	  ////        u(0,0)/3 * tan(u(1,0));
	  ////
	  ////x_ =  x_ + xdot;
	  //
	  //// compute jacobian...
	  //Eigen::MatrixXf J;
	  //J.resize(3,3);
	  //J << 0, 0, -u(0,0)*sin(x_(2,0)),
	  //     0, 0, u(0,0)*cos(x_(2,0)),
	  //     0, 0, 0;
          //
	  ////J << 0, 0, 0,
	  ////     0, 0, 0,
	  ////     0, 0, 0;
	  //
	  ////P_ = F_*P_*F_.transpose() + Q_;
	  //P_ = J*P_*J.transpose() + Q_;
	  return 0;
     }

     int EKF::update(const Eigen::MatrixXf &z)
     {
	  K_ = P_*H_.transpose()*(H_*P_*H_.transpose() + R_).inverse();
	  x_ = x_ + K_*(z - H_*x_);
	  P_ = (eye_ - K_*H_)*P_;
	  return 0;
     }

     Eigen::MatrixXf EKF::state()
     {
	  return x_;
     }

     Eigen::MatrixXf EKF::covariance()
     {
	  return P_;
     }
}


#include <boost/math/special_functions/prime.hpp>


//==============================================================================
//         Copyright 2003 - 2013 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013 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_STATISTICS_FUNCTIONS_TESTS_KSTEST_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_TESTS_KSTEST_HPP_INCLUDED

#include <nt2/statistics/functions/kstest.hpp>
#include <nt2/core/container/dsl.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/core/container/colon/colon.hpp>

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( kstest_
                            , tag::cpu_
                            , (A0)(A1)(A2)
                            ,  ((ast_<A0, nt2::container::domain>))
                              ((unspecified_< A1 >))
                              ((scalar_< floating_<A2> >))
                              ((scalar_< floating_<A2> >))
                            )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& cdf,A2 & d, A2&p) const
    {
      //a0 is supposed to be sorted;
      uint32_t nn = numel(a0);
      A2 n =  A2(nn);
      BOOST_AUTO_TPL(fn, nt2::colon(nt2::Zero<A2>(), n)/n);
      BOOST_AUTO_TPL(ff, cdf(rowvect(a0)));
      d = globalmax(nt2::max(dist(fn(_(1, nn)), ff),
                             dist(fn(_(2, nn+1)), ff)));
      n = nt2::sqrt(n);
      p = pks((n+ A2(0.12)+A2(0.11)/n)*d);
    }
  private :
    static inline A2 pks(const A2& lam)
    {
      const A2 eps1 = A2(0.001);
      const A2 eps2 = nt2::Sqrteps<A2>();
      A2 fac = Two<A2>();
      A2 a2 =  -fac*nt2::sqr(lam);
      A2 r = nt2::Zero<A2>();
      A2 termbf =   nt2::Zero<A2>();
      for(uint32_t i=1; i < 100u; ++i)
      {
        A2 term = fac*nt2::exp(a2*nt2::sqr(i));
        r+= term;
        if ((nt2::abs(term) <= eps1*termbf)||(nt2::abs(term) < eps2*r))
          return r;
        fac*= nt2::Mone<A2>();
        termbf =  nt2::abs(term);
      }
      return nt2::One<A2>();
    }

  };

  BOOST_DISPATCH_IMPLEMENT  ( kstest_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((unspecified_< A1 >))
                            )
  {
    typedef typename nt2::meta::scalar_of<A0>::type r_t;
    typedef std::pair<r_t, r_t>                      result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& cdf) const
    {
      r_t first, second;
      kstest(a0, cdf, first, second);
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( kstest_, tag::cpu_
                            , (A0)(A1)(A2)
                            , ((ast_<A0, nt2::container::domain>))
                              ((unspecified_<A1 >))
                              ((scalar_< floating_<A2> >))
                            )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A1 & cdf,A2 & a3) const
    {
      A2 a2;
      kstest(a0, cdf, a2, a3);
      return a2;
    }
  };

} }

#endif


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(convert_test)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;
  typedef MT::value_traits       VT;
  typedef MT::real_type          T;

  {
    V                   const center = V::zero();
    T                   const radius = VT::one();
    geometry::Sphere<V> const sphere = geometry::make_sphere(center, radius);
    geometry::DOP<T,6> const dop     = geometry::convert<6,V>(sphere);

    BOOST_CHECK_EQUAL(  dop.size(), 6u);
    BOOST_CHECK_CLOSE(  dop(0).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(1).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(2).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(0).upper(),  VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(1).upper(),  VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(2).upper(),  VT::one(), 0.01 );

  }

  {
    V                  const min_coord = V::make( -VT::one(), -VT::one(), -VT::one() );
    V                  const max_coord = V::make(  VT::one(),  VT::one(),  VT::one() );
    geometry::AABB<V>  const aabb      = geometry::make_aabb(min_coord, max_coord);
    geometry::DOP<T,6> const dop       = geometry::convert<6,V>(aabb);

    BOOST_CHECK_EQUAL(  dop.size(), 6u);
    BOOST_CHECK_CLOSE(  dop(0).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(1).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(2).lower(), -VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(0).upper(),  VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(1).upper(),  VT::one(), 0.01 );
    BOOST_CHECK_CLOSE(  dop(2).upper(),  VT::one(), 0.01 );

  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <fstream>
#include <complex>
#include <string>
#include <boost/multi_array.hpp>

using std::string;
using std::complex;
using std::cout;
using std::endl;

typedef boost::multi_array<double,2> rarray;
typedef boost::multi_array<complex<double>,3> harray;
//using rarray = boost::multi_array<double,2>; // for C++11 or later
//using harray = boost::multi_array<complex<double>,3>; //for C++11 or later

int input_hamiltonian(string &fname, rarray &rvec, harray &hop, const int &flag){
  const int no=hop.size(), nr=rvec.size();
  string fname_tmp,str;
  switch(flag){
  case 1:{
    fname_tmp=fname;
    break;
  }
  case 2:{
    fname_tmp=fname+"/irvec.txt";
    break;
  }
  case 3:{
    fname_tmp=fname+"_hr.dat";
    break;
  }
  case 4:{
    fname_tmp="Hopping.dat";
  }
  }
  std::ifstream rfile(fname_tmp.c_str(),std::ios::in);
  if(rfile.fail()){
    cout<<"fail input file"<<endl;
    return -1;
  }

  switch(flag){
  case 1:{
    for(int i=0; i<no; i++){
      for(int j=0; j<no;j++){
	for(int k=0;k<nr;k++){
	  getline(rfile,str);
	  sscanf(str.c_str(),"%lf %lf %lf %lf %lf",
		      &rvec[k][0], &rvec[k][1], &rvec[k][2],
		      &hop[j][i][k].real(), &hop[j][i][k].imag());
	}
      }
    }
    break;
  }
  case 2:{
    std::ifstream hfile((fname+"/ham_r.txt").c_str(),std::ios::in);
    if(hfile.fail()){
      cout<<"fail input file"<<endl;
      return -1;
    }
    for(int i=0; i<nr; i++){
      getline(rfile,str);
      sscanf(str.c_str(),"%lf %lf %lf",&rvec[i][0], &rvec[i][1], &rvec[i][2]);
      for(int j=0; j<no;j++){
	for(int k=0;k<no;k++){
          getline(hfile,str);
          sscanf(str.c_str(),"(%lf, %lf)",&hop[k][j][i].real(), &hop[k][j][i].imag());
	}
      }
    }
    break;
  }
  case 3:{
    break;
  }
  case 4:{
    int l,m;
    double tmp[3];
    for(int i=0;i<7+no;i++){
      getline(rfile,str);
    }
    for(int i=0;i<nr;i++){
      for(int j=0;j<no;j++){
	for(int k=0;k<no;k++){
	  getline(rfile,str);
	  sscanf(str.c_str(),"%lf %lf %lf %lf %lf %lf %ld %ld %lf %lf",
		 &rvec[i][0], &rvec[i][1], &rvec[i][2],
		 &tmp[0], &tmp[1], &tmp[2], &l, &m,
		 &hop[k][j][i].real(), &hop[k][j][i].imag());
	}
      }
    }
    break;
  }
  }
  return 0;
}

int main(){
  static const int no=1,nr=4,flag=1;

  int err;
  string fname="ham.dat";
  rarray rvec(boost::extents[nr][3]);
  harray hop(boost::extents[no][no][nr]);

  err=input_hamiltonian(fname,rvec,hop,flag);
  for(int i=0; i<nr; i++){
    for(int j=0;j<3; j++){
      cout<<rvec[i][j]<<" ";
    }
    for(int j=0; j<no;j++){
      for(int k=0;k<no;k++){
	cout<<hop[k][j][i];
      }
    }
    cout<<endl;
  }
  return 0;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINCOSPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINCOSPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object computes simultaneously and
    at lower cost the sine and cosine of the input in \f$\pi\f$ multiples.

    @par Header <boost/simd/function/sincospi.hpp>

    @see sincosd, sincos

    @par Example:

      @snippet sincospi.cpp sincospi

    @par Possible output:

      @snippet sincospi.txt sincospi

  **/
  std::pair<IEEEValue, IEEEValue> sincospi(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sincospi.hpp>
#include <boost/simd/function/simd/sincospi.hpp>

#endif


// Copyright (c) 2019 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <franka/lowpass_filter.h>

namespace franka {

double lowpassFilter(double sample_time, double y, double y_last, double cutoff_frequency) {
  if (sample_time < 0 || !std::isfinite(sample_time)) {
    throw std::invalid_argument("lowpass-filter: sample_time is negative, infinite or NaN.");
  }
  if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) {
    throw std::invalid_argument(
        "lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN.");
  }
  if (!std::isfinite(y) || !std::isfinite(y_last)) {
    throw std::invalid_argument(
        "lowpass-filter: current or past input value of the signal to be filtered is infinite or "
        "NaN.");
  }
  double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency)));
  return gain * y + (1 - gain) * y_last;
}

std::array<double, 16> cartesianLowpassFilter(double sample_time,
                                              std::array<double, 16> y,
                                              std::array<double, 16> y_last,
                                              double cutoff_frequency) {
  if (sample_time < 0 || !std::isfinite(sample_time)) {
    throw std::invalid_argument(
        "Cartesian lowpass-filter: sample_time is negative, infinite or NaN.");
  }
  if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) {
    throw std::invalid_argument(
        "Cartesian lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN.");
  }
  for (size_t i = 0; i < y.size(); i++) {
    if (!std::isfinite(y[i]) || !std::isfinite(y_last[i])) {
      throw std::invalid_argument(
          "Cartesian lowpass-filter: current or past input value of the signal to be filtered is "
          "infinite or NaN.");
    }
  }
  Eigen::Affine3d transform(Eigen::Matrix4d::Map(y.data()));
  Eigen::Affine3d transform_last(Eigen::Matrix4d::Map(y_last.data()));
  Eigen::Quaterniond orientation(transform.linear());
  Eigen::Quaterniond orientation_last(transform_last.linear());

  double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency)));
  transform.translation() =
      gain * transform.translation() + (1.0 - gain) * transform_last.translation();
  orientation = orientation_last.slerp(gain, orientation);

  transform.linear() << orientation.normalized().toRotationMatrix();
  std::array<double, 16> filtered_values{};
  Eigen::Map<Eigen::Matrix4d>(&filtered_values[0], 4, 4) = transform.matrix();

  return filtered_values;
}
}  // namespace franka


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REMQUO_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REMQUO_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing remquo capabilities

    Remainder and bits of quotient

    remquo computes the remainder (rem) and a part of the quotient (quo) upon division of
    @c x by @c y. By design, the value of the remainder is the same as that
    computed by the @ref remainder function. The value of the computed quotient has
    the sign of @c x/y and agrees with the actual quotient in at least the low
    order 3 bits.

    @par semantic:

    For any given value @c x, @c y of type @c T:

    @code
    as_integer_t<T>& quo;
    T rem = remquo(x, y, quo);
    @endcode

    or

    @code
    std::pair< T, as_integer_t<T> > p = remquo(x, y);
    @endcode

    computes the two values.

    @par Note

      - This function mimics a standard C library one that was mainly written in its time to
      help computation of periodic functions: three bits of quo allowing to know the
      'octant'

      - This implementation differs from std::remquo as the quotient is not returned as a pointer, and
      his type is not int the the signed integer type associated to the floating one, to allow
      proper SIMD implementation.

      - also note that the double implementation of std::remquo is flawed in GNU C
      Library until version 2.21 (2.22 been corrected).
  **/
  IntegerValue remquo(Value const & v0, Value const& y, IntegerValue const& quo);
} }
#endif

#include <boost/simd/function/scalar/remquo.hpp>
#include <boost/simd/function/simd/remquo.hpp>

#endif


#include <vector>

#include "ClothoidList.hh"
#include <boost/python.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

using d_vector = std::vector<double>;

d_vector extendClothoid(double x0, double y0, double th0, double k0, double dk, double L){
  G2lib::ClothoidCurve clothoid(x0, y0, th0, k0, dk, L);
  d_vector ret;
  ret.emplace_back(clothoid.xEnd());
  ret.emplace_back(clothoid.yEnd());
  ret.emplace_back(clothoid.thetaEnd());
  ret.emplace_back(k0 + dk * L);

  return ret;
}

d_vector g2Fitting(double x0, double y0, double th0, double k0, double x1, double y1, double th1, double k1){
  G2lib::G2solve3arc g2solve3arc;

  int iter = g2solve3arc.build(x0, y0, th0, k0, x1, y1, th1, k1);

  const G2lib::ClothoidCurve &first = g2solve3arc.getS0();
  const G2lib::ClothoidCurve &second = g2solve3arc.getSM();
  const G2lib::ClothoidCurve &third = g2solve3arc.getS1();

  d_vector ret;
  // k1, k2, k3
  ret.emplace_back(first.dkappa());
  ret.emplace_back(second.dkappa());
  ret.emplace_back(third.dkappa());
  ret.emplace_back(first.length());
  ret.emplace_back(second.length());
  ret.emplace_back(third.length());

  return ret;
}

d_vector pointsOnClothoid(double x0, double y0, double th0, double k0, double dk, double L, char XY, double tick){
  d_vector arc;
  G2lib::ClothoidCurve clothoid(x0, y0, th0, k0, dk, L);

  int npts = L / tick;
  clothoid.optimized_sample_ISO(0 /*offset*/,
                                npts /*number of points*/,
                                M_PI / 45 /*max angle*/,
                                arc);
  d_vector ret;
  for(const auto s : arc){
    if(XY == 'X') ret.emplace_back(clothoid.X(s));
    else          ret.emplace_back(clothoid.Y(s));
  }

  return ret;
}

BOOST_PYTHON_MODULE(PyClothoids){
  using namespace boost::python;
  def("extendClothoid", &extendClothoid);
  def("g2Fitting", &g2Fitting);
  def("pointsOnClothoid", &pointsOnClothoid);

  class_<::d_vector>("d_vector")
      .def(vector_indexing_suite<::d_vector>());
}


#include <mtl/dense1D.h>
#include <mtl/mtl.h>
#include <mtl/utils.h>

/*
  example output:

  [6,6,6,6,6,6,6,6,6,6]

  */

int
main()
{
  using namespace mtl;
  //begin
  dense1D<double> x(10,4), y(10,2), z(10);
  ele_div(scaled(x,3), y, z);
  //end
  print_vector(z);
  return 0;
}


#pragma once
#include <algorithm>
#include <varalgo/std_variant_traits.hpp>

#include <boost/range.hpp>
#include <boost/range/detail/range_return.hpp>

namespace varalgo
{
	/************************************************************************/
	/*                     includes                                         */
	/************************************************************************/
	template <class InputIterator1, class InputIterator2, class Pred>
	inline bool includes(InputIterator1 first1, InputIterator1 last1,
	                     InputIterator2 first2, InputIterator2 last2,
	                     Pred && pred)
	{
		auto alg = [&first1, &last1, &first2, &last2](auto && pred)
		{
			return std::includes(first1, last1, first2, last2, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange1, class SinglePassRange2, class Pred>
	inline bool includes(const SinglePassRange1 & rng1, const SinglePassRange2 & rng2, Pred && pred)
	{
		return varalgo::includes(
			boost::begin(rng1), boost::end(rng1),
			boost::begin(rng2), boost::end(rng2),
			std::forward<Pred>(pred));
	}

	/************************************************************************/
	/*                       set_difference                                 */
	/************************************************************************/
	template <class InputIterator1, class InputIterator2, class OutputIterator, class Pred>
	inline OutputIterator set_difference(InputIterator1 first1, InputIterator1 last1,
	                                     InputIterator2 first2, InputIterator2 last2,
	                                     OutputIterator out, Pred && pred)
	{
		auto alg = [&first1, &last1, &first2, &last2, &out](auto && pred)
		{
			return std::set_difference(first1, last1, first2, last2, out, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange1, class SinglePassRange2, class OutputIterator, class Pred>
	inline bool set_difference(const SinglePassRange1 & rng1,
	                           const SinglePassRange2 & rng2,
	                           OutputIterator out, Pred && pred)
	{
		return varalgo::set_difference(
			boost::begin(rng1), boost::end(rng1),
			boost::begin(rng2), boost::end(rng2),
			out, std::forward<Pred>(pred));
	}

	/************************************************************************/
	/*                       set_intersection                               */
	/************************************************************************/
	template <class InputIterator1, class InputIterator2, class OutputIterator, class Pred>
	inline OutputIterator set_intersection(InputIterator1 first1, InputIterator1 last1,
	                                       InputIterator2 first2, InputIterator2 last2,
	                                       OutputIterator out, Pred && pred)
	{
		auto alg = [&first1, &last1, &first2, &last2, &out](auto && pred)
		{
			return std::set_intersection(first1, last1, first2, last2, out, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange1, class SinglePassRange2, class OutputIterator, class Pred>
	inline bool set_intersection(const SinglePassRange1 & rng1,
	                             const SinglePassRange2 & rng2,
	                             OutputIterator out, Pred && pred)
	{
		return varalgo::set_intersection(
			boost::begin(rng1), boost::end(rng1),
			boost::begin(rng2), boost::end(rng2),
			out, std::forward<Pred>(pred));
	}

	/************************************************************************/
	/*                       set_symmetric_difference                       */
	/************************************************************************/
	template <class InputIterator1, class InputIterator2, class OutputIterator, class Pred>
	inline OutputIterator set_symmetric_difference(InputIterator1 first1, InputIterator1 last1,
	                                               InputIterator2 first2, InputIterator2 last2,
	                                               OutputIterator out, Pred && pred)
	{
		auto alg = [&first1, &last1, &first2, &last2, &out](auto && pred)
		{
			return std::set_symmetric_difference(first1, last1, first2, last2, out, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange1, class SinglePassRange2, class OutputIterator, class Pred>
	inline bool set_symmetric_difference(const SinglePassRange1 & rng1,
	                                     const SinglePassRange2 & rng2,
	                                     OutputIterator out, Pred && pred)
	{
		return varalgo::set_symmetric_difference(
			boost::begin(rng1), boost::end(rng1),
			boost::begin(rng2), boost::end(rng2),
			out, std::forward<Pred>(pred));
	}

	/************************************************************************/
	/*                       set_union                                      */
	/************************************************************************/
	template <class InputIterator1, class InputIterator2, class OutputIterator, class Pred>
	inline OutputIterator set_union(InputIterator1 first1, InputIterator1 last1,
	                                InputIterator2 first2, InputIterator2 last2,
	                                OutputIterator out, Pred && pred)
	{
		auto alg = [&first1, &last1, &first2, &last2, &out](auto && pred)
		{
			return std::set_union(first1, last1, first2, last2, out, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange1, class SinglePassRange2, class OutputIterator, class Pred>
	inline bool set_union(const SinglePassRange1 & rng1,
	                      const SinglePassRange2 & rng2,
	                      OutputIterator out, Pred && pred)
	{
		return varalgo::set_union(
			boost::begin(rng1), boost::end(rng1),
			boost::begin(rng2), boost::end(rng2),
			out, std::forward<Pred>(pred));
	}

}


//          Copyright Rein Halbersma 2010-2020.
// 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 <dctl/core/board/angle.hpp>    // angle, _deg
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE

using namespace dctl::core;
using namespace literals;

BOOST_AUTO_TEST_SUITE(AngleLiterals)

BOOST_AUTO_TEST_CASE(DegreesLiteralIsAngleObject)
{
        static_assert(  0_deg == angle{  0});
        static_assert( 45_deg == angle{ 45});
        static_assert( 90_deg == angle{ 90});
        static_assert(135_deg == angle{135});
        static_assert(180_deg == angle{180});
        static_assert(225_deg == angle{225});
        static_assert(270_deg == angle{270});
        static_assert(315_deg == angle{315});
        static_assert(360_deg == angle{360});
}

BOOST_AUTO_TEST_CASE(UnaryPlusAppliesToAngleObject)
{
        static_assert(  +0_deg == +angle{  0});
        static_assert( +45_deg == +angle{ 45});
        static_assert( +90_deg == +angle{ 90});
        static_assert(+135_deg == +angle{135});
        static_assert(+180_deg == +angle{180});
        static_assert(+225_deg == +angle{225});
        static_assert(+270_deg == +angle{270});
        static_assert(+315_deg == +angle{315});
        static_assert(+360_deg == +angle{360});
}

BOOST_AUTO_TEST_CASE(UnaryMinusAppliesToAngleObject)
{
        static_assert(  -0_deg == -angle{  0});
        static_assert( -45_deg == -angle{ 45});
        static_assert( -90_deg == -angle{ 90});
        static_assert(-135_deg == -angle{135});
        static_assert(-180_deg == -angle{180});
        static_assert(-225_deg == -angle{225});
        static_assert(-270_deg == -angle{270});
        static_assert(-315_deg == -angle{315});
        static_assert(-360_deg == -angle{360});
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <memory>
#include <boost/math/tools/roots.hpp>

#include "Config.hpp"
#include "MirrorPlasma.hpp"
#include "BatchRunner.hpp"

extern "C" {
#include <fenv.h>
}

int main( int argc, char** argv )
{
	std::string fname( "Mirror.conf" );
	if ( argc == 2 )
		fname = argv[ 1 ];
	if ( argc > 2 )
	{
		std::cerr << "Usage: MCTrans++ ConfigFile.conf" << std::endl;
		return 1;
	}

#if defined(DEBUG) && defined(FPEXCEPT)
	std::cerr << "Floating Point Exceptions Enabled" << std::endl;
	::feenableexcept( FE_INVALID | FE_DIVBYZERO | FE_OVERFLOW );
#endif

	BatchRunner runner(fname);
	runner.runBatchSolve();
	return 0;

}

std::shared_ptr<MirrorPlasma> MCTransConfig::Solve()
{
	switch ( Type ) {
		case SolveType::SteadyStateMachSolve:
			doMachSolve( *ReferencePlasmaState );
			break;
		case SolveType::SteadyStateTempSolve:
			doTempSolve( *ReferencePlasmaState );
			break;
		default:
			throw std::invalid_argument( "Unknown Solve Type" );
	}

	return std::move( ReferencePlasmaState );
}

void MCTransConfig::doMachSolve( MirrorPlasma& plasma ) const
{
	// NB This uses power densities in W/m^3
	auto PowerBalance = [ &plasma ]( double M ) {
		plasma.MachNumber = M;
		plasma.ComputeSteadyStateNeutrals();

		double HeatLoss = plasma.IonHeatLosses() + plasma.ElectronHeatLosses();
		double Heating = plasma.IonHeating() + plasma.ElectronHeating();

		return Heating - HeatLoss;
	};

	boost::uintmax_t iters = 1000;
	boost::math::tools::eps_tolerance<double> tol( 11 ); // only bother getting part in 1024 accuracy
	double InitialMach = plasma.initialMach(); // Usually M > 4 for these solutions
	double Factor = 1.25;
	bool rising = true; // Confinement gets uniformly better for increasing M, and Viscous heating increases with M
	auto M_bounds = boost::math::tools::bracket_and_solve_root( PowerBalance, InitialMach, Factor, rising, tol, iters );
	auto M_lower = M_bounds.first, M_upper = M_bounds.second;
	plasma.MachNumber = ( M_lower + M_upper )/2.0;
	plasma.ComputeSteadyStateNeutrals();
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <boost/variant.hpp>
#include <boost/lexical_cast.hpp>
#include <CGAL/boost/graph/dijkstra_shortest_paths.h>

typedef CGAL::Simple_cartesian<double>      Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3> Triangle_mesh;
typedef boost::graph_traits<Triangle_mesh>  Graph_traits;
typedef Graph_traits::vertex_descriptor     vertex_descriptor;

typedef std::vector<vertex_descriptor>                  VertexDescriptorList;
typedef std::map<vertex_descriptor, int>                VertexIndexMap;
typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;

typedef boost::iterator_property_map<VertexDescriptorList::iterator, VertexIdPropertyMap> PredecessorMap;
typedef boost::iterator_property_map<std::vector<double>::iterator, VertexIdPropertyMap>  DistanceMap;

int main(int argc, char** argv)
{
    if (argc < 3) {
        std::cerr << "ERROR: need to specify .off and selection files" << std::endl;
        return 1;
    }

    // read mesh
    Triangle_mesh tmesh;
    std::ifstream input(argv[1]);
    input >> tmesh;
    input.close();

    VertexIndexMap vertex_id_map;
    VertexIdPropertyMap vertex_index_pmap(vertex_id_map);
    int index = 0;
    for(vertex_descriptor vd : vertices(tmesh)) {
        vertex_id_map[vd] = index++;
    }

    input.open(argv[2]);

    std::ofstream out(std::string(argv[1])+".selection.txt");
    out << std::endl << std::endl;

    while (!input.eof()) {

        int start, end;
        input >> start >> end;

        vertex_descriptor vstart(start);
        vertex_descriptor vend(end);

        // We first declare a vector
        std::vector<vertex_descriptor> predecessor(num_vertices(tmesh));
        // and then turn it into a property map
        std::vector<double> distance(num_vertices(tmesh));
        PredecessorMap predecessor_pmap(predecessor.begin(), vertex_index_pmap);
        DistanceMap distance_pmap(distance.begin(), vertex_index_pmap);

        boost::dijkstra_shortest_paths(tmesh, vstart,
                                 distance_map(distance_pmap)
                                 .predecessor_map(predecessor_pmap)
                                 .vertex_index_map(vertex_index_pmap));

        vertex_descriptor it = vend;

        out << vertex_id_map[it] << " ";
        it = boost::get(predecessor_pmap, it);

        while (it != vstart) {
            out << vertex_id_map[it] << " " << vertex_id_map[it] << " ";
            it = boost::get(predecessor_pmap, it);
        }
        out << vertex_id_map[it] << " ";

    }
    input.close();

    return 0;
}


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/core/lightweight_test.hpp>
#include <boost/utility.hpp>
#include <boost/dynamic_bitset.hpp>
using namespace boost;

//////////////////////////////////////////
void case1()
{
    dynamic_bitset<> db1;
    dynamic_bitset<> db2(10);
    dynamic_bitset<> db3(0x16,
    BOOST_BINARY(10101));       //注意这里
    dynamic_bitset<> db4(string("0100"));
    dynamic_bitset<> db5(db3);

    dynamic_bitset<> db6;
    db6 = db4;

    cout << hex << db5.to_ulong() << endl;
    cout << db4[0] << db4[1] << db4[2] << endl;

}

//////////////////////////////////////////
void case2()
{
    dynamic_bitset<> db;

    db.resize(10, true);
    cout << db << endl;

    db.resize(5);
    cout << db << endl;

    {
        dynamic_bitset<> db(5,BOOST_BINARY(01110));

        cout << db << endl;
        assert(db.size() == 5);

        db.clear();
        assert(db.empty()&& db.size()==0);

    }

    assert(dynamic_bitset<>(64).num_blocks()==1);
    assert(dynamic_bitset<>(65).num_blocks()==2);

    {
        dynamic_bitset<> db(5,BOOST_BINARY(01001));
        db.push_back(true);
        assert(db.to_ulong() == BOOST_BINARY_UL(101001));

    }

    {
        dynamic_bitset<> db(5,BOOST_BINARY(01001));
        db.append(BOOST_BINARY(101));
        assert(db.size() == sizeof(unsigned long)*8 + 5);
        cout << db << endl;             //0000000000000000000000000000010101001

    }
}

//////////////////////////////////////////
void case3()
{
    dynamic_bitset<> db1(4, BOOST_BINARY(1010));

    db1[0] &= 1;
    db1[1] ^= 1;
    cout << db1 << endl;

    dynamic_bitset<> db2(4, BOOST_BINARY(0101));
    assert(db1 > db2);

    cout << (db1 ^ db2) << endl;
    cout << (db1 | db2) << endl;

}

//////////////////////////////////////////
void case4()
{
    dynamic_bitset<> db(4, BOOST_BINARY(0101));

    assert(db.test(0) && !db.test(1));
    assert(db.any() && !db.none());
    assert(db.count() == 2);

    {
        dynamic_bitset<> db(4, BOOST_BINARY(0101));

        db.flip();
        assert(db.to_ulong() == BOOST_BINARY(1010));

        db.set();
        assert(!db.none());

        db.reset();
        assert(!db.any() );

        db.set(1, 1);
        assert(db.count() == 1);

    }

    {
        dynamic_bitset<> db(5, BOOST_BINARY(00101));

        auto pos = db.find_first();
        assert(pos == 0);

        pos = db.find_next(pos);
        assert(pos == 2);

    }
}

//////////////////////////////////////////
void case5()
{
    dynamic_bitset<> db(10, BOOST_BINARY(1010101));
    cout << db.to_ulong() << endl;      //85

    db.append(10);
    cout << db.to_ulong() << endl;

    db.push_back(1);
    //cout << db.to_ulong() << endl;
    BOOST_TEST_THROWS(db.to_ulong(), std::overflow_error);

    string str;
    to_string(db, str);
    cout << str << endl;

    dump_to_string(db , str);
    cout << str << endl;
}

//////////////////////////////////////////
void case6()
{
    dynamic_bitset<> db1(5, BOOST_BINARY(10101));
    dynamic_bitset<> db2(5, BOOST_BINARY(10010));

    cout << (db1 | db2) << endl;
    cout << (db1 & db2) << endl;
    cout << (db1 - db2) << endl;

    dynamic_bitset<> db3(5, BOOST_BINARY(101));
    assert(db3.is_proper_subset_of(db1));

    dynamic_bitset<> db4(db2);
    assert(db4.is_subset_of(db2));
    assert(!db4.is_proper_subset_of(db2));
}

//////////////////////////////////////////
void func(int n)
{
    //cout << "test " << n << endl;

    dynamic_bitset<> db(n);
    db.set();
    //cout << db.size() << endl;

    for (dynamic_bitset<>::size_type i = db.find_next(1);
            i != dynamic_bitset<>::npos ;
            i = db.find_next(i ) )
    {
        for (dynamic_bitset<>::size_type j = db.find_next(i);
                j != dynamic_bitset<>::npos ;
                j = db.find_next(j ))
        {
            if ( j % i == 0)
            {
                db[j] = 0;
            }
        }
    }

    cout << dec ;
    for (dynamic_bitset<>::size_type i = db.find_next(2);
            i != dynamic_bitset<>::npos ;
            i = db.find_next(i) )
    {
        cout << i << ", ";
    }

}

void case7()
{
    func(10);
    func(50);
}

int main()
{
    case1();
    case2();
    case3();
    case4();
    case5();
    case6();
    case7();
}


#define PY_ARRAY_UNIQUE_SYMBOL pbcvt_ARRAY_API

#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>

namespace pbcvt {

    using namespace boost::python;

/**
 * @brief Example function. Basic inner matrix product using explicit matrix conversion.
 * @param left left-hand matrix operand (NdArray required)
 * @param right right-hand matrix operand (NdArray required)
 * @return an NdArray representing the dot-product of the left and right operands
 */
    PyObject *dot(PyObject *left, PyObject *right) {

        cv::Mat leftMat, rightMat;
        leftMat = pbcvt::fromNDArrayToMat(left);
        rightMat = pbcvt::fromNDArrayToMat(right);
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        // Check that the 2-D matrices can be legally multiplied.
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;
        PyObject *ret = pbcvt::fromMatToNDArray(result);
        return ret;
    }
/**
 * @brief Example function. Simply makes a new CV_16UC3 matrix and returns it as a numpy array.
 * @return The resulting numpy array.
 */

	PyObject* makeCV_16UC3Matrix(){
		cv::Mat image = cv::Mat::zeros(240,320, CV_16UC3);
		PyObject* py_image = pbcvt::fromMatToNDArray(image);
		return py_image;
	}

//
/**
 * @brief Example function. Basic inner matrix product using implicit matrix conversion.
 * @details This example uses Mat directly, but we won't need to worry about the conversion in the body of the function.
 * @param leftMat left-hand matrix operand
 * @param rightMat right-hand matrix operand
 * @return an NdArray representing the dot-product of the left and right operands
 */
    cv::Mat dot2(cv::Mat leftMat, cv::Mat rightMat) {
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;

        return result;
    }

    /**
     * \brief Example function. Increments all elements of the given matrix by one.
     * @details This example uses Mat directly, but we won't need to worry about the conversion anywhere at all,
     * it is handled automatically by boost.
     * \param matrix (numpy array) to increment
     * \return
     */
    cv::Mat increment_elements_by_one(cv::Mat matrix){
        matrix += 1.0;
        return matrix;
    }

#if (PY_VERSION_HEX >= 0x03000000)
#ifndef NUMPY_IMPORT_ARRAY_RETVAL
#define NUMPY_IMPORT_ARRAY_RETVAL NULL
#endif
    static void* init_ar() {
#else
#ifndef NUMPY_IMPORT_ARRAY_RETVAL
#define NUMPY_IMPORT_ARRAY_RETVAL
#endif
    static void init_ar(){
#endif
        Py_Initialize();

        import_array();
        return NUMPY_IMPORT_ARRAY_RETVAL;
    }

    BOOST_PYTHON_MODULE (pbcvt) {
        //using namespace XM;
        init_ar();

        //initialize converters
        to_python_converter<cv::Mat,pbcvt::matToNDArrayBoostConverter>();
        matFromNDArrayBoostConverter();

        //expose module-level functions
        def("dot", dot);
        def("dot2", dot2);
		def("makeCV_16UC3Matrix", makeCV_16UC3Matrix);

		//from PEP8 (https://www.python.org/dev/peps/pep-0008/?#prescriptive-naming-conventions)
        //"Function names should be lowercase, with words separated by underscores as necessary to improve readability."
        def("increment_elements_by_one", increment_elements_by_one);
    }

} //end namespace pbcvt