//=======================================================================
// Copyright (c) 2013 Attila Bernath, Piotr Wygocki
//               2014 Piotr Godlewski, Piotr Smulewicz
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file tree_augmentation.hpp
 * @brief
 * @author Attila Bernath, Piotr Smulewicz, Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-06-20
 */
#ifndef PAAL_TREE_AUGMENTATION_HPP
#define PAAL_TREE_AUGMENTATION_HPP

#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/utils/hash.hpp"

#include <boost/bimap.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/distance.hpp>

#include <algorithm>
#include <utility>

namespace paal {
namespace ir {

namespace detail {

/// This function returns the number of edges in a graph. The
/// reason this function was written is that BGL's num_edges()
/// function does not work properly for filtered_graph.  See
/// http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/filtered_graph.html#2
template <class EdgeListGraph>
int filtered_num_edges(const EdgeListGraph & g) {
    return boost::distance(edges(g));
}

/// A class that translates bool map on the edges to a filter, which can be used
/// with
/// boost::filtered_graph. That is, we translate operator[] to
/// operator().  We do a little more: we will also need the negated
/// version of the map (for the graph of the non-tree edges).
template <typename EdgeBoolMap> struct bool_map_to_tree_filter {
    bool_map_to_tree_filter() {}

    bool_map_to_tree_filter(EdgeBoolMap m) : ebmap(m) {}

    template <typename Edge> bool operator()(const Edge &e) const {
        return get(ebmap, e);
    }

    EdgeBoolMap ebmap;
};

template <typename EdgeBoolMap> struct bool_map_to_non_tree_filter {
    bool_map_to_non_tree_filter() {}

    bool_map_to_non_tree_filter(EdgeBoolMap m) : ebmap(m) {}

    template <typename Edge> bool operator()(const Edge &e) const {
        return !get(ebmap, e);
    }

    EdgeBoolMap ebmap;
};

/// A boost graph map that returns a constant integer value.
template <typename KeyType, int num>
class const_int_map
    : public boost::put_get_helper<int, const_int_map<KeyType, num>> {
  public:
    using category = boost::readable_property_map_tag;
    using value_type = int;
    using reference = int;
    using key_type = KeyType;
    reference operator[](KeyType e) const { return num; }
};

} // namespace detail

namespace {
struct ta_compare_traits {
    static const double EPSILON;
};

const double ta_compare_traits::EPSILON = 1e-10;
}

/**
 * Round Condition of the IR Tree Augmentation algorithm.
 */
struct ta_round_condition {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    ta_round_condition(double epsilon = ta_compare_traits::EPSILON)
        : m_round_half(epsilon) {}

    /**
     * Rounds a column up if it is at least half in the
     * current solution. If the column is rounded, the
     * corresponding edge is added to the result.
     */
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &problem, LP &lp,
                                       lp::col_id col) {
        auto res = m_round_half(problem, lp, col);
        if (res) {
            problem.add_to_solution(col);
        }
        return res;
    }

  private:
    round_condition_greater_than_half m_round_half;
};

/**
 * Set Solution component of the IR Tree Augmentation algorithm.
 */
struct ta_set_solution {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    ta_set_solution(double epsilon = ta_compare_traits::EPSILON)
        : m_compare(epsilon) {}

    /**
     * Creates the result form the LP (all edges corresponding to columns with
     * value 1).
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        for (auto e :
             boost::as_array(edges(problem.get_links_graph()))) {
            if (!problem.is_in_solution(e)) {
                auto col = problem.edge_to_col(e);
                if (m_compare.e(solution(col), 1)) {
                    problem.add_to_solution(col);
                }
            }
        }
    }

private:
    const utils::compare<double> m_compare;
};

/**
 * Initialization of the IR Tree Augmentation algorithm.
 */
class ta_init {
  public:
    /**
     * Initialize the cut LP.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        problem.init();
        lp.set_lp_name("Tree augmentation");
        lp.set_optimization_type(lp::MINIMIZE);

        add_variables(problem, lp);
        add_cut_constraints(problem, lp);
    }

  private:
    /**
     * Adds a variable to the LP for each link in the input graph.
     * Binds the LP columns to graph edges.
     */
    template <typename Problem, typename LP>
    void add_variables(Problem & problem, LP & lp) {
        for (auto e : boost::as_array(edges(problem.get_links_graph()))) {
            lp::col_id col_idx = lp.add_column(problem.get_cost(e), 0);
            problem.bind_edge_to_col(e, col_idx);
        }
    }

    /**
     * Adds a cut constraint to the LP for each edge in the input graph
     * and binds edges to LP rows.
     */
    template <typename Problem, typename LP>
    void add_cut_constraints(Problem &problem, LP &lp) {
        for (auto e :
             boost::as_array(edges(problem.get_tree_graph()))) {
            lp::linear_expression expr;
            for (auto pe : problem.get_covered_by(e)) {
                expr += problem.edge_to_col(pe);
            }

            lp::row_id row_idx = lp.add_row(std::move(expr) >= 1);
            problem.bind_edge_to_row(e, row_idx);
        }
    }
};

template <
    typename Init = ta_init,
    typename RoundCondition = ta_round_condition,
    typename RelaxContition = utils::always_false,
    typename SetSolution = ta_set_solution>
        using tree_augmentation_ir_components = IRcomponents<Init, RoundCondition,
                        RelaxContition, SetSolution>;

/**
 * @brief This is Jain's iterative rounding
 * 2-approximation algorithm for the Generalised Steiner Network
 * Problem, specialized for the Tree Augmentation Problem.
 *
 * The Tree Augmentation Problem is the following. Given a
 * 2-edge connected graph, in which a spanning tree is
 * designated. The non-tree edges are also called links. The
 * links have non-negative costs. The problem is to find a
 * minimum cost subset of links which, together with the
 * tree-edges give a 2-edge-connected graph.
 *
 * @tparam Graph the graph type used
 * @tparam TreeMap it is assumed to be a bool map on the edges of a graph of type Graph.
 *  It is used for designating a spanning tree in the graph.
 * @tparam CostMap type for the costs of the links.
 * @tparam VertexIndex type for the vertex index map.
 * @tparam EdgeSetOutputIterator type for the result edge set.
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator>
class tree_aug {
  public:
    using Edge = typename boost::graph_traits<Graph>::edge_descriptor;
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using CostValue = double;

    using TreeGraph =
        boost::filtered_graph<Graph, detail::bool_map_to_tree_filter<TreeMap>>;
    using NonTreeGraph = boost::filtered_graph<
        Graph, detail::bool_map_to_non_tree_filter<TreeMap>>;

    using EdgeList = std::vector<Edge>;
    using CoverMap = std::unordered_map<Edge, EdgeList, edge_hash<Graph>>;

    // cross reference between links and columns
    using EdgeToColId = boost::bimap<Edge, lp::col_id>;
    using RowIdToEdge = std::unordered_map<lp::row_id, Edge>;

    using ErrorMessage = boost::optional<std::string>;

    /**
     * Constructor.
     *
     * @param g  the graph to work with
     * @param tree_map designate a spanning tree in \c g
     * @param cost_map costs of the links (=non-tree edges). The costs assigned to tree edges are not used.
     * @param vertex_index vertex index map
     * @param solution result set of edges output iterator
     */
    tree_aug(const Graph & g, TreeMap tree_map, CostMap cost_map,
            VertexIndex vertex_index, EdgeSetOutputIterator solution) :
        m_g(g), m_tree_map(tree_map), m_cost_map(cost_map), m_index(vertex_index),
        m_solution(solution),
        m_tree(m_g, detail::bool_map_to_tree_filter<TreeMap>(m_tree_map)),
        m_ntree(m_g, detail::bool_map_to_non_tree_filter<TreeMap>(m_tree_map)),
        m_sol_cost(0)
    {}

    /// Checks validity of the input
    ErrorMessage check_input_validity() {
        // Num of edges == num of nodes-1 in the tree?
        int n_v = num_vertices(m_g);
        int n_e = filtered_num_edges(m_tree);

        if (n_e != n_v - 1) {
            return "Incorrect number of edges in the spanning tree. "
                        + std::string("Should be ") + std::to_string(n_v - 1)
                        + ", but it is " + std::to_string(n_e) + ".";
        }

        // Is the tree connected?
        std::vector<int> component(num_vertices(m_g));
        int num = boost::connected_components(m_tree, &component[0]);
        if (num > 1) {
            return ErrorMessage{ "The spanning tree is not connected." };
        }

        // Is the graph 2-edge-connected?
        detail::const_int_map<Edge, 1> const_1_edge_map;
        // TODO This stoer-wagner algorithm is unnecessarily slow for some reason
        int min_cut = boost::stoer_wagner_min_cut(m_g, const_1_edge_map);
        if (min_cut < 2) {
            return ErrorMessage{"The graph is not 2-edge-connected."};
        }

        return ErrorMessage{};
    }

    /**
     * Returns the non-tree graph (set of links).
     */
    const NonTreeGraph &get_links_graph() const { return m_ntree; }

    /**
     * Returns the spanning tree.
     */
    const TreeGraph &get_tree_graph() const { return m_tree; }

    /**
     * Returns the cost of an edge.
     */
    auto get_cost(Edge e)->decltype(get(std::declval<CostMap>(), e)) {
        return get(m_cost_map, e);
    }

    /**
     * Adds an edge corresponding to the given LP column to the result set.
     */
    void add_to_solution(lp::col_id col) {
        *m_solution = m_edge_to_col_id.right.at(col);
        ++m_solution;
        m_sol_cost += m_cost_map[m_edge_to_col_id.right.at(col)];
        m_edge_to_col_id.right.erase(col);
    }

    /**
     * Binds a graph edge to a LP column.
     */
    void bind_edge_to_col(Edge e, lp::col_id col) {
        auto tmp =
            m_edge_to_col_id.insert(typename EdgeToColId::value_type(e, col));
        assert(tmp.second);
    }

    /**
     * Binds a graph edge to a LP row.
     */
    void bind_edge_to_row(Edge e, lp::row_id row) {
        auto tmp =
            m_row_id_to_edge.insert(typename RowIdToEdge::value_type(row, e));
        assert(tmp.second);
    }

    /**
     * Initializes the necessary data structures.
     */
    void init() {
        // We need to fill very useful auxiliary data structures:
        //\c m_covered_by - containing lists of
        // edges. For a tree edge \c t the list \c m_covered_by[t]
        // contains the list of links covering \c t.

        std::vector<Edge> pred(num_vertices(m_g));
        auto pred_map =
            boost::make_iterator_property_map(pred.begin(), m_index);
        std::set<Vertex> seen;
        for (auto u : boost::as_array(vertices(m_g))) {
            auto tmp = seen.insert(u);
            assert(tmp.second);
            boost::breadth_first_search(
                m_tree, u, boost::visitor(boost::make_bfs_visitor(
                               boost::record_edge_predecessors(
                                   pred_map, boost::on_tree_edge()))));

            for (auto e : boost::as_array(out_edges(u, m_ntree))) {
                auto node = target(e, m_ntree);
                if (!seen.count(node)) {
                    while (node != u) {
                        m_covered_by[get(pred_map, node)].push_back(e);
                        node = source(get(pred_map, node), m_tree);
                    }
                }
            }
        }
    }

    /**
     * Returns the edge corresponding to an LP row.
     */
    Edge row_to_edge(lp::row_id row) const { return m_row_id_to_edge.at(row); }

    /**
     * Returns the LP columnn corresponding to a graph edge.
     */
    lp::col_id edge_to_col(Edge e) const { return m_edge_to_col_id.left.at(e); }

    /**
     * Returns the list of links covering a given edge.
     */
    EdgeList &get_covered_by(Edge e) { return m_covered_by[e]; }

    /**
     * Checks if an edge belongs to the solution.
     */
    bool is_in_solution(Edge e) const {
        return m_edge_to_col_id.left.find(e) == m_edge_to_col_id.left.end();
    }

    /**
     * Returns cost of the found solution.
     */
    CostValue get_solution_cost() const { return m_sol_cost; }

  private:

    /// Input graph
    const Graph &m_g;
    /// Input tree edges map
    TreeMap m_tree_map;
    /// Input edge cost map
    CostMap m_cost_map;
    /// Vertex index map
    VertexIndex m_index;

    /// Which links are chosen in the solution
    EdgeSetOutputIterator m_solution;

    /// Auxiliary data structures
    EdgeToColId m_edge_to_col_id;

    /// The spanning tree
    TreeGraph m_tree;
    /// The non-tree (=set of links)
    NonTreeGraph m_ntree;
    /// Cost of the solution found
    CostValue m_sol_cost;
    /// Structures for the "m_covered_by" relations
    CoverMap m_covered_by;
    /// Reference between tree edges and row ids
    RowIdToEdge m_row_id_to_edge;
};

namespace detail {
/**
 * @brief Creates a tree_aug object. Non-named parameters.
 *
 * @tparam Graph
 * @tparam TreeMap
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam EdgeSetOutputIterator
 * @param g
 * @param tree_map
 * @param cost_map
 * @param vertex_index
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator>
tree_aug<Graph, TreeMap, CostMap, VertexIndex, EdgeSetOutputIterator>
make_tree_aug(const Graph & g, TreeMap tree_map, CostMap cost_map,
                VertexIndex vertex_index, EdgeSetOutputIterator solution) {
    return paal::ir::tree_aug<Graph, TreeMap, CostMap,
            VertexIndex, EdgeSetOutputIterator>(g, tree_map, cost_map, vertex_index, solution);
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding.
* Non-named parameters.
 *
 * @tparam Graph
 * @tparam TreeMap
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param tree_map
 * @param cost_map
 * @param vertex_index
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult tree_augmentation_iterative_rounding(
        const Graph & g,
        TreeMap tree_map,
        CostMap cost_map,
        VertexIndex vertex_index,
        EdgeSetOutputIterator solution,
        IRcomponents components = IRcomponents(),
        Visitor visitor = Visitor()) {
    auto treeaug = make_tree_aug(g, tree_map, cost_map, vertex_index, solution);
    return solve_iterative_rounding(treeaug, std::move(components), std::move(visitor));
}
} // detail

/**
 * Creates a tree_aug object. Named parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param params
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename EdgeSetOutputIterator, typename P,
          typename T, typename R>
auto make_tree_aug(const Graph &g,
                   const boost::bgl_named_params<P, T, R> &params,
                   EdgeSetOutputIterator solution)
    ->tree_aug<
          Graph,
          decltype(choose_const_pmap(get_param(params, boost::edge_color), g,
                                     boost::edge_color)),
          decltype(choose_const_pmap(get_param(params, boost::edge_weight), g,
                                     boost::edge_weight)),
          decltype(choose_const_pmap(get_param(params, boost::vertex_index), g,
                                     boost::vertex_index)),
          EdgeSetOutputIterator> {
    return detail::make_tree_aug(
        g, choose_const_pmap(get_param(params, boost::edge_color), g,
                             boost::edge_color),
        choose_const_pmap(get_param(params, boost::edge_weight), g,
                          boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_index), g,
                          boost::vertex_index),
        solution);
}

/**
 * Creates a tree_aug object. All default parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @param g
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename EdgeSetOutputIterator>
auto make_tree_aug(const Graph &g, EdgeSetOutputIterator solution)
    ->decltype(make_tree_aug(g, boost::no_named_parameters(), solution)) {
    return make_tree_aug(g, boost::no_named_parameters(), solution);
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding. Named
* parameters.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param params
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor, typename P, typename T,
          typename R>
IRResult tree_augmentation_iterative_rounding(
    const Graph &g, const boost::bgl_named_params<P, T, R> &params,
    EdgeSetOutputIterator solution, IRcomponents components = IRcomponents(),
    Visitor visitor = Visitor()) {
    return detail::tree_augmentation_iterative_rounding(
        g, choose_const_pmap(get_param(params, boost::edge_color), g,
                             boost::edge_color),
        choose_const_pmap(get_param(params, boost::edge_weight), g,
                          boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_index), g,
                          boost::vertex_index),
        std::move(solution), std::move(components), std::move(visitor));
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding. All
* default parameters.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult tree_augmentation_iterative_rounding(const Graph &g,
                                              EdgeSetOutputIterator solution,
                                              IRcomponents components =
                                                  IRcomponents(),
                                              Visitor visitor = Visitor()) {
    return tree_augmentation_iterative_rounding(
        g, boost::no_named_parameters(), std::move(solution),
        std::move(components), std::move(visitor));
}

} // ir
} // paal

#endif // PAAL_TREE_AUGMENTATION_HPP


/**
3DSmoothNet
core.cpp

Purpose: containes all the functions used to generate the  Smoothed Density Value voxel representation
of the interest points neighborhood

@Author : Zan Gojcic, Caifa Zhou
@Version : 1.0

*/

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <omp.h>
#include "core.h"

namespace po = boost::program_options;

/*
	Utility functions
*/

// Processes command line arguments and returns the default values if the parameters are not specified by the user
bool processCommandLine(int argc, char** argv,
	std::string &file_cloud,
	float &support_radius,
	int &num_voxels,
	float &smoothing_kernel_width,
	std::string &file_keypoints,
	std::string &output_folder)
{
	try
	{
		po::options_description desc("Allowed options");
		desc.add_options()
			("help", "Given a point cloud and a support radius, this code generates a SDV voxel representation for the selected interest points.")
			("fileCloud,f", po::value<std::string>(&file_cloud)->required(), "Input point cloud file in .ply format")
			("supportRadius,r", po::value<float>(&support_radius)->default_value(0.150),
				"Half size of the voxel grid.")
			("numVoxels,n", po::value<int>(&num_voxels)->default_value(16),
				"Number of voxels in a side of the grid. Whole grid is nxnxn.")
			("smoothingKernelWidth,h", po::value<float>(&smoothing_kernel_width)->default_value(1.75),
				"Width of the Gaussia kernel used for smoothing.")
			("fileKeypoints,k", po::value<std::string>(&file_keypoints)->default_value("0"),
				"Path to the file with the indices of the interest points. If 0, SDV voxel grid representation if computed for all the points")
			("outputFolder,o", po::value<std::string>(&output_folder)->default_value("./data/sdv_voxel_grid/"),
				"Output folder path.")
			;

		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);

		if (vm.count("help"))
		{
			std::cout << desc << "\n";
			return false;
		}

		po::notify(vm);
	}
	catch (std::exception& e)
	{
		std::cerr << "ERROR: " << e.what() << "\n";
		return false;
	}
	catch (...)
	{
		std::cerr << "Unknown error!" << "\n";
		return false;
	}

	return true;
}

// Reads a file with the interest point indices file
std::vector<int> readKeypoints(std::string filename)
{
    char separator = ' ';
    std::vector<int> result;
    std::string row, item;

    std::ifstream in(filename);

    while (getline(in, row))
    {
        std::stringstream ss(row);
        std::getline(ss, item, separator);
        result.push_back(std::stoi(item.c_str()));
    }

    in.close();
    return result;
}

// Checks if the file exists
bool fileExist(const std::string& name)
{
    std::ifstream f(name.c_str());  // New enough C++ library will accept just name
    return f.is_open();
}

// Saves the descriptor to a binary csv file
void saveVector(std::string filename, const std::vector<std::vector<float>> descriptor)
{
    std::cout << "Saving Features to a CSV file:" << std::endl;
    std::cout << filename << std::endl;

    std::ofstream outFile;
    outFile.open(filename, std::ios::binary);

    float writerTemp;
    for (int i = 0; i < descriptor.size(); i++)
    {
        for (int j = 0; j < descriptor[i].size(); j++)
        {
            writerTemp = descriptor[i][j];
            outFile.write(reinterpret_cast<const char*>(&writerTemp), sizeof(float));
        }
    }
    outFile.close();
}

// Initizales a grid using the step size and the number of voxels per side
flann::Matrix<float> initializeGridMatrix(const int n, float x_step, float y_step, float z_step)
{
    int grid_size = n*n*n;
    flann::Matrix<float> input(new float[grid_size * 3], grid_size, 3);

    float xs = -(n / 2)*x_step + 0.5*x_step;
    float ys = -(n / 2)*y_step + 0.5*y_step;
    float zs = -(n / 2)*z_step + 0.5*z_step;

    for (int i = 0; i < n; i++)
    {
        //move on x axis
        for (int j = 0; j < n; j++)
        {
            //move on y axis
            for (int k = 0; k < n; k++)
            {
                //move on z axis
                input[i + n*j + n*n*k][0] = xs + x_step * i;
                input[i + n*j + n*n*k][1] = ys + y_step * j;
                input[i + n*j + n*n*k][2] = zs + z_step * k;
            }
        }
    }
    return input;
}

/*
	Functions for estimation of the local reference frame
*/

// Estimates the Z axis of the local reference frame
void toldiComputeZaxis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Vertex &z_axis, std::vector<float> point_dst)
{
    int i;
    pcl::PointXYZ query_point = cloud->points[0];
    // calculate covariance matrix
    Eigen::Matrix3f Cov = Eigen::Matrix3f::Zero();
    Eigen::Matrix<float, 4, 1> centroid;
    centroid[0] = query_point.x;
    centroid[1] = query_point.y;
    centroid[2] = query_point.z;

    Eigen::Vector4f queryPointVector = query_point.getVector4fMap();
    Eigen::Matrix<float, Eigen::Dynamic, 4> vij(point_dst.size(), 4);
    int valid_nn_points = 0;
    double distance = 0.0;
    double sum = 0.0;

    pcl::computeCovarianceMatrix(*cloud, centroid, Cov);

    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_min;
    EIGEN_ALIGN16 Eigen::Vector3f normal;
    pcl::eigen33(Cov, eigen_min, normal);
    z_axis.x = normal(0);
    z_axis.y = normal(1);
    z_axis.z = normal(2);

    // z-axis sign disambiguity
    float z_sign = 0;
    for (i = 0; i < cloud->points.size(); i++)
    {
        float vec_x = query_point.x - cloud->points[i].x;
        float vec_y = query_point.y - cloud->points[i].y;
        float vec_z = query_point.z - cloud->points[i].z;
        z_sign += (vec_x*z_axis.x + vec_y*z_axis.y + vec_z*z_axis.z);
    }
    if (z_sign < 0)
    {
        z_axis.x = -z_axis.x;
        z_axis.y = -z_axis.y;
        z_axis.z = -z_axis.z;
    }
}

// Estimates the X axis of the local reference frame
void toldiComputeXaxis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Vertex z_axis, float sup_radius, std::vector<float> point_dst, Vertex &x_axis)
{
    int i, j;
    pcl::PointXYZ query_point = cloud->points[0];
    //
    std::vector<Vertex> vec_proj;
    std::vector<float> dist_weight, sign_weight;//store weights w1,w2
    for (i = 0; i < cloud->points.size(); i++)
    {
        Vertex temp;
        Vertex pq = { cloud->points[i].x - query_point.x,cloud->points[i].y - query_point.y,cloud->points[i].z - query_point.z };
        float proj = z_axis.x*pq.x + z_axis.y*pq.y + z_axis.z*pq.z;
        if (proj >= 0)
            sign_weight.push_back(pow(proj, 2));
        else
            sign_weight.push_back(-pow(proj, 2));
        temp.x = pq.x - proj*z_axis.x;
        temp.y = pq.y - proj*z_axis.y;
        temp.z = pq.z - proj*z_axis.z;
        vec_proj.push_back(temp);
    }

    for (i = 0; i < point_dst.size(); i++)
    {
        float wei_temp = sup_radius - point_dst[i];
        wei_temp = pow(wei_temp, 2);
        dist_weight.push_back(wei_temp);
    }
    Vertex x_axis_temp = { 0.0f,0.0f,0.0f };
    for (i = 0; i < cloud->points.size(); i++)
    {
        float weight_sum = dist_weight[i] * sign_weight[i];
        x_axis_temp.x += weight_sum*vec_proj[i].x;
        x_axis_temp.y += weight_sum*vec_proj[i].y;
        x_axis_temp.z += weight_sum*vec_proj[i].z;
    }
    //Normalization
    float size = sqrt(pow(x_axis_temp.x, 2) + pow(x_axis_temp.y, 2) + pow(x_axis_temp.z, 2));
    x_axis_temp.x /= size;
    x_axis_temp.y /= size;
    x_axis_temp.z /= size;
    x_axis = x_axis_temp;
}

// Estimates the Y axis of the local reference frame
void toldiComputeYaxis(Vertex x_axis, Vertex z_axis, Vertex &y_axis)
{
    Eigen::Vector3f x(x_axis.x, x_axis.y, x_axis.z);
    Eigen::Vector3f z(z_axis.x, z_axis.y, z_axis.z);
    Eigen::Vector3f y;

    y = x.cross(z);//cross product

    y_axis.x = y(0);
    y_axis.y = y(1);
    y_axis.z = y(2);
}

// Compute the lrf accordin to the method from toldi paper, for the points selected with the indices
void toldiComputeLRF(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                     std::vector<int> indices,
                     float sup_radius,
                     float smoothingFactor,
                     std::vector<LRF> &cloud_LRF,
                     std::vector<std::vector <int>>& neighbors,
                     std::vector<std::vector <int>>& neighbors_smoothing_idx,
                     std::vector<std::vector <float>>& neighbors_smoothing_distance)
{
    int i, j, m;
    // Initialize all the variables
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    std::vector<int> point_idx;
    std::vector<float> point_dst;
    kdtree.setInputCloud(cloud);
    pcl::PointXYZ query_point;
    pcl::PointXYZ test;
    //LRF calculation
    for (i = 0; i < indices.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbor(new pcl::PointCloud<pcl::PointXYZ>);//local surface
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbor_z(new pcl::PointCloud<pcl::PointXYZ>);//local surface for computing the z-axis of LRF
        query_point = cloud->points[indices[i]];

        if (kdtree.radiusSearch(query_point, sup_radius, point_idx, point_dst) > 10)//only if there are more than 10 points in the local surface
        {

            for (j = 0; j < point_idx.size(); j++)
            {

                test.x = cloud->points[point_idx[j]].x - query_point.x;
                test.y = cloud->points[point_idx[j]].y - query_point.y;
                test.z = cloud->points[point_idx[j]].z - query_point.z;

                sphere_neighbor_z->points.push_back(test);
            }

            // Save points for feature computation
			neighbors.at(indices[i]) = point_idx;

            std::transform(point_dst.begin(), point_dst.end(), point_dst.begin(), std::ptr_fun<float, float>(std::sqrt));
            // Find first element that has a distance bigger than the smoothing threshold
            auto lower = std::lower_bound(point_dst.begin(), point_dst.end(), smoothingFactor);
            // Index of the last element smaller then the threshold
            int  index_last_element = lower - point_dst.begin();

            // Copy neighbours to vector
            std::vector<int> point_idx_smoothing(&point_idx[0], &point_idx[index_last_element]);
            std::vector<float> point_distance_smoothing(&point_dst[0], &point_dst[index_last_element]);

			neighbors_smoothing_idx.at(indices[i]) = point_idx_smoothing;
			neighbors_smoothing_distance.at(indices[i]) = point_distance_smoothing;

            for (j = 0; j < point_idx.size(); j++)
            {
                sphere_neighbor->points.push_back(cloud->points[point_idx[j]]);
            }

            Vertex x_axis, y_axis, z_axis;
            toldiComputeZaxis(sphere_neighbor_z, z_axis, point_dst);
            toldiComputeXaxis(sphere_neighbor, z_axis, sup_radius, point_dst, x_axis);
            toldiComputeYaxis(x_axis, z_axis, y_axis);
            LRF temp = { indices[i],x_axis,y_axis,z_axis };
			cloud_LRF.at(indices[i]) = temp;
        }
        else
        {
            std::cout << "Less then ten points in the neighborhood!!!" << std::endl;
            LRF temp = { NULL_POINTID,{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f} };
			cloud_LRF.at(indices[i]) = temp;
        }
    }
}

/*
	Functions used for computation of the SDV voxel grid representation
*/

// transform the local neighbhorhood of the selecred interest point to its canonical representation defined by the estimated local reference frame
void transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, LRF pointLRF, pcl::PointCloud<pcl::PointXYZ>::Ptr &transformed_cloud)
{
    pcl::PointXYZ point = cloud->points[0]; //the centroid of the local surface
    int number_of_points = cloud->points.size() - 1; // remove the first point which is centroid
    transformed_cloud->points.resize(number_of_points);
    Eigen::Matrix3f matrix;
    matrix(0, 0) = pointLRF.x_axis.x;
    matrix(0, 1) = pointLRF.x_axis.y;
    matrix(0, 2) = pointLRF.x_axis.z;
    matrix(1, 0) = pointLRF.y_axis.x;
    matrix(1, 1) = pointLRF.y_axis.y;
    matrix(1, 2) = pointLRF.y_axis.z;
    matrix(2, 0) = pointLRF.z_axis.x;
    matrix(2, 1) = pointLRF.z_axis.y;
    matrix(2, 2) = pointLRF.z_axis.z;

    // Iterrate over all the points and save the transfomed version (+1 because one point is ommited at the start )
    for (int i = 0; i < number_of_points; i++)
    {
        Eigen::Vector3f transformed_point(
            cloud->points[i + 1].x - point.x,
            cloud->points[i + 1].y - point.y,
            cloud->points[i + 1].z - point.z);

        transformed_point = matrix * transformed_point;

        pcl::PointXYZ new_point;
        new_point.x = transformed_point(0);
        new_point.y = transformed_point(1);
        new_point.z = transformed_point(2);
        transformed_cloud->points[i] = new_point;
    }
}

// estimate the SDV voxel grid for all interes points
void computeLocalDepthFeature(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                              std::vector<int> evaluation_points,
                              std::vector<std::vector<int>> indices_neighbors,
                              std::vector<LRF> cloud_LRF,
                              float sup_radius,
                              flann::Matrix<float> voxel_coordinates,
                              int num_voxels,
                              float smoothing_factor,
                              std::string saveFileName)
{
    // Iterrate over all the points for which the descriptor is to be computed
    pcl::PointXYZ queryPoint;
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    int counter_voxel = num_voxels * num_voxels * num_voxels;

    // Create the filtering object
    std::string saving_path_file;
    int threads_ = omp_get_max_threads();
    std::cout << "Starting SDV computation!" << std::endl;
    std::cout << threads_ << " threads will be used!!" << std::endl;

    // Initialize the space for the SDV values
    std::vector <std::vector <float>> DIMATCH_Descriptor(evaluation_points.size(), std::vector<float>(counter_voxel, 0));

    // Initialize the point to the descriptor for each thread used
    Eigen::VectorXf *descriptor = new Eigen::VectorXf[threads_];

    for (int i = 0; i < threads_; i++)
    {
        descriptor[i].setZero(counter_voxel);
    }

    int tid;

    // Create path for saving
    const char* path = "data";
    boost::filesystem::path dir(path);
    boost::filesystem::create_directory(dir);
    int progress_counter = 0;

    #pragma omp parallel for shared(cloud,evaluation_points,indices_neighbors,counter_voxel,DIMATCH_Descriptor,progress_counter) private(tid,extract)  num_threads(threads_)
    for (int i = 0; i < evaluation_points.size(); i++)
    {

        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbors(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbors_transformed(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        std::vector<std::vector<size_t>> indices;
        std::vector<std::vector<float>> dists;

        tid = omp_get_thread_num();
        descriptor[tid].setZero(counter_voxel);
        // Save query points coordinates
        queryPoint = cloud->points[evaluation_points[i]];

        // Extract neighbors from point cloud
        extract.setInputCloud(cloud);
        inliers->indices = indices_neighbors[evaluation_points[i]];
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*sphere_neighbors);

        // Transform the neighbors to the local reference frame
        if (sphere_neighbors->points.size() != 0)
        {
            transformCloud(sphere_neighbors, cloud_LRF[evaluation_points[i]], sphere_neighbors_transformed);

            flann::Matrix<float> data(new float[sphere_neighbors_transformed->size() * 3], sphere_neighbors_transformed->size(), 3);

            for (int j = 0; j < sphere_neighbors_transformed->size(); j++)
            {
                data[j][0] = sphere_neighbors_transformed->points[j].x;
                data[j][1] = sphere_neighbors_transformed->points[j].y;
                data[j][2] = sphere_neighbors_transformed->points[j].z;
            }

            flann::SearchParams search_parameters = flann::SearchParams();
            search_parameters.checks = -1;
            search_parameters.sorted = false;
            search_parameters.use_heap = flann::FLANN_True;

            flann::KDTreeSingleIndexParams index_parameters = flann::KDTreeSingleIndexParams();
            flann::KDTreeSingleIndex<flann::L2_3D<float> > index(data, index_parameters);
            index.buildIndex();

            // Square the smoothing factor as flann uses sqaured distances
            float smoothing_factor_sqrd = 9 * smoothing_factor*smoothing_factor;
            index.radiusSearch(voxel_coordinates, indices, dists, smoothing_factor_sqrd, search_parameters);

            // Computes the normalization term and the variance
            double normalization_term = 1 / (sqrt(2 * M_PI) * smoothing_factor);
            double variance_term = -1 / (2 * (smoothing_factor * smoothing_factor));

            for (int voxel_idx = 0; voxel_idx < counter_voxel - 1; voxel_idx++)
            {
                //cout << " thread: " << omp_get_thread_num() << endl;
                // Extract all the distances
                std::vector<float> point_distances = dists[voxel_idx];
                if (!point_distances.empty())
                {
                    // Multiply with variance term
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(),
                                   std::bind1st(std::multiplies<float>(), variance_term));

                    // Exponent
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(), std::ptr_fun<float, float>(std::exp));

                    // Normalization term
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(),
                                   std::bind1st(std::multiplies<float>(), normalization_term));

                    // Sum the elements
                    float sum_of_elems = std::accumulate(point_distances.begin(), point_distances.end(), 0.0f) / point_distances.size();

                    // Sum up all the depths value to the voxel depth value
                    descriptor[tid][voxel_idx] = sum_of_elems;

                }
                else
                {
                    // if no points in the neighborhood set the voxel depth to 0
                    descriptor[tid][voxel_idx] = 0;
                }

            }

            // Normalize the descriptor
            float descriptor_sum = descriptor[tid].sum();
            if (descriptor_sum != 0)
                descriptor[tid] = descriptor[tid] / descriptor_sum;

            for (int d = 0; d < descriptor[tid].size(); ++d)
                DIMATCH_Descriptor[i][d] = descriptor[tid][d];

        }
        else
        {
            for (int voxel_idx = 0; voxel_idx < counter_voxel - 1; voxel_idx++)
            {
                descriptor[tid][voxel_idx] = 0;
            }

            for (int d = 0; d < descriptor[tid].size(); ++d)
                DIMATCH_Descriptor[i][d] = descriptor[tid][d];

        }
    }

    // Create the name with the radius, number of voxels and the smoothing factor
    saving_path_file = saveFileName + "_" + std::to_string(sup_radius) + "_" + std::to_string(num_voxels) + "_" + std::to_string(smoothing_factor * num_voxels / sup_radius) + ".csv";

    // Write descriptor to CSV file
    saveVector(saving_path_file, DIMATCH_Descriptor);
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef SplitMSSM_FullMSSM_THRESHOLDS_H
#define SplitMSSM_FullMSSM_THRESHOLDS_H

#include <Eigen/Core>

/**
 * @file splitmssm_thresholds.hpp
 *
 * Contains function declarations for threshold corrections of the
 * MSSM to the Standard Model or the Split-MSSM from arXiv:1407.4081.
 *
 * Example for matching of the MSSM to the Standard Model:
@code
   Parameters pars;
   // fill parameters ...

   double lambda = lambda_tree_level(pars);

   if (loopLevel > 0) {
      lambda +=
         + delta_lambda_1loop_reg(pars)
         + delta_lambda_1loop_phi(pars)
         + delta_lambda_1loop_chi_1(pars)
         + delta_lambda_1loop_chi_2(pars);
   }

   if (loopLevel > 1)
      lambda += delta_lambda_2loop_phi_HSS(pars);
@endcode
 *
 * Example for matching of the MSSM to the Split-MSSM:
@code
   Parameters pars;
   // fill parameters ...
   // Note: g1 and g2 are defined in the Split-MSSM

   double lambda_split = lambda_tree_level(pars);
   double gYu = gYu_tree_level(pars);
   double gYd = gYd_tree_level(pars);
   double g2u = g2u_tree_level(pars);
   double g2d = g2d_tree_level(pars);

   if (loopLevel > 0) {
      lambda_split +=
         + delta_lambda_1loop_reg(pars)
         + delta_lambda_1loop_phi(pars);
      gYu += delta_gYu_1loop(pars);
      gYd += delta_gYd_1loop(pars);
      g2u += delta_g2u_1loop(pars);
      g2d += delta_g2d_1loop(pars);
   }

   if (loopLevel > 1)
      lambda_split += delta_lambda_2loop_phi(pars);
@endcode
 *
 * Example for matching of the Split-MSSM to the Standard Model:
@code
   Parameters pars;
   // fill parameters ...
   // Note: parameters are defined in the Split-MSSM

   double lambda_SM = lambda_split;

   if (loopLevel > 0) {
      lambda_SM += delta_lambda_1loop_chi_1(
         scale, mu, lambda_split, gYu, gYd, g2u, g2d, m1, m2);
   }
@endcode
 */

namespace flexiblesusy {
namespace splitmssm_thresholds {

/**
 * @class Parameters
 * @brief Parameters for MSSM threshold corrections to the SM or Split-MSSM
 *
 * Contains the running MS-bar parameters of the EFT (either the SM or
 * the Split-MSSM) and the runnign DR-bar parameters of the MSSM.  See
 * arXiv:1407.4081 for the parameter definition.
 */
struct Parameters {
   double g1{0.}, g2{0.}, g3{0.}; ///< MS-bar gauge couplings in the EFT (GUT normalized)
   double gt{0.};          ///< MS-bar top Yukawa coupling of the SM or Split-MSSM
   double At{0.};          ///< DR-bar trilinear coupling for the stops in the MSSM
   double mu{0.};          ///< bilinear Higgsino coupling
   double mA{0.};          ///< mass of the heavy Higgs doublett
   double m1{0.};          ///< bino mass parameter
   double m2{0.};          ///< wino mass parameter
   double tan_beta{0.};    ///< mixing angle of the heavy Higgs doublett in the MSSM
   double scale{0.};       ///< renormalization scale
   Eigen::Matrix<double,3,3> mq2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking left-handed squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> mu2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed up-squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> md2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed down-squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> ml2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking left-handed selectron mass parameters in the MSSM
   Eigen::Matrix<double,3,3> me2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed selectron mass parameters in the MSSM
};

std::ostream& operator<<(std::ostream&, const Parameters&);

double lambda_tree_level(const Parameters&);
double gYu_tree_level(const Parameters&);
double gYd_tree_level(const Parameters&);
double g2u_tree_level(const Parameters&);
double g2d_tree_level(const Parameters&);

double delta_lambda_1loop_reg(const Parameters&);
double delta_lambda_1loop_phi(const Parameters&);
double delta_lambda_1loop_chi_1(const Parameters&);
double delta_lambda_1loop_chi_1(
   double scale, double mu, double lambda, double gYu, double gYd,
   double g2u, double g2d, double m1, double m2);
double delta_lambda_1loop_chi_2(const Parameters&);
double delta_lambda_1loop_chi_2(
   double scale, double mu, double m2, double g1, double g2, double tan_beta);
double delta_lambda_2loop_phi(const Parameters&);
double delta_lambda_2loop_phi_HSS(const Parameters&);
double delta_gYu_1loop(const Parameters&);
double delta_gYd_1loop(const Parameters&);
double delta_g2u_1loop(const Parameters&);
double delta_g2d_1loop(const Parameters&);
double delta_gt_1loop_chi(
   double scale, double mu, double gYu, double gYd,
   double g2u, double g2d, double m1, double m2);
double delta_m2_1loop_chi(const Parameters&);

} // namespace splitmssm_thresholds
} // namespace flexiblesusy

#endif


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                      (C) 2006-2013 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <boost/python.hpp>

#include "wrap_E_vQ_Kernel.h"

namespace wrap_mccomponents {

  Wrap_E_vQ_Kernel::kernel_t *
  Wrap_E_vQ_Kernel::newKernel
  (const char * E_vQ,
   const char * S_vQ,
   double Emax,
   double absorption_cross_section,
   double scattering_cross_section
   )
  {
    return new kernel_t
      (fxyz_t(E_vQ, "Qx,Qy,Qz"),
       SvQ_t(S_vQ, "Qx,Qy,Qz,Q,s"),
       Emax,
       absorption_cross_section,
       scattering_cross_section
       );
  }

  Wrap_E_vQ_Kernel::Wrap_E_vQ_Kernel()
  {
    using namespace boost::python;

    def("create_E_vQ_Kernel",
	&newKernel,
	return_value_policy<manage_new_object>()
	);

    class_
      <kernel_t,
      bases<mccomponents::AbstractScatteringKernel>,
      boost::noncopyable>
      ("E_vQ_Kernel", no_init)
      ;
  }

}

// version
// $Id$

// End of file


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL 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 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 license and that you accept its terms.

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE eps_moea

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <iostream>
#include <sferes/phen/parameters.hpp>
#include <sferes/gen/evo_float.hpp>
#include <sferes/ea/eps_moea.hpp>
#include <sferes/eval/eval.hpp>
#include <sferes/stat/pareto_front.hpp>
#include <sferes/stat/best_fit.hpp>
#include <sferes/eval/parallel.hpp>
#include <sferes/modif/dummy.hpp>

using namespace sferes;
using namespace sferes::gen::evo_float;

struct Params {
  struct evo_float {

    SFERES_CONST float cross_rate = 0.5f;
    SFERES_CONST float mutation_rate = 1.0f/30.0f;
    SFERES_CONST float eta_m = 15.0f;
    SFERES_CONST float eta_c = 20.0f;
    SFERES_CONST mutation_t mutation_type = polynomial;
    SFERES_CONST cross_over_t cross_over_type = sbx;
  };
  struct pop {
    SFERES_CONST unsigned size = 100;
    SFERES_CONST int dump_period = -1;
    SFERES_ARRAY(float, eps, 0.0075f, 0.0075f);
    SFERES_ARRAY(float, min_fit, 0.0f, 0.0f);
    SFERES_CONST size_t grain = size / 4;
    SFERES_CONST unsigned nb_gen = 60000 / grain;
  };
  struct parameters {
    SFERES_CONST float min = 0.0f;
    SFERES_CONST float max = 1.0f;
  };
};

template<typename Indiv>
float _g(const Indiv &ind) {
  float g = 0.0f;
  assert(ind.size() == 30);
  for (size_t i = 1; i < 30; ++i)
    g += ind.data(i);
  g = 9.0f * g / 29.0f;
  g += 1.0f;
  return g;
}

SFERES_FITNESS(FitZDT2, sferes::fit::Fitness) {
public:
  template<typename Indiv>
  void eval(Indiv& ind) {
    this->_objs.resize(2);
    float f1 = ind.data(0);
    float g = _g(ind);
    float h = 1.0f - powf((f1 / g), 2.0f);
    float f2 = g * h;
    this->_objs[0] = -f1;
    this->_objs[1] = -f2;
  }
};

BOOST_AUTO_TEST_CASE(test_epsmoea) {
  srand(time(0));

  typedef gen::EvoFloat<30, Params> gen_t;
  typedef phen::Parameters<gen_t, FitZDT2<Params>, Params> phen_t;
  typedef eval::Parallel<Params> eval_t;
  typedef boost::fusion::vector<stat::ParetoFront<phen_t, Params> >  stat_t;
  typedef modif::Dummy<> modifier_t;
  typedef ea::EpsMOEA<phen_t, eval_t, stat_t, modifier_t, Params> ea_t;
  ea_t ea;

  ea.run();

  ea.stat<0>().show_all(std::cout, 0);
  BOOST_CHECK(ea.stat<0>().pareto_front().size() >= 101);
  std::cout<<"elite size :"<<ea.stat<0>().pareto_front().size()<<std::endl;

  BOOST_FOREACH(boost::shared_ptr<phen_t> p, ea.stat<0>().pareto_front()) {
    BOOST_CHECK(_g(*p) < 1.1);
  }

}


#include <boost/unordered_map.hpp>

#include <map>
#include <iostream>
#include <boost/lexical_cast.hpp>

#include <boost/unordered_set.hpp>
#include <string>
#include <cassert>

void example() {
    boost::unordered_set<std::string> strings;

    strings.insert("This");
    strings.insert("is");
    strings.insert("an");
    strings.insert("example");

    assert(strings.find("is") != strings.cend());
}

#include <set>
template <class T>
void output_example() {
    T strings;

    strings.insert("CZ");
    strings.insert("CD");
    strings.insert("A");
    strings.insert("B");

    std::copy(
        strings.begin(),
        strings.end(),
        std::ostream_iterator<std::string>(std::cout, "  ")
    );
}

// Boost.Timer has linkage issues on MSVC.
#ifndef BOOST_MSVC

#include <boost/timer/timer.hpp>

#ifndef BOOST_NO_CXX11_HDR_UNORDERED_MAP
#   include <unordered_map>
#endif
#include <boost/container/map.hpp>

template <class T>
double test_default(const std::size_t ii_max) {
    boost::timer::cpu_timer timer;

    const std::string s("Test string");

    T map;

    for (std::size_t ii = 0; ii < ii_max; ++ii) {
        map[s + boost::lexical_cast<std::string>(ii)] = ii;
    }

    // Asserting.
    for (std::size_t ii = 0; ii < ii_max; ++ii) {
        assert(map[s + boost::lexical_cast<std::string>(ii)] == ii);
    }

    return timer.elapsed().wall;
}

void measure_performance() {
    for (unsigned i = 10; i < 1000000; i *= 10) {
        std::cout << "For " << i << " elements:\n";

        const double boost_map = test_default< boost::container::map<std::string, std::size_t> >(i);
        const double boost_unordered = test_default< boost::unordered_map<std::string, std::size_t> >(i);
        std::cout << "Boost: map is " << boost_map / boost_unordered << " slower than unordered map\n";

#ifndef BOOST_NO_CXX11_HDR_UNORDERED_MAP
        const double std_map = test_default<std::map<std::string, std::size_t> >(i);
        const double std_unordered = test_default<std::unordered_map<std::string, std::size_t> >(i);
        std::cout << "Std: map is " << std_map / std_unordered << " slower than unordered map\n\n";
#endif
    }
}

#else // #ifndef BOOST_MSVC

void measure_performance() {}

#endif

struct my_type {
    int         val1_;
    std::string val2_;
};

inline bool operator == (const my_type& v1, const my_type& v2) {
    return v1.val1_ == v2.val1_ && v1.val2_ == v2.val2_;
}

std::size_t hash_value(const my_type& v) {
    std::size_t ret = 0u;

    boost::hash_combine(ret, v.val1_);
    boost::hash_combine(ret, v.val2_);

    return ret;
}

int main() {
    example();

    std::cout << "boost::unordered_set<std::string> : ";
    output_example<boost::unordered_set<std::string> >();
    std::cout << "\nstd::set<std::string> : ";
    output_example<std::set<std::string> >();
    std::cout << '\n';

    boost::unordered_set<my_type> mt;
    mt.insert(my_type());

    measure_performance();
}


#ifndef _ROTATION_BROADCASTER_H_
#define _ROTATION_BROADCASTER_H_

#include <Eigen/Core>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/TransformStamped.h>
#include <actionlib/server/simple_action_server.h>
#include <collect_calib_data/rotation_broadcasterAction.h>

namespace CalibrationVelodyne
{
    struct diff_xyz
    {
        float diff_x;
        float diff_y;
        float diff_z;
    };

    class RotationBroadcaster
    {
    private:

        int degree;
        bool cb_success;

        float pitch;
        float x;
        float y;
        float z;
        bool success;

        ros::Subscriber degree_sub;

        actionlib::SimpleActionServer<collect_calib_data::rotation_broadcasterAction> server;
        collect_calib_data::rotation_broadcasterFeedback feedback;
        collect_calib_data::rotation_broadcasterResult result;
        collect_calib_data::rotation_broadcasterGoalConstPtr goal;

        void set_R(
            Eigen::Matrix3f &R,
            const float roll,
            const float pitch,
            const float yaw);

        Eigen::Vector3f set_diff_xyz(
            const float roll,
            const float pitch,
            const float yaw,
            const float x,
            const float y,
            const float z);

        void set_ts(
            const float roll,
            const float pitch,
            const float yaw,
            geometry_msgs::TransformStamped &ts);

        void degree_cb(
            const std_msgs::Int16 &potentio_cb);

        void goal_callback();
        void preempt_callback();

    public:

        RotationBroadcaster(ros::NodeHandle &nh);
        ~RotationBroadcaster();
        void run();
    };
}

#endif


/*!
@file
Defines the `boost::hana::Identity` data type.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_IDENTITY_HPP
#define BOOST_HANA_IDENTITY_HPP

#include <boost/hana/applicative/mcd.hpp>
#include <boost/hana/comparable/equal_mcd.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/foldable/unpack_mcd.hpp>
#include <boost/hana/functor/fmap_mcd.hpp>
#include <boost/hana/monad/flatten_mcd.hpp>
#include <boost/hana/orderable/less_mcd.hpp>
#include <boost/hana/traversable/traverse_mcd.hpp>

namespace boost { namespace hana {
    //! @ingroup group-datatypes
    //! The identity data type.
    //!
    //! This is a trivial instance of several type classes. It is especially
    //! useful for unit testing purposes, where one wants a lightweight and
    //! easy-to-reason-with instance of a type class.
    //!
    //! ## Instance of
    //! `Comparable`, `Orderable`, `Functor`, `Applicative`, `Monad`,
    //! `Foldable` and `Traversable`.
    struct Identity { };

    namespace detail { namespace repr {
        template <typename X, typename = operators<Comparable, Orderable, Monad>>
        struct identity {
            X value;
            using hana_datatype = Identity;
        };
    }}

    //! Wrap a value in the `Identity` data type.
    BOOST_HANA_CONSTEXPR_LAMBDA auto identity = [](auto x) {
        return detail::repr::identity<decltype(x)>{x};
    };

    template <>
    struct Comparable::instance<Identity, Identity> : Comparable::equal_mcd {
        template <typename Id1, typename Id2>
        static constexpr auto equal_impl(Id1 x, Id2 y) {
            return equal(x.value, y.value);
        }
    };

    template <>
    struct Orderable::instance<Identity, Identity> : Orderable::less_mcd {
        template <typename Id1, typename Id2>
        static constexpr auto less_impl(Id1 x, Id2 y) {
            return less(x.value, y.value);
        }
    };

    template <>
    struct Functor::instance<Identity> : Functor::fmap_mcd {
        template <typename F, typename Id>
        static constexpr auto fmap_impl(F f, Id self) {
            return identity(f(self.value));
        }
    };

    template <>
    struct Applicative::instance<Identity> : Applicative::mcd {
        template <typename X>
        static constexpr auto lift_impl(X x) {
            return identity(x);
        }

        template <typename F, typename X>
        static constexpr auto ap_impl(F f, X x) {
            return identity(f.value(x.value));
        }
    };

    template <>
    struct Monad::instance<Identity> : Monad::flatten_mcd<Identity> {
        template <typename Id>
        static constexpr auto flatten_impl(Id self) {
            return self.value;
        }
    };

    template <>
    struct Foldable::instance<Identity> : Foldable::unpack_mcd {
        template <typename Id, typename F>
        static constexpr auto unpack_impl(Id self, F f) {
            return f(self.value);
        }
    };

    template <>
    struct Traversable::instance<Identity> : Traversable::traverse_mcd {
        template <typename A, typename F, typename Id>
        static constexpr auto traverse_impl(F f, Id self) {
            return fmap(identity, f(self.value));
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_IDENTITY_HPP


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/graph/graph_utility.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>

#include <boost/random/linear_congruential.hpp>

using namespace std;
using namespace boost;

struct cycle_validator
{
    cycle_validator(size_t& c)
        : cycles(c)
    { }

    template <typename Path, typename Graph>
    void cycle(const Path& p, const Graph& g)
    {
        ++cycles;
        // Check to make sure that each of the vertices in the path
        // is truly connected and that the back is connected to the
        // front - it's not validating that we find all paths, just
        // that the paths are valid.
        typename Path::const_iterator i, j, last = prior(p.end());
        for(i = p.begin(); i != last; ++i) {
            j = boost::next(i);
            BOOST_ASSERT(edge(*i, *j, g).second);
        }
        BOOST_ASSERT(edge(p.back(), p.front(), g).second);
    }

    size_t& cycles;
};

template <typename Graph>
void test()
{
    typedef erdos_renyi_iterator<boost::minstd_rand, Graph> er;

    // Generate random graphs with 15 vertices and 15% probability
    // of edge connection.
    static const size_t N = 20;
    static const double P = 0.1;
    boost::minstd_rand rng;

    Graph g(er(rng, N, P), er(), N);
    renumber_indices(g);
    print_edges(g, get(vertex_index, g));

    size_t cycles = 0;
    cycle_validator vis(cycles);
    tiernan_all_cycles(g, vis);
    cout << "# cycles: " << vis.cycles << "\n";
}

int
main(int, char *[])
{
    typedef undirected_graph<> Graph;
    typedef directed_graph<> DiGraph;

    std::cout << "*** undirected ***\n";
    test<Graph>();

    std::cout << "*** directed ***\n";
    test<DiGraph>();
}


#define BOOST_TEST_MODULE "C++ Unit Tests for metaLBM"
#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#define NPROCS 1
#include "Lattice.h"
typedef double dataType;
typedef lbm::Lattice<dataType, lbm::LatticeType::D1Q3> L;
#include "Domain.h"
#include "Field.h"
#include "Initialize.h"

using namespace lbm;

BOOST_AUTO_TEST_SUITE(TestInitializeT)

BOOST_AUTO_TEST_CASE(initGlobalAlphaT) {
  ::lbm::Field<dataType, 1, true> alphaField("alpha");

  auto alphaGlobalField = ::lbm::initGlobalAlpha<dataType>();

  alphaField.setGlobalField(alphaGlobalField);

  for(unsigned int i = 0; i < gD::volume(); ++i) {
  const auto alpha_ = alphaField.getGlobalField(i, 0);
    BOOST_CHECK_EQUAL(alpha_, (dataType) 2);
  }

  }

BOOST_AUTO_TEST_SUITE_END()


#include <boost/program_options.hpp>

#include <mips/mips.h>

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

int main(int argc, char *argv[]) {
    // OPTIONS

    std::string filenameExactResults;
    std::string filenameApproximateResults;
    std::string filenameQueryMatrix;
    std::string filenameProbeMatrix;
    bool querySideLeft;
    int numElements, k;
    int r, m, n;

    boost::program_options::options_description desc("Options");
    desc.add_options()
            ("help", "produce help message")
            ("exact", boost::program_options::value<string>(&filenameExactResults), "file containing the exact results")
            ("approximate", boost::program_options::value<string>(&filenameApproximateResults), "file containing the approximate results")
            ("Q^T", boost::program_options::value<string>(&filenameQueryMatrix), "file containing the query matrix (left side)")
            ("P", boost::program_options::value<string>(&filenameProbeMatrix), "file containing the probe matrix (right side)")
            ("querySideLeft", boost::program_options::value<bool>(&querySideLeft)->default_value(true), "1 if Q^T contains the queries (default). Interesting for Row-Top-k")
            ("k", boost::program_options::value<int>(&k), "top k")
            ("r", boost::program_options::value<int>(&r)->default_value(0), "num of coordinates in each vector (needed when reading from csv files)")
            ("m", boost::program_options::value<int>(&m)->default_value(0), "num of vectors in Q^T (needed when reading from csv files)")
            ("n", boost::program_options::value<int>(&n)->default_value(0), "num of vectors in P (needed when reading from csv files)")
            ;

    boost::program_options::positional_options_description pdesc;
    pdesc.add("exact", 1);
    pdesc.add("approximate", 2);
    pdesc.add("Q^T", 3);
    pdesc.add("P", 4);
    pdesc.add("k", 5);

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help") || vm.count("exact") == 0 || vm.count("approximate") == 0 || vm.count("Q^T") == 0 || vm.count("P") == 0 || vm.count("k") == 0) {
        std::cout << "run compare [options] <exact> <approximate> <Q^T> <P> <k>" << std::endl << std::endl;
        std::cout << desc << std::endl;
        return 1;
    }

    // COMPARING

    // reading matrices
    std::cout << "reading query matrix and probe matrix" << std::endl;
    mips::VectorMatrix queryMatrix, probeMatrix;
    if (querySideLeft) {
        queryMatrix.readFromFile(filenameQueryMatrix, r, m, true);
        probeMatrix.readFromFile(filenameProbeMatrix, r, n, false);
    } else {
        queryMatrix.readFromFile(filenameProbeMatrix, r, n, false);
        probeMatrix.readFromFile(filenameQueryMatrix, r, m, true);
    }
    numElements = queryMatrix.rowNum;

    std::ifstream fileExact(filenameExactResults.c_str(), std::ios_base::in);
    std::vector< std::vector <std::pair<int, double> > > resultsExact(numElements);
    std::ifstream fileApproximate(filenameApproximateResults.c_str(), std::ios_base::in);
    std::vector< std::vector <std::pair<int, double> > > resultsApproximate(numElements);

    if (fileExact && fileApproximate) {
        // reading exact
	std::cout << "reading " << filenameExactResults << " (exact results)" << std::endl;
        if(fileExact.good()) {
            for(int i = 0; i < numElements * k; i++) {
                int query;
                fileExact >> query;

                int id;
                fileExact >> id;

                // not used but need to read the value
                double value;
                fileExact >> value;

                double ip =  probeMatrix.innerProduct(id, queryMatrix.getMatrixRowPtr(query));

                resultsExact[query].push_back(std::make_pair(id, ip));
            }
        }

        // sorting exact
        std::cout << "sorting exact results (descending)" << std::endl;
        for(int i = 0; i < numElements; i++) {
            // descending
            std::sort(resultsExact[i].begin(), resultsExact[i].end(), [](const std::pair<int, double> &left, const std::pair<int, double> &right) {return left.second > right.second;});
        }

        // reading approximate
        std::cout << "reading " << filenameApproximateResults << " (approximate results)" << std::endl;
        if(fileApproximate.good()) {
            for(int i = 0; i < numElements * k; i++) {
                int query;
                fileApproximate >> query;

                int id;
                fileApproximate >> id;

                // not used but need to read the value
                double value;
                fileApproximate >> value;

                double ip =  probeMatrix.innerProduct(id, queryMatrix.getMatrixRowPtr(query));

                resultsApproximate[query].push_back(std::make_pair(id, ip));
            }
        }

        // sorting approximate
        std::cout << "sorting approximate results (descending)" << std::endl;
        for(int i = 0; i < numElements; i++) {
            // descending
            std::sort(resultsApproximate[i].begin(), resultsApproximate[i].end(), [](const std::pair<int, double> &left, const std::pair<int, double> &right) {return left.second > right.second;});
        }

        // calculate recall
        std::cout << "calculating recall" << std::endl;
	int same = 0;
        for(int i = 0; i < numElements; i++) {
            for(int j = 0; j < k; j++) {
                for(int n = 0; n < k; n++) {
                    if(resultsExact[i][j].first == resultsApproximate[i][n].first) {
                        same++;
			break;
                    }
		}
            }
	}
        double recall = static_cast<double>(same)/(numElements * k);

        // calculate rmse
        std::cout << "calculating rmse" << std::endl;
        double rmse = 0;
        double absoluteMax = 0;
        for(int i = 0; i < numElements; i++) {
            double sum = 0;
            for(int j = 0; j < k; j++) {
                double absoluteError = abs(resultsExact[i][j].second - resultsApproximate[i][j].second);

                if(absoluteError > absoluteMax) {
                    absoluteMax = absoluteError;
                }

                sum += absoluteError * absoluteError;
            }
            rmse += sqrt(sum/k);
        }
        double rmseAverage = rmse/numElements;

        // calculate are
        std::cout << "calculating are" << std::endl;
        double are = 0;
        double relativeMax = 0;
        for(int i = 0; i < numElements; i++) {
            double sum = 0;
            for(int j = 0; j < k; j++) {
                double relativeError = abs((resultsExact[i][j].second - resultsApproximate[i][j].second)/resultsExact[i][j].second);

                if(relativeError > relativeMax) {
                    relativeMax = relativeError;
                }

                sum += relativeError;
            }
            are += sum/k;
        }
        double areAverage = are/numElements;

        // print results
	std::cout << std::endl << same << " out of " << (numElements * k) << " results are the same" << std::endl;
        std::cout << "recall = " << recall << std::endl;
        std::cout << "rmse (average) = " << rmseAverage << std::endl;
        std::cout << "absolute error (maximum) = " << absoluteMax << std::endl;
        std::cout << "are (average) = " << areAverage << std::endl;
        std::cout << "relative error (maximum) = " << relativeMax << std::endl;
    }

}


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

#include "gtest/gtest.h"

#include <Eigen/Core>
#include "cpp/sensor_fusion/high_pass_filter.h"

namespace mahony_filter {

TEST(HighPassFilterTest, CreateFilter) {
  HighPassFilter filter(1.0);
  EXPECT_FALSE(filter.IsInitialized());
  EXPECT_EQ(filter.GetLastData(), Eigen::Vector3d::Zero());
  EXPECT_EQ(filter.GetFilteredData(), Eigen::Vector3d::Zero());
}

TEST(HighPassFilterTest, AddSampleDataToNewFilter) {
  HighPassFilter filter(1.0 / (2.0 * M_PI));
  const Eigen::Vector3d sample(3.0, 4.0, 0.0);
  filter.AddSampleData(sample, 1.0);
  EXPECT_TRUE(filter.IsInitialized());
  EXPECT_EQ(filter.GetFilteredData(), sample);
  EXPECT_EQ(filter.GetFilteredDataNorm(), 5.0);
  EXPECT_EQ(filter.GetFilteredDataDirection(), Eigen::Vector3d(0.6, 0.8, 0.0));
  EXPECT_EQ(filter.GetLastData(), Eigen::Vector3d::Zero());

  // The filter we implemented will cut the filtered data by half if it's feed
  // by the same sample. Eventually it will fast converge to 0.
  filter.AddSampleData(sample, 1.0);
  EXPECT_EQ(filter.GetFilteredData(), sample);
  EXPECT_EQ(filter.GetLastData(), sample);
  filter.AddSampleData(sample, 1.0);
  EXPECT_EQ(filter.GetFilteredData(), sample/2.0);
  EXPECT_EQ(filter.GetLastData(), sample);
  filter.AddSampleData(sample, 1.0);
  EXPECT_EQ(filter.GetFilteredData(), sample/4.0);
  EXPECT_EQ(filter.GetLastData(), sample);
}

}  // namespace mahony_filter


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

// Test real concept.

// real_concept is an archetype for User defined Real types.

// This file defines the features, constructors, operators, functions...
// that are essential to use mathematical and statistical functions.
// The template typename "RealType" is used where this type
// (as well as the normal built-in types, float, double & long double)
// can be used.
// That this is the minimum set is confirmed by use as a type
// in tests of all functions & distributions, for example:
//   test_spots(0.F); & test_spots(0.);  for float and double, but also
//   test_spots(boost::math::concepts::real_concept(0.));
// NTL quad_float type is an example of a type meeting the requirements,
// but note minor additions are needed - see ntl.diff and documentation
// "Using With NTL - a High-Precision Floating-Point Library".

#ifndef BOOST_MATH_REAL_CONCEPT_HPP
#define BOOST_MATH_REAL_CONCEPT_HPP

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/math/tools/big_constant.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/special_functions/asinh.hpp>
#include <boost/math/special_functions/atanh.hpp>
#if defined(__SGI_STL_PORT)
#  include <boost/math/tools/real_cast.hpp>
#endif
#include <ostream>
#include <istream>
#include <boost/config/no_tr1/cmath.hpp>
#include <math.h> // fmodl

#if defined(__SGI_STL_PORT) || defined(_RWSTD_VER) || defined(__LIBCOMO__)
#  include <cstdio>
#endif

namespace boost{ namespace math{

namespace concepts
{

#ifdef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   typedef double real_concept_base_type;
#else
   typedef long double real_concept_base_type;
#endif

class real_concept
{
public:
   // Constructors:
   real_concept() : m_value(0){}
   real_concept(char c) : m_value(c){}
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   real_concept(wchar_t c) : m_value(c){}
#endif
   real_concept(unsigned char c) : m_value(c){}
   real_concept(signed char c) : m_value(c){}
   real_concept(unsigned short c) : m_value(c){}
   real_concept(short c) : m_value(c){}
   real_concept(unsigned int c) : m_value(c){}
   real_concept(int c) : m_value(c){}
   real_concept(unsigned long c) : m_value(c){}
   real_concept(long c) : m_value(c){}
#if defined(__DECCXX) || defined(__SUNPRO_CC)
   real_concept(unsigned long long c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(long long c) : m_value(static_cast<real_concept_base_type>(c)){}
#elif defined(BOOST_HAS_LONG_LONG)
   real_concept(boost::ulong_long_type c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(boost::long_long_type c) : m_value(static_cast<real_concept_base_type>(c)){}
#elif defined(BOOST_HAS_MS_INT64)
   real_concept(unsigned __int64 c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(__int64 c) : m_value(static_cast<real_concept_base_type>(c)){}
#endif
   real_concept(float c) : m_value(c){}
   real_concept(double c) : m_value(c){}
   real_concept(long double c) : m_value(c){}
#ifdef BOOST_MATH_USE_FLOAT128
   real_concept(BOOST_MATH_FLOAT128_TYPE c) : m_value(c){}
#endif

   // Assignment:
   real_concept& operator=(char c) { m_value = c; return *this; }
   real_concept& operator=(unsigned char c) { m_value = c; return *this; }
   real_concept& operator=(signed char c) { m_value = c; return *this; }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   real_concept& operator=(wchar_t c) { m_value = c; return *this; }
#endif
   real_concept& operator=(short c) { m_value = c; return *this; }
   real_concept& operator=(unsigned short c) { m_value = c; return *this; }
   real_concept& operator=(int c) { m_value = c; return *this; }
   real_concept& operator=(unsigned int c) { m_value = c; return *this; }
   real_concept& operator=(long c) { m_value = c; return *this; }
   real_concept& operator=(unsigned long c) { m_value = c; return *this; }
#ifdef BOOST_HAS_LONG_LONG
   real_concept& operator=(boost::long_long_type c) { m_value = static_cast<real_concept_base_type>(c); return *this; }
   real_concept& operator=(boost::ulong_long_type c) { m_value = static_cast<real_concept_base_type>(c); return *this; }
#endif
   real_concept& operator=(float c) { m_value = c; return *this; }
   real_concept& operator=(double c) { m_value = c; return *this; }
   real_concept& operator=(long double c) { m_value = c; return *this; }

   // Access:
   real_concept_base_type value()const{ return m_value; }

   // Member arithmetic:
   real_concept& operator+=(const real_concept& other)
   { m_value += other.value(); return *this; }
   real_concept& operator-=(const real_concept& other)
   { m_value -= other.value(); return *this; }
   real_concept& operator*=(const real_concept& other)
   { m_value *= other.value(); return *this; }
   real_concept& operator/=(const real_concept& other)
   { m_value /= other.value(); return *this; }
   real_concept operator-()const
   { return -m_value; }
   real_concept const& operator+()const
   { return *this; }
   real_concept& operator++()
   { ++m_value;  return *this; }
   real_concept& operator--()
   { --m_value;  return *this; }

private:
   real_concept_base_type m_value;
};

// Non-member arithmetic:
inline real_concept operator+(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result += b;
   return result;
}
inline real_concept operator-(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result -= b;
   return result;
}
inline real_concept operator*(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result *= b;
   return result;
}
inline real_concept operator/(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result /= b;
   return result;
}

// Comparison:
inline bool operator == (const real_concept& a, const real_concept& b)
{ return a.value() == b.value(); }
inline bool operator != (const real_concept& a, const real_concept& b)
{ return a.value() != b.value();}
inline bool operator < (const real_concept& a, const real_concept& b)
{ return a.value() < b.value(); }
inline bool operator <= (const real_concept& a, const real_concept& b)
{ return a.value() <= b.value(); }
inline bool operator > (const real_concept& a, const real_concept& b)
{ return a.value() > b.value(); }
inline bool operator >= (const real_concept& a, const real_concept& b)
{ return a.value() >= b.value(); }

// Non-member functions:
inline real_concept acos(real_concept a)
{ return std::acos(a.value()); }
inline real_concept cos(real_concept a)
{ return std::cos(a.value()); }
inline real_concept asin(real_concept a)
{ return std::asin(a.value()); }
inline real_concept atan(real_concept a)
{ return std::atan(a.value()); }
inline real_concept atan2(real_concept a, real_concept b)
{ return std::atan2(a.value(), b.value()); }
inline real_concept ceil(real_concept a)
{ return std::ceil(a.value()); }
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
// I've seen std::fmod(long double) crash on some platforms
// so use fmodl instead:
#ifdef _WIN32_WCE
//
// Ugly workaround for macro fmodl:
//
inline long double call_fmodl(long double a, long double b)
{  return fmodl(a, b); }
inline real_concept fmod(real_concept a, real_concept b)
{ return call_fmodl(a.value(), b.value()); }
#else
inline real_concept fmod(real_concept a, real_concept b)
{ return fmodl(a.value(), b.value()); }
#endif
#endif
inline real_concept cosh(real_concept a)
{ return std::cosh(a.value()); }
inline real_concept exp(real_concept a)
{ return std::exp(a.value()); }
inline real_concept fabs(real_concept a)
{ return std::fabs(a.value()); }
inline real_concept abs(real_concept a)
{ return std::abs(a.value()); }
inline real_concept floor(real_concept a)
{ return std::floor(a.value()); }
inline real_concept modf(real_concept a, real_concept* ipart)
{
#ifdef __MINGW32__
   real_concept_base_type ip;
   real_concept_base_type result = boost::math::modf(a.value(), &ip);
   *ipart = ip;
   return result;
#else
   real_concept_base_type ip;
   real_concept_base_type result = std::modf(a.value(), &ip);
   *ipart = ip;
   return result;
#endif
}
inline real_concept frexp(real_concept a, int* expon)
{ return std::frexp(a.value(), expon); }
inline real_concept ldexp(real_concept a, int expon)
{ return std::ldexp(a.value(), expon); }
inline real_concept log(real_concept a)
{ return std::log(a.value()); }
inline real_concept log10(real_concept a)
{ return std::log10(a.value()); }
inline real_concept tan(real_concept a)
{ return std::tan(a.value()); }
inline real_concept pow(real_concept a, real_concept b)
{ return std::pow(a.value(), b.value()); }
#if !defined(__SUNPRO_CC)
inline real_concept pow(real_concept a, int b)
{ return std::pow(a.value(), b); }
#else
inline real_concept pow(real_concept a, int b)
{ return std::pow(a.value(), static_cast<real_concept_base_type>(b)); }
#endif
inline real_concept sin(real_concept a)
{ return std::sin(a.value()); }
inline real_concept sinh(real_concept a)
{ return std::sinh(a.value()); }
inline real_concept sqrt(real_concept a)
{ return std::sqrt(a.value()); }
inline real_concept tanh(real_concept a)
{ return std::tanh(a.value()); }

//
// C++11 ism's
// Note that these must not actually call the std:: versions as that precludes using this
// header to test in C++03 mode, call the Boost versions instead:
//
inline boost::math::concepts::real_concept asinh(boost::math::concepts::real_concept a)
{
   return boost::math::asinh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}
inline boost::math::concepts::real_concept acosh(boost::math::concepts::real_concept a)
{
   return boost::math::acosh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}
inline boost::math::concepts::real_concept atanh(boost::math::concepts::real_concept a)
{
   return boost::math::atanh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}

//
// Conversion and truncation routines:
//
template <class Policy>
inline int iround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::iround(v.value(), pol); }
inline int iround(const concepts::real_concept& v)
{ return boost::math::iround(v.value(), policies::policy<>()); }
template <class Policy>
inline long lround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::lround(v.value(), pol); }
inline long lround(const concepts::real_concept& v)
{ return boost::math::lround(v.value(), policies::policy<>()); }

#ifdef BOOST_HAS_LONG_LONG
template <class Policy>
inline boost::long_long_type llround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::llround(v.value(), pol); }
inline boost::long_long_type llround(const concepts::real_concept& v)
{ return boost::math::llround(v.value(), policies::policy<>()); }
#endif

template <class Policy>
inline int itrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::itrunc(v.value(), pol); }
inline int itrunc(const concepts::real_concept& v)
{ return boost::math::itrunc(v.value(), policies::policy<>()); }
template <class Policy>
inline long ltrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::ltrunc(v.value(), pol); }
inline long ltrunc(const concepts::real_concept& v)
{ return boost::math::ltrunc(v.value(), policies::policy<>()); }

#ifdef BOOST_HAS_LONG_LONG
template <class Policy>
inline boost::long_long_type lltrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::lltrunc(v.value(), pol); }
inline boost::long_long_type lltrunc(const concepts::real_concept& v)
{ return boost::math::lltrunc(v.value(), policies::policy<>()); }
#endif

// Streaming:
template <class charT, class traits>
inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const real_concept& a)
{
   return os << a.value();
}
template <class charT, class traits>
inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, real_concept& a)
{
   real_concept_base_type v;
   is >> v;
   a = v;
   return is;
}

} // namespace concepts

namespace tools
{

template <>
inline concepts::real_concept make_big_value<concepts::real_concept>(boost::math::tools::largest_float val, const char* , std::false_type const&, std::false_type const&)
{
   return val;  // Can't use lexical_cast here, sometimes it fails....
}

template <>
inline concepts::real_concept max_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return max_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept min_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return min_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept log_max_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return log_max_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept log_min_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return log_min_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept epsilon<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
#ifdef __SUNPRO_CC
   return std::numeric_limits<concepts::real_concept_base_type>::epsilon();
#else
   return tools::epsilon<concepts::real_concept_base_type>();
#endif
}

template <>
inline BOOST_MATH_CONSTEXPR int digits<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept)) BOOST_NOEXCEPT
{
   // Assume number of significand bits is same as real_concept_base_type,
   // unless std::numeric_limits<T>::is_specialized to provide digits.
   return tools::digits<concepts::real_concept_base_type>();
   // Note that if numeric_limits real concept is NOT specialized to provide digits10
   // (or max_digits10) then the default precision of 6 decimal digits will be used
   // by Boost test (giving misleading error messages like
   // "difference between {9.79796} and {9.79796} exceeds 5.42101e-19%"
   // and by Boost lexical cast and serialization causing loss of accuracy.
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_REAL_CONCEPT_HPP


/*-----------------------------------------------------------------------------+
Copyright (c) 2007-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#include <boost/config.hpp>
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4996) // This function or variable may be unsafe. Consider using fopen_s instead. To disable deprecation, use _CRT_SECURE_NO_WARNINGS. See online help for details.
#endif

namespace boost{namespace icl
{

/** Time is a toy-class to demonstrate a class that conforms the requirements of
    a template parameter for class template icl::interval.

    In real world applications you may want to use the integer representation of a
    time variable. That way intervals and their containers are working most efficiently.
*/

enum {sunday=0, monday, tuesday, wednesday, thursday, friday, saturday};
static const char* daynames[] = {"sun", "mon", "tue", "wed", "thu", "fri", "sat"};

class Time
{
public:
    Time(): m_time(0) {}
    Time(int hours, int minutes): m_time(60*hours+minutes) {}
    Time(int day, int hours, int minutes): m_time((24*60)*day+60*hours+minutes) {}
    int getDay()const { return m_time/(24*60); }
    int getHours()const { return (m_time%(24*60))/60; }
    int getMinutes()const { return (m_time%(24*60))%60; }
    int asInt()const { return m_time; }
    std::string getDayString()const { return daynames[getDay()]; }

    std::string as_string()const
    {
        const int MAX_TIMESTING_LEN = 256;
        char repr[MAX_TIMESTING_LEN];
        sprintf(repr, "%3s:%02d:%02d", getDayString().c_str(), getHours(), getMinutes());
        return std::string(repr);
    }

    Time& operator ++ () { m_time++; return *this; }
    Time& operator -- () { m_time--; return *this; }

private:
    int m_time;
};

bool operator < (const Time& x1, const Time& x2) { return x1.asInt() < x2.asInt(); }
bool operator == (const Time& x1, const Time& x2) { return x1.asInt() == x2.asInt(); }
bool operator <= (const Time& x1, const Time& x2) { return x1.asInt() <= x2.asInt(); }

template<class CharType, class CharTraits>
std::basic_ostream<CharType, CharTraits> &operator<<
  (std::basic_ostream<CharType, CharTraits> &stream, Time const& value)
{
    return stream << value.as_string();
}

}} // namespace icl boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/reverse.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

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

#include <algorithms/test_reverse.hpp>

template <typename P>
void test_all()
{
    // Multi point, should happen nothing.
    test_geometry<bg::model::multi_point<P> >(
        "MULTIPOINT((0 0),(1 1))",
        "MULTIPOINT((0 0),(1 1))");

    test_geometry<bg::model::multi_linestring<bg::model::linestring<P> > >(
        "MULTILINESTRING((0 0,1 1),(3 3,4 4))",
        "MULTILINESTRING((1 1,0 0),(4 4,3 3))");

    typedef bg::model::multi_polygon<bg::model::polygon<P> > mp;
    test_geometry<mp>(
        "MULTIPOLYGON(((4 0,8 2,8 7,4 9,0 7,0 2,2 1,4 0)))",
        "MULTIPOLYGON(((4 0,2 1,0 2,0 7,4 9,8 7,8 2,4 0)))");
    test_geometry<mp>(
        "MULTIPOLYGON(((4 0,8 2,8 7,4 9,0 7,0 2,2 1,4 0),(7 3,7 6,1 6,1 3,4 3,7 3)))",
        "MULTIPOLYGON(((4 0,2 1,0 2,0 7,4 9,8 7,8 2,4 0),(7 3,4 3,1 3,1 6,7 6,7 3)))");
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


//  french.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2011 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

// Adapted to Boost from the original Hawards's code

#include <boost/chrono/chrono_io.hpp>
#include <boost/chrono/process_cpu_clocks.hpp>
#include <boost/chrono/thread_clock.hpp>
#include <iostream>
#include <locale>

int main()
{
    using std::cout;
    using std::locale;
    using namespace boost;
    using namespace boost::chrono;

    cout.imbue(locale(locale(), new duration_punct<char>
        (
            duration_punct<char>::use_long,
            "secondes", "minutes", "heures",
            "s", "m", "h"
        )));
    hours h(5);
    minutes m(45);
    seconds s(15);
    milliseconds ms(763);
    cout << h << ", " << m << ", " << s << " et " << ms << '\n';
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// Distributed under the Boost
// Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

// example demonstrating how to overload default hash function
// in order to support user-defined type
#include <boost/bloom_filter/basic_bloom_filter.hpp>
#include <sstream>
#include <string>
#include <iostream>
using namespace boost::bloom_filters;
using namespace std;

class URL {
public:
  URL() : _data() {}
  explicit URL(const string& url) : _data(url) {}

  const string data() const {
    return _data;
  }
private:
  string _data;
};

// provide an overload for your class
// alternatively, implement own Hasher that can handle your type
namespace boost {
  namespace bloom_filters {
    template <size_t Seed>
    struct boost_hash<URL, Seed> {
      size_t operator()(const URL& t) {
	return boost::hash_value(t.data()) + Seed;
      }
    };
  }
}

const URL gen_url(const size_t num)
{
  static const string start_url("https://www.");
  static const string end_url(".com/");
  static stringstream stringer;
  string result;

  stringer << num;
  stringer >> result;
  stringer.clear();

  return URL(start_url + result + end_url);
}

int main () {
  static const size_t INSERT_MAX = 5000;
  static const size_t CONTAINS_MAX = 10000;
  static const size_t NUM_BITS = 32768; // 8KB

  basic_bloom_filter<URL, NUM_BITS> bloom;
  size_t collisions = 0;

  for (size_t i = 0; i < INSERT_MAX; ++i) {
    bloom.insert(gen_url(i));
  }

  for (size_t i = INSERT_MAX; i < CONTAINS_MAX; ++i) {
    if (bloom.probably_contains(gen_url(i))) ++collisions;
  }

  cout << "collisions: " << collisions << endl;

  return 0;
}


// The MIT License
// (c) 2019 Daniel Williams

#include <algorithm>
#include <cmath>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <memory>
#include <vector>

using std::cerr;
using std::endl;
using std::hex;
using std::setfill;
using std::setw;
using std::string;
using std::unique_ptr;

#include <Eigen/Geometry>

#include <nlohmann/json/json.hpp>

#include "geometry/layout.hpp"

#include "grid/grid.hpp"

using Eigen::Vector2d;

using terrain::geometry::cell_value_t;
using terrain::geometry::Polygon;
using terrain::geometry::Layout;
using terrain::grid::Grid;

Grid::Grid():
    layout(Layout())
{
    reset();
}

Grid::Grid(const Layout& _layout):
    layout(_layout)
{
    reset();
}

bool Grid::contains(const Vector2d& p) const {
    return layout.contains(p);
}

cell_value_t Grid::classify(const Vector2d& p) const {
    if(contains(p)){
        return storage[layout.rhash(p.x(), p.y())];
    }

    return geometry::cell_default_value;
}

void Grid::fill(const cell_value_t value){
    memset(storage.data(), value, size());
}

cell_value_t& Grid::get_cell(const size_t xi, const size_t yi) {
    return storage[layout.rhash(static_cast<uint32_t>(xi), static_cast<uint32_t>(yi))];
}

cell_value_t Grid::get_cell(const size_t xi, const size_t yi) const {
    return storage[layout.rhash(static_cast<uint32_t>(xi), static_cast<uint32_t>(yi))];
}

size_t Grid::get_memory_usage() const {
    return layout.get_size() * sizeof(cell_value_t);
}

void Grid::reset() {
    storage.resize( layout.get_size() );
}

void Grid::reset(const Layout& new_layout){
    layout = new_layout;

    reset();
}

size_t Grid::size() const {
    return storage.size();
}

bool Grid::store(const Vector2d& p, const cell_value_t new_value) {
    if(contains(p)){
        storage[layout.rhash(p.x(), p.y())] = new_value;
        return true;
    }

    return false;
}


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil/color_base_algorithm.hpp>
#include <boost/gil/pixel.hpp>
#include <boost/gil/typedefs.hpp>

#include <cassert>
#include <cstdint>

namespace gil = boost::gil;

int main()
{
    // K-element index must be less than size of channel_mapping_t sequence
    // of the *destination* color base.

    // RGB (wider space) transformation to Gray (narrower space) takes only R channel value
    {
        gil::cmyk8_pixel_t src{16, 32, 64, 128};
        gil::rgb8_pixel_t  dst{0, 0, 0};
        gil::static_transform(src, dst, [](std::uint8_t src_channel) {
            return src_channel; // copy
            });
        assert(gil::at_c<0>(dst) == std::uint16_t{16});
        assert(gil::at_c<0>(dst) == std::uint16_t{32});
        assert(gil::at_c<0>(dst) == std::uint16_t{64});
    }

    // RGB (narrower space) to CMYK (wider space) transformation FAILS to compile
    {
        gil::rgb8_pixel_t  src{16, 32, 64};
        gil::cmyk8_pixel_t dst{0, 0, 0, 0};
        gil::static_transform(src, dst, [](std::uint8_t src_channel) {
            return src_channel; // copy
        });
    }
}