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

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// other samrai stuff
#include <HierarchyDataOpsManager.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/equation_systems.h>
#include <libmesh/linear_partitioner.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>
#include <libmesh/periodic_boundary.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/LEInteractor.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// test stuff
#include "../tests.h"

// This file is the main driver for force spreading tests (i.e.,
// IBFEmethod::spreadForce). At the moment it simply prints out the force
// values.

// Coordinate mapping function.
void
coordinate_mapping_function(libMesh::Point& X, const libMesh::Point& s, void* /*ctx*/)
{
    // We have to be careful with how we pick these offsets since these tests
    // validate spreading by comparing pointwise values. In particular: with
    // an odd-order quadrature rule and a quad or hex mesh the cell midpoint
    // is a quadrature point. Due to roundoff this could work out to be, e.g.,
    // 0.5 +/- eps: this is problematic since we will get a different Eulerian
    // cell now based on roundoff accumulation.
    //
    // Get around these rounding issues by picking strange-looking shifts that
    // guarantee no quadrature point will be near 0.5, 0.75, or any other
    // possible Eulerian cell midpoint coordinate.
    X(0) = s(0) + 0.612345;
    X(1) = s(1) + 0.512345;
#if (NDIM == 3)
    X(2) = s(2) + 0.512345;
#endif
    return;
} // coordinate_mapping_function

static constexpr double R = 0.2;
static constexpr double w = 0.0625;
static constexpr double dx0 = 1.0 / 64.0;

// alternative mapping function for periodic domains.
void
periodic_coordinate_mapping_function(libMesh::Point& X, const libMesh::Point& s, void* /*ctx*/)
{
    static constexpr double gamma = 0.15;
    X(0) = (R + s(1)) * cos(s(0) / R) + 0.505;
    X(1) = (R + gamma + s(1)) * sin(s(0) / R) + 0.505;
    return;
} // periodic_coordinate_mapping_function

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    PetscOptionsSetValue(nullptr, "-ksp_rtol", "1e-16");
    PetscOptionsSetValue(nullptr, "-ksp_atol", "1e-16");

    // prevent a warning about timer initializations
    TimerManager::createManager(nullptr);
    {
        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");

        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Create a simple FE mesh.
        std::vector<std::unique_ptr<ReplicatedMesh> > meshes;
        meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        const std::string elem_str = input_db->getString("ELEM_TYPE");
        const auto elem_type = Utility::string_to_enum<ElemType>(elem_str);

        const std::string geometry = input_db->getStringWithDefault("geometry", "sphere");

        if (geometry == "sphere")
        {
            ReplicatedMesh& mesh = *meshes[0];
            // libMesh circa version 1.5 fixed a bug with the way they read
            // Triangle input which results in vertices being numbered in a
            // different way after the patch. This actually makes a
            // substantial difference for this test since changing the vertex
            // numbering changes the quadrature points, which in turn changes
            // where we spread forces. Hence, unlike the examples, just avoid
            // Triangle altogether here.
            //
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = 2.0 * M_PI * R / ds;
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(mesh, R, r, elem_type);
        }
        else if (geometry == "cube")
        {
            ReplicatedMesh& mesh = *meshes[0];
            const double L = input_db->getDouble("L");
            if (NDIM == 2)
                MeshTools::Generation::build_square(mesh, 10, 12, 0.0, L, 0.0, L, elem_type);
            else
                MeshTools::Generation::build_cube(mesh, 10, 12, 14, 0.0, L, 0.0, L, 0.0, L, elem_type);
        }
        else if (geometry == "periodic")
        {
            const int n_x = ceil(2.0 * M_PI * R / ds);
            const int n_y = ceil(w / ds);
            MeshTools::Generation::build_square(*meshes[0], n_x, n_y, 0.0, 2.0 * M_PI * R, 0.0, w, elem_type);
        }
        else
        {
            TBOX_ASSERT(geometry == "composite_cube");
            const double L = input_db->getDouble("L");
            if (NDIM == 2)
            {
                ReplicatedMesh& mesh_0 = *meshes[0];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_1 = *meshes[1];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_2 = *meshes[2];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_3 = *meshes[3];
                MeshTools::Generation::build_square(mesh_0, 10, 12, 0.0, L / 2, 0.0, L / 2, elem_type);
                MeshTools::Generation::build_square(mesh_1, 10, 12, L / 2, L, 0.0, L / 2, elem_type);
                MeshTools::Generation::build_square(mesh_2, 10, 12, 0.0, L / 2, L / 2, L, elem_type);
                MeshTools::Generation::build_square(mesh_3, 10, 12, L / 2, L, L / 2, L, elem_type);
            }
            else
            {
                ReplicatedMesh& mesh_0 = *meshes[0];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_1 = *meshes[1];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_2 = *meshes[2];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_3 = *meshes[3];
                MeshTools::Generation::build_cube(mesh_0, 5, 6, 7, 0.0, L / 2, 0.0, L / 2, 0.0, L / 2, elem_type);
                MeshTools::Generation::build_cube(mesh_1, 5, 6, 7, L / 2, L, 0.0, L / 2, 0.0, L / 2, elem_type);
                MeshTools::Generation::build_cube(mesh_2, 5, 6, 7, 0.0, L / 2, L / 2, L, 0.0, L / 2, elem_type);
                MeshTools::Generation::build_cube(mesh_3, 5, 6, 7, L / 2, L, L / 2, L, 0.0, L / 2, elem_type);

                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_4 = *meshes[4];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_5 = *meshes[5];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_6 = *meshes[6];
                meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
                ReplicatedMesh& mesh_7 = *meshes[7];
                MeshTools::Generation::build_cube(mesh_4, 5, 6, 7, 0.0, L / 2, 0.0, L / 2, L / 2, L, elem_type);
                MeshTools::Generation::build_cube(mesh_5, 5, 6, 7, L / 2, L, 0.0, L / 2, L / 2, L, elem_type);
                MeshTools::Generation::build_cube(mesh_6, 5, 6, 7, 0.0, L / 2, L / 2, L, L / 2, L, elem_type);
                MeshTools::Generation::build_cube(mesh_7, 5, 6, 7, L / 2, L, L / 2, L, L / 2, L, elem_type);
            }
        }

        // metis does a good job partitioning, but the partitioning relies on
        // random numbers: the seed changed in libMesh commit
        // 98cede90ca8837688ee13aac5e299a3765f083da (between 1.3.1 and
        // 1.4.0). Hence, to achieve consistent partitioning, use a simpler partitioning scheme:
        for (const auto& mesh : meshes)
        {
            mesh->prepare_for_use();
            LinearPartitioner partitioner;
            partitioner.partition(*mesh);
        }

        VectorValue<double> boundary_translation(2.0 * M_PI * R, 0.0, 0.0);
        PeriodicBoundary pbc(boundary_translation);
        pbc.myboundary = 3;
        pbc.pairedboundary = 1;

        std::size_t n_elem = 0;
        for (const auto& mesh : meshes) n_elem += mesh->n_active_elem();
        plog << "Number of elements: " << n_elem << std::endl;

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"), false);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy =
            new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry, false);
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();

        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"),
                false);
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"),
                false);
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }

        std::vector<libMesh::MeshBase*> mesh_ptrs;
        for (auto& mesh : meshes)
        {
            mesh_ptrs.emplace_back(mesh.get());
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           mesh_ptrs,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           false);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator,
                                              false);

        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer,
                                        false);

        // Configure the IBFE solver.
        if (geometry == "sphere")
        {
            ib_method_ops->registerInitialCoordinateMappingFunction(coordinate_mapping_function);
        }
        else if (geometry == "periodic")
        {
            ib_method_ops->registerInitialCoordinateMappingFunction(periodic_coordinate_mapping_function);
        }
        else
        {
            // other meshes are already centered correctly
        }
        ib_method_ops->initializeFEEquationSystems();
        if (geometry == "periodic")
        {
            EquationSystems* equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();
            for (unsigned int k = 0; k < equation_systems->n_systems(); ++k)
            {
                System& system = equation_systems->get_system(k);
                system.get_dof_map().add_periodic_boundary(pbc);
            }
        }
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // only regrid if we are in parallel - do this to avoid weird problems
        // with inconsistent initial partitionings (see
        // IBFEMethod::d_skip_initial_workload_log)
        if (IBTK_MPI::getNodes() != 1) time_integrator->regridHierarchy();

        // Now for the actual test. Set up a new variable containing ghost data:
        VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
        const Pointer<SAMRAI::hier::Variable<NDIM> > f_var = time_integrator->getBodyForceVariable();
        const Pointer<VariableContext> f_ghost_ctx = var_db->getContext("f_ghost");

        int n_ghosts = input_db->keyExists("IB_DELTA_FUNCTION") ?
                           LEInteractor::getMinimumGhostWidth(input_db->getString("IB_DELTA_FUNCTION")) :
                           3;
        if (app_initializer->getComponentDatabase("IBFEMethod")->keyExists("min_ghost_cell_width"))
        {
            n_ghosts = app_initializer->getComponentDatabase("IBFEMethod")->getInteger("min_ghost_cell_width");
        }
        const int f_ghost_idx = var_db->registerVariableAndContext(f_var, f_ghost_ctx, n_ghosts);

        for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
        {
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            level->allocatePatchData(f_ghost_idx);
            for (PatchLevel<NDIM>::Iterator p(level); p; p++)
            {
                Pointer<Patch<NDIM> > patch = level->getPatch(p());
                Pointer<SideData<NDIM, double> > f_data = patch->getPatchData(f_ghost_idx);
                f_data->fillAll(0.0);
            }
        }

        // synch ghost data
        using InterpolationTransactionComponent = HierarchyGhostCellInterpolation::InterpolationTransactionComponent;
        std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
        ghost_cell_components[0] = InterpolationTransactionComponent(f_ghost_idx,
                                                                     "CONSERVATIVE_LINEAR_REFINE",
                                                                     true,
                                                                     "CONSERVATIVE_COARSEN",
                                                                     "LINEAR",
                                                                     false,
                                                                     {}, // f_bc_coefs
                                                                     nullptr);
        HierarchyGhostCellInterpolation ghost_fill_op;
        ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
        ghost_fill_op.fillData(/*time*/ 0.0);

        const double dt = time_integrator->getMaximumTimeStepSize();
        time_integrator->preprocessIntegrateHierarchy(
            time_integrator->getIntegratorTime(), time_integrator->getIntegratorTime() + dt, 1 /*???*/);
        for (unsigned int part_n = 0; part_n < meshes.size(); ++part_n)
        {
            auto& fe_data_manager = *ib_method_ops->getFEDataManager(part_n);
            auto& equation_systems = *fe_data_manager.getEquationSystems();
            auto& force_system = equation_systems.get_system(IBFEMethod::FORCE_SYSTEM_NAME);
            auto& half_f_vector = dynamic_cast<libMesh::PetscVector<double>&>(force_system.get_vector("half"));
            for (unsigned int i = half_f_vector.first_local_index(); i < half_f_vector.last_local_index(); ++i)
            {
                half_f_vector.set(i, i % 10);
            }
            half_f_vector.close();
        }

        // the partitioning isn't relevant when we only have one processor
        if (IBTK_MPI::getNodes() != 1)
        {
            print_partitioning_on_plog_0(
                patch_hierarchy, patch_hierarchy->getFinestLevelNumber(), patch_hierarchy->getFinestLevelNumber());
        }

        // Test the accumulation code when we have meshes that are against the
        // boundary (i.e., the cube and composite cube geometries)
        const double data_time = time_integrator->getIntegratorTime() + dt / 2;
        RobinPhysBdryPatchStrategy* bdry_op = geometry == "sphere" ? nullptr : time_integrator->getVelocityPhysBdryOp();
        if (input_db->getBoolWithDefault("spread_with_ibfemethod", true))
            ib_method_ops->spreadForce(f_ghost_idx, bdry_op, {}, data_time);
        else
        {
            TBOX_ASSERT(meshes.size() == 1); // not implemented for multiple parts
            auto& fe_data_manager = *ib_method_ops->getFEDataManager(0);
            auto& equation_systems = *fe_data_manager.getEquationSystems();
            auto& force_system = equation_systems.get_system(IBFEMethod::FORCE_SYSTEM_NAME);
            auto& F_vec = dynamic_cast<libMesh::PetscVector<double>&>(force_system.get_vector("half"));
            auto& position_system = equation_systems.get_system(IBFEMethod::COORDS_SYSTEM_NAME);
            auto& X_vec = dynamic_cast<libMesh::PetscVector<double>&>(*position_system.current_local_solution);
            fe_data_manager.spread(
                f_ghost_idx, F_vec, X_vec, IBFEMethod::FORCE_SYSTEM_NAME, nullptr, data_time, false, false);

            // here's the real test for the bug in this case: FEDataManager::spread()
            // previously did not copy values spread into ghost regions, so the values
            // computed by these two combined calls would be wrong.
            bdry_op->setPatchDataIndex(f_ghost_idx);
            const int ln = patch_hierarchy->getFinestLevelNumber();
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            for (PatchLevel<NDIM>::Iterator p(level); p; p++)
            {
                const Pointer<Patch<NDIM> > patch = level->getPatch(p());
                Pointer<PatchData<NDIM> > f_data = patch->getPatchData(f_ghost_idx);
                bdry_op->accumulateFromPhysicalBoundaryData(*patch, data_time, f_data->getGhostCellWidth());
            }
        }
        const double cutoff = input_db->getDoubleWithDefault("output_cutoff_value", 0.0);
        std::ostringstream out;
        {
            const int ln = patch_hierarchy->getFinestLevelNumber();
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);

            // We don't need to print this if we are running in serial
            if (IBTK_MPI::getNodes() != 1)
            {
                out << "\nrank: " << IBTK_MPI::getRank() << '\n';
            }
            for (PatchLevel<NDIM>::Iterator p(level); p; p++)
            {
                bool printed_value = false;
                std::ostringstream patch_out;
                patch_out << "patch number " << p() << '\n';
                patch_out.precision(16);
                Pointer<Patch<NDIM> > patch = level->getPatch(p());
                Pointer<SideData<NDIM, double> > f_data = patch->getPatchData(f_ghost_idx);
                const Box<NDIM> patch_box = patch->getBox();

                // same as SideData::print, but elides zero values. We don't
                // print any information about the patch when no values are
                // above the cutoff.
                for (int axis = 0; axis < NDIM; ++axis)
                {
                    patch_out << "Array side normal = " << axis << std::endl;
                    for (int d = 0; d < f_data->getDepth(); ++d)
                    {
                        patch_out << "Array depth = " << d << std::endl;
                        const ArrayData<NDIM, double>& data = f_data->getArrayData(axis);
                        for (SideIterator<NDIM> i(patch_box, axis); i; i++)
                        {
                            const double value = data(i(), d);
                            if (std::abs(value) > cutoff)
                            {
                                patch_out << "array" << i() << " = " << value << '\n';
                                printed_value = true;
                            }
                        }
                    }
                }
                if (printed_value) out << patch_out.str();
            }
        }
        IBTK_MPI::barrier();

        print_strings_on_plog_0(out.str());
    } // cleanup dynamically allocated objects prior to shutdown
} // main


////////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::example::data::exponential.cpp                             //
//                                                                                //
//  Copyright 2009 Erwann Rogard. 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 <vector>
#include <algorithm>
#include <string>
#include <ext/algorithm>
#include <fstream>

#include <boost/mpl/int.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/range.hpp>
#include <boost/function.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/utility.hpp>

#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/include/make_map.hpp>

#include <boost/statistics/detail/math/constant/shortcut.hpp>

#include <boost/statistics/detail/fusion/joint_view/bind_iterator.hpp>

#include <boost/statistics/detail/distribution_common/meta/random/generator.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>

#include <boost/statistics/detail/fusion/joint_view/bind_iterator.hpp>

#include <boost/statistics/detail/distribution/survival/record/key/include.hpp>
#include <boost/statistics/detail/distribution/survival/record/random/generate.hpp>

#include <boost/statistics/detail/distribution/survival/response/common/factory2.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/include.hpp>
#include <boost/statistics/detail/distribution/survival/response/common/generator.hpp>

#include <boost/statistics/detail/distribution/survival/models/common/meta/include.hpp>
#include <boost/statistics/detail/distribution/survival/models/common/importance_sampling/cg.hpp>
#include <boost/statistics/detail/distribution/survival/models/common/importance_sampling/manager.hpp>
#include <boost/statistics/detail/distribution/survival/models/common/importance_sampling/proposals_manager.hpp>

#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/include.hpp>

#include <libs/statistics/detail/distribution/survival/example/models/exponential.h>

void example_model_exponential(
    long n_runs,                 // = 5e4;
    long n_r,     // records     // =
    long n_mod,                  // = 1e3;
    long n_p,     // proposals   // = 1e4;
    long n_t,     // targets     // = 1e4;
    double time_analysis,        // = const_::inf_;
    std::string out_path,        // = "/Users/erwann/projets/2009/Xcode/survival/build/Release/
    std::ostream& out
)
{

    out << "-> example_model_exponential : ";

    out.flush();

    // Steps shown in this example:
    //
    // Given a likelihood model, a covariate level, and a prior.
    // for each of a set of runs,
    // - samples a parameter, p, from the prior
    // - samples failure times given the model at p
    // - sample from the posterior distribution using importance sampling
    // - computes importance sampling statistics such as ESS
    // - saves the cook-gelman statistic for that run

    using namespace boost;
    using namespace statistics;
    namespace stat = boost::statistics::detail;
    namespace is = stat::importance_sampling;
    namespace dist = stat::distribution;
    namespace ds = dist::survival;
    namespace dm = dist::model;
	namespace jv = stat::fusion::joint_view;

    typedef std::string                 str_;
    typedef std::ofstream               ofs_;

    typedef dm::tag::covariate          tag_x_; // nec?
    typedef dm::tag::response           tag_y_; // nec?

    typedef ds::record::tag::entry_time tag_et_;
    typedef ds::record::tag::failure_time tag_ft_;
    typedef dm::tag::dataset            tag_dset_;
    typedef dm::tag::model              tag_m_;
    typedef dm::tag::prior              tag_pr_;
    typedef mt19937                     urng_;

    // Cg
    const str_ cg_path = out_path + "cg";
    ofs_ ofs_cg( cg_path.c_str() );

    typedef double val_;
	typedef stat::math::constant::shortcut<val_> const_;
    typedef boost::math::normal_distribution<val_> prior_; // prior
	typedef ds::importance_sampling::proposals_manager<prior_> proposals_mngr_;

    const val_ max_log      = 100.0;
    const val_ prior_mu     = 0.0;
    const val_ prior_sigma  = 5.0;
    const val_ x_val        = -1.0;
    const val_ et_val       = 0.0;  // entry_time
    typedef val_ p_; // parameter
    typedef dist::survival::response::event<val_> y_val_; // response
    typedef std::vector<val_> 	vec_val_;
    typedef std::vector<p_> 	vec_p_;

    typedef boost::fusion::result_of::make_map<tag_ft_,val_>::type 	ft_;
    typedef boost::fusion::result_of::make_map<tag_et_,val_>::type 	et_;
    typedef boost::fusion::result_of::make_map<tag_x_,val_>::type 	x_;

    typedef std::vector<ft_> vec_ft_;

    typedef ds::response::factory2<y_val_> 		factory2_;
    typedef ds::response::generator<factory2_> 	y_gen_;
    typedef std::vector<y_gen_::y_>				vec_y_;

    typedef proposals_mngr_::tag::parameter tag_par_;
    typedef proposals_mngr_::tag::log_pdf  	tag_lpdf_;
    typedef boost::fusion::result_of::make_map<
    	tag_par_,tag_lpdf_,p_, val_
    >::type p_lpdf_;
    typedef std::vector<p_lpdf_> proposals_;
    typedef ds::exponential_model<val_> model_;

    // typedef detail::fusion::joint_view::result_of::flatten_bind_range;

    urng_ urng;
    prior_ prior(prior_mu,prior_sigma);
    proposals_mngr_ proposals_mngr(prior);
    proposals_mngr.refresh(urng,n_p);

    model_ model;
    et_ et(boost::fusion::make_pair<tag_et_>(et_val));
    // Note that a dataset with alternating covariate values say
    // -1,-1,-1, 1, 1, 1 can be created with joint_view::flatten_range

	x_ x(boost::fusion::make_pair<tag_x_>(x_val));

    vec_ft_	vec_ft;	// failure times
    vec_ft.reserve(n_r);
    vec_val_	vec_w;   // importance weights
    vec_p_ 		targets;
    vec_y_		vec_y; vec_y.reserve(n_r);
    p_ p;
    {
        BOOST_AUTO(g_p,dist::make_random_generator(urng,prior));
        for(unsigned i_run = 0; i_run<n_runs; i_run++)
        {
            {   // sample records given (x,model)
                p = g_p();
                model.set_parameter( p );
                vec_ft.clear();
                ds::record::generate_n(
                	std::back_inserter(vec_ft),
                    n_r, urng, model,
                    boost::fusion::at_key<tag_x_>(x)
                );
            }
            {

                // Generate responses
                vec_y.clear();
                ds::response::generator<factory2_>::call(
                	std::back_inserter(vec_y),
                	et_val,
                    boost::begin(vec_ft),
                    boost::end(vec_ft),
                    time_analysis
                );

                // Generate posterior sample
				typedef ds::importance_sampling_manager<
                	tag_par_,tag_lpdf_,val_> is_mngr_;
                is_mngr_ is_mngr(max_log);

				typedef boost::range_iterator<vec_y_>::type it_y_;
				typedef jv::result_of::bind_iterator<x_,it_y_>::type it_xy_;
                it_xy_ xy_b = jv::bind_iterator(x,boost::begin(vec_y));
                it_xy_ xy_e = jv::bind_iterator(x,boost::end(vec_y));

                is_mngr.refresh(
                	std::back_inserter( targets ),
                    n_t,
					xy_b,
                    xy_e,
        			boost::begin(proposals_mngr()),
                    boost::end(proposals_mngr()),
                    prior, model,
                    urng
                );

                if(i_run%n_mod == 0)
                {
                    const str_ str = "i = %1%, %2%";
                    boost::format f(str);
                    f % i_run  % is_mngr.description();
                    out << f.str() << std::endl;
                    out.flush();
                }
            }
            {
                ofs_cg
                	<< ds::cg(boost::begin(targets),boost::end(targets),p)
                    << ' ';
                ofs_cg.flush();
            }
        }// runs
    }

    out << "<-" << std::endl;
}


// Copyright (c) 2019-2020 Ricardo Tonet
// Use of this source code is governed by the MIT license, see LICENSE

/**
 * \file trajectory_planner.cpp
 * \brief Defines TrajectoryPlanner class for robot arm trajectory planning.
 */

#include <smalldrop_toolpath/trajectory_planner.h>
#include <smalldrop_state/exceptions.h>

#include <Eigen/Dense>

namespace smalldrop
{
namespace smalldrop_toolpath
{
/*****************************************************************************************
 * Public methods & constructors/destructors
 *****************************************************************************************/

/**
 * \copybrief TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE
 * plan_mode)
 */
TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode)
  : duration_(duration), frequency_(frequency), plan_mode_(plan_mode), max_speed_(50)
{
}

/**
 * \copybrief TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode, const double max_speed)
 */
TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode, const double max_speed)
  : duration_(duration), frequency_(frequency), plan_mode_(plan_mode), max_speed_(max_speed)
{
}

/**
 * \copybrief TrajectoryPlanner::plan(const Path& path)
 */
Trajectory TrajectoryPlanner::plan(const Path& path)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && path.length()/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  poses_t poses = path.poses();
  unsigned int npoints = poses.size();
  double t_interval = duration_ / ((double)npoints-1);

  Trajectory t(trajectory, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < npoints - 1; i++)
    {
      double t = 0;
      while (t <= t_interval + 0.0001)
      {
        switch (plan_mode_)
        {
          case PLAN_MODE::LSPB:
            trajectory.push_back(lspb(poses[i], poses[i + 1], LSPB_ACCEL, 0, t_interval, t));
            break;
          default:  // PLAN_MODE::POLY3
            trajectory.push_back(poly3(poses[i], poses[i + 1], 0, t_interval, t));
            break;
        }
        t += 1 / frequency_;
      }
    }

    Trajectory traj(trajectory, path.length(), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::plan(const paths_t paths)
 */
Trajectory TrajectoryPlanner::plan(const paths_t paths)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && getFullPathLength(paths)/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  double path_duration = duration_ / paths.size();

  Trajectory t(trajectory, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < paths.size(); i++)
    {
      poses_t poses = paths[i].poses();
      unsigned int npoints = poses.size();
      double t_interval = path_duration / ((double)npoints-1);

      for (size_t j = 0; j < npoints - 1; j++)
      {
        double t = 0;
        while (t <= t_interval + 0.0001)
        {
          switch (plan_mode_)
          {
            case PLAN_MODE::LSPB:
              trajectory.push_back(lspb(poses[j], poses[j + 1], LSPB_ACCEL, 0, t_interval, t));
              break;
            default:  // PLAN_MODE::POLY3
              trajectory.push_back(poly3(poses[j], poses[j + 1], 0, t_interval, t));
              break;
          }
          t += 1 / frequency_;
        }
      }
    }

    Trajectory traj(trajectory, getFullPathLength(paths), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::plan(const Toolpath& toolpath)
 */
Trajectory TrajectoryPlanner::plan(const ToolPath& toolpath)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && toolpath.length()/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  path_actions_t trajectory_actions;
  poses_t poses = toolpath.poses();
  unsigned int npoints = poses.size();
  double t_interval = duration_ / ((double)npoints-1);

  Trajectory t(trajectory, trajectory_actions, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < npoints - 1; i++)
    {
      double t = 0;
      while (t <= t_interval + 0.0001)
      {
        switch (plan_mode_)
        {
          case PLAN_MODE::LSPB:
            trajectory.push_back(lspb(poses[i], poses[i + 1], LSPB_ACCEL, 0, t_interval, t));
            break;
          default:  // PLAN_MODE::POLY3
            trajectory.push_back(poly3(poses[i], poses[i + 1], 0, t_interval, t));
            break;
        }
        t += 1 / frequency_;
      }
    }

    Trajectory traj(trajectory, toolpath.actions(), toolpath.length(), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief Trajectory TrajectoryPlanner::plan(const toolpaths_t toolpaths)
 */
Trajectory TrajectoryPlanner::plan(const toolpaths_t toolpaths)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && getFullPathLength(toolpaths)/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  path_actions_t trajectory_actions;
  double path_duration = duration_ / toolpaths.size();

  Trajectory t(trajectory, trajectory_actions, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < toolpaths.size(); i++)
    {
      poses_t poses = toolpaths[i].poses();
      path_actions_t actions = toolpaths[i].actions();
      unsigned int npoints = poses.size();
      double t_interval = path_duration / ((double)npoints-1);

      for (size_t j = 0; j < npoints - 1; j++)
      {
        PRINT_ACTION action = actions[j];
        double t = 0;
        while (t <= t_interval + 0.0001)
        {
          switch (plan_mode_)
          {
            case PLAN_MODE::LSPB:
              trajectory.push_back(lspb(poses[j], poses[j + 1], LSPB_ACCEL, 0, t_interval, t));
              trajectory_actions.push_back(action);
              break;
            default:  // PLAN_MODE::POLY3
              trajectory.push_back(poly3(poses[j], poses[j + 1], 0, t_interval, t));
              trajectory_actions.push_back(action);
              break;
          }
          t += 1 / frequency_;
        }
      }
    }

    Trajectory traj(trajectory, trajectory_actions, getFullPathLength(toolpaths), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::setDuration()
 */
void TrajectoryPlanner::setDuration(const double duration)
{
  duration_ = duration;
}

/**
 * \copybrief TrajectoryPlanner::setFrequency()
 */
void TrajectoryPlanner::setFrequency(const double frequency)
{
  frequency_ = frequency;
}

/**
 * \copybrief TrajectoryPlanner::setPlanMode()
 */
void TrajectoryPlanner::setPlanMode(const PLAN_MODE plan_mode)
{
  plan_mode_ = plan_mode;
}

/**
 * \copybrief TrajectoryPlanner::setMaxSpeed(const double max_speed)
 */
void TrajectoryPlanner::setMaxSpeed(const double max_speed)
{
  max_speed_ = max_speed;
}

/*****************************************************************************************
 * Private methods
 *****************************************************************************************/

/**
 * \copybrief double viaVelocity(const double pos_before, const double pos, const double pos_after, const double
 * t_before, const double t, const double t_after) const
 */
double TrajectoryPlanner::viaVelocity(const double pos_before, const double pos, const double pos_after,
                                      const double t_before, const double t, const double t_after) const
{
  double velocity_i, velocity_ii;

  velocity_i = (pos - pos_before) / (t - t_before);  // Mean velocity before via point
  velocity_ii = (pos_after - pos) / (t_after - t);   // Mean velocity after via point

  if ((velocity_i * velocity_ii) < 0)  // Mean velocities change signal at via point
    return 0.0;                        // Velocity at via point is zero
  else
    return 0.5 * (velocity_ii + velocity_i);  // Velocity at via point is the average of the mean velocities
}

/**
 * \copybrief pose_t poly3(const pose_t pose_i, const pose_t pose_f, const double t0, const double tf, const double t)
 * const
 */
pose_t TrajectoryPlanner::poly3(const pose_t pose_i, const pose_t pose_f, const double t0, const double tf,
                                const double t) const
{
  std::vector<double> a0 = { pose_i.position.x, pose_i.position.y, pose_i.position.z };
  std::vector<double> a1 = { 0, 0, 0 };
  std::vector<double> a2 = { (3 / pow(tf - t0, 2)) * (pose_f.position.x - pose_i.position.x),
                             (3 / pow(tf - t0, 2)) * (pose_f.position.y - pose_i.position.y),
                             (3 / pow(tf - t0, 2)) * (pose_f.position.z - pose_i.position.z) };
  std::vector<double> a3 = { -(2 / pow(tf - t0, 3)) * (pose_f.position.x - pose_i.position.x),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.y - pose_i.position.y),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.z - pose_i.position.z) };

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);

  pose_t pose;
  pose.position.x = a0[0] + a1[0] * (t - t0) + a2[0] * pow(t - t0, 2) + a3[0] * pow(t - t0, 3);
  pose.position.y = a0[1] + a1[1] * (t - t0) + a2[1] * pow(t - t0, 2) + a3[1] * pow(t - t0, 3);
  pose.position.z = a0[2] + a1[2] * (t - t0) + a2[2] * pow(t - t0, 2) + a3[2] * pow(t - t0, 3);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  return pose;
}

/**
 * \copybrief pose_t poly3c(const pose_t pose_i, const pose_t pose_f, const std::vector<double> velocity0, const
 * std::vector<double> velocityf, const double t0, const double tf, const double t) const
 */
pose_t TrajectoryPlanner::poly3c(const pose_t pose_i, const pose_t pose_f, const std::vector<double> velocity0,
                                 const std::vector<double> velocityf, const double t0, const double tf,
                                 const double t) const
{
  std::vector<double> a0 = { pose_i.position.x, pose_i.position.y, pose_i.position.z };
  std::vector<double> a1 = { velocity0[0], velocity0[1], velocity0[2] };
  std::vector<double> a2 = { (3 / pow(tf - t0, 2)) * (pose_f.position.x - pose_i.position.x) -
                                 (2 / (tf - t0)) * velocity0[0] - (1 / (tf - t0)) * velocityf[0],
                             (3 / pow(tf - t0, 2)) * (pose_f.position.y - pose_i.position.y) -
                                 (2 / (tf - t0)) * velocity0[1] - (1 / (tf - t0)) * velocityf[1],
                             (3 / pow(tf - t0, 2)) * (pose_f.position.z - pose_i.position.z) -
                                 (2 / (tf - t0)) * velocity0[2] - (1 / (tf - t0)) * velocityf[2] };
  std::vector<double> a3 = { -(2 / pow(tf - t0, 3)) * (pose_f.position.x - pose_i.position.x) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[0] + velocity0[0]),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.y - pose_i.position.y) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[1] + velocity0[1]),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.z - pose_i.position.z) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[2] + velocity0[2]) };

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);

  pose_t pose;
  pose.position.x = a0[0] + a1[0] * (t - t0) + a2[0] * pow(t - t0, 2) + a3[0] * pow(t - t0, 3);
  pose.position.y = a0[1] + a1[1] * (t - t0) + a2[1] * pow(t - t0, 2) + a3[1] * pow(t - t0, 3);
  pose.position.z = a0[2] + a1[2] * (t - t0) + a2[2] * pow(t - t0, 2) + a3[2] * pow(t - t0, 3);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  return pose;
}

/**
 * \copybrief pose_t poly3cVias(const poses_t poses, const std::vector<double> times, const double t) const
 */
pose_t TrajectoryPlanner::poly3cVias(const poses_t poses, const std::vector<double> times, const double t) const
{
  if (poses.size() != times.size())
  {
    throw std::runtime_error("The poses vector should be the same size as times vector.");
  }

  std::vector<double> velocity0;
  std::vector<double> velocityf;
  velocity0.resize(3, 0);
  velocityf.resize(3, 0);

  pose_t pose;
  for (size_t i = 1; i < times.size(); i++)
  {
    if (t >= times[i - 1] && t <= times[i])
    {
      if (i == 1)
      {
        // velocity0 is already {0, 0, 0}
        velocityf[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
      }
      else if (i == times.size() - 1)
      {
        velocity0[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
        // velocityf is already {0, 0, 0}
      }
      else
      {
        velocity0[0] = viaVelocity(poses[i - 2].position.x, poses[i - 1].position.x, poses[i].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[1] = viaVelocity(poses[i - 2].position.y, poses[i - 1].position.y, poses[i].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[2] = viaVelocity(poses[i - 2].position.z, poses[i - 1].position.z, poses[i].position.z, times[i - 1],
                                   times[i], times[i + 1]);

        velocityf[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
      }

      pose = poly3c(poses[i - 1], poses[i], velocity0, velocityf, times[i - 1], times[i], t);
      break;
    }
  }

  return pose;
}

/**
 * \copybrief pose_t lspb(const pose_t pose_i, const pose_t pose_f, const double accel, const double t0, const double
 * tf, const double t) const
 */
pose_t TrajectoryPlanner::lspb(const pose_t pose_i, const pose_t pose_f, const double accel, const double t0,
                               const double tf, const double t) const
{
  // It is assumed that accel param is always positive

  if (accel < (4 * abs(pose_f.position.x - pose_i.position.x)) / pow(tf - t0, 2) ||
      accel < (4 * abs(pose_f.position.y - pose_i.position.y)) / pow(tf - t0, 2) ||
      accel < (4 * abs(pose_f.position.z - pose_i.position.z)) / pow(tf - t0, 2))
  {
    throw std::runtime_error("Invalid acceleration! The value needs to be larger.");
  }

  // Change accel sign depending on growth direction of position (eg. x0 > xf => accel0 < 0)
  std::vector<double> accel_vec;
  accel_vec.resize(3, 0);
  accel_vec[0] = (pose_i.position.x > pose_f.position.x) ? -accel : accel;
  accel_vec[1] = (pose_i.position.y > pose_f.position.y) ? -accel : accel;
  accel_vec[2] = (pose_i.position.z > pose_f.position.z) ? -accel : accel;

  pose_t pose;
  std::vector<double> velocity, acceleration;
  velocity.resize(3, 0);
  acceleration.resize(3, 0);
  std::vector<double> delta_tb;
  delta_tb.resize(3, 0);
  delta_tb[0] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.x - pose_i.position.x) / abs(accel_vec[0]));
  delta_tb[1] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.y - pose_i.position.y) / abs(accel_vec[1]));
  delta_tb[2] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.z - pose_i.position.z) / abs(accel_vec[2]));
  std::vector<double> tb;
  tb.resize(3, 0);
  tb[0] = t0 + delta_tb[0];
  tb[1] = t0 + delta_tb[1];
  tb[2] = t0 + delta_tb[2];
  std::vector<double> velocity_b;
  velocity_b.resize(3, 0);
  velocity_b[0] = accel_vec[0] * delta_tb[0];
  velocity_b[1] = accel_vec[1] * delta_tb[1];
  velocity_b[2] = accel_vec[2] * delta_tb[2];
  std::vector<double> poseb;
  poseb.resize(3, 0);
  poseb[0] = pose_i.position.x + 0.5 * accel_vec[0] * pow(delta_tb[0], 2);
  poseb[1] = pose_i.position.y + 0.5 * accel_vec[1] * pow(delta_tb[1], 2);
  poseb[2] = pose_i.position.z + 0.5 * accel_vec[2] * pow(delta_tb[2], 2);
  std::vector<double> posebf;
  posebf.resize(3, 0);
  posebf[0] = poseb[0] + velocity_b[0] * ((tf - t0) - 2 * delta_tb[0]);
  posebf[1] = poseb[1] + velocity_b[1] * ((tf - t0) - 2 * delta_tb[1]);
  posebf[2] = poseb[2] + velocity_b[2] * ((tf - t0) - 2 * delta_tb[2]);

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  for (size_t i = 0; i < 3; i++)
  {
    if (t >= t0 && t <= tb[i])
    {
      acceleration[i] = accel_vec[i];
      velocity[i] = accel_vec[i] * (t - t0);
      switch (i)
      {
        case 1:
          pose.position.y = pose_i.position.y + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
        case 2:
          pose.position.z = pose_i.position.z + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
        default:
          pose.position.x = pose_i.position.x + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
      }
    }
    else if (t >= (tf - delta_tb[i]) && t <= tf + 0.0001)
    {
      acceleration[i] = -accel_vec[i];
      velocity[i] = velocity_b[i] + acceleration[i] * (t - (tf - delta_tb[i]));
      switch (i)
      {
        case 1:
          pose.position.y = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
        case 2:
          pose.position.z = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
        default:
          pose.position.x = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
      }
    }
    else
    {
      acceleration[i] = 0;
      velocity[i] = velocity_b[i];
      switch (i)
      {
        case 1:
          pose.position.y = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
        case 2:
          pose.position.z = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
        default:
          pose.position.x = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
      }
    }
  }

  return pose;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathLength(const paths_t path)
 */
double TrajectoryPlanner::getFullPathLength(const paths_t paths)
{
  double total_length = 0;
  for (size_t i = 0; i < paths.size(); i++)
    total_length += paths[i].length();

  return total_length;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathLength(const toolpaths_t path)
 */
double TrajectoryPlanner::getFullPathLength(const toolpaths_t toolpaths)
{
  double total_length = 0;
  for (size_t i = 0; i < toolpaths.size(); i++)
    total_length += toolpaths[i].length();

  return total_length;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathSize(const paths_t path)
 */
double TrajectoryPlanner::getFullPathSize(const paths_t paths)
{
  double total_size = 0;
  for (size_t i = 0; i < paths.size(); i++)
    total_size += paths[i].poses().size();

  return total_size;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathSize(const toolpaths_t toolpaths)
 */
double TrajectoryPlanner::getFullPathSize(const toolpaths_t toolpaths)
{
  double total_size = 0;
  for (size_t i = 0; i < toolpaths.size(); i++)
    total_size += toolpaths[i].poses().size();

  return total_size;
}

}  // namespace smalldrop_toolpath

}  // namespace smalldrop


// a sorting example that uses the worst-case for conventional MSD radix sorts.
//
//  Copyright Steven Ross 2009-2014.
//
// 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)

//  See http://www.boost.org/libs/sort for library home page.

#include <boost/sort/spreadsort/spreadsort.hpp>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
using namespace boost::sort::spreadsort;
using namespace std;

#define DATA_TYPE boost::uint64_t

#define ALR_THRESHOLD 3

const unsigned max_count = ALR_THRESHOLD - 1;
const unsigned bit_shift = detail::rough_log_2_size(max_count) -
  detail::int_log_mean_bin_size;
const unsigned radix_threshold = detail::rough_log_2_size(max_count) + 1;
//Increase this size if too fast to test accurately
const unsigned top_splits = 12;

const DATA_TYPE typed_one = 1;

void
fill_vector(vector<DATA_TYPE> & input, const DATA_TYPE base_value,
            unsigned remaining_bits)
{
  if (remaining_bits >= radix_threshold) {
    input.push_back((base_value << remaining_bits) +
                    ((typed_one << remaining_bits) - 1));
    fill_vector(input, base_value << bit_shift, remaining_bits - bit_shift);
  }
  else {
    for (unsigned u = 0; u < max_count; ++u)
      input.push_back((base_value << remaining_bits) +
                      (rand() % (1 << remaining_bits)));
  }
}

//Tests spreadsort on the worst-case distribution for standard MSD radix sorts.
int main(int, const char **) {
  vector<DATA_TYPE> input;
  for (int ii = (1 << top_splits) - 1; ii >= 0; --ii)
    fill_vector(input, ii, (sizeof(DATA_TYPE) * 8) - top_splits);

  //Run both std::sort and spreadsort
  for (unsigned u = 0; u < 2; ++u) {
    vector<DATA_TYPE> array = input;
    clock_t start, end;
    double elapsed;
    start = clock();
    if (u)
      std::sort(array.begin(), array.end());
    else
      boost::sort::spreadsort::spreadsort(array.begin(), array.end());
    end = clock();
    elapsed = static_cast<double>(end - start);
    if (u)
      printf("std::sort elapsed time %f\n", elapsed / CLOCKS_PER_SEC);
    else
      printf("spreadsort elapsed time %f\n", elapsed / CLOCKS_PER_SEC);
    array.clear();
  }
  return 0;
}


#ifndef _ALGEBRA_HPP_
#define _ALGEBRA_HPP_
/* algebra.hpp - an NTL-compatibility thin layer
 * Copyright (C) 2021, LWE-PVSS
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 **/
#include <NTL/ZZ.h>
#include <NTL/mat_ZZ.h>
#include <NTL/mat_ZZ_pE.h>
#include <NTL/RR.h>

/* This header provides copmatibility with NTL, inside the ALGEBRA namespace.
 * Modules that use it should use the names that it provides rather than
 * directly use NTL. Specifically it provides the following names:
 * - BigInt  -> NTL:ZZ
 * - Scalar  -> NTL::ZZ_p
 * - Element -> NTL::ZZ_pE (element in GF(p^e))
 * - BIVector-> NTL::vec_ZZ
 * - SVector -> NTL::vec_ZZ_p
 * - EVector -> NTL::vec_ZZ_pE
 * - SMatrix -> NTL::mat_ZZ_p
 * - EMatrix -> NTL::mat_ZZ_pE
 *
 * The users can assume a function conv(x,y) that converts between these
 * types as apropriate. Everything else is provided explicitly in this header.
 */
namespace ALGEBRA {
// in liue of "using X as Y", bring the relevant NTL types to this namespace
typedef NTL::ZZ BigInt;
typedef NTL::ZZ_p Scalar;
typedef NTL::ZZ_pX SPoly;
typedef NTL::ZZ_pE Element;
typedef NTL::vec_ZZ BIVector;
typedef NTL::vec_ZZ_p SVector;
typedef NTL::mat_ZZ_p SMatrix;
typedef NTL::vec_ZZ_pE EVector;
typedef NTL::mat_ZZ_pE EMatrix;

inline BigInt divCeil(const BigInt& x, const BigInt& overY) {
    return (x+overY-1)/overY;
}
inline BigInt divCeil(const BigInt& x, long overY) {
    return (x+overY-1)/overY;
}

inline int bytesPerScalar() { return (NumBits(NTL::ZZ_p::modulus())+7)/8; }
inline int scalarsPerElement() { return NTL::ZZ_pE::degree(); }

inline const Scalar& zeroScalar() { return NTL::ZZ_p::zero(); }
inline Scalar& randomizeScalar(Scalar& s) { NTL::random(s); return s; }
inline Element& randomizeElement(Element& e) { NTL::random(e); return e; }
inline long randBitsize(size_t n) {return NTL::RandomBits_long(n);}
inline BigInt& randBitsize(BigInt& bi, size_t n) {
    NTL::RandomBits(bi, n);
    return bi;
}
inline size_t randomBit() {return randBitsize(1);}

inline BIVector& resize(BIVector& vec, size_t n) {vec.SetLength(n); return vec;}
inline SVector& resize(SVector& vec, size_t n) {vec.SetLength(n); return vec;}
inline EVector& resize(EVector& vec, size_t n) {vec.SetLength(n); return vec;}
inline SMatrix& resize(SMatrix& mat, size_t n, size_t m) {
    mat.SetDims(n,m); return mat;
}
inline EMatrix& resize(EMatrix& mat, size_t n, size_t m) {
    mat.SetDims(n,m); return mat;
}

inline BigInt square(const BigInt& x) {return NTL::sqr(x);}
inline BigInt sqrt(const BigInt& x) {return NTL::SqrRoot(x);}

// returns the smallest ell such that 2^{ell} >= n
inline size_t log2roundUp(const BigInt& n) {
    if (NTL::IsZero(n)) return 0;
    return NTL::NumBits(n-1);
}

// some conversions
inline BigInt toBigInt(long n) {
    NTL::ZZ num(NTL::INIT_SIZE,4);
    conv(num, n);
    return num;
}
inline BigInt scalar2bigInt(const Scalar& s) {return NTL::conv<NTL::ZZ>(s);}
inline const Scalar& coeff(const Element& e, size_t idx) {
    return NTL::coeff(NTL::rep(e), idx);
}

inline void bigIntBytes(unsigned char *buf, const BigInt& bi, size_t bufSize){
    NTL::BytesFromZZ(buf, bi, bufSize);
}
inline void scalarBytes(unsigned char *buf, const Scalar& s, size_t bufSize){
    bigIntBytes(buf, NTL::rep(s), bufSize);
}
inline void elementBytes(unsigned char *buf, const Element& e, size_t bufSize){
    const SVector& v = NTL::rep(e).rep; // the underlying representation as vec_ZZ_p
    size_t len = bytesPerScalar(); // =32
    for (size_t i=0; i<v.length() && bufSize>0; i++) {
        if (len > bufSize)
            len = bufSize;
        NTL::BytesFromZZ(buf, NTL::rep(v[i]), len); // gen the next few bytes
        bufSize -= len;
        buf += len;
    }
    if (bufSize > 0) // zero-out the rest of the buffer
        memset(buf, 0, bufSize);
}

inline void scalarFromBytes(Scalar& s, const unsigned char *buf, size_t bufSize){
    NTL::ZZ n(NTL::INIT_SIZE,4);
    NTL::ZZFromBytes(n, buf, bufSize);
    NTL::conv(s,n);
}
inline void elementFromBytes(Element& e, const unsigned char *buf, size_t bufSize){
    NTL::ZZ_pX px;
    size_t len = bytesPerScalar();
    for (size_t i=0; i<scalarsPerElement() && bufSize>0; i++) {
        if (len > bufSize)
            len = bufSize;
        NTL::ZZ n(NTL::INIT_SIZE,4);
        NTL::ZZFromBytes(n, buf, len);
        SetCoeff(px, i, NTL::conv<NTL::ZZ_p>(n));
        bufSize -= len;
        buf += len;
    }
    conv(e,px);
}

// Other utilities
inline Scalar innerProduct(const SVector& v1, const SVector& v2) {
    Scalar s;
    NTL::InnerProduct(s,v1,v2);
    return s;
}
inline Element innerProduct(const EVector& v1, const SVector& v2) {
    Element e;
    int len = std::min(v1.length(), v2.length());
    for (int i=0; i<len; i++)
        e += v1[i]*v2[i];
    return e;
}
inline Element innerProduct(const SVector& v1, const EVector& v2) {
    return innerProduct(v2,v1);
}
inline Element innerProduct(const EVector& v1, const EVector& v2) {
    Element e;
    NTL::InnerProduct(e,v1,v2);
    return e;
}

template<class VecType, class ElemType>
VecType& push_back(VecType &v, const ElemType& s) {
    v.append(s);
    return v;
}

// convert between an element and a vector of scalars
inline void conv(Element& e, const SVector& v) {
    NTL::ZZ_pX& px = (NTL::ZZ_pX&) rep(e); // dirty: discards const-ness
    size_t n = std::min(scalarsPerElement(), (int)v.length());
    for (size_t i=0; i<n; i++) {
        NTL::SetCoeff(px, i, v[i]);
    }
}
inline void conv(SVector& v, const Element& e) {
    resize(v, scalarsPerElement());
    for (size_t i=0; i<v.length(); i++) {
        v[i] = NTL::coeff(rep(e), i);
    }
}
inline void conv(BIVector& to, const EVector& from) {
    resize(to, scalarsPerElement() *from.length());
    int idx = 0;
    for (int i=0; i<from.length(); i++) for (int j=0; j<scalarsPerElement(); j++) {
        conv(to[idx++], ALGEBRA::coeff(from[i], j));
    }
}

// Implemet a version of BigInt-by-double multiplication
inline BigInt multDbl(double x, const BigInt& y) {
    NTL::RR ry = NTL::to_RR(y);
    ry *= x;
    return NTL::to_ZZ(ry);
}
inline BigInt multDbl(const BigInt& y, double x) {
    return multDbl(x,y);
}
inline double log2BI(const BigInt& x) {
    return NTL::conv<double>(NTL::log(NTL::to_RR(x))/0.69314718056);
}

typedef NTL::RandomStreamPush PRGbackupClass; // backup/restore of PRG state
inline void initRandomness(const std::string& st) {
    NTL::SetSeed((unsigned char*)st.data(), st.length());
}

// Break a vector v over GF(p^ell) (with coefficients in [+-P/2]) into
// two "digit" vectors hi,lo, where lo = (v mod radix) \in [+-radix/2]
// and hi = (v-lo)/radix.
void breakTwoDigits(EVector& hi, EVector& lo, const EVector& v, const BigInt& radix);

inline BigInt balanced(const Scalar& s) {
    static auto Pover2 = NTL::ZZ_p::modulus()/2;
    if (rep(s) > Pover2)
        return rep(s)-NTL::ZZ_p::modulus();
    else
        return rep(s);
}
inline BIVector balanced(const SVector& sv) {
    BIVector bv; resize(bv, sv.length());
    for (int i=0; i<sv.length(); i++)
        bv[i] = balanced(sv[i]);
    return bv;
}
inline BIVector balanced(const Element& e) {
    SVector sv; conv(sv, e);
    return balanced(sv);
}
inline NTL::Vec<BIVector> balanced(const EVector& ev) {
    NTL::Vec<BIVector> vbv;
    vbv.SetLength(ev.length());
    for (int i=0; i<ev.length(); i++)
        vbv[i] = balanced(ev[i]);
    return vbv;
}
inline NTL::mat_ZZ balanced(const SMatrix& sm) {
    static auto Pover2 = NTL::ZZ_p::modulus()/2;
    NTL::mat_ZZ mat;
    conv(mat, sm);
    for (int i=0; i<mat.NumRows(); i++) for (int j=0; j<mat.NumCols(); j++) {
        if (mat[i][j]>Pover2)
            mat[i][j] -= NTL::ZZ_p::modulus();
    }
    return mat;
}
inline NTL::Mat<BIVector> balanced(const EMatrix& emat) {
    NTL::Mat<BIVector> mbv;
    mbv.SetDims(emat.NumRows(), emat.NumCols());
    for (int i=0; i<mbv.NumRows(); i++) for (int j=0; j<mbv.NumCols(); j++) {
        mbv[i][j] = balanced(emat[i][j]);
    }
    return mbv;
}

inline std::ostream& printScalar(std::ostream& st, const Scalar& sc) {
    BigInt szz(NTL::INIT_SIZE,4);
    conv(szz,sc);
    if (szz > NTL::ZZ_p::modulus()/2)
        st <<  (szz - NTL::ZZ_p::modulus());
    else
        st << szz;
    return st;
}
inline std::ostream& printSvec(std::ostream& st, const SVector& sv) {
    st << "[";
    for (size_t i=0; i<sv.length()-1; i++)
        printScalar(st, sv[i]) << " ";
    return (printScalar(st, sv[sv.length()-1]) << "]");
}
inline std::ostream& printElement(std::ostream& st, const Element& el) {
    const NTL::ZZ_pX& ex = rep(el);
    size_t ell = scalarsPerElement();
    st << "[";
    for (size_t i=0; i<ell-1; i++)
        printScalar(st, NTL::coeff(ex, i)) << " ";
    return (printScalar(st, NTL::coeff(ex, ell-1)) << "]");
}
inline std::ostream& printEvec(std::ostream& st, const EVector& ev) {
    st << "[";
    for (size_t i=0; i<ev.length()-1; i++)
        printElement(st, ev[i]) << " ";
    return (printElement(st, ev[ev.length()-1]) << "]");
}

} // end of namespace ALGEBRA
#endif // ifndef _ALGEBRA_HPP_


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "tfidf_v2.h"

#include <fstream>

#include <boost/filesystem.hpp>

#include "argsort.h"
#include "tfidf_data.pb.h"
#include "timing.h"
#include "weighter_v2.h"

tfidfV2::tfidfV2(
        protoIndex const *iidx,
        protoIndex const *fidx,
        std::string tfidfFn,
        featGetter const *featGetter_obj,
        fastann::nn_obj<float> const *nn_obj,
        softAssigner const *SA_obj )
        : retrieverFromIter(iidx, fidx, false, false),
          iidx_(iidx),
          featGetter_obj_(featGetter_obj),
          nn_obj_(nn_obj),
          SA_obj_(SA_obj),
          numDims_(featGetter_obj==NULL ? 0 : featGetter_obj->numDims()) {

    if ( tfidfFn.length()>0 && boost::filesystem::exists( tfidfFn ) ){

        load(tfidfFn, idf_, docL2_);
        numDocs_= docL2_.size();

    } else {

        computeIdf();
        computeDocL2();
        numDocs_= docL2_.size();

        if (tfidfFn.length()>0)
            save(tfidfFn, idf_, docL2_);

    }

}

void
tfidfV2::load(std::string tfidfFn, std::vector<double> &idf, std::vector<double> &docL2){

    rr::tfidfData data;

    std::ifstream in(tfidfFn.c_str(), std::ios::binary);
    ASSERT(data.ParseFromIstream(&in));
    in.close();

    idf.clear();
    idf.reserve(data.idf_size());
    for (int i= 0; i<data.idf_size(); ++i)
        idf.push_back( data.idf(i) );

    docL2.clear();
    docL2.reserve(data.docl2_size());
    for (int i= 0; i<data.docl2_size(); ++i)
        docL2.push_back( data.docl2(i) );

}

void
tfidfV2::save(std::string tfidfFn, std::vector<double> const &idf, std::vector<double> const &docL2){

    rr::tfidfData data;
    data.mutable_idf()->Reserve(idf.size());
    for (uint32_t i= 0; i<idf.size(); ++i)
        data.add_idf(idf[i]);
    data.mutable_docl2()->Reserve(docL2.size());
    for (uint32_t i= 0; i<docL2.size(); ++i)
        data.add_docl2(docL2[i]);

    std::ofstream of(tfidfFn.c_str(), std::ios::binary);
    data.SerializeToOstream(&of);
    of.close();

}

void
tfidfV2::queryExecute( rr::indexEntry &queryRep, ueIterator *ueIter, std::vector<indScorePair> &queryRes, uint32_t toReturn ) const {
    std::vector<double> scores;
    queryExecute(queryRep, ueIter, scores);
    retriever::sortResults( scores, queryRes, toReturn );
}

void
tfidfV2::queryExecute( rr::indexEntry &queryRep, ueIterator *ueIter, std::vector<double> &scores ) const {

    // weight query BoW with idf
    weight(queryRep);

    // query
    weighterV2::queryExecute(queryRep, ueIter, idf_, docL2_, scores);

}

void
tfidfV2::weightStatic(rr::indexEntry &entry, double *weight, std::vector<double> const *idf) {

    ASSERT(weight!=NULL || idf!=NULL);
    bool hasWeigth= (entry.weight_size()!=0);
    bool hasCount= (entry.count_size()!=0);

    if (!hasWeigth)
        entry.mutable_weight()->Reserve( entry.id_size() );
    else
        ASSERT( entry.id_size() == entry.weight_size() );

    if (hasCount)
        ASSERT( entry.id_size() == entry.count_size() );

    double w= (weight==NULL ? 0.0 : *weight);

    for (int i= 0; i < entry.id_size(); ++i) {
        if (weight==NULL)
            w= idf->at( entry.id(i) );

        if (hasWeigth)
            entry.set_weight( i, w * entry.weight(i) );
        else
            entry.add_weight( hasCount ? w * entry.count(i) : w );
    }

}

void
tfidfV2::computeIdf(protoIndex const &iidx, std::vector<double> &idf, protoIndex const *fidx){

    uint32_t numWords= iidx.numIDs();
    ASSERT(fidx!=NULL); // if needed, this could be replaced by computing numDocs as max(all ids)
    uint32_t numDocs= fidx->numIDs();

    idf.clear();
    idf.resize( numWords, 0.0 );

    uint32_t numWords_printStep= std::max(static_cast<uint32_t>(1),numWords/20);

    std::cout<<"tfidfV2::computeIdf\n";

    double time= timing::tic();

    // compute idf (pretend a word appears in 1 document if it appears in 0 to prevent division by 0)

    for (uint32_t wordID= 0; wordID < numWords; ++wordID){

        if (wordID % numWords_printStep == 0)
            std::cout<<"tfidfV2::computeIdf: wordID= "<<wordID<<" / "<<numWords<<" "<<timing::toc(time)<<" ms\n";

        idf[ wordID ]=
            log(
                static_cast<double>(numDocs) /
                std::max( static_cast<uint32_t>(1), iidx.getUniqNumWithID( wordID ) )
                );

    }

    std::cout<<"tfidfV2::computeIdf: DONE ("<<timing::toc(time)<<" ms)\n";

}

void
tfidfV2::computeDocL2() {

    uint32_t numWords= iidx_->numIDs();
    ASSERT(fidx_!=NULL); // if needed, this could be replaced by computing numDocs as max(all ids)
    uint32_t numDocs= fidx_->numIDs();

    docL2_.clear();
    docL2_.resize( numDocs, 0.0 );
    std::vector<rr::indexEntry> entries;

    uint32_t numWords_printStep= std::max(static_cast<uint32_t>(1),numWords/20);

    std::cout<<"tfidfV2::computeDocL2\n";
    double time= timing::tic();

    for (uint32_t wordID= 0; wordID < numWords; ++wordID){
        if (wordID % numWords_printStep == 0)
            std::cout<<"tfidfV2::computeDocL2: wordID= "<<wordID<<" / "<<numWords<<" "<<timing::toc(time)<<" ms\n";

        iidx_->getEntries( wordID, entries );

        // set/add weights and multiply by idf
        for (uint32_t iEntry= 0; iEntry<entries.size(); ++iEntry)
            weight(entries[iEntry], &idf_[wordID]);

        indexEntryVector iev(entries);
        ievIterator it= iev.beginIter(), end= iev.endIter();
        uint32_t docID;
        double weight;

        while (it != end){
            docID= *it;
            weight= 0.0;
            for (; it != end && *it==docID; ++it){
                std::pair<uint32_t, int> p= iev.getInds(it.getInd());
                weight+= entries[p.first].weight(p.second);
            }
            docL2_[ docID ]+= weight * weight;
        }
    }

    for (uint32_t docID= 0; docID < numDocs; ++docID){
        if ( docL2_[docID] <= 1e-7 )
            docL2_[docID]= 1.0;
        else
            docL2_[docID]= sqrt( docL2_[docID] );
    }

    std::cout<<"tfidfV2::computeDocL2: DONE ("<<timing::toc(time)<<" ms)\n";

}

void
tfidfV2::externalQuery_computeData( std::string imageFn, query const &queryObj ) const {

    ASSERT( featGetter_obj_!=NULL );
    ASSERT( nn_obj_!=NULL );

    // compute features

    uint32_t numFeats;
    std::vector<ellipse> regions;
    float *descs;

    std::cout<<"tfidfV2::externalQuery_computeData: Extracting features\n";
    featGetter_obj_->getFeats(imageFn.c_str(), numFeats, regions, descs);
    std::cout<<"tfidfV2::externalQuery_computeData: Extracting features - DONE\n";

    // get visual words

    uint KNN= (SA_obj_==NULL) ? 1 : 3;

    rr::indexEntry queryRep0;
    google::protobuf::RepeatedField<uint32_t> *wordIDs0= queryRep0.mutable_id();
    google::protobuf::RepeatedField<float> *weights0= queryRep0.mutable_weight();
    google::protobuf::RepeatedField<float> *x0= queryRep0.mutable_x();
    google::protobuf::RepeatedField<float> *y0= queryRep0.mutable_y();
    google::protobuf::RepeatedField<float> *a0= queryRep0.mutable_a();
    google::protobuf::RepeatedField<float> *b0= queryRep0.mutable_b();
    google::protobuf::RepeatedField<float> *c0= queryRep0.mutable_c();
    wordIDs0->Reserve( numFeats * KNN );
    weights0->Reserve( numFeats * KNN );
    x0->Reserve( numFeats * KNN );
    y0->Reserve( numFeats * KNN );
    a0->Reserve( numFeats * KNN );
    b0->Reserve( numFeats * KNN );
    c0->Reserve( numFeats * KNN );

    std::cout<<"tfidfV2::externalQuery_computeData: assigning to clusters\n";

    for (uint32_t iFeat=0; iFeat<numFeats; ++iFeat){

        // assign to clusters

        unsigned *clusterID= new unsigned[KNN];
        float *distSq= new float[KNN];

        nn_obj_->search_knn(descs+iFeat*numDims_, 1, KNN, clusterID, distSq);

        quantDesc ww;

        for (uint iNN=0; iNN < KNN; ++iNN)
            ww.rep.push_back( std::make_pair(clusterID[iNN], distSq[iNN]) );

        delete []clusterID;
        delete []distSq;

        ellipse const &region= regions[iFeat];

        // assign weights to visual words (soft or hard)

        if (SA_obj_!=NULL && KNN>1)
            SA_obj_->getWeights(ww);
        else
            ww.rep[0].second= 1.0f;

        for (uint32_t i= 0; i<ww.rep.size(); ++i){
            wordIDs0->AddAlreadyReserved( ww.rep[i].first );
            weights0->AddAlreadyReserved( ww.rep[i].second );
            x0->AddAlreadyReserved( region.x );
            y0->AddAlreadyReserved( region.y );
            a0->AddAlreadyReserved( region.a );
            b0->AddAlreadyReserved( region.b );
            c0->AddAlreadyReserved( region.c );
        }

    }
    std::cout<<"tfidfV2::externalQuery_computeData: assigning to clusters - DONE\n";

    // cleanup
    delete []descs;

    // sort IDs
    std::vector<uint32_t> inds;
    argSortArray<uint32_t>::sort( queryRep0.id().data(), numFeats * KNN, inds );

    // apply the sort
    rr::indexEntry queryRep= queryRep0; // to get all repeated field sizes correctly
    uint32_t *wordIDs= queryRep.mutable_id()->mutable_data();
    float *weights= queryRep.mutable_weight()->mutable_data();
    float *x= queryRep.mutable_x()->mutable_data();
    float *y= queryRep.mutable_y()->mutable_data();
    float *a= queryRep.mutable_a()->mutable_data();
    float *b= queryRep.mutable_b()->mutable_data();
    float *c= queryRep.mutable_c()->mutable_data();
    uint32_t size= wordIDs0->size();
    for (uint32_t i= 0; i<size; ++i, ++wordIDs, ++weights, ++x, ++y, ++a, ++b, ++c){
        uint32_t ind= inds[i];
        *wordIDs= wordIDs0->Get(ind);
        *weights= weights0->Get(ind);
        *x= x0->Get(ind);
        *y= y0->Get(ind);
        *a= a0->Get(ind);
        *b= b0->Get(ind);
        *c= c0->Get(ind);
    }

    // save to file
    std::ofstream of(queryObj.compDataFn.c_str(), std::ios::binary);
    queryRep.SerializeToOstream(&of);
    of.close();

}


//
// Copyright (c) 2011-2012 Krzysztof Jusiak (krzysztof at jusiak dot net)
//
// 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 QFSM_TEST_COMMON_EQUAL_HPP
#define QFSM_TEST_COMMON_EQUAL_HPP

#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/utility/enable_if.hpp>

namespace QFsm
{
namespace Test
{
namespace Common
{

template<typename T1, typename T2, typename Enable = void> class Equal
{
    struct ContainsAll : boost::mpl::fold
        <
            T1,
            boost::mpl::vector0<>,
            boost::mpl::if_
            <
                boost::mpl::contains<T2, boost::mpl::_2>,
                boost::mpl::_1,
                boost::mpl::push_back<boost::mpl::_1, boost::mpl::_2>
            >
        >::type
    { };

    BOOST_MPL_ASSERT_MSG(
        boost::mpl::size<ContainsAll>::value == 0,
        TypesAreNotEqual,
        (typename ContainsAll::type)
    );

public:
    static const bool value = boost::mpl::size<ContainsAll>::value == 0;
};

template<typename T1, typename T2>
class Equal<T1, T2, typename boost::enable_if< boost::mpl::not_equal_to< boost::mpl::size<T1>, boost::mpl::size<T2> > >::type> : public boost::mpl::false_
{
    BOOST_MPL_ASSERT_MSG(
        false,
        TypesAreNotEqualDueToDifferentSizes,
        (typename boost::mpl::size<T1>::type, typename boost::mpl::size<T2>::type)
    );
};

} // namespace Common
} // namespace Test
} // namespace QFsm

#endif


/******************************
      Author: Joel Veness
        Date: 2011-2013
******************************/

#include <iostream>
#include <fstream>
#include <bitset>
#include <cmath>
#include <sstream>

#include "common.hpp"
#include "ctw.hpp"
#include "facmodels.hpp"
#include "cts.hpp"
#include "ac.hpp"
#include "skipcts.hpp"

// boost includes
#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/filesystem/operations.hpp>

namespace po = boost::program_options;

static po::variables_map options;
static po::options_description options_desc("Usage", 100);

// bit history data structure
history_t history;

/* display the program usage options and quit. */
static void showHelp(void) {

    std::cout << std::endl << options_desc << std::endl;
    exit(0);
}

/* load program options into a variable map */
static void initOptions(int argc, char *argv[], po::variables_map &vm) {

    std::string file_msg;

    file_msg += "File to compress/decompress. A compressed file with the\n"
                "same name plus an extension (e.g. foo.cts) will be\n"
                "produced. Decompression is chosen automatically if\n"
                "the file extension matches a method (i.e. .ctw for\n"
                "ctw.)";

    options_desc.add_options()

        ("help", "")

        ("depth",
         po::value<unsigned int>()->default_value(48),
         "Maximum depth to look back in bits. Higher values use\nmore RAM.")

        ("skips",
         po::value<unsigned int>()->default_value(0),
         "Number of context skips, if a skipping model is used.")

        ("slots",
         po::value<unsigned int>()->default_value(18),
         "The base 2 logarithm of the number of slots in the \n"
          "hashtable (SkipCTS only).")

        ("method",
         po::value<std::string>()->default_value("faccts"),
         "Compression method. (ctw, cts, facctw, faccts, skipcts)")

        ("file",
         po::value<std::string>(),
         file_msg.c_str())
    ;

    po::store(po::parse_command_line(argc, argv, options_desc), vm);
    po::notify(vm);

    if (argc == 1) showHelp();
}

/* checks for illegal combinations of configuration parameters,
   throwing an exception if an illegal combination is detected. */
static void processOptions(const po::variables_map &vm) {

    if (vm.count("help") > 0) showHelp();
}

/* We initialize the context with zeros for convenience. */
void zeroFill(history_t &h, size_t n) {

    for (size_t i=0; i < n; ++i) h.push_back(0);
}

/* create the file compressor, or die if creation fails */
static Compressor *buildCompressor() {

    unsigned int d     = options["depth"].as<unsigned int>();
    unsigned int skips = options["skips"].as<unsigned int>();
    unsigned int slots = options["slots"].as<unsigned int>();

    if (slots < 10) throw std::invalid_argument("slots must be at least 10.");

    // ensure history always has enough context
    history.clear();
    zeroFill(history, d);

    Compressor *c = NULL;
    if (options["method"].as<std::string>() == "cts") {
        c = new SwitchingTree(history, d);
    } else if (options["method"].as<std::string>() == "ctw") {
        c = new ContextTree(history, d);
    } else if (options["method"].as<std::string>() == "facctw") {
        c = new FactoredContextTree(history, d);
    } else if (options["method"].as<std::string>() == "faccts") {
        c = new FactoredSwitchingTree(history, d);
    } else if (options["method"].as<std::string>() == "skipcts") {
        c = new SkipCTS(history, d, skips, slots);
    } else if (options["method"].as<std::string>() == "facskipcts") {

        // MGB: there are unresolved issues with FacSkipCTS.
        std::cerr << "Factored SkipCTS is not currently fully functional. See readme.txt for details." << std::endl;
        std::exit(1);
        // c = new FactoredSkipCTS(history, d);
    }

    if (c == NULL) {
        std::cout << "Unknown compression method." << std::endl;
        std::exit(1);
    }

    return c;
}

/* compress the input file */
static void compress() {

    std::auto_ptr<Compressor> c(buildCompressor());
    std::ifstream::pos_type size;

    // open the file to be compressed
    std::string fn = options["file"].as<std::string>();
    std::ifstream file(fn.c_str(), std::ios::in | std::ios::binary | std::ios::ate);
    if (!file.is_open()) {
        throw std::runtime_error("cannot load or open input file");
    }
    size = file.tellg(); file.seekg(0, std::ios::beg);
    std::cerr << "Compressing " << fn << " (" << size << " bytes)" << std::endl;

    // determine the destination filename
    std::string ext = std::string(".") + c->fileExtension();
    std::string ofn = fn + ext;

    // now compress the file, bit by bit
    BinaryArithmeticEncoder bae(ofn);
    std::ostringstream oss; boost::progress_timer ti(oss);
    boost::progress_display show_progress(static_cast<unsigned long>(size));

    for (int i=0; i < size; ++i) {

        char buffer = file.get();

        for (size_t j=0; j < 8; j++) {

            int bit = buffer & 1;
            double p = c->prob(1);
            double before = c->logBlockProbability();
            bae.encode(bit, p);
            c->update(bit);
            double after = c->logBlockProbability();

            assert(after < before);
            assert(p > 0.0 && p < 1.0);

            buffer >>= 1;
        }
        ++show_progress;
    }
    bae.close();
    file.close();
    std::cerr << std::endl;

    // display compression statistics
    boost::uintmax_t compr_size = boost::filesystem::file_size(ofn) * 8;
    double bpc = double(compr_size) / double(size);
    double ratio = double(compr_size) / double(size*8) * 100.0;
    double kbsec = double(size / 1024) / ti.elapsed();
    std::cout << "Compression statistics: " << std::endl;
    std::cout << "    Ratio:          " <<  ratio << "%" << std::endl;
    std::cout << "    Size:           " <<  (compr_size/8) << " bytes" << std::endl;
    std::cout << "    Bits per Byte:  " <<  bpc << std::endl;
    std::cout << "    Time:           " << ti.elapsed() << "s" << std::endl;
    std::cout << "    KB/s:           " << kbsec << std::endl;
}

/* decompress the input file */
static void decompress() {

    std::string fn  = options["file"].as<std::string>();
    BinaryArithmeticDecoder dec(fn.c_str());
    std::auto_ptr<Compressor> dc(buildCompressor());

    std::cerr << "Decompressing " << fn << " (" << (dec.size()/8) << " bytes)" << std::endl;

    // open destination filename
    std::string decfn = fn.substr(0, fn.find_last_of("."));
    std::ofstream decoded(decfn.c_str(), std::ios::out | std::ios::binary);
    if (!decoded.is_open()) {
        throw std::runtime_error("cannot open/write to ");
    }

    // now decode the file, one bit at a time
    char buf = 0, upto = 0;
    std::ostringstream oss; boost::progress_timer ti(oss);
    boost::progress_display dec_progress(dec.size());
    for (uint32_t i=0; i < dec.size(); ++i) {

        double p = dc->prob(1);

        bit_t bit = dec.decode(p);
        dc->update(bit);

        buf |= char(bit) << upto;
        upto++;
        if (upto == 8) {
            decoded.put(buf);
            buf = 0; upto = 0;
        }

        ++dec_progress;
    }

    dec.close();
    decoded.close();

    // display decompression statistics
    double kbsec = double(dec.size() / 8 / 1024) / ti.elapsed();

    std::cerr << std::endl;
    std::cout << "Decompression statistics: " << std::endl;
    std::cout << "    Time:           " << ti.elapsed() << "s" << std::endl;
    std::cout << "    KB/s:           " << kbsec << std::endl;
}

/* determine whether we are compressing or decompressing */
static bool compressionMode() {

    // if file ends in ".cts" or ".ctw" then we are in decompression mode
    std::string fn  = options["file"].as<std::string>();
    size_t pos = fn.find_last_of(".");
    if (pos == std::string::npos) return true;

    std::string ext = fn.substr(pos);
    if (ext == ".ctw"    || ext == ".cts" ||
        ext == ".faccts" || ext == ".facctw" ||
        ext == ".skipcts" || ext == ".facskipcts") return false;

    return true;
}

/* application entry point */
int main(int argc, char* argv[]) {

    // load program configuration
    initOptions(argc, argv, options);

    // check and apply program configuration
    processOptions(options);

    // determine whether we are compressing or decompressing
    try {
        if (compressionMode()) {
            compress();
        } else {
            decompress();
        }
    } catch (std::exception &e) {
        std::cout << "error: " << e.what() << std::endl;
        return 1;
    }

    return 0;
}