/** Copyright : Arthur Bit-Monnot (2013)  arthur.bit-monnot@laas.fr

This software is a computer program whose purpose is to [describe
functionalities and technical features of your software].

This software is governed by the CeCILL-B license under French law and
abiding by the rules of distribution of free software.  You can  use,
modify and/ or redistribute the software under the terms of the CeCILL-B
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and  rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty  and the software's author,  the holder of the
economic rights,  and the successive licensors  have only  limited
liability.

In this respect, the user's attention is drawn to the risks associated
with loading,  using,  modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean  that it is complicated to manipulate,  and  that  also
therefore means  that it is reserved for developers  and  experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and,  more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL-B license and that you accept its terms.
*/

#include <boost/foreach.hpp>

#include "node_filter_utils.h"

#include "nodes_filter.h"
#include "AlgoTypedefs.h"

using namespace RLC;

NodeSet * isochrone ( const RLC::AbstractGraph * g, const int center, const int max_time )
{
    typedef AspectMaxCostPruning<AspectMinCost<DRegLC> > Dij;
    NodeSet * ns = new NodeSet( g->transport->num_vertices() );

    Dij::ParamType p(
        DRegLCParams( g, 0, 1 ),
        AspectMaxCostPruningParams( max_time )
    );

    Dij dij( p );
    BOOST_FOREACH( int state, g->dfa_start_states() ) {
        dij.add_source_node( RLC::Vertice(center, state), 0, 0 );
    }
    while( !dij.finished() ) {
        RLC::Label lab = dij.treat_next();
        ns->addNode( lab.node.first );
    }

    return ns;
}

VisualResult show_isochrone ( const RLC::AbstractGraph * g, const int center, const int max_time )
{
    NodeSet * ns = isochrone( g, center, max_time );
    return ns->visualization();
}

BBNodeFilter* rectangle_containing ( const Transport::Graph* trans, const int node1, const int node2, const float margin )
{

    float lat1 = trans->latitude( node1 );
    float lon1 = trans->longitude( node1 );
    float lat2 = trans->latitude( node2 );
    float lon2 = trans->longitude( node2 );

    float max_lat, min_lat, max_lon, min_lon;
    if(lat1 > lat2) {
        max_lat = lat1 + margin;
        min_lat = lat2 - margin;
    } else {
        max_lat = lat2 + margin;
        min_lat = lat1 - margin;
    }

    if(lon1 > lon2) {
        max_lon = lon1 + margin;
        min_lon = lon2 - margin;
    } else {
        max_lon = lon2 + margin;
        min_lon = lon1 - margin;
    }

    return new BBNodeFilter(trans, max_lon, min_lon, max_lat, min_lat);
}

BBNodeFilter* rectangle_containing ( const Transport::Graph* trans, const std::vector<int> nodes, const float margin )
{
    float max_lat = std::numeric_limits< float >::min();
    float min_lat = std::numeric_limits< float >::max();
    float max_lon = std::numeric_limits< float >::min();
    float min_lon = std::numeric_limits< float >::max();

    BOOST_FOREACH(int node, nodes) {
        float lat = trans->latitude( node );
        float lon = trans->longitude( node );

        if( lat > max_lat ) {
            max_lat = lat;
        }
        if( lat < min_lat ) {
            min_lat = lat;
        }
        if( lon > max_lon ) {
            max_lon = lon;
        }
        if( lon < min_lon ) {
            min_lon = lon;
        }
    }
    return new BBNodeFilter(trans, max_lon + margin, min_lon - margin, max_lat + margin, min_lat - margin);
}

BBNodeFilter * bordeaux_bb( const Transport::Graph * trans )
{
    return new BBNodeFilter(trans, -0.455148, -0.860322, 45.0317, 44.7266);
}

BBNodeFilter * toulouse_bb( const Transport::Graph * trans )
{
    return new BBNodeFilter(trans, 1.68514, 1.2078, 43.7794, 43.3935);
}


#include <iostream>
#include <vector>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
#  include <amgcl/backend/vexcl_static_matrix.hpp>
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#endif

#include <amgcl/mpi/util.hpp>
#include <amgcl/mpi/make_solver.hpp>
#include <amgcl/mpi/preconditioner.hpp>
#include <amgcl/mpi/solver/runtime.hpp>

#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

#ifndef AMGCL_BLOCK_SIZES
#  define AMGCL_BLOCK_SIZES (3)(4)
#endif

namespace amgcl {
    profiler<> prof;
}

namespace math = amgcl::math;

//---------------------------------------------------------------------------
ptrdiff_t assemble_poisson3d(amgcl::mpi::communicator comm,
        ptrdiff_t n, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    ptrdiff_t n3 = n * n * n;

    ptrdiff_t chunk = (n3 + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }
    ptrdiff_t row_beg = std::min(n3, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n3, row_beg + chunk);
    chunk = row_end - row_beg;

    ptr.clear(); ptr.reserve(chunk + 1);
    col.clear(); col.reserve(chunk * 7);
    val.clear(); val.reserve(chunk * 7);

    rhs.resize(chunk);
    std::fill(rhs.begin(), rhs.end(), 1.0);

    const double h2i = (n - 1) * (n - 1);
    ptr.push_back(0);

    for (ptrdiff_t idx = row_beg; idx < row_end; ++idx) {
        ptrdiff_t k = idx / (n * n);
        ptrdiff_t j = (idx / n) % n;
        ptrdiff_t i = idx % n;

        if (k > 0)  {
            col.push_back(idx - n * n);
            val.push_back(-h2i);
        }

        if (j > 0)  {
            col.push_back(idx - n);
            val.push_back(-h2i);
        }

        if (i > 0) {
            col.push_back(idx - 1);
            val.push_back(-h2i);
        }

        col.push_back(idx);
        val.push_back(6 * h2i);

        if (i + 1 < n) {
            col.push_back(idx + 1);
            val.push_back(-h2i);
        }

        if (j + 1 < n) {
            col.push_back(idx + n);
            val.push_back(-h2i);
        }

        if (k + 1 < n) {
            col.push_back(idx + n * n);
            val.push_back(-h2i);
        }

        ptr.push_back( col.size() );
    }

    return chunk;
}

//---------------------------------------------------------------------------
ptrdiff_t read_matrix_market(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    amgcl::io::mm_reader A_mm(A_file);
    ptrdiff_t n = A_mm.rows();

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    A_mm(ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        amgcl::io::mm_reader rhs_mm(rhs_file);
        rhs_mm(rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
ptrdiff_t read_binary(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    ptrdiff_t n = amgcl::io::crs_size<ptrdiff_t>(A_file);

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    amgcl::io::read_crs(A_file, n, ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        ptrdiff_t rows, cols;
        amgcl::io::read_dense(rhs_file, rows, cols, rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
template <class Backend, class Matrix>
std::shared_ptr< amgcl::mpi::distributed_matrix<Backend> >
partition(amgcl::mpi::communicator comm, const Matrix &Astrip,
        typename Backend::vector &rhs, const typename Backend::params &bprm,
        amgcl::runtime::mpi::partition::type ptype, int block_size = 1)
{
    typedef typename Backend::value_type val_type;
    typedef typename amgcl::math::rhs_of<val_type>::type rhs_type;
    typedef amgcl::mpi::distributed_matrix<Backend> DMatrix;

    using amgcl::prof;

    auto A = std::make_shared<DMatrix>(comm, Astrip);

    if (comm.size == 1 || ptype == amgcl::runtime::mpi::partition::merge)
        return A;

    prof.tic("partition");
    boost::property_tree::ptree prm;
    prm.put("type", ptype);
    amgcl::runtime::mpi::partition::wrapper<Backend> part(prm);

    auto I = part(*A, block_size);
    auto J = transpose(*I);
    A = product(*J, *product(*A, *I));

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> new_rhs(J->loc_rows());
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<rhs_type> new_rhs(bprm.q, J->loc_rows());
#elif defined(SOLVER_BACKEND_CUDA)
    thrust::device_vector<rhs_type> new_rhs(J->loc_rows());
#endif

    J->move_to_backend(bprm);

    amgcl::backend::spmv(1, *J, rhs, 0, new_rhs);
    rhs.swap(new_rhs);
    prof.toc("partition");

    return A;
}

//---------------------------------------------------------------------------
#if defined(SOLVER_BACKEND_BUILTIN) || defined(SOLVER_BACKEND_VEXCL)
template <int B>
void solve_block(
        amgcl::mpi::communicator comm,
        ptrdiff_t chunk,
        const std::vector<ptrdiff_t>      &ptr,
        const std::vector<ptrdiff_t>      &col,
        const std::vector<double>         &val,
        const boost::property_tree::ptree &prm,
        const std::vector<double>         &f,
        amgcl::runtime::mpi::partition::type ptype
        )
{
    typedef amgcl::static_matrix<double, B, B> val_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;

#if defined(SOLVER_BACKEND_BUILTIN)
    typedef amgcl::backend::builtin<val_type> Backend;
#elif defined(SOLVER_BACKEND_VEXCL)
    typedef amgcl::backend::vexcl<val_type> Backend;
#endif

    typedef
        amgcl::mpi::make_solver<
            amgcl::runtime::mpi::preconditioner<Backend>,
            amgcl::runtime::mpi::solver::wrapper<Backend>
            >
        Solver;

    using amgcl::prof;

    typename Backend::params bprm;

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> rhs(
            reinterpret_cast<const rhs_type*>(&f[0]),
            reinterpret_cast<const rhs_type*>(&f[0]) + chunk / B
            );
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    bprm.q = ctx;

    vex::scoped_program_header header(ctx,
            amgcl::backend::vexcl_static_matrix_declaration<double,B>());

    if (comm.rank == 0) std::cout << ctx << std::endl;

    vex::vector<rhs_type> rhs(ctx, chunk / B, reinterpret_cast<const rhs_type*>(&f[0]));
#endif

    prof.tic("setup");
    std::shared_ptr<Solver> solve;
    if (ptype) {
        auto A = partition<Backend>(comm,
                amgcl::adapter::block_matrix<val_type>(std::tie(chunk, ptr, col, val)),
                rhs, bprm, ptype, prm.get("precond.coarsening.aggr.block_size", 1));

        solve = std::make_shared<Solver>(comm, A, prm, bprm);
        chunk = A->loc_rows();
    } else {
        solve = std::make_shared<Solver>(comm,
                amgcl::adapter::block_matrix<val_type>(std::tie(chunk, ptr, col, val)),
                prm, bprm);
    }
    prof.toc("setup");

    if (comm.rank == 0) {
        std::cout << *solve << std::endl;
    }

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> x(chunk);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<rhs_type> x(ctx, chunk);
    x = math::zero<rhs_type>();
#endif

    int    iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = (*solve)(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}
#endif

//---------------------------------------------------------------------------
void solve_scalar(
        amgcl::mpi::communicator comm,
        ptrdiff_t chunk,
        const std::vector<ptrdiff_t> &ptr,
        const std::vector<ptrdiff_t> &col,
        const std::vector<double> &val,
        const boost::property_tree::ptree &prm,
        const std::vector<double> &f,
        amgcl::runtime::mpi::partition::type ptype
        )
{
#if defined(SOLVER_BACKEND_BUILTIN)
    typedef amgcl::backend::builtin<double> Backend;
#elif defined(SOLVER_BACKEND_VEXCL)
    typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
    typedef amgcl::backend::cuda<double> Backend;
#endif

    typedef
        amgcl::mpi::make_solver<
            amgcl::runtime::mpi::preconditioner<Backend>,
            amgcl::runtime::mpi::solver::wrapper<Backend>
            >
        Solver;

    using amgcl::prof;

    typename Backend::params bprm;

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<double> rhs(f);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    bprm.q = ctx;

    if (comm.rank == 0) std::cout << ctx << std::endl;

    vex::vector<double> rhs(ctx, f);
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
    thrust::device_vector<double> rhs(f);
#endif

    prof.tic("setup");
    std::shared_ptr<Solver> solve;
    if (ptype) {
        auto A = partition<Backend>(comm,
                std::tie(chunk, ptr, col, val), rhs, bprm, ptype,
                prm.get("precond.coarsening.aggr.block_size", 1));

        solve = std::make_shared<Solver>(comm, A, prm, bprm);
        chunk = A->loc_rows();
    } else {
        solve = std::make_shared<Solver>(comm, std::tie(chunk, ptr, col, val), prm, bprm);
    }
    prof.toc("setup");

    if (comm.rank == 0) {
        std::cout << *solve << std::endl;
    }

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<double> x(chunk);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<double> x(ctx, chunk);
    x = 0.0;
#elif defined(SOLVER_BACKEND_CUDA)
    thrust::device_vector<double> x(chunk, 0.0);
#endif

    int    iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = (*solve)(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    amgcl::mpi::init_thread mpi(&argc, &argv);
    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    if (comm.rank == 0)
        std::cout << "World size: " << comm.size << std::endl;

    using amgcl::prof;

    // Read configuration from command line
    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        ("matrix,A",
         po::value<std::string>(),
         "System matrix in the MatrixMarket format. "
         "When not specified, a Poisson problem in 3D unit cube is assembled. "
        )
        (
         "rhs,f",
         po::value<std::string>()->default_value(""),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "Ap",
         po::value< std::vector<std::string> >()->multitoken(),
         "Pre-partitioned matrix (single file per MPI process)"
        )
        (
         "fp",
         po::value< std::vector<std::string> >()->multitoken(),
         "Pre-partitioned RHS (single file per MPI process)"
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "block-size,b",
         po::value<int>()->default_value(1),
         "The block size of the system matrix. "
         "When specified, the system matrix is assumed to have block-wise structure. "
         "This usually is the case for problems in elasticity, structural mechanics, "
         "for coupled systems of PDE (such as Navier-Stokes equations), etc. "
        )
        (
         "partitioner,r",
         po::value<amgcl::runtime::mpi::partition::type>()->default_value(
#if defined(AMGCL_HAVE_SCOTCH)
             amgcl::runtime::mpi::partition::ptscotch
#elif defined(AMGCL_HAVE_PARMETIS)
             amgcl::runtime::mpi::partition::parmetis
#else
             amgcl::runtime::mpi::partition::merge
#endif
             ),
         "Repartition the system matrix"
        )
        (
         "size,n",
         po::value<ptrdiff_t>()->default_value(128),
         "domain size"
        )
        ("prm-file,P",
         po::value<std::string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::positional_options_description p;
    p.add("prm", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (comm.rank == 0) std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<std::string>(), prm);
    }

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as<std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    ptrdiff_t n;
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    int block_size = vm["block-size"].as<int>();
    int aggr_block = prm.get("precond.coarsening.aggr.block_size", 1);

    bool binary = vm["binary"].as<bool>();
    amgcl::runtime::mpi::partition::type ptype = vm["partitioner"].as<amgcl::runtime::mpi::partition::type>();

    if (vm.count("matrix")) {
        prof.tic("read");
        if (binary) {
            n = read_binary(comm,
                    vm["matrix"].as<std::string>(),
                    vm["rhs"].as<std::string>(),
                    block_size * aggr_block, ptr, col, val, rhs);
        } else {
            n = read_matrix_market(comm,
                    vm["matrix"].as<std::string>(),
                    vm["rhs"].as<std::string>(),
                    block_size * aggr_block, ptr, col, val, rhs);
        }
        prof.toc("read");
    } else if (vm.count("Ap")) {
        prof.tic("read");
        ptype = static_cast<amgcl::runtime::mpi::partition::type>(0);

        std::vector<std::string> Aparts = vm["Ap"].as<std::vector<std::string>>();
        comm.check(Aparts.size() == static_cast<size_t>(comm.size),
                "--Ap should have single entry per MPI process");

        if (binary) {
            amgcl::io::read_crs(Aparts[comm.rank], n, ptr, col, val);
        } else {
            ptrdiff_t m;
            std::tie(n, m) = amgcl::io::mm_reader(Aparts[comm.rank])(ptr, col, val);
        }

        if (vm.count("fp")) {
            std::vector<std::string> fparts = vm["fp"].as<std::vector<std::string>>();
            comm.check(fparts.size() == static_cast<size_t>(comm.size),
                    "--fp should have single entry per MPI process");

            ptrdiff_t rows;
            ptrdiff_t cols;

            if (binary) {
                amgcl::io::read_dense(fparts[comm.rank], rows, cols, rhs);
            } else {
                std::tie(rows, cols) = amgcl::io::mm_reader(fparts[comm.rank])(rhs);
            }

            comm.check(rhs.size() == static_cast<size_t>(n), "Wrong RHS size");
        } else {
            rhs.resize(n, 1);
        }
        prof.toc("read");
    } else {
        prof.tic("assemble");
        n = assemble_poisson3d(comm,
                vm["size"].as<ptrdiff_t>(),
                block_size * aggr_block, ptr, col, val, rhs);
        prof.toc("assemble");
    }

    switch(block_size) {

#if defined(SOLVER_BACKEND_BUILTIN) || defined(SOLVER_BACKEND_VEXCL)
#  define AMGCL_CALL_BLOCK_SOLVER(z, data, B)                        \
        case B:                                                      \
            solve_block<B>(comm, n, ptr, col, val, prm, rhs, ptype); \
            break;

        BOOST_PP_SEQ_FOR_EACH(AMGCL_CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)

#  undef AMGCL_CALL_BLOCK_SOLVER
#endif

        case 1:
            solve_scalar(comm, n, ptr, col, val, prm, rhs, ptype);
            break;
        default:
            if (comm.rank == 0)
                std::cout << "Unsupported block size!" << std::endl;
    }
}


/*
 * FunctionWrapper.hpp
 *
 *  Created on: Feb 7, 2015
 *      Author: dagothar
 */

#ifndef SRC_RW_MATH_FUNCTIONWRAPPER_HPP_
#define SRC_RW_MATH_FUNCTIONWRAPPER_HPP_

#include <rw/core/Ptr.hpp>
#include <rw/math/Function.hpp>
#include <boost/function.hpp>

namespace rw {
namespace math {

/** @addtogroup math */
/*@{*/

/**
 * A wrapper to create a Function from a callback
 */
template<class RES_T = double, class ARG_T = double>
class FunctionWrapper: virtual public Function<RES_T, ARG_T> {
public:
	//! Smart pointer type
	typedef rw::core::Ptr<FunctionWrapper> Ptr;

public:
	/**
	 * @brief Constructor
	 *
	 * @param f [in] a callback that returns the function value
	 */
	FunctionWrapper(typename boost::function<RES_T(ARG_T)> f) :
			_f(f) {
	}

	//! Destructor
	virtual ~FunctionWrapper() {
	}

	/**
	 * @copydoc Function::f
	 *
	 * Redirects function call to the wrapped callback.
	 */
	virtual RES_T f(ARG_T q) {
		return _f(q);
	}

	//! Get the wrapped callback.
	typename boost::function<RES_T(ARG_T)> getCallback() {
		return _f;
	}

private:
	typename boost::function<RES_T(ARG_T)> _f;
};

/**
 * A wrapper to create a Function1Diff from a callback.
 */
template<class RES_T = double, class ARG_T = double, class GRAD_T = double>
class Function1DiffWrapper: public FunctionWrapper<RES_T, ARG_T>,
		virtual public Function1Diff<RES_T, ARG_T, GRAD_T> {
public:
	//! Smart pointer type
	typedef rw::core::Ptr<Function1DiffWrapper> Ptr;

public:
	/**
	 * @brief Constructor
	 *
	 * @param f [in] a callback that returns the function value
	 * @param df [in] a callback that returns the function derivative
	 */
	Function1DiffWrapper(typename boost::function<RES_T(ARG_T)> f,
			typename boost::function<GRAD_T(ARG_T)> df) :
			FunctionWrapper<RES_T, ARG_T>(f), _df(df) {
	}

	//! Destructor
	virtual ~Function1DiffWrapper() {
	}

	/**
	 * @copydoc Function::df
	 *
	 * Redirects gradient evaluation to the wrapped callback.
	 */
	virtual GRAD_T df(ARG_T q) {
		return _df(q);
	}

	//! Get the wrapped callback.
	typename boost::function<RES_T(ARG_T)> getGradientCallback() {
		return _df;
	}

private:
	typename boost::function<GRAD_T(ARG_T)> _df;
};

} /* namespace math */
} /* namespace rw */

#endif /* SRC_RW_MATH_FUNCTIONWRAPPER_HPP_ */


// Copyright (C) 2001-2003
// William E. Kempf
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  William E. Kempf makes no representations
// about the suitability of this software for any purpose.
// It is provided "as is" without express or implied warranty.

#include <boost/thread/thread.hpp>
#include <iostream>

const int NUM_CALCS=5;

class factorial
{
public:
    factorial(int x, int& res) : x(x), res(res) { }
    void operator()() { res = calculate(x); }
    int result() const { return res; }

private:
    int calculate(int x) { return x <= 1 ? 1 : x * calculate(x-1); }

private:
    int x;
    int& res;
};

int main()
{
    int results[NUM_CALCS];
    boost::thread_group thrds;
    for (int i=0; i < NUM_CALCS; ++i)
        thrds.create_thread(factorial(i*10, results[i]));
    thrds.join_all();
    for (int j=0; j < NUM_CALCS; ++j)
        std::cout << j*10 << "! = " << results[j] << std::endl;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2012-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
* \file
* \author Martin Weiser
* This file contains an implementation of an additive hierarchical basis preconditioner.
*/

#ifndef HB_HH
#define HB_HH

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

#include <boost/utility/result_of.hpp>

#include "dune/grid/common/grid.hh"
#include "dune/istl/preconditioners.hh"

#include "fem/barycentric.hh"

namespace Kaskade {

  /**
  * \ingroup linalgsolution
  * \brief A hierarchical basis preconditioner.
  *
  * This preconditioner realizes an approximative inverse of a Laplace operator with constant diffusion coefficient
  * discretized by linear finite elements in nodal basis over the leaf view of the provided grid.
  *
  * This implementation is geometry- and coefficient-agnostic, i.e. it assumes that the diagonal of the stiffness matrix is
  * just the identity. The advantage of this approach are its extreme simplicity, both in terms of interface and implementation,
  * a minimal memory footprint, and very cheap set-up phase and application. The drawback is, of course, a worse resulting
  * condition number in case non-uniform coarse grids, anisotropic coarse grids, or spatially varying or anisotropic
  * diffusion coefficients are to be treated.
  *
  * Extending the implementation to take more structure (stiffness matrices, non-uniform grids) into account is probably
  * not worth the while, as the iteration count grows anyway in proportion to the number of refinement levels in 2D
  * or even faster in 3D. If the simplicity of the PrecondType::HB preconditioner does'nt pay off, have a look at \ref MultigridSolver
  * or \ref BPXYPreconditioner.
  *
  * We refer to Deuflhard/Weiser Chapter 7.3.
  *
  * \tparam Grid the grid on which the Laplace operator is discretized. This implementation assumes that the grid
  *              is purely simplicial and that on refinement new nodes are created only at edge midpoints.
  * \tparam Domain the type of domain space vectors
  * \tparam Range the type of range space vectors
  */

  template <class Grid, class Domain, class Range>
  class HierarchicalBasisPreconditioner: public Dune::Preconditioner<Domain,Range> {
    private:
      typedef typename Grid::LevelGridView::IndexSet::IndexType Index;
      typedef std::vector<Index>                                NodeSet;

      static int const dim = Grid::dimension;

    public:
      enum { category = Dune::SolverCategory::sequential };

      /**
      * \brief Constructor.
      * \param grid the grid
      * \param mat_ a matrix containing (at least) the diagonal of the stiffness matrix
      */
      explicit HierarchicalBasisPreconditioner(Grid const& grid)
      {
	// make sure the grid satisfies our assumptions: a purely simplicial grid
	assert(grid.leafIndexSet().geomTypes(0).size()==1 && grid.leafIndexSet().geomTypes(0)[0].isSimplex()
	      && "HierarchicalBasisPreconditioner currently requires purely simplicial grids");

	// extract and store relevant sizes
	j = grid.maxLevel();
	n = grid.leafIndexSet().size(dim);

	typedef typename Grid::template Codim<0>::LevelIterator  CellLevelIterator;
	typedef typename Grid::template Codim<dim>::LevelIterator LeafVertexIterator;

	// extract the coarse grid nodes
	nodesOnLevel.resize(j+1);
	for (LeafVertexIterator i=grid.template lbegin<dim>(0); i!=grid.template lend<dim>(0); ++i)
	  nodesOnLevel[0].push_back(grid.leafIndexSet().index(*i));

	// extract the coarse grid elements' vertices
	coarseElement.reserve(grid.levelIndexSet(0).size(0));
	for (CellLevelIterator ci=grid.template lbegin<0>(0); ci!=grid.template lend<0>(0); ++ci)
	{
	  Dune::FieldVector<Index,dim+1> idx;
	  for (int i=0; i<=dim; ++i)
	    idx[i] = grid.leafIndexSet().index(*(ci->template subEntity<dim>(i)));
	  coarseElement.push_back(idx);
	}

	// compute the parents of each node on level >0
	parents.resize(n,std::make_pair(n,n)); // invalid sentinel (n,n) denotes not yet processed nodes.
	for (int k=1; k<=j; ++k)
	{
	  // On each level, look out where nodes of this level are located in their father cell. Their barycentric coordinates
	  // should have exactly two nonzero entries of size 0.5. The corresponding corners of the father cell are the parents.
	  for (CellLevelIterator ci=grid.template lbegin<0>(k); ci!=grid.template lend<0>(k); ++ci)
	  {
	    typedef typename Grid::template Codim<0>::EntityPointer CellPointer;
	    CellPointer father = ci->father();

	    // Extract indices of father's vertices
	    // @TODO Variable is never read! Remove or keep for future extension of the preconditioner??
//            Index fatherCornerIndex[dim+1];
//            for (int i=0; i<=dim; ++i)
//              fatherCornerIndex[i] = grid.leafIndexSet().index(*(father->template subEntity<dim>(i)));

	    // consider each vertex of current cell in turn
	    for (int i=0; i<=dim; ++i)
	    {
	      typename Grid::template Codim<dim>::EntityPointer vertexPointer = ci->template subEntity<dim>(i);
	      Index idx = grid.leafIndexSet().index(*vertexPointer);

	      // Edge midpoints can be reached from multiple cells depending on the spatial dimension. The parent nodes
	      // are independent of from which cell we look. Thus we check wether we've already done the work.
	      if (parents[idx].first==n && vertexPointer->level()>0) // not yet processed (but has parents)
	      {
		// Obtain barycentric coordinates of child corner in father. For child nodes located on an edge,
		// there will be exactly two nonzero entries associated to the vertices. Note that the barycentric
		// coordinates as implemented in fem/barycentric.hh are shifted by index 1 compared to the
		// reference element vertex numbering.
		Dune::FieldVector<typename Grid::ctype,dim+1> relativeCoord = barycentric(ci->geometryInFather().corner(i));

		// Extract the two coordinates with value 0.5
		int coords[dim+1];
		int pos = 0;
		for (int m=0; m<=dim; ++m)
		  if (relativeCoord[m] > 0.4)
		    coords[pos++] = (m+1)%(dim+1);

		if (pos==2)
		{
		  // that's an edge midpoint: obtain the leaf indices of those father corners
		  parents[idx].first  = grid.leafIndexSet().index(*(father->template subEntity<dim>(coords[0])));
		  parents[idx].second = grid.leafIndexSet().index(*(father->template subEntity<dim>(coords[1])));

		  // write down that this node appeared first on level k
		  nodesOnLevel[k].push_back(idx);
		}
	      }
	    }
	  }
	}

	// perform sanity checks
	#ifndef NDEBUG
	// check that each level >0 node has parents
	int count = nodesOnLevel[0].size();
	for (int k=1; k<=j; ++k)
	{
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	    assert(parents[*i].first<n && parents[*i].second<n);
	  count += nodesOnLevel[k].size();
	}
	assert(count==n);
	#endif
      }

      /**
      * \brief Has to be called before the first call to apply
      */
      virtual void pre (Domain& x, Range& b)
      {
	// actually, does nothing :)
      }

      /**
      * \brief applies the preconditioner to the residual \arg d, which results in the correction \arg v
      */
      virtual void apply (Domain& v, const Range& d)
      {
	using namespace boost::fusion;
      // We modify the residual (in-place), hence we have to copy it here.
	Range r = d;

	// apply smoother and recursively restrict the residual downwards through the mesh hierarchy
	for (int k=j; k>0; --k)
	{
	  // correct scaling for diagonal coefficients
	  typename Domain::field_type a = std::pow(2.0,(Grid::dimension-2.0)*(k-j));

	  //  for all nodes on level k
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	  {
	    // apply one Jacobi step, care for correct scaling of lower level hierarchical basis functions
	    // Beispiel at_c<0>(estSol.data)[*j];

	    at_c<0>(v.data)[*i] = at_c<0>(r.data)[*i];
	    at_c<0>(v.data)[*i] /= a;

	    // restrict the residual
	    at_c<0>(r.data)[parents[*i].first]  += 0.5 * at_c<0>(r.data)[*i];
	    at_c<0>(r.data)[parents[*i].second] += 0.5 * at_c<0>(r.data)[*i];
	  }
	}

	// "solve" on coarse grid.
	coarseGridSolution(v,r);

	// prolongate the correction recursively upwards through the mesh hierarchy, adding up
	// all the corrections from different levels
	for (int k=1; k<=j; ++k)
	  // the nodes on level k get contributions of 0.5 from each of their parent nodes - that's all
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	    at_c<0>(v.data)[*i] += 0.5*(at_c<0>(v.data)[parents[*i].first]+at_c<0>(v.data)[parents[*i].second]);
      }

      /**
      * \brief Has to be called after the last call to apply
      */
      virtual void post (Domain& x)
      {
	// actually, does nothing :)
      }

    private:
      std::vector<NodeSet>                       nodesOnLevel;   // hierarchical basis node sets
      std::vector<std::pair<Index,Index> >       parents;        // parent nodes of child nodes on level >0
      int                                        j;              // maximum grid level
      Index                                      n;              // total number of nodes
      std::vector<Dune::FieldVector<Index,dim+1> > coarseElement;

      // This coarse grid solution employs a simple Jacobi iteration, where the
      // matrix is simply patched together from identical elemental stiffness matrices.
      // TODO: maybe it would be better to explicitly form the coarse grid matrix and use a direct solver
      void coarseGridSolution(Domain& v, Range& r) const {
	using namespace boost::fusion;
	// get the correct scaling for the coarse grid
	typename Domain::field_type a = std::pow(2.0,-(Grid::dimension-2.0));

	// Initialize correction to zero
	for (typename NodeSet::const_iterator i=nodesOnLevel[0].begin(); i!=nodesOnLevel[0].end(); ++i)
	  at_c<0>(v.data)[*i] = 0;
	Domain dv = v; // TODO: that's a looooong vector (overkill)

	// perform a couple of Jacobi iterations
	for (int k=0; k<20; ++k)
	{
	  // add correction: solve with diagonal and set residual to zero
	  for (typename NodeSet::const_iterator i=nodesOnLevel[0].begin(); i!=nodesOnLevel[0].end(); ++i)
	  {
	    at_c<0>(dv.data)[*i] = at_c<0>(r.data)[*i];
	    at_c<0>(v.data)[*i] += at_c<0>(dv.data)[*i] / a;
	  }

	  // update the residual. We are geometry agnostic here and use a rough approximation of the Laplacian: On each
	  // coarse grid element we assume we have the following element matrices in 1D, 2D, 3D, respectively:
	  // [ 1 -1 ]       [ 2 -1 -1 ]        [  3 -1 -1 -1 ]
	  // [ -1 1 ], 1/12 [-1  2 -1 ], 1/60  [ -1  3 -1 -1 ]
	  //                [-1 -1  2 ]        [ -1 -1  3 -1 ]
	  //                                   [ -1 -1 -1  3 ]
	  // Obviously, the matrixes are just  s*(-e*e^T + (d+1)*I), where e is the vector with all entries zero. In order
	  // to have an invertible matrix, we add a little bit more of the identity, i.e. we end up with element matrices
	  // s*(-e*e^T + (d+1.1)*I) with s=1, 1/12, 1/60 for d=1,2,3, respectively. In this form, multiplication with the
	  // total stiffness matrix is just summing up the contributions of the elemental matrices that can efficiently
	  // been computed.
	  typename Dune::Preconditioner<Domain,Range>::field_type s = dim==1? 1.0: dim==2? 1/12.0 : 1/60.0;
	  for (int i=0; i<coarseElement.size(); ++i)
	  {
	    //typename Domain::block_type eTdv = 0;
	    double eTdv = 0;
	    for (int m=0; m<=dim; ++m)
	      eTdv += at_c<0>(dv.data)[coarseElement[i][m]];
	    for (int m=0; m<=dim; ++m)
	      at_c<0>(r.data)[coarseElement[i][m]] -= a*s*(-eTdv+(dim+1.1)*at_c<0>(dv.data)[coarseElement[i][m]]);
	  }
	}
      }
  };

}

#endif


//Plugin Template

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
#include <iostream>
#include <string>
#include <fstream>
#include <math.h>

namespace gazebo {

double max(double a, double b) {
	return (a>b)?a:b;
}

double min(double a, double b) {
	return (a<b)?a:b;
}

void Print_Vec(math::Vector3 vect) {
	std::cout << vect.x<<", "<<vect.y<<", "<<vect.z<<"\n";
}

class Push : public ModelPlugin {
	public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {

		//Getting model pointer
		this->model = _parent;

		//Getting links
		body = this->model->GetLink("link_0");
		lb_wheel = this->model->GetLink("link_1_clone_clone");
		rb_wheel = this->model->GetLink("link_1_clone_clone_0");
		lf_wheel = this->model->GetLink("link_1_clone");
		rf_wheel = this->model->GetLink("link_1");

		//Getting joints
		lb_drive = this->model->GetJoint("lb_drive");
		rf_drive = this->model->GetJoint("rf_drive");
		lf_drive = this->model->GetJoint("lf_drive");
		rb_drive = this->model->GetJoint("rb_drive");
		lf_steer = this->model->GetJoint("lf_steer");
		rf_steer = this->model->GetJoint("rf_steer");

		//Initializing Joint Controller
		this->j_con = this->model->GetJointController();

		//Joint Pointer Vectors -> Drive and Steer
		std::vector<physics::JointPtr> j_drive;
		std::vector<physics::JointPtr> j_steer;

		j_drive.push_back(lb_drive);
		j_drive.push_back(rb_drive);
		j_drive.push_back(rf_drive);
		j_drive.push_back(lf_drive);

		j_steer.push_back(lf_steer);
		j_steer.push_back(rf_steer);

		//Initializing PID Controllers
		this->pidrive = common::PID(10000,0,0);
		this->pisteer1 = common::PID(10000,0,0);
		this->pisteer2 = common::PID(10000,0,0);

		//Assigning PID controllers to Joints
		for (int i = 0 ; i < j_drive.size(); i++) {
			j_con->SetPositionPID(j_drive[i]->GetScopedName(),pidrive);
		}
		j_con->SetPositionPID(lf_steer->GetScopedName(),pisteer1);
		j_con->SetPositionPID(rf_steer->GetScopedName(),pisteer2);

		//A couple of state flags, for future use
		flag = 0;
		state = 0;

		radius = 20*(lb_wheel->GetWorldCoGPose().pos.x);

		//Getting front wheels' parameters for ackermann
		lfd = math::Vector3(-radius*.95,0,0);
		lfd = (lf_wheel->GetWorldCoGPose().pos - lb_wheel->GetWorldCoGPose().pos)+lfd;
		rfd = math::Vector3(-radius*1.05,0,0);
		rfd = (rf_wheel->GetWorldCoGPose().pos - rb_wheel->GetWorldCoGPose().pos)+rfd;

		//Radius Setting with test output
		std::cout << "Radius of trajectory is "<<radius<<std::endl;
		//Update Call
		this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&Push::OnUpdate,this,_1));

	}

	public: void  OnUpdate(const common::UpdateInfo& _info) {
		//Getting Time
		common::Time T_curr = this->model->GetWorld()->GetSimTime();
		double t = T_curr.Double();

		//driver value for wheels
		double driver = 3*max(0,t-1);

		//Control of rear wheels' drive
		j_con->SetPositionTarget(rb_drive->GetScopedName(), 1.05*driver);
		j_con->SetPositionTarget(lb_drive->GetScopedName(), 0.95*driver);

		//Control of front wheels' drive
		std::cout << lfd.GetLength() << " | "<< rfd.GetLength()<<std::endl;
		j_con->SetPositionTarget(lf_drive->GetScopedName(), driver*lfd.GetLength()/-radius);
		j_con->SetPositionTarget(rf_drive->GetScopedName(), driver*rfd.GetLength()/-radius);

		//Getting steering angles
		double l_angle = lf_steer->GetAngle(0).Radian();
		double r_angle = rf_steer->GetAngle(0).Radian();

		//Left Ackermann Check
		math::Vector3 v1(radius,lb_wheel->GetWorldCoGPose().pos.y,lb_wheel->GetWorldCoGPose().pos.z);
		v1 = lf_wheel->GetWorldCoGPose().pos - v1;
		math::Vector3 v2(-sin(l_angle),cos(l_angle),0);
		if ((1000*std::abs(v1.Normalize().Dot(v2)) < 1) && (flag == 0)) {
			final_l = l_angle;
			std::cout << "final_l is "<<final_l<<std::endl;
			flag = 1;
			Print_Vec(v1.Normalize());
			Print_Vec(v2);
			std::cout << std::abs(v1.Normalize().Dot(v2)) << std::endl;
		}

		//Right Ackermann Check
		math::Vector3 v3(radius, rb_wheel->GetWorldCoGPose().pos.y, rb_wheel->GetWorldCoGPose().pos.z);
		v3 = rf_wheel->GetWorldCoGPose().pos - v3;
		math::Vector3 v4(-sin(r_angle),cos(r_angle),0);
		if ((1000*std::abs(v3.Normalize().Dot(v4)) < 1) && (state == 0)) {
			final_r = r_angle;
			std::cout << "final_r is "<<final_r<<std::endl;
			state = 1;
			Print_Vec(v3.Normalize());
			Print_Vec(v4);
			std::cout << std::abs(v3.Normalize().Dot(v4)) << std::endl;
		}

		//Steer Updates wrt Ackermann
		if (flag == 0)
			j_con->SetPositionTarget(lf_steer->GetScopedName(), t);
		else
			j_con->SetPositionTarget(lf_steer->GetScopedName(), final_l);

		if (state == 0)
			j_con->SetPositionTarget(rf_steer->GetScopedName(), t);
		else
			j_con->SetPositionTarget(rf_steer->GetScopedName(), final_r);

		//Debugging print statement
		if (t > 1)
			std::cout << final_l << " | "<<final_r<<" | "<<l_angle<<" | "<<r_angle<<"\n";

	}

	public: math::Vector3 lfd,rfd;
	public: double radius, final_l, final_r;
	private: int flag;
	private: int state;
	private: double max_y;
	private: double min_y;
	private: common::PID pisteer1;
	private: common::PID pisteer2;
	private: common::PID pidrive;
	private: common::Time T_prev;
	private: physics::LinkPtr body, lb_wheel, rb_wheel, lf_wheel, rf_wheel;
	private: physics::JointControllerPtr j_con;
	private: physics::JointPtr lb_drive,rb_drive,lf_drive,rf_drive,lf_steer,rf_steer;
	private: physics::ModelPtr model;
	private: event::ConnectionPtr updateConnection;
};
	GZ_REGISTER_MODEL_PLUGIN(Push)
}


#include <iostream>
#include <cstdio>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/connected_components.hpp>
// #include <boost/unordered_set.hpp>
#include <ctime>
#include <stack>
#include <queue>
#include <fstream>
// #include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/functional/hash.hpp>
#include <random>
#include <unordered_set>
#include <unordered_map>
// using namespace std;
using namespace boost;

typedef std::vector < std::vector <int> > vvi;

typedef adjacency_list< listS, vecS, undirectedS, no_property, property<edge_weight_t, float> > Graph;
// typedef adjacency_list< listS,vecS, undirectedS > Graph;
typedef graph_traits < Graph> ::vertex_descriptor vertex_descriptor;
typedef graph_traits < Graph> ::edge_descriptor edge_descriptor;
typedef Graph::vertex_iterator vertex_iterator;
typedef Graph::edge_iterator edge_iterator;
typedef std::pair <int, int> Edge;
typedef graph_traits < Graph> ::adjacency_iterator my_adjacency_iterator;
property_map<Graph, edge_weight_t>::type weight;

std::unordered_set<int> union_(std::unordered_set<int> &set1, std::unordered_set<int> &set2)
{
	// returns the union of two unordered sets of integers
	std::unordered_set<int> new_set = set1;

	for(auto thing : set2)
		new_set.insert(thing);
	return new_set;
}

std::vector< std::vector <int> > sample(std::vector <int> &things, int &k)
{
	// Returns the set of sampled nodes needed for the whole process of spanner construction as vector of integer vectors
	std::vector< std::vector<int> > master_v;
	std::vector<int> v = things;

	int i = 1, n = v.size(), s;
	float f;

	std::vector<int> counts;

	for(i = 1; i < k; i ++)
	{
		f = i / (float) k;
		s = ceil(pow(n, 1 - f));
		counts.push_back(s);
	}

	master_v.push_back(v);

	std::default_random_engine engine(std::random_device {}());
	std::shuffle(master_v[0].begin(), master_v[0].end(), engine);

	for(i = 1; i < k; i ++)
	{
		std::vector<int> temp;
		for(int j = 0; j < counts[i - 1]; j ++)
			temp.push_back(master_v[i - 1][j]);
		std::shuffle(temp.begin(), temp.end(), engine);
		master_v.push_back(temp);
	}
	return master_v;
}

tuple<Graph, Graph, Graph> read_graphs(std::string filename)
{
	// Reads the graphs from the edgelist. At first, the graphs G, G_comm and G_prime are all the same
	std::ifstream fp(filename);
	Graph G, G_comm, G_prime;
	int u, v;
	float w;

	while(fp >> u >> v >> w)
	{
		add_edge(u, v, w, G);
		add_edge(u, v, w, G_comm);
		add_edge(u, v, w, G_prime);
	}
	fp.close();
	return make_tuple(G, G_comm, G_prime);
}

Graph make_spanner(std::string filename, int k)
{
	// constructs the (2k - 1)-spanner using Baswana and Sen's randomized algorithm
	Graph G, G_comm, G_prime;
	Graph G_spanner;

	tie(G, G_comm, G_prime) = read_graphs(filename);

	std::unordered_set <std::pair<int, int>, hash< std::pair<int, int> > > old_Ei;
	std::unordered_set <std::pair<int, int>, hash< std::pair<int, int> > > new_Ei;

	std::unordered_set <int> v_prime_flag;
	std::unordered_map <int, int> membership_oldc;
	std::unordered_map <int, int> membership_newc;
	std::unordered_map <int, int> neighborhood;

	std::unordered_map <int, std::unordered_set <int> > old_c;

	std::pair <vertex_iterator, vertex_iterator> vp;

	std::vector <int> all_nodes;

	for(vp = vertices(G); vp.first != vp.second; ++vp.first) // iterating over all the nodes
	{
		auto node = *vp.first;
		old_c[node].insert(node);
		v_prime_flag.insert(node);
		membership_oldc[node] = node;
		membership_newc[node] = -1;
		all_nodes.push_back(node);
	}

	my_adjacency_iterator start, end;

	std::vector < std::vector<int> > R_vector; //this stores all the randomly sampled cluster heads for all iterations

	R_vector = sample(all_nodes, k); // R_vector stores the set of sampled nodes required at each iteration

	for(int i = 1; i < k; i ++)
	{
		/*****1. Forming a sample of clusters **********************/

		std::unordered_set<int> R_i(R_vector[i].begin(), R_vector[i].end()); // R_vector[i] gives the randomly picked cluster heads for ith iteration
		std::unordered_map <int, std::unordered_set <int> > new_c;

		for(auto item : old_c)
		{
			int v = item.first;

			if(R_i.find(v) != R_i.end())
				new_c[v] = old_c[v];
		}

		std::unordered_set <int> sampled_nodes;

		for(auto v : R_i)
			sampled_nodes = union_(sampled_nodes, old_c[v]);

		std::unordered_set <int> unsampled_nodes;

		for(vp = vertices(G_prime); vp.first != vp.second; ++vp.first)
		{
			int node = *(vp.first);
			if(sampled_nodes.find(node) == sampled_nodes.end())
				unsampled_nodes.insert(node);
		}

		neighborhood.clear();
		for(auto node : unsampled_nodes)
			neighborhood[node] = -1;

		if(i == 1)
			for(auto v : R_i)
				membership_newc[v] = v;

		else
		{
			std::pair <vertex_iterator, vertex_iterator> vp;
			for(vp = vertices(G_prime); vp.first != vp.second; ++ vp.first)
			{
				auto v = *(vp.first);
				if(membership_newc[v] != -1)
					if(R_i.find(membership_newc[v]) == R_i.end())
						membership_newc[v] = -1;
			}
		}

		new_Ei = old_Ei;
		int u, v;

		for(auto edge : old_Ei)
		{
			tie(u, v) = edge;
			if(R_i.find(membership_newc[u]) == R_i.end() || R_i.find(membership_newc[v]) == R_i.end())
			{
				std::pair<int, int> e = std::make_pair(u, v);
				new_Ei.erase(e);
			}
		}

		/***********************end of step 1**************************/

		/********2. finding nearest neighboring sampled cluster*********/

		for(int v : unsampled_nodes)
		{
			if(v_prime_flag.find(v) == v_prime_flag.end()) // if v is not in G_prime, we don't consider it
				continue;

			std::unordered_map <int, std::pair<float, int> > min_table;
			edge_descriptor e;
			vertex_descriptor src, dest;

			for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
			{
				int neighbor = *start;
				float wt;
				src = vertex(v, G);
				dest = vertex(neighbor, G);

				std::tie(e, std::ignore) = edge(src, dest, G);

				wt = get(weight, e);

				if(membership_newc[neighbor] != -1) // checking if the neighbor is sampled
				{
					if(min_table.find(membership_newc[neighbor]) == min_table.end())
						min_table[membership_newc[neighbor]] = std::make_pair(wt, neighbor);

					else if(wt < min_table[membership_newc[neighbor]].first)
					{
						min_table[membership_newc[neighbor]].first = wt;
						min_table[membership_newc[neighbor]].second = neighbor;
					}
				}
			}

			float min_ = INT_MAX;
			int nearest_neighbor = -1;

			for(auto item : min_table)
			{
				if(item.second.first  < min_)
				{
					min_ = item.second.first;
					nearest_neighbor = item.second.second;
				}
			}

			neighborhood[v] = nearest_neighbor;
		}

		/************end of step 2 ********************************/

		/* **********3. adding edges to spanner ******************/

		for(int v : unsampled_nodes)
		{
			if(v_prime_flag.find(v) == v_prime_flag.end())
				continue;

			if(neighborhood[v] == -1) //v is not adjacent to any sampled nodes
			{
				std::unordered_map <int, std::pair<float, int> > min_table;

				edge_descriptor e;
				vertex_descriptor src, dest;
				std::vector <std::pair<int, int> > edges_to_be_removed;
				my_adjacency_iterator start, end;

				for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
				{
					int neighbor = *start;
					float wt;

					src = vertex(v, G);
					dest = vertex(neighbor, G);

					std::tie(e, std::ignore) = edge(src, dest, G);

					wt = get(weight, e);

					if(membership_oldc[neighbor] != -1)
					{
						if(min_table.find(neighbor) == min_table.end())
							min_table[neighbor] = std::make_pair(wt, neighbor);

						if(wt <= min_table[neighbor].first)
						{
							min_table[neighbor].first = wt;
							min_table[neighbor].second = neighbor;

							edges_to_be_removed.push_back(std::make_pair(v, neighbor));
						}
					}
				}

				for(auto edge : edges_to_be_removed)
					remove_edge(edge.first, edge.second, G_prime);

				for(auto item : min_table)
				{
					add_edge(v, item.second.second, item.second.first, G_spanner);
					remove_edge(v, item.second.second, G_comm);
				}
			}

			else
			{
				my_adjacency_iterator start, end;
				float wt;

				edge_descriptor e;
				std::tie(e, std::ignore) = edge(v, neighborhood[v], G);
				wt = get(weight, e);

				add_edge(v, neighborhood[v], wt, G_spanner);
				remove_edge(v, neighborhood[v], G_comm);

				new_Ei.insert(std::make_pair(v, neighborhood[v]));

				std::vector <int> neighbors;
				for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
				{
					int neighbor = *start;
					neighbors.push_back(neighbor);
				}

				for(auto neighbor : neighbors)
				{
					if(membership_newc[neighbor] == membership_newc[neighborhood[v]])
						remove_edge(v, neighbor, G_prime);
				}

				std::unordered_map <int, std::pair<float, int> > min_table;
				edge_descriptor e1;
				vertex_descriptor src, dest;

				for(auto neighbor : neighbors)
				{
					float wt1, wt2;
					src = vertex(v, G);
					dest = vertex(neighbor, G);

					std::tie(e1, std::ignore) = edge(src, dest, G);
					wt1 = get(weight, e1);

					dest = vertex(neighborhood[v], G);
					std::tie(e1, std::ignore) = edge(src, dest, G);
					wt2 = get(weight, e1);

					if(wt1 < wt2)
					{
						if(min_table.find(membership_oldc[neighbor]) == min_table.end())
							min_table[membership_oldc[neighbor]] = std::make_pair(wt1, neighbor);

						else if(wt1 < min_table[membership_oldc[neighbor]].first)
						{
							min_table[membership_oldc[neighbor]].first = wt2;
							min_table[membership_oldc[neighbor]].second = neighbor;
							remove_edge(v, neighbor, G_prime);
						}
					}
				}

				for(auto item : min_table)
				{
					add_edge(v, item.second.second, item.second.first, G_spanner);
					remove_edge(v, item.second.second, G_comm);
					remove_edge(v, item.second.second, G_prime);
				}

			}

		}

		/************end of step 3*****************************************/

		/******4. removing intra cluster edges ***********************/

		std::vector< std::pair <int, int> > intracluster_edges;

		for(auto item : new_Ei)
		{
			int u = item.first;
			int v = item.second;

			if(membership_newc[u] != -1)
			{
				membership_newc[v] = membership_newc[u];
				new_c[membership_newc[u]].insert(v);
			}

			else if(membership_newc[v] != -1)
			{
				membership_newc[u] = membership_newc[v];
				new_c[membership_newc[v]].insert(u);
			}
		}

		edge_iterator e_start, e_end;

		for(std::tie(e_start, e_end) = edges(G_prime); e_start != e_end; ++ e_start)
		{
			int u = source(*e_start, G_prime), v = target(*e_start, G_prime);

			if(membership_newc[u] == membership_newc[v])
				intracluster_edges.push_back(std::make_pair(u, v));
		}

		for(auto edge : intracluster_edges)
		{
			// // std:: cout << "\n(" << edge.first << ", " << edge.second << ")";
			remove_edge(edge.first, edge.second, G_prime);
		}

		/*************end of step 4*************************************************/

		/**** updations needed at the end of iteration******************************/

		v_prime_flag.clear();

		int count = 0;
		// // std:: cout << "E_prime: \n";
		for(std::tie(e_start, e_end) = edges(G_prime); e_start != e_end; ++ e_start)
		{
			int u = source(*e_start, G_prime), v = target(*e_start, G_prime);
			v_prime_flag.insert(u);
			v_prime_flag.insert(v);
			count ++;
		}

		for(auto item : new_Ei)
		{
			v_prime_flag.insert(item.first);
			v_prime_flag.insert(item.second);
		}

		old_c = new_c;
		membership_oldc = membership_newc;

		/***********The end of iteration i *****************************/
	}

	/******* Phase 2 **************************/

	vertex_iterator v_start, v_end;

	for(std::tie(v_start, v_end) = vertices(G_prime); v_start != v_end; ++ v_start)
	{
		int v = *v_start;
		if(v_prime_flag.find(v) == v_prime_flag.end())
			continue;

		std::unordered_map <int, std::pair<float, int> > min_table;
		edge_descriptor e;
		vertex_descriptor src, dest;

		std::vector <std::pair<int, int> > edges_to_be_removed;

		for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
		{
			int neighbor = *start;
			float wt;
			src = vertex(v, G);
			dest = vertex(neighbor, G);

			std::tie(e, std::ignore) = edge(src, dest, G);

			wt = get(weight, e);

			if(membership_newc[neighbor] == -1)
				continue;

			if(min_table.find(membership_newc[neighbor]) == min_table.end())
				min_table[membership_newc[neighbor]] = std::make_pair(wt, neighbor);

			if(wt <= min_table[membership_newc[neighbor]].first)
			{
				min_table[membership_newc[neighbor]].first = wt;
				min_table[membership_newc[neighbor]].second = neighbor;

				edges_to_be_removed.push_back(std::make_pair(v, neighbor));
			}

			if(min_table.find(membership_newc[neighbor]) == min_table.end())
				edges_to_be_removed.push_back(std::make_pair(v, neighbor));
		}

		for(auto edge : edges_to_be_removed)
			remove_edge(edge.first, edge.second, G_prime);

		for(auto item : min_table)
		{
			add_edge(v, item.second.second, item.second.first, G_spanner);
			remove_edge(v, item.second.second, G_comm);
		}
	}
	return G_comm;
}

float THRESHOLD = 0.66;

std::stack <int> Broker_Stack;
std::queue <int> Community_Queue;
std::unordered_set <int> isolated;
std::unordered_set <int> influenced;
std::unordered_map<int, int> labels;
std::unordered_set <int> traversed;
std::unordered_set <int> brokers;
std::unordered_set <int> marked;
std::unordered_map<int, float> scores;

float INS_score(Graph &g, int node)
{
	int count = 0;
	int total_count = 0;

	my_adjacency_iterator start, end;
	vertex_descriptor v = vertex(node, g);

	for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
	{
		auto neighbor = *start;
		if(influenced.find(neighbor) != influenced.end())
			count += 1;
		total_count += 1;
	}

	return count * 1.0 / total_count;
}

void INS(Graph &g, int starting_node)
{
	vertex_iterator v_st, v_end;
	influenced.insert(starting_node);
	traversed.insert(starting_node);
	Broker_Stack.push(starting_node);
	brokers.insert(starting_node);
	scores[starting_node] = 0;
	labels[starting_node] = starting_node;

	my_adjacency_iterator start, end;

	while(Broker_Stack.size() + Community_Queue.size() > 0)
	{
		int node;
		if(! Community_Queue.empty())
		{
			node = Community_Queue.front();
			Community_Queue.pop();
		}
		else
		{
			node = Broker_Stack.top();
			Broker_Stack.pop();
		}

		isolated.erase(node);

		vertex_descriptor v = vertex(node, g);

		for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
			influenced.insert(*start);

		for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
		{
			int neighbor = *start;

			if(traversed.find(neighbor) != traversed.end())
				continue;
			else
			{
				traversed.insert(neighbor);
			}

			auto score = INS_score(g, neighbor);
			// std:: cout << "node: " << neighbor << " score: " << score << std:: endl;
			scores[neighbor] = score;

			if(score < THRESHOLD) //broker
			{
				labels[neighbor] = neighbor;
				Broker_Stack.push(neighbor);
				brokers.insert(neighbor);
			}

			else
			{
				if(marked.find(neighbor) == marked.end())
				{
					labels[neighbor] = labels[node];
					marked.insert(neighbor);
				}

				isolated.erase(neighbor);
				if(score != 1)
					Community_Queue.push(neighbor);
			}
		}
	}
}

void run_INS(Graph &G, std::string filename)
{
	for(unsigned int i = 0; i < num_vertices(G); i ++)
		isolated.insert(i);

	std::vector<int> component(num_vertices(G));
	int num = connected_components(G, &component[0]);

	vvi comp_lst(num);
	for(unsigned int i = 0; i != component.size(); ++i)
		comp_lst[component[i]].push_back(i);

	std::vector <int> starting_nodes;
	int min_deg = INT_MAX;
	int starting_node, deg;

	for(int i = 0; i < num; i ++)
	{
		starting_node = -1;
		min_deg = INT_MAX;
		for(auto node : comp_lst[i])
		{
			vertex_descriptor v = vertex(node, G);
			deg = degree(v, G);
			if(deg == 1)
			{
				starting_node = node;
				min_deg = deg;
				break;
			}
			else if(deg < min_deg)
			{
				starting_node = node;
				min_deg = deg;
			}
		}
		starting_nodes.push_back(starting_node);
	}

	for(auto start_node : starting_nodes)
		INS(G, start_node);

	std::string st = filename;
	std::ofstream fout(st + "_cover.part");

	for(auto it = labels.begin(); it != labels.end(); ++ it)
	{
		fout << it->first << " " << it->second << std::endl;
	}

	for(auto thing : isolated)
	{
		fout << thing << " " << thing << std::endl;
	}
	fout.close();

}

int main(int argc, char const *argv[])
{
	if(argc < 3)
	{
		std::cout << "Enter filename and k";
		return 0;
	}
	Graph G_comm;
	G_comm = make_spanner(argv[1], std::stoi(argv[2]));

	run_INS(G_comm, argv[1]);
	return 0;
}


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

// Copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_POLYGON_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_POLYGON_HPP

#include <cstddef>
#ifdef BOOST_GEOMETRY_TEST_DEBUG
#include <iostream>
#endif // BOOST_GEOMETRY_TEST_DEBUG

#include <algorithm>
#include <deque>
#include <iterator>
#include <set>
#include <vector>

#include <boost/core/ignore_unused.hpp>
#include <boost/range.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/range.hpp>

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

#include <boost/geometry/iterators/point_iterator.hpp>

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/num_interior_rings.hpp>
#include <boost/geometry/algorithms/validity_failure_type.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/algorithms/detail/check_iterator_range.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>

#include <boost/geometry/algorithms/detail/is_valid/complement_graph.hpp>
#include <boost/geometry/algorithms/detail/is_valid/has_valid_self_turns.hpp>
#include <boost/geometry/algorithms/detail/is_valid/is_acceptable_turn.hpp>
#include <boost/geometry/algorithms/detail/is_valid/ring.hpp>

#include <boost/geometry/algorithms/detail/is_valid/debug_print_turns.hpp>
#include <boost/geometry/algorithms/detail/is_valid/debug_validity_phase.hpp>
#include <boost/geometry/algorithms/detail/is_valid/debug_complement_graph.hpp>

#include <boost/geometry/algorithms/dispatch/is_valid.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template <typename Polygon, bool CheckRingValidityOnly = false>
class is_valid_polygon
{
protected:
    typedef debug_validity_phase<Polygon> debug_phase;

    template <typename VisitPolicy, typename Strategy>
    struct per_ring
    {
        per_ring(VisitPolicy& policy, Strategy const& strategy)
            : m_policy(policy)
            , m_strategy(strategy)
        {}

        template <typename Ring>
        inline bool apply(Ring const& ring) const
        {
            return detail::is_valid::is_valid_ring
                <
                    Ring, false, true
                >::apply(ring, m_policy, m_strategy);
        }

        VisitPolicy& m_policy;
        Strategy const& m_strategy;
    };

    template <typename InteriorRings, typename VisitPolicy, typename Strategy>
    static bool has_valid_interior_rings(InteriorRings const& interior_rings,
                                         VisitPolicy& visitor,
                                         Strategy const& strategy)
    {
        return
            detail::check_iterator_range
                <
                    per_ring<VisitPolicy, Strategy>,
                    true // allow for empty interior ring range
                >::apply(boost::begin(interior_rings),
                         boost::end(interior_rings),
                         per_ring<VisitPolicy, Strategy>(visitor, strategy));
    }

    struct has_valid_rings
    {
        template <typename VisitPolicy, typename Strategy>
        static inline bool apply(Polygon const& polygon,
                                 VisitPolicy& visitor,
                                 Strategy const& strategy)
        {
            typedef typename ring_type<Polygon>::type ring_type;

            // check validity of exterior ring
            debug_phase::apply(1);

            if (! detail::is_valid::is_valid_ring
                     <
                         ring_type,
                         false // do not check self intersections
                     >::apply(exterior_ring(polygon), visitor, strategy))
            {
                return false;
            }

            // check validity of interior rings
            debug_phase::apply(2);

            return has_valid_interior_rings(geometry::interior_rings(polygon),
                                            visitor,
                                            strategy);
        }
    };

    // structs for partition -- start
    struct expand_box
    {
        template <typename Box, typename Iterator>
        static inline void apply(Box& total, Iterator const& it)
        {
            geometry::expand(total, geometry::return_envelope<Box>(*it));
        }

    };

    struct overlaps_box
    {
        template <typename Box, typename Iterator>
        static inline bool apply(Box const& box, Iterator const& it)
        {
            return ! geometry::disjoint(*it, box);
        }
    };

    struct item_visitor_type
    {
        bool items_overlap;

        item_visitor_type() : items_overlap(false) {}

        template <typename Item1, typename Item2>
        inline void apply(Item1 const& item1, Item2 const& item2)
        {
            if (! items_overlap
                && (geometry::within(*points_begin(*item1), *item2)
                    || geometry::within(*points_begin(*item2), *item1))
                )
            {
                items_overlap = true;
            }
        }
    };
    // structs for partition -- end

    template
    <
        typename RingIterator,
        typename ExteriorRing,
        typename TurnIterator,
        typename VisitPolicy
    >
    static inline bool are_holes_inside(RingIterator rings_first,
                                        RingIterator rings_beyond,
                                        ExteriorRing const& exterior_ring,
                                        TurnIterator turns_first,
                                        TurnIterator turns_beyond,
                                        VisitPolicy& visitor)
    {
        boost::ignore_unused(visitor);

        // collect the interior ring indices that have turns with the
        // exterior ring
        std::set<signed_size_type> ring_indices;
        for (TurnIterator tit = turns_first; tit != turns_beyond; ++tit)
        {
            if (tit->operations[0].seg_id.ring_index == -1)
            {
                BOOST_GEOMETRY_ASSERT(tit->operations[1].seg_id.ring_index != -1);
                ring_indices.insert(tit->operations[1].seg_id.ring_index);
            }
            else if (tit->operations[1].seg_id.ring_index == -1)
            {
                BOOST_GEOMETRY_ASSERT(tit->operations[0].seg_id.ring_index != -1);
                ring_indices.insert(tit->operations[0].seg_id.ring_index);
            }
        }

        signed_size_type ring_index = 0;
        for (RingIterator it = rings_first; it != rings_beyond;
             ++it, ++ring_index)
        {
            // do not examine interior rings that have turns with the
            // exterior ring
            if (ring_indices.find(ring_index) == ring_indices.end()
                && ! geometry::covered_by(range::front(*it), exterior_ring))
            {
                return visitor.template apply<failure_interior_rings_outside>();
            }
        }

        // collect all rings (exterior and/or interior) that have turns
        for (TurnIterator tit = turns_first; tit != turns_beyond; ++tit)
        {
            ring_indices.insert(tit->operations[0].seg_id.ring_index);
            ring_indices.insert(tit->operations[1].seg_id.ring_index);
        }

        // put iterators for interior rings without turns in a vector
        std::vector<RingIterator> ring_iterators;
        ring_index = 0;
        for (RingIterator it = rings_first; it != rings_beyond;
             ++it, ++ring_index)
        {
            if (ring_indices.find(ring_index) == ring_indices.end())
            {
                ring_iterators.push_back(it);
            }
        }

        // call partition to check is interior rings are disjoint from
        // each other
        item_visitor_type item_visitor;

        geometry::partition
            <
                geometry::model::box<typename point_type<Polygon>::type>
            >::apply(ring_iterators, item_visitor, expand_box(), overlaps_box());

        if (item_visitor.items_overlap)
        {
            return visitor.template apply<failure_nested_interior_rings>();
        }
        else
        {
            return visitor.template apply<no_failure>();
        }
    }

    template
    <
        typename InteriorRings,
        typename ExteriorRing,
        typename TurnIterator,
        typename VisitPolicy
    >
    static inline bool are_holes_inside(InteriorRings const& interior_rings,
                                        ExteriorRing const& exterior_ring,
                                        TurnIterator first,
                                        TurnIterator beyond,
                                        VisitPolicy& visitor)
    {
        return are_holes_inside(boost::begin(interior_rings),
                                boost::end(interior_rings),
                                exterior_ring,
                                first,
                                beyond,
                                visitor);
    }

    struct has_holes_inside
    {
        template <typename TurnIterator, typename VisitPolicy>
        static inline bool apply(Polygon const& polygon,
                                 TurnIterator first,
                                 TurnIterator beyond,
                                 VisitPolicy& visitor)
        {
            return are_holes_inside(geometry::interior_rings(polygon),
                                    geometry::exterior_ring(polygon),
                                    first,
                                    beyond,
                                    visitor);
        }
    };

    struct has_connected_interior
    {
        template <typename TurnIterator, typename VisitPolicy>
        static inline bool apply(Polygon const& polygon,
                                 TurnIterator first,
                                 TurnIterator beyond,
                                 VisitPolicy& visitor)
        {
            boost::ignore_unused(visitor);

            typedef typename std::iterator_traits
                <
                    TurnIterator
                >::value_type turn_type;
            typedef complement_graph<typename turn_type::point_type> graph;

            graph g(geometry::num_interior_rings(polygon) + 1);
            for (TurnIterator tit = first; tit != beyond; ++tit)
            {
                typename graph::vertex_handle v1 = g.add_vertex
                    ( tit->operations[0].seg_id.ring_index + 1 );
                typename graph::vertex_handle v2 = g.add_vertex
                    ( tit->operations[1].seg_id.ring_index + 1 );
                typename graph::vertex_handle vip = g.add_vertex(tit->point);

                g.add_edge(v1, vip);
                g.add_edge(v2, vip);
            }

#ifdef BOOST_GEOMETRY_TEST_DEBUG
            debug_print_complement_graph(std::cout, g);
#endif // BOOST_GEOMETRY_TEST_DEBUG

            if (g.has_cycles())
            {
                return visitor.template apply<failure_disconnected_interior>();
            }
            else
            {
                return visitor.template apply<no_failure>();
            }
        }
    };

public:
    template <typename VisitPolicy, typename Strategy>
    static inline bool apply(Polygon const& polygon,
                             VisitPolicy& visitor,
                             Strategy const& strategy)
    {
        if (! has_valid_rings::apply(polygon, visitor, strategy))
        {
            return false;
        }

        if (BOOST_GEOMETRY_CONDITION(CheckRingValidityOnly))
        {
            return true;
        }

        // compute turns and check if all are acceptable
        debug_phase::apply(3);

        typedef has_valid_self_turns<Polygon> has_valid_turns;

        std::deque<typename has_valid_turns::turn_type> turns;
        bool has_invalid_turns
            = ! has_valid_turns::apply(polygon, turns, visitor, strategy);
        debug_print_turns(turns.begin(), turns.end());

        if (has_invalid_turns)
        {
            return false;
        }

        // check if all interior rings are inside the exterior ring
        debug_phase::apply(4);

        if (! has_holes_inside::apply(polygon,
                                      turns.begin(), turns.end(),
                                      visitor))
        {
            return false;
        }

        // check whether the interior of the polygon is a connected set
        debug_phase::apply(5);

        return has_connected_interior::apply(polygon,
                                             turns.begin(),
                                             turns.end(),
                                             visitor);
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// A Polygon is always a simple geometric object provided that it is valid.
//
// Reference (for validity of Polygons): OGC 06-103r4 (6.1.11.1)
template <typename Polygon, bool AllowEmptyMultiGeometries>
struct is_valid
    <
        Polygon, polygon_tag, AllowEmptyMultiGeometries
    > : detail::is_valid::is_valid_polygon<Polygon>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_POLYGON_HPP


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

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

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test0(Env& runtime)
{
  using p_t = bs::pack<T, N>;
  std::array<T,N> ref;

  T k = 0;
  for(auto& e : ref) e = k++;

  STF_ALL_EQUAL(bs::enumerate<p_t>(),ref);
}

STF_CASE_TPL( "Check enumerate<T>() behavior", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test0<T, N>(runtime);
  test0<T, N/2>(runtime);
  test0<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void test1(Env& runtime)
{
  using p_t = bs::pack<T, N>;
  std::array<T,N> ref;

  T k = 42;
  for(auto& e : ref) e = k++;

  STF_ALL_EQUAL(bs::enumerate<p_t>(42),ref);
}

STF_CASE_TPL( "Check enumerate<T>(SEED) behavior", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test1<T, N>(runtime);
  test1<T, N/2>(runtime);
  test1<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void test2(Env& runtime)
{
  using p_t = bs::pack<T, N>;
  std::array<T,N> ref;

  for(std::size_t i = 0; i< N; ++i)
    ref[i] = 5 + T(i)*3;

  STF_ALL_EQUAL(bs::enumerate<p_t>(5,3),ref);
}

STF_CASE_TPL( "Check enumerate<T>(SEED,STEP) behavior", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test2<T, N>(runtime);
  test2<T, N/2>(runtime);
  test2<T, N*2>(runtime);
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm core cover
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_array.h"
#include "fern/algorithm/algebra/boole/and.h"
#include "fern/algorithm/core/cover.h"
#include "fern/algorithm/core/test/test_utils.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::core::cover<>(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    {
        verify_value<>( 4, 5, 4);
        verify_value<>( 5, 4, 5);
    }
}

BOOST_AUTO_TEST_CASE(array_2d_masked_0d)
{
    size_t const nr_rows{3};
    size_t const nr_cols{3};
    fern::MaskedArray<int, 2> value1(fern::extents[nr_rows][nr_cols]);
    int const value2{3};
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<2>>,
        fa::SkipNoData> input_no_data_policy{{value1.mask(), true}, {}};

    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    value1[1][2] = 4;
    value1[2][1] = 5;
    value1[2][2] = -1;
    value1.mask().fill(true);
    value1.mask()[1][2] = false;
    value1.mask()[2][1] = false;
    value1.mask()[2][2] = false;

    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    result_we_want[0][0] = value2;
    result_we_want[0][1] = value2;
    result_we_want[0][2] = value2;
    result_we_want[1][0] = value2;
    result_we_want[1][1] = value2;
    result_we_want[1][2] = value1[1][2];
    result_we_want[2][0] = value2;
    result_we_want[2][1] = value1[2][1];
    result_we_want[2][2] = value1[2][2];

    fa::SequentialExecutionPolicy sequential;

    fa::core::cover(input_no_data_policy, output_no_data_policy,
        sequential, value1, value2, result_we_got);

    BOOST_CHECK(fern::compare(sequential, result_we_got, result_we_want));
}

BOOST_AUTO_TEST_CASE(array_2d_masked_2d)
{
    size_t const nr_rows{3};
    size_t const nr_cols{3};
    fern::MaskedArray<int, 2> value1(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> value2(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> value3(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got1(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got2(fern::extents[nr_rows][nr_cols]);

    // +---+---+---+
    // | X | X | X |
    // +---+---+---+
    // | X | X | 4 |
    // +---+---+---+
    // | X | X | 1 |
    // +---+---+---+
    value1[1][2] = 4;
    value1[2][2] = 1;
    value1.mask().fill(true);
    value1.mask()[1][2] = false;
    value1.mask()[2][2] = false;

    // +---+---+---+
    // | 0 | X | X |
    // +---+---+---+
    // | X | X | 8 |
    // +---+---+---+
    // | X | 2 | X |
    // +---+---+---+
    value2[0][0] = 0;
    value2[1][2] = 8;
    value2[2][1] = 2;
    value2.mask().fill(true);
    value2.mask()[0][0] = false;
    value2.mask()[1][2] = false;
    value2.mask()[2][1] = false;

    // +---+---+---+
    // | 4 | 4 | 4 |
    // +---+---+---+
    // | 4 | 4 | 4 |
    // +---+---+---+
    // | X | 4 | 4 |
    // +---+---+---+
    value3.fill(4);
    value3[2][0] = -9;
    value3.mask().fill(false);
    value3.mask()[2][0] = true;

    // +---+---+---+
    // | 0 | 4 | 4 |
    // +---+---+---+
    // | 4 | 4 | 8 |
    // +---+---+---+
    // | X | 2 | 1 |
    // +---+---+---+
    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    result_we_want[0][0] = value2[0][0];
    result_we_want[0][1] = value3[0][1];
    result_we_want[0][2] = value3[0][2];
    result_we_want[1][0] = value3[1][0];
    result_we_want[1][1] = value3[1][1];
    result_we_want[1][2] = value1[1][2];
    result_we_want[2][0] = 0;
    result_we_want[2][1] = value2[2][1];
    result_we_want[2][2] = value1[2][2];
    result_we_want.mask().fill(false);
    result_we_want.mask()[2][0] = true;

    fa::SequentialExecutionPolicy sequential;

    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got1.mask(), true);

    // result1 = cover(value1, value2)
    {
        fa::InputNoDataPolicies<
            fa::DetectNoDataByValue<fern::Mask<2>>,
            fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
                {value1.mask(), true}, {value2.mask(), true}};

        fa::algebra::and_(sequential, value1.mask(), value2.mask(),
            result_we_got1.mask());

        fa::core::cover(input_no_data_policy, output_no_data_policy,
            sequential, value1, value2, result_we_got1);

    }

    // result2 = cover(result1, value3)
    {
        fa::InputNoDataPolicies<
            fa::DetectNoDataByValue<fern::Mask<2>>,
            fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
                {result_we_got1.mask(), true}, {value3.mask(), true}};

        fa::algebra::and_(sequential, result_we_got1.mask(),
            value3.mask(), result_we_got2.mask());

        fa::core::cover(input_no_data_policy, output_no_data_policy,
            sequential, result_we_got1, value3, result_we_got2);
    }

    BOOST_CHECK(fern::compare(sequential, result_we_got2, result_we_want));
}


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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_ERROR_BOUND_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_ERROR_BOUND_HPP

#include <limits>

#include <boost/mp11/integral.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/map.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/coefficient_list.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template <typename KV> using second_is_coeff_list =
    is_coeff_list< boost::mp11::mp_second<KV> >;

template <typename EM> using is_error_map = boost::mp11::mp_and
    <
        boost::mp11::mp_is_map<EM>,
        boost::mp11::mp_all_of<EM, second_is_coeff_list>,
        boost::mp11::mp_similar<EM, boost::mp11::mp_list<>
    >
    //TODO: It would be desirable to also validate that the keys are good
>;

template <typename KV, typename M>
struct error_map_insert_impl
{
private:
    using key = boost::mp11::mp_front<KV>;
    using value = boost::mp11::mp_second<KV>;
    using other_value = typename mp_map_at_second_or_void<M, key>::type;
    using merged_value = coeff_merge<value, other_value>;
    using nkv = boost::mp11::mp_list<key, merged_value>;
public:
    using type = boost::mp11::mp_map_replace<M, nkv>;
};

template <typename KV, typename M> using error_map_insert =
    typename error_map_insert_impl<KV, M>::type;

template <typename M1, typename M2>
struct add_fold_operator
{
public:
    template <typename M, typename K> using fn = boost::mp11::mp_map_insert
        <
            M,
            boost::mp11::mp_list
                <
                    K,
                    coeff_merge
                        <
                            typename mp_map_at_second_or_void<M1, K>::type,
                            typename mp_map_at_second_or_void<M2, K>::type
                        >
                >
        >;
};

template <typename M1, typename M2> using add_children = boost::mp11::mp_fold
    <
        boost::mp11::mp_set_union
            <
                boost::mp11::mp_map_keys<M1>,
                boost::mp11::mp_map_keys<M2>
            >,
        boost::mp11::mp_list<>,
        add_fold_operator<M1, M2>::template fn
    >;

template
<
    typename Exp,
    typename EM,
    typename Out,
    typename LErr = typename val_or_empty_list<EM, typename Exp::left>::type,
    typename RErr = typename val_or_empty_list<EM, typename Exp::right>::type,
    typename skip_decompose =
        boost::mp11::mp_or
            <
                boost::mp11::mp_not
                    <
                        boost::mp11::mp_same
                            <
                                typename Exp::error_type,
                                sum_error_type
                            >
                    >,
                boost::mp11::mp_same
                    <
                        boost::mp11::mp_list<>,
                        LErr,
                        RErr
                    >
            >
>
struct decompose_add_impl
{
private:
    using decomp_left = typename decompose_add_impl
        <
            typename Exp::left,
            EM,
            Out
        >::type;
    using decomp_right = typename decompose_add_impl
        <
            typename Exp::right,
            EM,
            decomp_left
        >::type;
public:
    using type = decomp_right;
};

template
<
    typename Exp,
    typename EM,
    typename Out,
    typename LErr,
    typename RErr
>
struct decompose_add_impl
    <
        Exp,
        EM,
        Out,
        LErr,
        RErr,
        boost::mp11::mp_true
    >
{
    using type = error_map_insert
        <
            boost::mp11::mp_list
                <
                    Exp,
                    boost::mp11::mp_list<boost::mp11::mp_int<1>>
                >,
            Out
        >;
};

template
<
    typename Exp,
    typename EM,
    typename Out
>
using decompose_add = typename decompose_add_impl<Exp, EM, Out>::type;

template
<
    typename Exp,
    typename EM,
    typename LErr = typename val_or_empty_list<EM, typename Exp::left>::type,
    typename RErr = typename val_or_empty_list<EM, typename Exp::right>::type,
    typename Children_Empty =
        boost::mp11::mp_and
            <
                typename empty_or_void<LErr>::type,
                typename empty_or_void<RErr>::type
            >
>
struct sum_err_impl
{
private:
    static_assert(is_error_map<LErr>::value, "LErr needs to be a valid error map.");
    static_assert(is_error_map<RErr>::value, "RErr needs to be a valid error map.");
    using children = add_children<LErr, RErr>;
    static_assert(
            is_error_map<children>::value, "children needs to be a valid error map.");
public:
    /*
    using type = boost::mp11::mp_map_insert<
        children,
        boost::mp11::mp_list<Exp, boost::mp11::mp_list<boost::mp11::mp_int<1>>>
    >;*/
    using type = decompose_add<Exp, EM, children>;
    static_assert(is_error_map<type>::value, "type needs to be a valid error map.");
};

template
<
    typename Exp,
    typename EM,
    typename LErr,
    typename RErr
>
struct sum_err_impl
    <
        Exp,
        EM,
        LErr,
        RErr,
        boost::mp11::mp_true
    >
{
    static_assert(is_error_map<LErr>::value, "LErr needs to be a valid error map.");
    static_assert(is_error_map<RErr>::value, "RErr needs to be a valid error map.");
    using type = boost::mp11::mp_list
        <
            boost::mp11::mp_list
                <
                    Exp,
                    boost::mp11::mp_list<boost::mp11::mp_int<1>>
                >
        >;
    static_assert(is_error_map<type>::value, "type needs to be a valid error map.");
};

template <typename Exp, typename EM> using sum_err =
    typename sum_err_impl<Exp, EM>::type;

template <typename L> using pad_second = boost::mp11::mp_list
    <
        boost::mp11::mp_front<L>,
        boost::mp11::mp_push_front
            <
                boost::mp11::mp_second<L>,
                boost::mp11::mp_int<0>
            >
    >;

template <typename L> using pop_front_second = boost::mp11::mp_list
    <
        boost::mp11::mp_front<L>,
        boost::mp11::mp_pop_front<boost::mp11::mp_second<L>>
    >;

template <typename K, typename V> using increment_first_of_second =
    boost::mp11::mp_transform_front<V, inc>;

template <typename KV1, typename KV2> using prod_entry_merge =
    boost::mp11::mp_list
        <
            boost::mp11::mp_flatten
                <
                    boost::mp11::mp_list
                        <
                            boost::mp11::mp_first<KV1>,
                            boost::mp11::mp_first<KV2>
                        >
                >,
            list_product
                <
                    boost::mp11::mp_second<KV1>,
                    boost::mp11::mp_second<KV2>
                >
        >;

template
<
    typename Exp,
    typename EM,
    typename LErr = typename val_or_empty_list<EM, typename Exp::left>::type,
    typename RErr = typename val_or_empty_list<EM, typename Exp::right>::type
>
struct prod_children_impl
{
private:
    static_assert(is_error_map<LErr>::value, "LErr needs to be a valid error map.");
    static_assert(is_error_map<RErr>::value, "RErr needs to be a valid error map.");
    using left = typename Exp::left;
    using right = typename Exp::right;
    using padded_lerr = boost::mp11::mp_transform<pad_second, LErr>;
    using added_left_lerr = decompose_add<left, EM, padded_lerr>;
    using padded_rerr = boost::mp11::mp_transform<pad_second, RErr>;
    using added_right_rerr = decompose_add<right, EM, padded_rerr>;
    using prod = boost::mp11::mp_product
        <
            prod_entry_merge,
            added_left_lerr,
            added_right_rerr
        >;
    using stripped_prod = boost::mp11::mp_transform<pop_front_second, prod>;
public:
    using type = stripped_prod;
    static_assert(is_error_map<type>::value, "type needs to be a valid error map.");
};

template <typename Exp, typename EM> using prod_children =
    typename prod_children_impl<Exp, EM>::type;

template
<
    typename Exp,
    typename EM,
    typename LErr = typename val_or_empty_list<EM, typename Exp::left>::type,
    typename RErr = typename val_or_empty_list<EM, typename Exp::right>::type
>
struct product_err_impl
{
private:
    static_assert(is_error_map<LErr>::value, "LErr needs to be a valid error map.");
    static_assert(is_error_map<RErr>::value, "RErr needs to be a valid error map.");
    using children = prod_children<Exp, EM>;
    static_assert(is_error_map<children>::value, "children needs to be a valid error map.");
public:
    using type = boost::mp11::mp_map_insert
        <
            children,
            boost::mp11::mp_list
                <
                    Exp,
                    boost::mp11::mp_list< boost::mp11::mp_int<1> >
                >
        >;
    static_assert(is_error_map<type>::value, "type needs to be a valid error map.");
};

template <typename Exp, typename EM> using product_err =
    typename product_err_impl<Exp, EM>::type;

template
<
    typename Errors,
    typename Exp,
    typename IsSum = boost::mp11::mp_same
        <
            typename Exp::error_type,
            sum_error_type
        >
>
struct sum_or_product_err_impl
{
    using type = sum_err<Exp, Errors>;
};

template
<
    typename Errors,
    typename Exp
>
struct sum_or_product_err_impl<Errors, Exp, boost::mp11::mp_false>
{
    using type = product_err<Exp, Errors>;
};

template <typename Errors, typename Exp> using sum_or_product_err =
    typename sum_or_product_err_impl<Errors, Exp>::type;

template
<
    typename Errors,
    typename Exp
>
struct error_fold_impl
{
private:
    using lerr = typename val_or_empty_list<Errors, typename Exp::left>::type;
    using rerr = typename val_or_empty_list<Errors, typename Exp::right>::type;
    using err = sum_or_product_err<Errors, Exp>;
public:
    using type = boost::mp11::mp_map_insert
        <
            Errors,
            boost::mp11::mp_list<Exp, err>
        >;
};

template <typename Errors, typename Exp> using error_fold =
    typename error_fold_impl<Errors, Exp>::type;

template <typename Evals> using evals_error =
    boost::mp11::mp_fold
        <
            Evals,
            boost::mp11::mp_list<>,
            error_fold
        >;

template <typename T> using is_mp_list = boost::mp11::mp_similar
    <
        boost::mp11::mp_list<>,
        T
    >;

template <typename KV>
struct list_to_product_impl
{
private:
    using key = boost::mp11::mp_front<KV>;
    using value = boost::mp11::mp_second<KV>;
    using multiplications = boost::mp11::mp_int<boost::mp11::mp_size<key>::value - 1>;
    using nvalue = mult_by_1_p_eps_pow<value, multiplications>;
    using nkey = boost::mp11::mp_reverse_fold<
        boost::mp11::mp_pop_back<key>,
        boost::mp11::mp_back<key>,
        product
    >;
public:
    using type = boost::mp11::mp_list<nkey, nvalue>;
};

template <typename KV> using list_to_product = typename list_to_product_impl<KV>::type;

template
<
    typename M,
    typename KV,
    typename KeyMPList = is_mp_list< boost::mp11::mp_front<KV> >
>
struct error_map_list_to_product_fold_impl
{
    using type = error_map_insert<list_to_product<KV>, M>;
};

template <typename M, typename KV>
struct error_map_list_to_product_fold_impl
    <
        M,
        KV,
        boost::mp11::mp_false
    >
{
    using type = error_map_insert<KV, M>;
};

template <typename M, typename KV> using error_map_list_to_product_fold =
    typename error_map_list_to_product_fold_impl<M, KV>::type;

template <typename M>
struct error_map_list_to_product_impl
{
    using type = boost::mp11::mp_fold
        <
            M,
            boost::mp11::mp_list<>,
            error_map_list_to_product_fold
        >;
};

template <typename M> using error_map_list_to_product =
    typename error_map_list_to_product_impl<M>::type;

template <typename Expression>
struct abs_unless_non_negative
{
    using type = boost::mp11::mp_if_c
        <
            Expression::non_negative,
            Expression,
            abs<Expression>
        >;
};

template <typename EM, typename KV> using abs_fold =
    boost::mp11::mp_push_back
        <
            EM,
            boost::mp11::mp_list
                <
                    typename abs_unless_non_negative<boost::mp11::mp_front<KV>>
                        ::type,
                    boost::mp11::mp_second<KV>
                >
        >;

template<typename EM> using abs_all =
    boost::mp11::mp_fold
        <
            EM,
            boost::mp11::mp_list<>,
            abs_fold
        >;

template
<
    typename KV1,
    typename KV2
>
struct abs_sum_error_term_impl
{
private:
    using key1 = boost::mp11::mp_front<KV1>;
    using key2 = boost::mp11::mp_front<KV2>;
    using nkey = sum<key1, key2>;
    using val1 = boost::mp11::mp_second<KV1>;
    using val2 = boost::mp11::mp_second<KV2>;
    using mval = coeff_max<val1, val2>;
    static_assert(is_coeff_list<mval>::value, "merged coefficient list fails coefficient list check");
    using nval = mult_by_1_p_eps<mval>;
public:
    using type = boost::mp11::mp_list<nkey, nval>;
};

template<typename KV1, typename KV2> using abs_sum_error_term = typename abs_sum_error_term_impl<KV1, KV2>::type;

//TODO: The following could be probably optimized for potentially produce better error bounds in some cases
//      if the error map is treated as a minheap by ordering of epsilon-polynomial.
template<typename M> using error_map_sum_up =
    boost::mp11::mp_fold
        <
            boost::mp11::mp_pop_front<M>,
            boost::mp11::mp_first<M>,
            abs_sum_error_term
        >;

template<typename M, std::size_t MS = boost::mp11::mp_size<M>::value>
struct error_map_balanced_sum_up_impl
{
private:
    static constexpr std::size_t mid = MS/2;
    using left = typename error_map_balanced_sum_up_impl
        <
            boost::mp11::mp_erase_c<M, 0, mid>
        >::type;
    using right = typename error_map_balanced_sum_up_impl
        <
            boost::mp11::mp_erase_c<M, mid, MS>
        >::type;
public:
    using type = abs_sum_error_term<left, right>;
};

template<typename M>
struct error_map_balanced_sum_up_impl<M, 1>
{
    using type = boost::mp11::mp_front<M>;
};

template<typename M> using error_map_balanced_sum_up =
    typename error_map_balanced_sum_up_impl<M>::type;

template
<
    typename Real,
    typename Exp
>
struct eps_pow
{
    static constexpr Real value =
          std::numeric_limits<Real>::epsilon()/2.0
        * eps_pow<Real, boost::mp11::mp_size_t< Exp::value - 1 >>::value;
};

template <typename Real> struct eps_pow<Real, boost::mp11::mp_size_t<0>>
{
    static constexpr Real value = 1.0;
};

template
<
    typename Real,
    typename L,
    typename S = boost::mp11::mp_size<L>
>
struct eval_eps_polynomial
{
private:
    using last = boost::mp11::mp_back<L>;
    using s2last = boost::mp11::mp_back< boost::mp11::mp_pop_back<L> >;
public:
    static constexpr Real value =
          s2last::value
        * eps_pow<Real, boost::mp11::mp_size_t<S::value - 1>>::value
        + last::value * eps_pow<Real, S>::value;
};

template
<
    typename Real,
    typename L
>
struct eval_eps_polynomial<Real, L, boost::mp11::mp_size_t<1>>
{
    static constexpr Real value =
          boost::mp11::mp_front<L>::value
        * std::numeric_limits<Real>::epsilon() / 2.0;
};

template
<
    typename Real,
    typename L
>
struct eval_eps_polynomial<Real, L, boost::mp11::mp_size_t<0>>
{
    static constexpr Real value = 0;
};

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_ERROR_BOUND_HPP