#include "LatexWriter.hpp"
#include "../solvers/FluentTimeResource.hpp"
#include <boost/algorithm/string.hpp>

using namespace moreorg;

namespace templ {
namespace io {

std::string LatexWriter::toLatex(const Mission::Ptr& mission)
{
    std::stringstream ss;
    std::vector<solvers::FluentTimeResource> ftrs = Mission::getResourceRequirements(mission);

    ss << "\\begin{align*}" << std::endl;

    // General available
    ss << "    \\widehat{GA}\n&= \\big \\{";
    ModelPool available = mission->getAvailableResources();
    for(const ModelPool::value_type& v : available)
    {
        const owlapi::model::IRI& model = v.first;
        size_t cardinality = v.second;

        ss << "(" << escape( model.getFragment() ) << "," << cardinality << ")";
    }
    ss << "\\big \\} \\\\" << std::endl;

    // Requirements
    ss << "    STR" << std::endl;
    bool init = false;
    for(const solvers::FluentTimeResource& ftr : ftrs)
    {
        if(!init)
        {
            ss << "     &= \\big \\{";
            init = true;
        } else {
            ss << "     & ";
        }

        ss << toLatex(ftr) << ", \\\\" << std::endl;
    }
    ss << "    & \\big \\}\\\\" << std::endl;

    // Constraints
    ss << "    \\mathcal{X}" << std::endl;
    ss << "    &= \\big \\{ ";

    std::string row;
    for(const Constraint::Ptr& c : mission->getConstraints())
    {
        using namespace solvers::temporal::point_algebra;
        if(c->getCategory() == Constraint::TEMPORAL_QUALITATIVE)
        {
            QualitativeTimePointConstraint::Ptr qtpc =
                dynamic_pointer_cast<QualitativeTimePointConstraint>(c);

            std::string constraintString = qtpc->getLVal()->getLabel() +
                QualitativeTimePointConstraint::TypeSymbol[ qtpc->getType() ]
                + qtpc->getRVal()->getLabel();
            row = wrap(ss, row, constraintString);
        }
    }
    ss << row << " \\\\" << std::endl;
    ss << "    & \\big \\} \\\\" << std::endl;

    // Organization Model
    ss << "    \\mathcal{OM}" << std::endl;
    ss << "    &= \\big \\{ \\\\"  << std::endl;
    ss << "    & \\big \\} \\\\" << std::endl;

    // Timepoints
    ss << "    T\n";
    ss << "     &= \\big \\{ ";
    row = "";
    init = false;
    for(const solvers::temporal::point_algebra::TimePoint::Ptr& tp :
            mission->getTimepoints())
    {
        row = wrap(ss, row, tp->getLabel());
    }
    ss << row <<  " \\\\" << std::endl;
    ss << "    & \\big \\}\\\\" << std::endl;

    // Locations
    ss << "    L" << std::endl;
    init = false;
    for(const symbols::constants::Location::Ptr& location :
            mission->getLocations(true))
    {
        if(!init)
        {
            ss << "     &= \\big \\{ ";
            init = true;
        } else {
            ss << "     & ";
        }
        ss << toLatex(location) << ", \\\\" << std::endl;
    }
    ss << "    & \\big \\} \\\\" << std::endl;

    ss << "\\end{align*}" << std::endl;
    return ss.str();
}

std::string LatexWriter::toLatex(const symbols::constants::Location::Ptr& location)
{
    std::stringstream ss;
    base::Point point = location->getPosition();
    std::string locationLabel = location->getInstanceName();
    locationLabel = suffixNumber(locationLabel);

    ss << escape( locationLabel ) << "= (" << point.x() << ","
        << point.y() << "," << point.z() << ") ";
    return ss.str();
}

std::string LatexWriter::toLatex(const solvers::FluentTimeResource& ftr)
{
    owlapi::model::IRIList functionalities;
    std::stringstream ssAgents;

    moreorg::OrganizationModelAsk ask = ftr.getOrganizationModelAsk();

    ModelPool pool = ftr.getMinCardinalities();
    for(ModelPool::value_type& v : pool)
    {
        const owlapi::model::IRI& model = v.first;
        size_t count  = v.second;
        if(ask.ontology().isSubClassOf(model, vocabulary::OM::Functionality()))
        {
            functionalities.push_back(model);
        } else {
            ssAgents << "(" << escape( model.getFragment() ) << "," << count << ")";
        }
    }

    std::stringstream ss;
    ss << "(";
    Resource::Set resources = ftr.getRequiredResources();
    if(resources.empty())
    {
        ss << "\\emptyset";
    } else {
        ss << "\\{";
        Resource::Set::const_iterator cit = resources.begin();
        for(; cit != resources.end(); ++cit)
        {
            const Resource& r = *cit;
            ss << r.getModel().getFragment();
            if( std::distance(cit,resources.end()) != 1)
            {
                ss << ",";
            }
        }
        ss << "\\}";
    }
    ss << ",";
    ss << "\\{" << ssAgents.str() << "\\})";
    ss << "@(" << suffixNumber( ftr.getLocation()->getInstanceName() ) << ",";
    ss << "[" << suffixNumber( ftr.getInterval().getFrom()->getLabel() )
        << "," << suffixNumber( ftr.getInterval().getTo()->getLabel() )
        << "])";

   return ss.str();
}

std::string LatexWriter::suffixNumber(const std::string& label)
{
    std::string newlabel = label;
    std::vector<char> numbers{'0','1','2','3','4','5','6','7','8','9'};
    std::string::iterator it = std::find_first_of(newlabel.begin(), newlabel.end(), numbers.begin(), numbers.end());
    std::string suffixStart("_{");
    if(it != newlabel.end())
    {
        newlabel.insert(it,suffixStart.begin(), suffixStart.end());
        newlabel += "}";
    }
    return newlabel;
}

std::string LatexWriter::escape(const std::string& label,
        const std::string& env)
{
    std::string escapedLabel = label;
    boost::replace_all(escapedLabel, "_", + "\\_");
    if(!env.empty())
    {
        return env + "{" + escapedLabel + "}";
    } else {
        return escapedLabel;
    }
}

std::string LatexWriter::wrap(std::ostream& os,
        const std::string& currentRow,
        const std::string& newLabel,
        const std::string& suffixNoWrap,
        const std::string& suffixWrap,
        size_t lineWidth)
{
    std::string row;
    // Check row length
    if(currentRow.size() + newLabel.size() > lineWidth)
    {
        // write the remaining label
        os << currentRow <<  suffixWrap;
        row = "    & " + newLabel + suffixNoWrap;
    } else {
        row = currentRow + newLabel + suffixNoWrap;
    }
    return row;
}

} // end namespace io
} // end namespace templ


// ---------------------------------------------------------------------
//
// Copyright (c) 2017 - 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 <IBAMR_config.h>
#include <IBTK_config.h>

#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>
#include <VariableDatabase.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/CIBMethod.h>
#include <ibamr/CIBMobilitySolver.h>
#include <ibamr/CIBSaddlePointSolver.h>
#include <ibamr/CIBStaggeredStokesSolver.h>
#include <ibamr/DirectMobilitySolver.h>
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBHydrodynamicForceEvaluator.h>
#include <ibamr/IBStandardForceGen.h>
#include <ibamr/IBStandardInitializer.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>
#include <ibamr/INSStaggeredPressureBcCoef.h>
#include <ibamr/KrylovMobilitySolver.h>
#include <ibamr/app_namespaces.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/LData.h>
#include <ibtk/LDataManager.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

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

// Center of mass velocity
void
ConstrainedCOMOuterVel(double /*data_time*/, IBTK::Vector3d& U_com, IBTK::Vector3d& W_com, void* /*ctx*/)
{
    U_com.setZero();
    W_com.setZero();
    U_com[0] = 1.0;
    U_com[1] = 2.0;
    W_com[1] = 5.0;

    return;
} // ConstrainedCOMOuterVel

// Center of mass velocity
void
ConstrainedCOMInnerVel(double /*data_time*/, IBTK::Vector3d& U_com, IBTK::Vector3d& W_com, void* /*ctx*/)
{
    U_com.setZero();
    W_com.setZero();
    return;
} // ConstrainedCOMInnerVel

void
ConstrainedNodalVel(Vec /*U_k*/, const RigidDOFVector& /*U*/, const IBTK::Vector3d& /*X_com*/, void* /*ctx*/)
{
    // intentionally left blank.
    return;
} // ConstrainedNodalVel

// These forces on the structure are computed when the "above" velocities are prescribed on them.
void
NetExternalForceTorqueOuter(double /*data_time*/, IBTK::Vector3d& F_ext, IBTK::Vector3d& T_ext, void* /*ctx*/)
{
    F_ext << 100.916, 1351.74, 0.000916801;
    T_ext << -2.23858e-09, 58206.1, -1.12567e-09;

    return;
} // NetExternalForceTorqueOuter

void
NetExternalForceTorqueInner(double /*data_time*/, IBTK::Vector3d& F_ext, IBTK::Vector3d& T_ext, void* /*ctx*/)
{
    F_ext << -116.762, -201.82, -0.00128781;
    T_ext << 1.46423e-09, -1393.33, 9.03505e-10;

    return;
} // NetExternalForceTorqueInner

// Function prototypes
void output_data(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                 LDataManager* l_data_manager,
                 const int iteration_num,
                 const double loop_time,
                 const string& data_dump_dirname);

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/
int
main(int argc, char* argv[])
{
    // Initialize PETSc, MPI, and SAMRAI.
    PetscInitialize(&argc, &argv, NULL, NULL);
    SAMRAI_MPI::setCommunicator(PETSC_COMM_WORLD);
    SAMRAI_MPI::setCallAbortInSerialInsteadOfExit();
    SAMRAIManager::startup();
    SAMRAIManager::setMaxNumberPatchDataEntries(2054);

    { // cleanup dynamically allocated objects prior to shutdown

        // 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, "CIB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Read default Petsc options
        if (input_db->keyExists("petsc_options_file"))
        {
            std::string petsc_options_file = input_db->getString("petsc_options_file");
            PetscOptionsInsertFile(PETSC_COMM_WORLD, NULL, petsc_options_file.c_str(), PETSC_TRUE);
        }

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && !app_initializer->getVisItDataWriter().isNull();

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // 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.

        // INS integrator
        Pointer<INSStaggeredHierarchyIntegrator> navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
            "INSStaggeredHierarchyIntegrator",
            app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));

        // CIB method
        const unsigned int num_structures = input_db->getIntegerWithDefault("num_structures", 1);
        Pointer<CIBMethod> ib_method_ops =
            new CIBMethod("CIBMethod", app_initializer->getComponentDatabase("CIBMethod"), num_structures);

        // Krylov solver for INS integrator that solves for [u,p,U,L]
        Pointer<CIBStaggeredStokesSolver> CIBSolver =
            new CIBStaggeredStokesSolver("CIBStaggeredStokesSolver",
                                         input_db->getDatabase("CIBStaggeredStokesSolver"),
                                         navier_stokes_integrator,
                                         ib_method_ops,
                                         "SP_");

        // Register the Krylov solver with INS integrator
        navier_stokes_integrator->setStokesSolver(CIBSolver);

        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator);
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);
        // Configure the IB solver.
        Pointer<IBStandardInitializer> ib_initializer = new IBStandardInitializer(
            "IBStandardInitializer", app_initializer->getComponentDatabase("IBStandardInitializer"));
        ib_method_ops->registerLInitStrategy(ib_initializer);

        // Specify structure kinematics
        FreeRigidDOFVector outer_free_dofs, inner_free_dofs;

        // Make some DOFs prescribed and some free.
        outer_free_dofs << 0, 0, 1, 1, 1, 1;
        inner_free_dofs << 1, 1, 0, 1, 1, 0;
        ib_method_ops->setSolveRigidBodyVelocity(0, outer_free_dofs);
        ib_method_ops->setSolveRigidBodyVelocity(1, inner_free_dofs);

        ib_method_ops->registerConstrainedVelocityFunction(NULL, &ConstrainedCOMOuterVel, NULL, 0);
        ib_method_ops->registerConstrainedVelocityFunction(NULL, &ConstrainedCOMInnerVel, NULL, 1);

        ib_method_ops->registerExternalForceTorqueFunction(&NetExternalForceTorqueOuter, NULL, 0);
        ib_method_ops->registerExternalForceTorqueFunction(&NetExternalForceTorqueInner, NULL, 1);

        // Create initial condition specification objects.
        Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
            "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
        navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
            "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
        navier_stokes_integrator->registerPressureInitialConditions(p_init);

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        Pointer<LSiloDataWriter> silo_data_writer = app_initializer->getLSiloDataWriter();
        if (uses_visit)
        {
            ib_initializer->registerLSiloDataWriter(silo_data_writer);
            ib_method_ops->registerLSiloDataWriter(silo_data_writer);
            ib_method_ops->registerVisItDataWriter(visit_data_writer);
            time_integrator->registerVisItDataWriter(visit_data_writer);
        }

        // Create boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                Pointer<Database> bc_coefs_db = app_initializer->getComponentDatabase(bc_coefs_db_name);
                u_bc_coefs[d] = new muParserRobinBcCoefs(bc_coefs_name, bc_coefs_db, grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        // Initialize hierarchy configuration and data on all patches.
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Set physical boundary operator used in spreading.
        ib_method_ops->setVelocityPhysBdryOp(time_integrator->getVelocityPhysBdryOp());

        // Register mobility matrices (if needed)
        std::string mobility_solver_type = input_db->getString("MOBILITY_SOLVER_TYPE");
        if (mobility_solver_type == "DIRECT")
        {
            std::string mat_name1 = "struct-1";
            std::string mat_name2 = "struct-2";
            std::vector<std::vector<unsigned> > struct_ids1;
            std::vector<std::vector<unsigned> > struct_ids2;
            std::vector<unsigned> prototype_structs1;
            std::vector<unsigned> prototype_structs2;

            // Dense matrix
            prototype_structs1.push_back(0);
            prototype_structs2.push_back(1);

            struct_ids1.push_back(prototype_structs1);
            struct_ids2.push_back(prototype_structs2);

            DirectMobilitySolver* direct_solvers = NULL;
            CIBSolver->getSaddlePointSolver()->getCIBMobilitySolver()->getMobilitySolvers(NULL, &direct_solvers, NULL);

            direct_solvers->registerMobilityMat(
                mat_name1, prototype_structs1, EMPIRICAL, std::make_pair(LAPACK_SVD, LAPACK_SVD), 0);
            direct_solvers->registerStructIDsWithMobilityMat(mat_name1, struct_ids1);

            int next_proc = 0;
            if (SAMRAI_MPI::getNodes() > 1) next_proc = 1;
            direct_solvers->registerMobilityMat(
                mat_name2, prototype_structs2, EMPIRICAL, std::make_pair(LAPACK_SVD, LAPACK_SVD), next_proc);
            direct_solvers->registerStructIDsWithMobilityMat(mat_name2, struct_ids2);
        }
        navier_stokes_integrator->setStokesSolverNeedsInit();

        // Set up the hydro force objects
        double rho_fluid = input_db->getDouble("RHO");
        double mu_fluid = input_db->getDouble("MU");
        double start_time = time_integrator->getIntegratorTime();
        Pointer<IBHydrodynamicForceEvaluator> hydro_force =
            new IBHydrodynamicForceEvaluator("IBHydrodynamicForce", rho_fluid, mu_fluid, start_time, true);

        // Get the initial box position and velocity from input
        const string init_hydro_force_box_out_db_name = "InitHydroForceBox_0";
        IBTK::Vector3d box_X_lower_out, box_X_upper_out, box_init_vel_out;

        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("lower_left_corner", &box_X_lower_out[0], 3);
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("upper_right_corner", &box_X_upper_out[0], 3);
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("init_velocity", &box_init_vel_out[0], 3);

        const string init_hydro_force_box_in_db_name = "InitHydroForceBox_1";
        IBTK::Vector3d box_X_lower_in, box_X_upper_in, box_init_vel_in;

        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("lower_left_corner", &box_X_lower_in[0], 3);
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("upper_right_corner", &box_X_upper_in[0], 3);
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("init_velocity", &box_init_vel_in[0], 3);

        // Register the control volumes
        hydro_force->registerStructure(box_X_lower_out, box_X_upper_out, patch_hierarchy, box_init_vel_out, 0);
        hydro_force->registerStructure(box_X_lower_in, box_X_upper_in, patch_hierarchy, box_init_vel_in, 1);

        // Set up optional visualization of select boxes
        hydro_force->registerStructurePlotData(visit_data_writer, patch_hierarchy, 0);
        hydro_force->registerStructurePlotData(visit_data_writer, patch_hierarchy, 1);

        // Set the origin of the torque evaluation
        IBTK::Vector3d torque_origin_out, torque_origin_in;
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("torque_origin", &torque_origin_out[0], 3);
        input_db->getDatabase(init_hydro_force_box_out_db_name)
            ->getDoubleArray("torque_origin", &torque_origin_in[0], 3);

        hydro_force->setTorqueOrigin(torque_origin_out, 0);
        hydro_force->setTorqueOrigin(torque_origin_in, 1);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Get velocity and pressure variables from integrator
        VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();

        const Pointer<Variable<NDIM> > u_var = navier_stokes_integrator->getVelocityVariable();
        const Pointer<VariableContext> u_ctx = navier_stokes_integrator->getCurrentContext();
        const int u_idx = var_db->mapVariableAndContextToIndex(u_var, u_ctx);

        const Pointer<Variable<NDIM> > p_var = navier_stokes_integrator->getPressureVariable();
        const Pointer<VariableContext> p_ctx = navier_stokes_integrator->getCurrentContext();
        const int p_idx = var_db->mapVariableAndContextToIndex(p_var, p_ctx);

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();

        if (dump_viz_data && uses_visit)
        {
            pout << "\n\nWriting visualization files...\n\n";
            time_integrator->setupPlotData();
            visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            silo_data_writer->writePlotData(iteration_num, loop_time);
        }
        if (dump_postproc_data)
        {
            output_data(patch_hierarchy,
                        ib_method_ops->getLDataManager(),
                        iteration_num,
                        loop_time,
                        postproc_data_dump_dirname);
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double current_time, new_time;
        double dt = 0.0;

        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();
            current_time = loop_time;

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            loop_time += dt;
            new_time = loop_time;

            pout << "Advancing hierarchy by timestep size dt = " << dt << "\n";

            if (time_integrator->atRegridPoint()) navier_stokes_integrator->setStokesSolverNeedsInit();
            if (ib_method_ops->flagRegrid())
            {
                time_integrator->regridHierarchy();
                navier_stokes_integrator->setStokesSolverNeedsInit();
            }

            // Update the location of the box for time n + 1
            hydro_force->updateStructureDomain(IBTK::Vector3d::Zero(), dt, patch_hierarchy, 0);
            hydro_force->updateStructureDomain(IBTK::Vector3d::Zero(), dt, patch_hierarchy, 1);

            // Compute the momentum of u^n in box n+1 on the newest hierarchy
            hydro_force->computeLaggedMomentumIntegral(
                u_idx, patch_hierarchy, navier_stokes_integrator->getVelocityBoundaryConditions());

            // Advance the hierarchy
            time_integrator->advanceHierarchy(dt);

            pout << "\n\nNet rigid force and torque on structure 0 is : \n"
                 << ib_method_ops->getNetRigidGeneralizedForce(0) << "\n\n";
            pout << "\n\nNet rigid force and torque on structure 1 is : \n"
                 << ib_method_ops->getNetRigidGeneralizedForce(1) << "\n\n";

            RDV U;
            ib_method_ops->getNewRigidBodyVelocity(0, U);
            pout << "\n\nRigid body velocity of structure 0 is : \n" << U << "\n\n";
            ib_method_ops->getNewRigidBodyVelocity(1, U);
            pout << "\n\nRigid body velocity of structure 1 is : \n" << U << "\n\n";

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // Set the linear and angular momentum of the spheres (zero in Stokes flow)
            IBTK::Vector3d Sphere_mom;
            IBTK::Vector3d Sphere_ang_mom;
            Sphere_mom.setZero();
            Sphere_ang_mom.setZero();

            // Store the new momenta of the sphere
            hydro_force->updateStructureMomentum(Sphere_mom, Sphere_ang_mom, 0);
            hydro_force->updateStructureMomentum(Sphere_mom, Sphere_ang_mom, 1);

            // Evaluate hydrodynamic force on the sphere.
            hydro_force->computeHydrodynamicForce(u_idx,
                                                  p_idx,
                                                  /*f_idx*/ -1,
                                                  patch_hierarchy,
                                                  dt,
                                                  navier_stokes_integrator->getVelocityBoundaryConditions(),
                                                  navier_stokes_integrator->getPressureBoundaryConditions());
            // Post processing call for writing data
            hydro_force->postprocessIntegrateData(current_time, new_time);

            // Update optional visualization of select boxes
            hydro_force->updateStructurePlotData(patch_hierarchy, 0);
            hydro_force->updateStructurePlotData(patch_hierarchy, 1);

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && uses_visit && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                silo_data_writer->writePlotData(iteration_num, loop_time);
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                output_data(patch_hierarchy,
                            ib_method_ops->getLDataManager(),
                            iteration_num,
                            loop_time,
                            postproc_data_dump_dirname);
            }
        }

        // Cleanup boundary condition specification objects (when necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown

    SAMRAIManager::shutdown();
    PetscFinalize();
} // main

void
output_data(Pointer<PatchHierarchy<NDIM> > /*patch_hierarchy*/,
            LDataManager* /*l_data_manager*/,
            const int iteration_num,
            const double loop_time,
            const string& /*data_dump_dirname*/)
{
    plog << "writing hierarchy data at iteration " << iteration_num << " to disk" << endl;
    plog << "simulation time is " << loop_time << endl;

    return;
} // output_data


/**
 * @example studies/reverberation/reverb_extra_test.cc
 *
 * Perform reverb tests that run too slow to be included
 * in the normal suite of regression tests.
 */
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <usml/ocean/ocean_shared.h>
#include <usml/sensors/beams.h>
#include <usml/sensors/sensors.h>
#include <usml/eigenverb/wavefront_generator.h>
#include <list>
#include <boost/progress.hpp>
#include <boost/foreach.hpp>

BOOST_AUTO_TEST_SUITE(reverb_extra_test)

using namespace boost::unit_test;
using namespace usml::waveq3d;

static const double time_step = 0.100 ;
static const double src_lat = 45.0;        // location = mid-Atlantic
static const double src_lng = -45.0;
static const double c0 = 1500.0;           // constant sound speed

/**
 * Tests the the envelope generation process using conditions like
 * those used in the eigenverb_demo.m scenario.
 *
 *   - Profile: constant 1500 m/s sound speed, no absorption
 *   - Surface: Perfectly reflecting, Lambert scattering with -37.0 dB
 *   - Bottom: 200 meters, sand, Lambert scattering with -27.0 dB
 *   - Source: 45N, 45W, on surface, 1000 Hz
 *   - Interfaces: bottom and surface, but limit to 2 bounces
 *   - Time Step: 100 msec
 *   - Launch D/E: 91 tangent spaced rays from -90 to +90 degrees
 *   - Launch AZ: Rays in the range [0,360) with 20 degree spacing.
 *
 * The primary motivation for this test is to generate an envelope netCDF
 * file that can be used to support off-line comparisons to the
 * eigenverb_demo.m scenario.  It also serves as a standalone prototype
 * for the calculations in the envelope_generator::run() method.
 *
 * This test artificially limits its processing to downward D/E launch
 * angles and receivers AZ launch angles to zero to match the conditions
 * used in eigenverb_demo.m.
 *
 * It also artificially limits the initial time (using the round trip time to
 * bottom) to test the ability to limit the initial time. Such a limitation is
 * not normally used for a monostatic sensor.
 */
BOOST_AUTO_TEST_CASE( envelope_analytic ) {
    cout << "=== eigenverb_test: envelope_analytic ===" << endl;
    const char* ncname = USML_STUDIES_DIR "/reverberation/envelope_analytic_";
    const char* ncname_env = USML_STUDIES_DIR "/reverberation/envelope_analytic_env.nc";
    const double time_max = 4.0;
    const double depth = 200.0 ;
    const double threshold = 1e-17 ;

    // initialize propagation model

    attenuation_model* attn = new attenuation_constant(0.0);
	profile_model* profile = new profile_linear(c0, attn);

	boundary_model* surface = new boundary_flat();
	surface->scattering( new scattering_lambert(-37.0)  ) ;

	reflect_loss_model* bottom_loss = new reflect_loss_rayleigh(reflect_loss_rayleigh::SAND) ;
	boundary_model* bottom = new boundary_flat(depth,bottom_loss);
	bottom->scattering( new scattering_lambert()  ) ;

	ocean_model ocean(surface, bottom, profile);

    seq_log freq( 1000.0, 10.0, 1 );
    wposition1 pos( src_lat, src_lng, 0.0 );
    seq_rayfan de( -90.0, 0.0, 91 );
    seq_linear az( 0.0, 20.0, 359.0 );

    // build a wavefront that generates eigenverbs

    eigenverb_collection* eigenverbs = new eigenverb_collection( ocean.num_volume() ) ;
    wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
    wave.add_eigenverb_listener(eigenverbs) ;
    wave.max_bottom(2) ;
    wave.max_surface(2) ;

    // propagate wavefront and compute eigenverbs
    // time the duration of this calculation

    cout << "computing eigenverbs" << endl;
    {
		boost::progress_timer timer ;
		while ( wave.time() < time_max ) {
			wave.step();
		}
    }

    // record eigenverbs for each interface to their own disk file

    for ( int n=0 ; n < eigenverbs->num_interfaces() ; ++n ) {
    	std::ostringstream filename ;
    	filename << ncname << n << ".nc" ;
        cout << "writing eigenverbs to " << filename.str().c_str() << endl;
    	eigenverbs->write_netcdf( filename.str().c_str(),n) ;
    }

	// construct an envelope_collection

	const seq_vector* travel_time = new seq_linear(0.0,0.001,70.0) ;
	envelope_collection envelopes(
		&freq,			// envelope_freq
		0,				// src_freq_first
		travel_time,	// travel_time, cloned by model
		7.0,			// reverb_duration
		0.25, 			// pulse_length
		threshold,		// threshold
		az.size(), 		// num_azimuths
		1, 				// num_src_beams
		1,              // num_rcv_beams
		2.0*depth/c0,   // initial_time = first fathometer
        1,              // source_id - fill the api
        1,              // receiver_id - fill the api
        wposition1(src_lat, src_lng),   // src_pos - fill the api
        wposition1(src_lat, src_lng) ); // rcv_pos - fill the api
    delete travel_time;

    // compute the envelope from eigenverbs on each interface

	vector<double> scatter( freq.size() ) ;
	matrix<double> src_beam( freq.size(), 1, 1.0 ) ;
	matrix<double> rcv_beam( freq.size(), 1, 1.0 ) ;

    cout << "computing envelopes" << endl;
    {
//    	int n=0 ;
		boost::progress_timer timer ;
		double distance_threshold = 6.0 ;
		for ( size_t interface_num=0 ; interface_num < 1 ; ++interface_num) {
			BOOST_FOREACH( eigenverb rcv_verb, eigenverbs->eigenverbs(interface_num) ) {
//				cout << "rcv_verb #" << n++ << endl ;
				if ( abs(rcv_verb.source_az) > 1e-6 ) continue ; // just for this test
				BOOST_FOREACH( eigenverb src_verb, eigenverbs->eigenverbs(interface_num) ) {

					// determine relative range and bearing between the projected Gaussians
					// skip this combo if source peak too far away

					double bearing ;
					const double range = rcv_verb.position.gc_range( src_verb.position, &bearing ) ;
					if ( range > distance_threshold * max(rcv_verb.length,rcv_verb.width)) continue ;

					if ( range < 1e-6 ) bearing = 0 ;	// fixes bearing = NaN
					bearing -= rcv_verb.direction ;		// relative bearing

					const double ys = range * cos( bearing ) ;
					const double ys2 = ys * ys ;
					if ( abs(ys) > distance_threshold * rcv_verb.length ) continue ;

					const double xs = range * sin( bearing ) ;
					const double xs2 = xs * xs ;
					if ( abs(xs) > distance_threshold * rcv_verb.width ) continue ;

					// compute interface_num scattering strength
					// continue if scattering strength does not meet threshold

					ocean.bottom().scattering(
						rcv_verb.position, freq,
						src_verb.grazing, rcv_verb.grazing,
						src_verb.direction, rcv_verb.direction,
						&scatter )  ;

					// create envelope contribution

					envelopes.add_contribution( src_verb, rcv_verb,
							src_beam, rcv_beam, scatter, xs2, ys2 ) ;
				}
			}
		}
    }
    cout << "writing envelopes to " << ncname_env << endl;
	envelopes.write_netcdf(ncname_env) ;

    // clean up and exit

    delete eigenverbs ;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


#ifndef ASLAM_CAMERAS_OMNI_PROJECTION_HPP
#define ASLAM_CAMERAS_OMNI_PROJECTION_HPP

#include "StaticAssert.hpp"
#include "FiniteDifferences.hpp"
#include <sm/PropertyTree.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <aslam/cameras/GridCalibrationTargetObservation.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <sm/logging.hpp>

namespace aslam {
namespace cameras {

template<typename DISTORTION_T>
class OmniProjection {
 public:

  enum {
    KeypointDimension = 2
  };
  enum {
    IntrinsicsDimension = 5
  };
  enum {
    DesignVariableDimension = IntrinsicsDimension
  };

  typedef DISTORTION_T distortion_t;
  typedef Eigen::Matrix<double, KeypointDimension, 1> keypoint_t;
  typedef Eigen::Matrix<double, KeypointDimension, IntrinsicsDimension> jacobian_intrinsics_t;

  /// \brief Default constructor
  OmniProjection();

  OmniProjection(double xi, double focalLengthU, double focalLengthV,
                 double imageCenterU, double imageCenterV, int resolutionU,
                 int resolutionV, distortion_t distortion);

  OmniProjection(double xi, double focalLengthU, double focalLengthV,
                 double imageCenterU, double imageCenterV, int resolutionU,
                 int resolutionV);

  OmniProjection(const sm::PropertyTree & config);

  /// \brief destructor.
  virtual ~OmniProjection();

  /// \brief resize the intrinsics based on a scaling of the image.
  void resizeIntrinsics(double scale);

  template<typename DERIVED_P, typename DERIVED_K>
  bool euclideanToKeypoint(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P> & p,
                           const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
                           const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  template<typename DERIVED_P, typename DERIVED_K>
  bool homogeneousToKeypoint(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P> & p,
                             const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
                             const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  template<typename DERIVED_K, typename DERIVED_P>
  bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                           const Eigen::MatrixBase<DERIVED_P> & outPoint) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                           const Eigen::MatrixBase<DERIVED_P> & outPoint,
                           const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_K, typename DERIVED_P>
  bool keypointToHomogeneous(
      const Eigen::MatrixBase<DERIVED_K> & keypoint,
      const Eigen::MatrixBase<DERIVED_P> & outPoint) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                             const Eigen::MatrixBase<DERIVED_P> & outPoint,
                             const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void euclideanToKeypointIntrinsicsJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JD>
  void euclideanToKeypointDistortionJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void homogeneousToKeypointIntrinsicsJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JD>
  void homogeneousToKeypointDistortionJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;

  template<typename DERIVED_K>
  bool isValid(const Eigen::MatrixBase<DERIVED_K> & keypoint) const;

  template<typename DERIVED_K>
  bool isLiftable(const Eigen::MatrixBase<DERIVED_K> & keypoint) const;

  bool isUndistortedKeypointValid(const double rho2_d) const;

  template<typename DERIVED_P>
  bool isEuclideanVisible(const Eigen::MatrixBase<DERIVED_P> & p) const;

  template<typename DERIVED_P>
  bool isHomogeneousVisible(const Eigen::MatrixBase<DERIVED_P> & ph) const;

  /// \brief initialize the intrinsics based on one view of a gridded calibration target
  /// \return true on success
  bool initializeIntrinsics(const std::vector<GridCalibrationTargetObservation> &observations);

  /// \brief estimate the transformation of the camera with respect to the calibration target
  ///        On success out_T_t_c is filled in with the transformation that takes points from
  ///        the camera frame to the target frame
  /// \return true on success
  bool estimateTransformation(const GridCalibrationTargetObservation & obs,
                              sm::kinematics::Transformation & out_T_t_c) const;

  /// \brief compute the reprojection error based on a checkerboard observation.
  /// \return the number of corners successfully observed and projected
  size_t computeReprojectionError(
      const GridCalibrationTargetObservation & obs,
      const sm::kinematics::Transformation & T_target_camera,
      double & outErr) const;

  // aslam::backend compatibility
  void update(const double * v);
  int minimalDimensions() const;
  void getParameters(Eigen::MatrixXd & P) const;
  void setParameters(const Eigen::MatrixXd & P);
  Eigen::Vector2i parameterSize() const;

  enum {
    CLASS_SERIALIZATION_VERSION = 0
  };BOOST_SERIALIZATION_SPLIT_MEMBER();
  template<class Archive>
  void load(Archive & ar, const unsigned int version);
  template<class Archive>
  void save(Archive & ar, const unsigned int version) const;

  /// \brief creates a random valid keypoint.
  virtual Eigen::VectorXd createRandomKeypoint() const;

  /// \brief creates a random visible point. Negative depth means random between 0 and 100 meters.
  virtual Eigen::Vector3d createRandomVisiblePoint(double depth = -1.0) const;

  /// \brief is the projection invertible?
  bool isProjectionInvertible() const {
    return false;
  }

  /// \brief set the distortion model.
  void setDistortion(const distortion_t & distortion) {
    _distortion = distortion;
  }
  /// \brief returns a reference to the distortion model
  distortion_t & distortion() {
    return _distortion;
  }
  ;
  const distortion_t & distortion() const {
    return _distortion;
  }
  ;

  Eigen::Matrix3d getCameraMatrix() {
    Eigen::Matrix3d K;
    K << _fu, 0.0, _cu, 0.0, _fv, _cv, 0.0, 0.0, 1.0;
    return K;
  }
  ;

  /// \brief the xi parameter that controls the spherical projection.
  double xi() const {
    return _xi;
  }
  /// \brief The horizontal focal length in pixels.
  double fu() const {
    return _fu;
  }
  /// \brief The vertical focal length in pixels.
  double fv() const {
    return _fv;
  }
  /// \brief The horizontal image center in pixels.
  double cu() const {
    return _cu;
  }
  /// \brief The vertical image center in pixels.
  double cv() const {
    return _cv;
  }
  /// \brief The horizontal resolution in pixels.
  int ru() const {
    return _ru;
  }
  /// \brief The vertical resolution in pixels.
  int rv() const {
    return _rv;
  }
  /// \brief The horizontal resolution in pixels.
  int width() const {
    return _ru;
  }
  /// \brief The vertical resolution in pixels.
  int height() const {
    return _rv;
  }

  double focalLengthCol() const {
    return _fu;
  }
  double focalLengthRow() const {
    return _fv;
  }
  double opticalCenterCol() const {
    return _cu;
  }
  double opticalCenterRow() const {
    return _cv;
  }

  int keypointDimension() const {
    return KeypointDimension;
  }

  bool isBinaryEqual(const OmniProjection<distortion_t> & rhs) const;

  static OmniProjection<distortion_t> getTestProjection();
 private:

  /// \brief the xi parameter that controls the spherical projection.
  double _xi;
  /// \brief The horizontal focal length in pixels.
  double _fu;
  /// \brief The vertical focal length in pixels.
  double _fv;
  /// \brief The horizontal image center in pixels.
  double _cu;
  /// \brief The vertical image center in pixels.
  double _cv;
  /// \brief The horizontal resolution in pixels.
  int _ru;
  /// \brief The vertical resolution in pixels.
  int _rv;

  /// \brief A computed value for speeding up computation.
  void updateTemporaries();

  double _recip_fu;
  double _recip_fv;
  double _fu_over_fv;
  double _one_over_xixi_m_1;
  double _fov_parameter;  //PM: is = xi for xi=<1, = 1/xi for x>1. Used for determining valid projections. Better name?

  distortion_t _distortion;

};

}  // namespace cameras
}  // namespace aslam

#include "implementation/OmniProjection.hpp"

SM_BOOST_CLASS_VERSION_T1 (aslam::cameras::OmniProjection);

#endif /* ASLAM_CAMERAS_OMNI_PROJECTION_HPP */


////////////////////////////////////////////////////////////////////////////////
#include "Mesh.h"
#include <cellogram/State.h>
#include <cellogram/delaunay.h>
#include <cellogram/load_points.h>
#include <cellogram/convex_hull.h>
#include <cellogram/delaunay.h>
#include <cellogram/tri2hex.h>
#include <cellogram/voronoi.h>
#include <cellogram/vertex_degree.h>
#include <cellogram/laplace_energy.h>
#include <points_untangler/points_untangler.h>
#include <igl/list_to_matrix.h>
#include <igl/bounding_box.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <fstream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

	namespace
	{
		template<typename T>
		void removeRow(T& matrix, unsigned int rowToRemove)
		{
			unsigned int numRows = matrix.rows() - 1;
			unsigned int numCols = matrix.cols();

			if (rowToRemove < numRows)
				matrix.block(rowToRemove, 0, numRows - rowToRemove, numCols) = matrix.bottomRows(numRows - rowToRemove);

			matrix.conservativeResize(numRows, numCols);
		}

		void replaceTriangles(const Eigen::MatrixXi tNew, const Eigen::MatrixXi &to_remove, Eigen::MatrixXi &triangles)
		{
			std::vector<int> removeIdx(to_remove.data(), to_remove.data() + to_remove.rows() * to_remove.cols());
			std::sort(removeIdx.begin(), removeIdx.end());
			// remove all the triangles that connect to the internal of ROI
			for (int i = removeIdx.size() - 1; i >= 0; i--)
			{
				removeRow(triangles, removeIdx[i]);
			}

			//std::cout << tNew << "\n\n" << std::endl;

			// add new rows at the end of triangles
			Eigen::MatrixXi tmp = triangles;
			triangles.resize(triangles.rows() + tNew.rows(), triangles.cols());

			triangles << tmp, tNew;
		}

		template<typename Matrix>
		bool load_data(const std::string & path, Matrix & data_matrix)
		{
			typedef typename Matrix::Scalar Scalar;
			std::fstream file;
			file.open(path);

			if (!file.good())
			{
				std::cerr << "Failed to open file : " << path << std::endl;
				file.close();
				return false;
			}

			std::string s;
			std::vector<std::vector<Scalar>> matrix;

			while (getline(file, s))
			{
				std::stringstream input(s);
				double temp;
				matrix.emplace_back();

				auto &currentLine = matrix.back();

				while (input >> temp)
					currentLine.push_back(temp);
			}

			if (!igl::list_to_matrix(matrix, data_matrix))
			{
				std::cerr << "list to matrix error" << std::endl;
				file.close();
				return false;
			}

			return true;
		}
	}

	bool Mesh::load(const nlohmann::json &data)
	{
		params = {};
		detected.resize(0, 0); // detected (unmoved) point positions
		moved.resize(0, 0); // manually moved
		points.resize(0, 0); // relaxed point positions
		triangles.resize(0, 0); // triangular mesh
		adj.clear(); // adjaceny list of triangluar mesh
		vertex_to_tri.clear();
		boundary.resize(0); // list of vertices on the boundary
		vertex_status_fixed.resize(0);

		read_json_mat(data["detected"], detected);
		read_json_mat(data["points"], points);
		read_json_mat(data["moved"], moved);
		read_json_mat(data["triangles"], triangles);

		params.load(data["params"]);

		solved_vertex.resize(points.rows(), 1);
		solved_vertex.setConstant(false);

		adjacency_list(triangles, adj);
		generate_vertex_to_tri();

		vertex_status_fixed.resize(points.rows(), 1);
		vertex_status_fixed.setZero();

		return true;
	}

	// bool Mesh::load(const std::string & path)
	// {
	// 	if (path.empty()) { return false; }
	// 	// clear previous
	// 	params = {};
	// 	detected.resize(0, 0); // detected (unmoved) point positions
	// 	moved.resize(0, 0); // manually moved
	// 	points.resize(0, 0); // relaxed point positions
	// 	triangles.resize(0, 0); // triangular mesh
	// 	adj.clear(); // adjaceny list of triangluar mesh
	// 	vertex_to_tri.clear();
	// 	boundary.resize(0); // list of vertices on the boundary
	// 	vertex_status_fixed.resize(0);

	// 	// Load data
	// 	load_data(path + "/cellogram/detected.vert", detected);
	// 	load_data(path + "/cellogram/points.vert", points);
	// 	load_data(path + "/cellogram/moved.vert", moved);
	// 	load_data(path + "/cellogram/mesh.tri", triangles);

	// 	// change when moved is also saved
	// 	//moved = detected;

	// 	params.load(path + "/cellogram/params.json");

	// 	solved_vertex.resize(points.rows(), 1);
	// 	solved_vertex.setConstant(false);

	// 	adjacency_list(triangles, adj);
	// 	generate_vertex_to_tri();

	// 	vertex_status_fixed.resize(points.rows(), 1);
	// 	vertex_status_fixed.setZero();

	// 	return true;
	// }

	void Mesh::relax_with_lloyd(const int lloyd_iterations, const Eigen::MatrixXd &hull_vertices,const Eigen::MatrixXi &hull_faces, const bool fix_regular_regions)
	{
		//reset the state
		//points = detected;
		points = moved;
		compute_triangulation();

		Eigen::VectorXi fixed_V;
		if (fix_regular_regions)
		{
			Eigen::VectorXd energy;
			laplace_energy(moved, triangles, energy);
			// Determine whether each vertex passes the criterium for bad mesh
			double avg = energy.mean();

			// Find the degree of each vertex
			Eigen::VectorXi degree;
			vertex_degree(degree);

			Eigen::Matrix<bool, 1, Eigen::Dynamic> low_energy(moved.rows());
			low_energy.setConstant(false);
			int count = 0;
			for (int i = 0; i < moved.rows(); i++)
			{
				if (energy(i) < 0.6*avg && degree(i) == 6)
				{
					low_energy(i) = true;
					count++;
				}
			}
			for (int i = 0; i < boundary.size(); i++)
			{
				low_energy(boundary(i)) = true;
				count++;
			}
			fixed_V.resize(low_energy.size());
			count = 0;
			for (int i = 0; i < low_energy.size(); i++)
			{
				if (low_energy(i))
				{
					fixed_V(count) = i;
					count++;
				}
				//check if the vertices were manually moved
				else if((moved.row(i) - detected.row(i)).squaredNorm() > 1.)
				{
					fixed_V(count) = i;
					count++;
				}
			}
			fixed_V.conservativeResize(count);
		}
		else
			fixed_V = boundary;

		lloyd_relaxation(points, fixed_V, lloyd_iterations, hull_vertices, hull_faces);
		compute_triangulation();
	}

	void Mesh::vertex_degree(Eigen::VectorXi & degree)
	{
		cellogram::vertex_degree(triangles, degree);
	}

	void Mesh::detect_vertices(const Eigen::MatrixXd &V, const DetectionParams &params)
	{
		detected.resize(0, 0); // detected (unmoved) point positions
		points.resize(0, 0); // relaxed point positions
		triangles.resize(0, 0); // triangular mesh
		adj.clear(); // adjaceny list of triangluar mesh
		vertex_to_tri.clear();
		boundary.resize(0); // list of vertices on the boundary

		if (V.size() == 0)
			return;

		this->params = params;
		detected = V;

		moved = detected;
		points = detected;
		vertex_status_fixed.resize(points.rows(), 1);
		vertex_status_fixed.setZero();
		solved_vertex.resize(points.rows(), 1);
		solved_vertex.setConstant(false);

		if (V.rows() < 3)
			return;

		//loose_convex_hull(moved, boundary, 6); moved to compute_triangulation
		compute_triangulation();

		// automatically load params if available

	}

	void Mesh::delete_vertex(const int index, bool recompute_triangulation)
	{
		// Delete vertex
		removeRow(detected, index);
		removeRow(moved, index);
		removeRow(solved_vertex, index);
		removeRow(vertex_status_fixed, index);

		// Delete entry in params
		if (params.A.size() > 0)
		{
			params.remove_index(index);
		}

		for (int i = 0; i < boundary.size(); ++i)
		{
			if (boundary(i) > index)
				--boundary(i);
		}
		for (int i = 0; i < added_by_untangler.size(); ++i)
		{
			if (added_by_untangler(i) == index){
				removeRow(added_by_untangler, index);
				break;
			}
		}

		for (int i = 0; i < added_by_untangler.size(); ++i)
		{
			if (added_by_untangler(i) > index)
				--added_by_untangler(i);
		}
		if (recompute_triangulation)
		{
			reset();
		}
		else
		{
			// delete from points
			removeRow(points, index);

			// delete triangles
			std::vector<int> ind;
			ind = vertex_to_tri[index];
			std::sort(ind.begin(), ind.end());

			for (int i = ind.size()-1; i >= 0; i--)
			{
				removeRow(triangles,ind[i]);
			}
			// lower index of values higher than index
			for (int i = 0; i < triangles.rows(); i++)
			{
				for (int j = 0; j < triangles.cols(); j++)
				{
					if (triangles(i, j) > index)
						triangles(i, j) = triangles(i, j) - 1;
				}
			}
			adjacency_list(triangles, adj);
			generate_vertex_to_tri();
		}
	}

	void Mesh::add_vertex(Eigen::Vector3d & new_point, bool must_reset)
	{
		// Check if vertex already exists
		Eigen::MatrixXd tmp = detected.rowwise() - new_point.transpose();
		Eigen::MatrixXd d = tmp.rowwise().norm();
		if (d.minCoeff() == 0)
		{
			// point is a duplicate
			return;
		}

		// Add vertex
		{
			Eigen::MatrixXd tmp(detected.rows() + 1, detected.cols());
			tmp.block(0, 0, detected.rows(), detected.cols()) = detected;
			tmp.row(detected.rows()) = new_point.transpose();
			detected = tmp;
		}
		// Add vertex to moved
		{
			Eigen::MatrixXd tmp(moved.rows() + 1, moved.cols());
			tmp.block(0, 0, moved.rows(), moved.cols()) = moved;
			tmp.row(moved.rows()) = new_point.transpose();
			moved = tmp;
		}
		// Add new entry to solved_vertex
		Eigen::Matrix<bool, Eigen::Dynamic,1> tmp_bool;
		tmp_bool.resize(solved_vertex.rows()+1);
		tmp_bool.block(0, 0, solved_vertex.rows(), solved_vertex.cols()) = solved_vertex;
		tmp_bool(solved_vertex.rows()) = false;
		solved_vertex = tmp_bool;

		// Add new entry to vertex_status_fixed
		Eigen::MatrixXi tmp_i;
		tmp_i.resize(vertex_status_fixed.rows() + 1, vertex_status_fixed.cols());
		tmp_i.block(0, 0, vertex_status_fixed.rows(), vertex_status_fixed.cols()) = vertex_status_fixed;
		tmp_i(vertex_status_fixed.rows()) = 0;
		vertex_status_fixed = tmp_i;

		// Add zero row to params
		params.push_back_const(0);

		if(must_reset)
			reset();
	}

	void Mesh::local_update(Eigen::VectorXi &local2global, const int global_to_remove, Eigen::MatrixXi & new_triangles)
	{
		//decide if to remove is local or global (better local)
		//remove vertex to_remove from points detected params (ie fix the existing method) delete_vertex(local2global(local_to_remove), false); and update triangulation and the local2global important
		delete_vertex(global_to_remove, false);
		Eigen::MatrixXi tGlobal = Eigen::MatrixXi(new_triangles.rows(), 3);
		for (int i = 0; i < new_triangles.rows(); i++)
		{
			for (int j = 0; j < 3; j++)
			{
				const int gindex = local2global(new_triangles(i, j));
				assert(gindex != global_to_remove);
				tGlobal(i, j) = gindex -(gindex >= global_to_remove ? 1 : 0);
			}

		}

		Eigen::MatrixXi tmp = triangles;
		triangles.resize(triangles.rows() + tGlobal.rows(), triangles.cols());

		triangles << tmp, tGlobal;

		adjacency_list(triangles, adj);
		generate_vertex_to_tri();
	}

	void Mesh::update_triangles_from_split(const Eigen::VectorXi & t_ind_old, const  Eigen::MatrixXi & t1, const Eigen::MatrixXi & t2)
	{
		// first remove all the old triangles
		for (int i = t_ind_old.size()-1; i >= 0; i--)
		{
			removeRow(triangles, t_ind_old(i));
		}

		int nT = triangles.rows();

		// append the new triangles and return their indices in triangle
		int index = triangles.rows();

		triangles.conservativeResize(triangles.rows() + t1.rows() + t2.rows(), 3);
		if (t1.size() > 0)
		{
			triangles.block(index, 0, t1.rows(), 3) = t1;
			index += t1.rows();
		}
		if (t2.size() > 0)
		{
			triangles.block(index, 0, t2.rows(), 3) = t2;
			index += t2.rows();
		}
		assert(index == triangles.rows());
		adjacency_list(triangles, adj);
		generate_vertex_to_tri();
	}

	void Mesh::mark_vertex_as_solved(const Eigen::VectorXi & region_interior)
	{
		for (int i = 0; i < region_interior.size(); i++)
		{
			solved_vertex(region_interior(i)) = true;
		}
	}

	void Mesh::get_physical_bounding_box(double scaling, Eigen::Vector2d & min, Eigen::Vector2d & max) const
	{
		Eigen::RowVector3d maxVal = points.colwise().maxCoeff();
		Eigen::RowVector3d minVal = points.colwise().minCoeff();

		max = maxVal.block<1,2>(0, 0)*scaling;
		min = minVal.block<1,2>(0, 0)*scaling;

		// std::cout << "max:\n" << max << "\n\nmin:\n" << min << std::endl;
	}

	void Mesh::get_background_mesh(double scaling, Eigen::MatrixXd &V, Eigen::MatrixXi &F, Eigen::VectorXd &S, double padding) const {
		Eigen::MatrixXd BV;
		Eigen::MatrixXi BF;
		V = points.leftCols<2>();
		igl::bounding_box(V, padding / scaling, BV, BF);
		assert(BV.rows() == 4);

		V.resize(points.rows() + BV.rows(), 2);
		V.topRows(points.rows()) = points.leftCols<2>();
		V.bottomRows(BV.rows()) = BV;

		delaunay_triangulation(V, F);
		V *= scaling;

		S.resize(V.rows());
		S.head(points.rows()) = (detected - points).rowwise().norm() * scaling;
		S.tail(BV.rows()).setZero();
	}

	void Mesh::local_update(Eigen::VectorXi & local2global, Eigen::MatrixXd & new_points, Eigen::MatrixXi & new_triangles, Eigen::VectorXi & old_triangles)
	{
		for (int i = 0; i < local2global.size(); i++)
		{
			const int global_index = local2global(i);
			points.row(global_index) = new_points.row(i);

		}

		Eigen::MatrixXi tGlobal = Eigen::MatrixXi(new_triangles.rows(), 3);
		for (int i = 0; i < new_triangles.rows(); i++)
		{
			for (int j = 0; j < 3; j++)
			{
				tGlobal(i, j) = local2global(new_triangles(i, j));
			}

		}

		replaceTriangles(tGlobal, old_triangles, triangles);

		adjacency_list(triangles, adj);
		generate_vertex_to_tri();
	}

	void Mesh::final_relax(const Eigen::VectorXi & expanded_boundary)
	{
		int n = points.rows();
		////Find vertices that have lower connectivity than 6

		Eigen::VectorXi neighCount;
		vertex_degree(neighCount);

		// Determine fixed vertices based on connectivity and bounding box
		Eigen::VectorXi indFixed = Eigen::VectorXi::Zero(n);

		for (size_t i = 0; i < n; i++)
		{
			if (neighCount(i) != 6)
			{
				indFixed(i) = 1;
			}
		}

		// fix vertices on the boundary and the ones connected to the boundary
		for (size_t i = 0; i < expanded_boundary.rows(); i++)
		{
			indFixed(expanded_boundary(i)) = 1;
		}

		//////PLEASE USE ME
		//igl::opengl::glfw::Viewer viewer;

		//viewer.data().set_mesh(points, triangles);
		//Eigen::MatrixXd cc(n, 3);
		//for (int i = 0; i < indFixed.rows(); ++i) {
		//	if(indFixed(i) == 1)
		//		cc.row(i) = Eigen::RowVector3d(1, 0, 0);
		//	else
		//		cc.row(i) = Eigen::RowVector3d(0, 0, 0);
		//}
		//viewer.data().set_points(points, cc);
		//viewer.data().point_size = float(8);
		//viewer.core.set_rotation_type(igl::opengl::ViewerCore::RotationType::ROTATION_TYPE_NO_ROTATION);
		//viewer.core.orthographic = true;
		//viewer.core.is_animating = true;
		//viewer.launch();

		// Rearrange coordinates and triangle indices to have free vertices first...
		//MatrixXd xRearranged = MatrixXd::Zero(n, 1);
		Eigen::SparseVector<double> xRearranged(n);
		Eigen::SparseVector<double> yRearranged(n);
		Eigen::VectorXi indicesMapping = Eigen::VectorXi::Zero(n);
		Eigen::MatrixXi trianglesRearranged = Eigen::MatrixXi::Zero(triangles.rows(), 3);

		int c = 0;
		for (size_t i = 0; i < n; i++)
		{
			if (indFixed(i) == 0)
			{
				indicesMapping(i) = c;
				xRearranged.fill(c) = points(i, 0);
				yRearranged.fill(c) = points(i, 1);

				c++;
			}
		}
		int nFree = c;

		// and then the fixed vertices
		for (size_t i = 0; i < n; i++)
		{
			if (indFixed(i) == 1)
			{
				indicesMapping(i) = c;
				xRearranged.fill(c) = points(i, 0);
				yRearranged.fill(c) = points(i, 1);

				c++;
			}
		}

		// rearrange triangles such that it is congruent with the newly arranged coordinates
		for (size_t j = 0; j < triangles.rows(); j++)
		{
			for (size_t k = 0; k < 3; k++)
			{
				trianglesRearranged(j, k) = indicesMapping(triangles(j, k));
			}
		}

		// Generate Laplacian
		Eigen::VectorXd diag = Eigen::VectorXd::Zero(n);
		Eigen::SparseMatrix<double> L(n, n);
		typedef Eigen::Triplet<int> Trip;
		std::vector< Trip > tripletList;
		tripletList.reserve(n * 7);

		for (size_t i = 0; i < trianglesRearranged.rows(); i++)
		{
			for (size_t j = 0; j < 3; j++)
			{
				tripletList.push_back(Trip(trianglesRearranged(i, j), trianglesRearranged(i, (j + 1) % 3), -1));
				tripletList.push_back(Trip(trianglesRearranged(i, (j + 1) % 3), trianglesRearranged(i, j), -1));
				diag(trianglesRearranged(i, j))++;
			}
		}

		for (size_t i = 0; i < diag.rows(); i++)
		{
			tripletList.push_back(Trip(i, i, diag(i)));
		}

		L.setFromTriplets(tripletList.begin(), tripletList.end());

		// Force all non-zeros to be one
		for (int k = 0; k<L.outerSize(); ++k)
		{
			for (Eigen::SparseMatrix<double>::InnerIterator it(L, k); it; ++it)
			{
				if (it.value() < 0)
				{
					L.coeffRef(it.row(), it.col()) = -1;
				}
			}
		}

		// Solve for xNew and yNew
		Eigen::SparseMatrix<double>  Li = L.block(0, 0, nFree, nFree);
		Eigen::SparseMatrix<double>  Lb = L.block(0, nFree, nFree, L.rows() - nFree);

		//SparseMatrix<int>  Lii = Li.inverse();
		Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
		solver.compute(Li);
		if (solver.info() != Eigen::Success) {
			// decomposition failed
			return;
		}

		Eigen::SparseVector<double> xB(L.rows() - nFree);
		Eigen::SparseVector<double> yB(L.rows() - nFree);
		for (size_t i = 0; i < L.rows() - nFree; i++)
		{
			xB.fill(i) = xRearranged.coeffRef(nFree + i);
			yB.fill(i) = yRearranged.coeffRef(nFree + i);
		}
		Eigen::SparseVector<double> tmp = -Lb * xB;

		Eigen::VectorXd xNew = solver.solve(Eigen::VectorXd(tmp));

		tmp = -Lb * yB;
		Eigen::VectorXd yNew = solver.solve(Eigen::VectorXd(tmp));

		// Overwrite the free vertices of xRearranged and yRearranged
		for (size_t i = 0; i < nFree; i++)
		{
			xRearranged.coeffRef(i) = xNew(i);
			yRearranged.coeffRef(i) = yNew(i);
		}

		// use indices mapping to overwrite "points" with the newly calculated coordinates
		for (size_t i = 0; i < indicesMapping.rows(); i++)
		{
			points(i,0) = xRearranged.coeffRef(indicesMapping(i));
			points(i,1) = yRearranged.coeffRef(indicesMapping(i));
		}
	}

	void Mesh::generate_vertex_to_tri()
	{
		vertex_to_tri.clear();
		vertex_to_tri.resize(points.rows());

		for (int f = 0; f < triangles.rows(); ++f) {
			for (int lv = 0; lv < triangles.cols(); ++lv)
			{
				const int v_index = triangles(f, lv);
				vertex_to_tri[v_index].push_back(f);
			}
		}
	}

	bool Mesh::untangle()
	{
		bool has_changed_points = false;
		triangles.resize(0, 0); // triangular mesh
		adj.clear(); // adjaceny list of triangluar mesh
		vertex_to_tri.clear();

		added_by_untangler.resize(0);
		deleted_by_untangler.resize(0, 0);

		Eigen::MatrixXd newPts;
		std::vector<int> dropped;
		// std::cout<<points<<std::endl;
		cellogram::PointsUntangler::pointsUntangler(moved, triangles, dropped, newPts);
		// assert(moved.rows() - dropped.size() + newPts.rows() == triangles.maxCoeff() - 1);

		if (newPts.rows() > 0 || !dropped.empty())
		{
			has_changed_points = true;
		}

		int old_size = added_by_untangler.size();
		added_by_untangler.conservativeResize(old_size + newPts.rows());

		for (int i = 0; i < newPts.rows(); ++i) {

			Eigen::Vector3d tmp; tmp.setZero();
			for (int j = 0; j < newPts.cols(); ++j)
				tmp(j) = newPts(i, j);

			added_by_untangler(old_size + i) = detected.rows();
			add_vertex(tmp, false);
		}
		points = moved;

		solved_vertex.setConstant(false);
		vertex_status_fixed.setZero();

		//Now mesh is valid, but it has to many points....
		generate_vertex_to_tri();

		old_size = deleted_by_untangler.size();
		deleted_by_untangler.conservativeResize(old_size + dropped.size(), moved.cols());
		std::sort(dropped.begin(), dropped.end());

		for (int i = dropped.size() - 1; i >= 0; i--)
		{
			const int gid = dropped[i];
			deleted_by_untangler.row(old_size++) = moved.row(gid);
			delete_vertex(gid, false);
		}

		assert(triangles.maxCoeff() < points.rows());
		assert(triangles.minCoeff() >= 0);

		adjacency_list(triangles, adj);
		generate_vertex_to_tri();

		return has_changed_points;
	}

	void Mesh::clear()
	{
		detected.resize(0, 0); // detected (unmoved) point positions
		moved.resize(0, 0); // relaxed point positions
		points.resize(0, 0); // relaxed point positions
		triangles.resize(0, 0); // triangular mesh
		adj.clear(); // adjaceny list of triangluar mesh
		vertex_to_tri.clear();
		boundary.resize(0); // list of vertices on the boundary

		solved_vertex.resize(0);
		vertex_status_fixed.resize(0);
		added_by_untangler.resize(0);
		deleted_by_untangler.resize(0, 0);

		params.clear();
	}

	void Mesh::save(nlohmann::json &data)
	{
		data["detected"] = json::object();
		write_json_mat(detected, data["detected"]);

		data["moved"] = json::object();
		write_json_mat(moved, data["moved"]);

		data["points"] = json::object();
		write_json_mat(points, data["points"]);

		data["displacement"] = json::object();
		write_json_mat((detected-points).eval(), data["displacement"]);

		data["triangles"] = json::object();
		write_json_mat(triangles, data["triangles"]);

		data["boundary"] = json::object();
		write_json_mat(boundary, data["boundary"]);

		data["params"] = json::object();

		params.save(data["params"]);
	}

	// void Mesh::save(const std::string & path)
	// {
	// 	{
	// 		std::ofstream out(path + "/detected.vert");
	// 		out << detected << std::endl;
	// 		out.close();
	// 	}

	// 	{
	// 		std::ofstream out(path + "/moved.vert");
	// 		out << moved << std::endl;
	// 		out.close();
	// 	}

	// 	{
	// 		std::ofstream out(path + "/points.vert");
	// 		out << points << std::endl;
	// 		out.close();
	// 	}

	// 	{
	// 		std::ofstream out(path + "/displacement.txt");
	// 		out << (points-detected) << std::endl;
	// 		out.close();
	// 	}

	// 	{
	// 		std::ofstream out(path + "/mesh.tri");
	// 		out << triangles << std::endl;
	// 		out.close();
	// 	}

	// 	{
	// 		std::ofstream out(path + "/boundary.txt");
	// 		out << boundary << std::endl;
	// 		out.close();
	// 	}

	// 	params.save(path);
	// }

	void Mesh::reset()
	{
		//points = detected;
		points = moved;
		solved_vertex.setConstant(false);

		//recompute boundary

		//compute_triangulation();
	}

	void Mesh::compute_triangulation()
	{
		if (points.size() == 0)
			return;
		// delaunay_triangulation(points, triangles);

		loose_convex_hull(moved, boundary, 6);
		constrained_delaunay_triangulation(points, boundary, triangles);

		// Calculate the graph adjancency
		adjacency_list(triangles, adj);
		generate_vertex_to_tri();

	}

}// namespace cellogram


#ifndef HSL_ALGORITHMS_HPP
#define HSL_ALGORITHMS_HPP

#include <boost/gil/gil_all.hpp>

namespace boost { namespace gil {
namespace toolbox {

template< typename HSL_VIEW >
void shift_hue( const HSL_VIEW& img
              , const bits32f   value )
{
   // make sure src and dst are views
   // make sure src and dst are either hsl or hsv

   for( int y = 0; y < img.height(); ++y )
   {
      HSL_VIEW::x_iterator img_it = img.row_begin( y );

      for( int x = 0; x < img.width(); ++x )
      {
         bits32f& hue = get_color( img_it[x], boost::gil::hsl_color_space::hue_t() );

         hue += value;

         if( hue > 1.f )
         {
            hue -= 1.f;
         }

         if( hue < 0.f )
         {
            hue = 0.f;
         }
      }
   }
}

} //namespace toolbox
} //namespace gil
} //namespace boost

#endif // HSL_ALGORITHMS_HPP


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_IS_NGEZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NGEZ_HPP_INCLUDED

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

 /*!

    @ingroup group-predicates
    Function object implementing is_ngez capabilities

    Returns @ref True if x is not greater or equal to @ref Zero else returns @ref False.

    @par Semantic:

    @code
    as_logical_t<T> r = is_ngez(x);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = !(x >= 0);
    @endcode

    @par Note:

    Due to existence of @ref Nan, this is not equivalent to a call to @ref is_lt
z    for floating types

  **/
  const boost::dispatch::functor<tag::is_ngez_> is_ngez = {};
} }
#endif

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

#endif


//
// Created by briand on 2/22/19.
//

#include "SimulatedAnnealing.h"

#include <ensmallen.hpp>

#undef BOOST_LOG

#include <boost/log/trivial.hpp>
#include <Engines/GlobalHeuristics/Utilities/MetropolisHastings.h>

namespace SmolDock::Heuristics {

    SimulatedAnnealing::SimulatedAnnealing(Score::ScoringFunction* scorFunc_, Optimizer::Optimizer* optimizer_,
                                           unsigned int seed_, SimulatedAnnealing::Parameters params_) :
            scorFunc(scorFunc_), optimizer(optimizer_), rndGenerator(seed_), params(params_) {

    }

    bool SimulatedAnnealing::search() {

        arma::mat currentState = scorFunc->getStartingConditions();

        auto coolingType = ens::ExponentialSchedule();

        unsigned int maxIterations = this->params.maxIterations;
        double initTemp = this->params.initTemp;
        unsigned int initialNoTempDropMoves = this->params.initialNoTempDropMoves;
        unsigned int moveCtrlSweep = this->params.moveCtrlSweep;
        double tolerance = this->params.tolerance;
        unsigned int maxToleranceSweep = this->params.maxToleranceSweep;
        double maxMoveSize = this->params.maxMoveSize;
        double initMoveSize = this->params.initMoveSize;
        double gain = this->params.gain;

        ens::SA<> SimulatedAnnealer(coolingType,
                                    maxIterations,
                                    initTemp ,
                                    initialNoTempDropMoves,
                                    moveCtrlSweep,
                                    tolerance,
                                    maxToleranceSweep,
                                    maxMoveSize,
                                    initMoveSize,
                                    gain);

        SimulatedAnnealer.Optimize(*this->scorFunc, currentState);

        //double scoreBeforeLocalOptim = this->scorFunc->Evaluate(currentState);

        this->optimizer->optimize(currentState);
        //double scoreAfterLocalOptim = this->optimizer->getScore();

        this->result = this->optimizer->getRawResultMatrix();

//        BOOST_LOG_TRIVIAL(debug) << "SimulatedAnnealing: Score after SA          = " << scoreBeforeLocalOptim;
//        BOOST_LOG_TRIVIAL(debug) << "SimulatedAnnealing: Score after local optim = " << scoreAfterLocalOptim;

        return true;
    }

    arma::mat SimulatedAnnealing::getResultMatrix() {
        return this->result;
    }

}


/* Copyright 2017-2018 PaGMO development team

This file is part of the PaGMO library.

The PaGMO library is free software; you can redistribute it and/or modify
it under the terms of either:

  * the GNU Lesser General Public License as published by the Free
    Software Foundation; either version 3 of the License, or (at your
    option) any later version.

or

  * the GNU General Public License as published by the Free Software
    Foundation; either version 3 of the License, or (at your option) any
    later version.

or both in parallel, as here.

The PaGMO library is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
for more details.

You should have received copies of the GNU General Public License and the
GNU Lesser General Public License along with the PaGMO library.  If not,
see https://www.gnu.org/licenses/. */

#ifndef PAGMO_CUSTOM_COMPARISONS_HPP
#define PAGMO_CUSTOM_COMPARISONS_HPP

#include <algorithm>
#include <boost/functional/hash.hpp> // boost::hash_combine
#include <cstddef>
#include <type_traits>
#include <vector>

#include <pagmo/type_traits.hpp>

namespace pagmo
{
namespace detail
{
// Less than compares floating point types placing nans after inf or before -inf
// It is a useful function when calling e.g. std::sort to guarantee a weak strict ordering
// and avoid an undefined behaviour
template <typename T, bool After = true>
inline bool less_than_f(T a, T b)
{
    static_assert(std::is_floating_point<T>::value, "less_than_f can be used only with floating-point types.");
    if (!std::isnan(a)) {
        if (!std::isnan(b))
            return a < b; // a < b
        else
            return After; // a < nan
    } else {
        if (!std::isnan(b))
            return !After; // nan < b
        else
            return false; // nan < nan
    }
}

// Greater than compares floating point types placing nans after inf or before -inf
// It is a useful function when calling e.g. std::sort to guarantee a weak strict ordering
// and avoid an undefined behaviour
template <typename T, bool After = true>
inline bool greater_than_f(T a, T b)
{
    static_assert(std::is_floating_point<T>::value, "greater_than_f can be used only with floating-point types.");
    if (!std::isnan(a)) {
        if (!std::isnan(b))
            return a > b; // a > b
        else
            return !After; // a > nan
    } else {
        if (!std::isnan(b))
            return After; // nan > b
        else
            return false; // nan > nan
    }
}

// equal_to than compares floating point types considering nan==nan
template <typename T>
inline bool equal_to_f(T a, T b)
{
    static_assert(std::is_floating_point<T>::value, "equal_to_f can be used only with floating-point types.");
    if (!std::isnan(a) && !std::isnan(b)) {
        return a == b;
    }
    return std::isnan(a) && std::isnan(b);
}

// equal_to_vf than compares vectors of floating point types considering nan==nan
template <typename T>
struct equal_to_vf {
    bool operator()(const std::vector<T> &lhs, const std::vector<T> &rhs) const
    {
        static_assert(std::is_floating_point<T>::value,
                      "This class (equal_to_vf) can be used only with floating-point types.");
        if (lhs.size() != rhs.size()) {
            return false;
        } else {
            return std::equal(lhs.begin(), lhs.end(), rhs.begin(), equal_to_f<T>);
        }
    }
};

// hash_vf can be used to hash vectors of floating point types
template <typename T>
struct hash_vf {
    std::size_t operator()(std::vector<T> const &in) const
    {
        static_assert(std::is_floating_point<T>::value,
                      "This class (hash_vf) can be used only with floating-point types.");
        std::size_t retval = 0u;
        for (T el : in) {
            // Combine the hash of the current vector with the hashes of the previous ones
            boost::hash_combine(retval, el);
        }
        return retval;
    }
};
} // namespace detail
} // namespace pagmo

#endif


// Copyright 2019 Apex.AI, Inc.
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <ndt/ndt_representations.hpp>
#include <optimization/optimization_problem.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <Eigen/Core>
#include <tuple>

namespace autoware{
namespace localization{
namespace ndt{

  class P2DNDTObjective : public common::optimization::Expression<P2DNDTObjective, geometry_msgs::msg::Transform, 1U, 6U>{
  // getting aliases from the base class.
  using ExpressionT = Expression<P2DNDTObjective, geometry_msgs::msg::Transform, 1U, 6U>;
  using DomainValue = typename ExpressionT::DomainValue;
  using Value = typename ExpressionT::Value;
  using Jacobian = typename ExpressionT::Jacobian;
  using Hessian = typename ExpressionT::Hessian;

  public:
      P2DNDTObjective(const DomainValue & init_guess, const P2DNDTScan & scan, const NDTMap & map):
      m_init_guess_ref(init_guess),
      m_scan_ref(scan),
      m_map_ref(map){}

  Value operator()(const DomainValue & x);
  Jacobian & jacobian(const DomainValue & x);
  Hessian & hessian(const DomainValue & x);

  private:
  // references as class members to be initialized at constructor.
  const DomainValue & m_init_guess_ref;
  const P2DNDTScan & m_scan_ref;
  const NDTMap & m_map_ref;
};

// Here the class P2DNDTObjective
class P2DNDTOptimizationProblem : public common::optimization::UnconstrainedOptimizationProblem<
        P2DNDTOptimizationProblem, geometry_msgs::msg::Transform, 6U, P2DNDTObjective>{
// Forward everything to the P2DNDTObjective? Possibly unnecessary.
};

}
}
}


/**
 * @file adaboost_test.cpp
 * @author Nikhil Goel
 *
 * Test mlpackMain() of adaboost_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST
static const std::string testName = "AdaBoost";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/adaboost/adaboost_main.cpp>

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

using namespace mlpack;

struct AdaBoostTestFixture
{
 public:
  AdaBoostTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~AdaBoostTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(AdaBoostMainTest, AdaBoostTestFixture);

/**
 * Check that number of output labels and number of input
 * points are equal.
 */
BOOST_AUTO_TEST_CASE(AdaBoostOutputDimensionTest)
{
  arma::mat trainData;
  if (!data::Load("vc2.csv", trainData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Unable to load label dataset vc2_labels.txt!");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Unable to load test dataset vc2.csv!");

  size_t testSize = testData.n_cols;

  SetInputParam("training", std::move(trainData));
  SetInputParam("labels", std::move(labels));

  SetInputParam("test", std::move(testData));

  mlpackMain();

  // Check that number of predicted labels is equal to the input test points.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("output").n_cols,
                      testSize);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("output").n_rows, 1);
}

/**
 * Ensure that saved model can be used again.
 */
BOOST_AUTO_TEST_CASE(AdaBoostModelReuseTest)
{
  arma::mat trainData;
  if (!data::Load("vc2.csv", trainData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Unable to load label dataset vc2_labels.txt!");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Unable to load test dataset vc2.csv!");

  SetInputParam("training", std::move(trainData));
  SetInputParam("labels", std::move(labels));

  SetInputParam("test", testData);

  mlpackMain();

  arma::Row<size_t> output;
  output = std::move(CLI::GetParam<arma::Row<size_t>>("output"));

  CLI::GetSingleton().Parameters()["training"].wasPassed = false;
  CLI::GetSingleton().Parameters()["labels"].wasPassed = false;
  CLI::GetSingleton().Parameters()["test"].wasPassed = false;

  SetInputParam("test", std::move(testData));
  SetInputParam("input_model",
                CLI::GetParam<AdaBoostModel*>("output_model"));

  mlpackMain();

  // Check that initial output and output using saved model are same.
  CheckMatrices(output, CLI::GetParam<arma::Row<size_t>>("output"));
}

/**
 * Test that iterations in adaboost is always non-negative.
 */
BOOST_AUTO_TEST_CASE(AdaBoostItrTest)
{
  arma::mat trainData;
  if (!data::Load("trainSet.csv", trainData))
    BOOST_FAIL("Unable load train dataset trainSet.csv!");

  SetInputParam("training", std::move(trainData));
  SetInputParam("iterations", (int) -1);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Check that the last dimension of the training set is
 * used as labels when labels are not passed specifically
 * and results are same from both label and without label models.
 */
BOOST_AUTO_TEST_CASE(AdaBoostWithoutLabelTest)
{
  // Train adaboost without providing labels.
  arma::mat trainData;
  if (!data::Load("trainSet.csv", trainData))
    BOOST_FAIL("Unable to load train dataset trainSet.csv!");

  // Give labels.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);

  arma::mat testData;
  if (!data::Load("testSet.csv", testData))
    BOOST_FAIL("Unable to load test dataset testSet.csv!");

  // Delete the last row containing labels from test dataset.
  testData.shed_row(testData.n_rows - 1);

  SetInputParam("training", trainData);

  SetInputParam("test", testData);

  mlpackMain();

  CLI::GetSingleton().Parameters()["training"].wasPassed = false;
  CLI::GetSingleton().Parameters()["test"].wasPassed = false;

  arma::Row<size_t> output;
  output = std::move(CLI::GetParam<arma::Row<size_t>>("output"));

  bindings::tests::CleanMemory();

  trainData.shed_row(trainData.n_rows - 1);

  // Now train Adaboost with labels provided.
  SetInputParam("training", std::move(trainData));
  SetInputParam("test", std::move(testData));
  SetInputParam("labels", std::move(labels));

  mlpackMain();

  // Check that initial output and final output matrix are same.
  CheckMatrices(output, CLI::GetParam<arma::Row<size_t>>("output"));
}

/**
 * Testing that only one of training data or pre-trained model is passed.
 */
BOOST_AUTO_TEST_CASE(AdaBoostTrainingDataOrModelTest)
{
  arma::mat trainData;
  if (!data::Load("trainSet.csv", trainData))
    BOOST_FAIL("Unable to load train dataset trainSet.csv!");

  SetInputParam("training", std::move(trainData));

  mlpackMain();

  SetInputParam("input_model",
                CLI::GetParam<AdaBoostModel*>("output_model"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Weak learner should be either Decision Stump or Perceptron.
 */
BOOST_AUTO_TEST_CASE(AdaBoostWeakLearnerTest)
{
  arma::mat trainData;
  if (!data::Load("trainSet.csv", trainData))
    BOOST_FAIL("Unable to load train dataset trainSet.csv!");

  SetInputParam("training", std::move(trainData));
  SetInputParam("weak_learner", std::string("decision tree"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Different Weak learner should give different outputs.
 */
BOOST_AUTO_TEST_CASE(AdaBoostDiffWeakLearnerOutputTest)
{
  arma::mat trainData;
  if (!data::Load("vc2.csv", trainData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Unable to load label dataset vc2_labels.txt!");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Unable to load test dataset vc2.csv!");

  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("test", testData);

  mlpackMain();

  arma::Row<size_t> output;
  output = std::move(CLI::GetParam<arma::Row<size_t>>("output"));

  bindings::tests::CleanMemory();

  CLI::GetSingleton().Parameters()["training"].wasPassed = false;
  CLI::GetSingleton().Parameters()["labels"].wasPassed = false;
  CLI::GetSingleton().Parameters()["test"].wasPassed = false;

  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("test", testData);
  SetInputParam("weak_learner", std::string("perceptron"));

  mlpackMain();

  arma::Row<size_t> outputPerceptron;
  outputPerceptron = std::move(CLI::GetParam<arma::Row<size_t>>("output"));

  BOOST_REQUIRE_GT(arma::accu(output != outputPerceptron), 1);
}

/**
 * Accuracy increases as Number of Iterations increases.
 * (Or converges and remains same)
 */
BOOST_AUTO_TEST_CASE(AdaBoostDiffItrTest)
{
  arma::mat trainData;
  if (!data::Load("vc2.csv", trainData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Unable to load label dataset vc2_labels.txt!");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Unable to load test dataset vc2.csv!");

  arma::Row<size_t> testLabels;
  if (!data::Load("vc2_test_labels.txt", testLabels))
    BOOST_FAIL("Unable to load labels for vc2__test_labels.txt");

  // Iterations = 1
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("weak_learner", std::string("perceptron"));
  SetInputParam("iterations", (int) 1);

  mlpackMain();

  // Calculate accuracy.
  arma::Row<size_t> output;
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  size_t correct = arma::accu(output == testLabels);
  double accuracy1 = (double(correct) / double(testLabels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // Iterations = 10
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("weak_learner", std::string("perceptron"));
  SetInputParam("iterations", (int) 10);

  mlpackMain();

  // Calculate accuracy.
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  correct = arma::accu(output == testLabels);
  double accuracy10 = (double(correct) / double(testLabels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // Iterations = 100
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("weak_learner", std::string("perceptron"));
  SetInputParam("iterations", (int) 100);

  mlpackMain();

  // Calculate accuracy.
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  correct = arma::accu(output == testLabels);
  double accuracy100 = (double(correct) / double(testLabels.n_elem) * 100);

  BOOST_REQUIRE_LE(accuracy1, accuracy10);
  BOOST_REQUIRE_LE(accuracy10, accuracy100);
}

/**
 * Accuracy increases as tolerance decreases.
 * (Execution Time also increases)
 */
BOOST_AUTO_TEST_CASE(AdaBoostDiffTolTest)
{
  arma::mat trainData;
  if (!data::Load("vc2.csv", trainData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Unable to load label dataset vc2_labels.txt!");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Unable to load test dataset vc2.csv!");

  arma::Row<size_t> testLabels;
  if (!data::Load("vc2_test_labels.txt", testLabels))
    BOOST_FAIL("Unable to load labels for vc2__test_labels.txt");

  // tolerance = 0.001
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("tolerance", (double) 0.001);

  mlpackMain();

  // Calculate accuracy.
  arma::Row<size_t> output;
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  size_t correct = arma::accu(output == testLabels);
  double accuracy1 = (double(correct) / double(testLabels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // tolerance = 0.01
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("tolerance", (double) 0.01);

  mlpackMain();

  // Calculate accuracy.
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  correct = arma::accu(output == testLabels);
  double accuracy2 = (double(correct) / double(testLabels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // tolerance = 0.1
  SetInputParam("training", trainData);
  SetInputParam("labels", labels);
  SetInputParam("tolerance", (double) 0.1);

  mlpackMain();

  // Calculate accuracy.
  CLI::GetParam<AdaBoostModel*>("output_model")->Classify(testData,
       output);

  correct = arma::accu(output == testLabels);
  double accuracy3 = (double(correct) / double(testLabels.n_elem) * 100);

  BOOST_REQUIRE_LE(accuracy1, accuracy2);
  BOOST_REQUIRE_LE(accuracy2, accuracy3);
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/range/algorithm.hpp>
#include <boost/range/adaptors.hpp>
#include <array>
#include <iterator>
#include <iostream>

int main()
{
  std::array<int, 6> a{{0, 5, 2, 1, 3, 4}};
  boost::copy(boost::adaptors::filter(a, [](int i){ return i > 2; }),
    std::ostream_iterator<int>{std::cout, ","});
}


///////////////////////////////////////////////////////////////////////////////
// mpl::most.hpp                              				 				 //
//                                                                           //
//  Copyright 2010 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MPL_MOST_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_MPL_MOST_HPP_ER_2010
#include <boost/mpl/iterator_range.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/prior.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace mpl{

	template<typename Seq>
    struct most : boost::mpl::iterator_range<
        typename boost::mpl::begin<Seq>::type,
        typename boost::mpl::prior<
            typename boost::mpl::end<Seq>::type
        >::type
    >{};

}// mpl
}// detail
}// statistics
}// boost

#endif