// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"

#include <boost/graph/isomorphism.hpp>

using namespace graph_tool;
using namespace boost;

struct check_iso
{

    template <class Graph1, class Graph2, class IsoMap, class InvMap,
              class VertexIndexMap>
    void operator()(Graph1& g1, Graph2& g2, InvMap cinv_map1, InvMap cinv_map2,
                    int64_t max_inv, IsoMap map, VertexIndexMap index1,
                    VertexIndexMap index2, bool& result) const
    {
        auto inv_map1 = cinv_map1.get_unchecked(num_vertices(g1));
        auto inv_map2 = cinv_map2.get_unchecked(num_vertices(g2));

        vinv_t<decltype(inv_map1)> vinv1(inv_map1, max_inv);
        vinv_t<decltype(inv_map2)> vinv2(inv_map2, max_inv);

        result = isomorphism(g1, g2,
                             isomorphism_map(map.get_unchecked(num_vertices(g1))).
                             vertex_invariant1(vinv1).
                             vertex_invariant2(vinv2).
                             vertex_index1_map(index1).
                             vertex_index2_map(index2));
    }

    template <class Prop>
    struct vinv_t
    {
        vinv_t(Prop& prop, int64_t max)
            : _prop(prop), _max(max) {}
        Prop& _prop;
        int64_t _max;

        template <class Vertex>
        int64_t operator()(Vertex v) const
        {
            return _prop[v];
        };

        int64_t max() const { return _max; }

        typedef int64_t result_type;
        typedef size_t argument_type;
    };
};

typedef property_map_types::apply<integer_types,
                                  GraphInterface::vertex_index_map_t,
                                  mpl::bool_<false> >::type
    vertex_props_t;

bool check_isomorphism(GraphInterface& gi1, GraphInterface& gi2,
                       boost::any ainv_map1, boost::any ainv_map2,
                       int64_t max_inv, boost::any aiso_map)
{
    bool result;

    typedef vprop_map_t<int32_t>::type iso_map_t;
    auto iso_map = any_cast<iso_map_t>(aiso_map);

    typedef vprop_map_t<int64_t>::type inv_map_t;
    auto inv_map1 = any_cast<inv_map_t>(ainv_map1);
    auto inv_map2 = any_cast<inv_map_t>(ainv_map2);

    if (gi1.get_directed() != gi2.get_directed())
        return false;
    if (gi1.get_directed())
    {
        gt_dispatch<>()
            (std::bind(check_iso(),
                       std::placeholders::_1, std::placeholders::_2,
                       inv_map1, inv_map2, max_inv, iso_map,
                       gi1.get_vertex_index(),
                       gi2.get_vertex_index(), std::ref(result)),
             always_directed(), always_directed())
            (gi1.get_graph_view(), gi2.get_graph_view());
    }
    else
    {
        gt_dispatch<>()
            (std::bind(check_iso(),
                       std::placeholders::_1, std::placeholders::_2,
                       inv_map1, inv_map2, max_inv, iso_map,
                       gi1.get_vertex_index(),
                       gi2.get_vertex_index(), std::ref(result)),
             never_directed(), never_directed())
            (gi1.get_graph_view(), gi2.get_graph_view());
    }

    return result;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_REDUCE_ON_GPU_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_REDUCE_ON_GPU_HPP

#include <iterator>

#include <boost/compute/program.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/detail/vendor.hpp>
#include <boost/compute/detail/work_size.hpp>
#include <boost/compute/detail/meta_kernel.hpp>
#include <boost/compute/type_traits/result_of.hpp>
#include <boost/compute/type_traits/type_name.hpp>
#include <boost/compute/utility/program_cache.hpp>
#include <boost/compute/utility/source.hpp>

namespace boost {
namespace compute {
namespace detail {

/// \internal
/// body reduction inside a warp
template<typename T,bool isNvidiaDevice>
struct ReduceBody
{
    static std::string body()
    {
        std::stringstream k;
        // local reduction
        k << "for(int i = 1; i < TPB; i <<= 1){\n" <<
             "   barrier(CLK_LOCAL_MEM_FENCE);\n"  <<
             "   uint mask = (i << 1) - 1;\n"      <<
             "   if((lid & mask) == 0){\n"         <<
             "       scratch[lid] += scratch[lid+i];\n" <<
             "   }\n" <<
            "}\n";
        return k.str();
    }
};

/// \internal
/// body reduction inside a warp
/// for nvidia device we can use the "unsafe"
/// memory optimisation
template<typename T>
struct ReduceBody<T,true>
{
    static std::string body()
    {
        std::stringstream k;
        // local reduction
        // we use TPB to compile only useful instruction
        // local reduction when size is greater than warp size
        k << "barrier(CLK_LOCAL_MEM_FENCE);\n" <<
        "if(TPB >= 1024){\n" <<
            "if(lid < 512) { sum += scratch[lid + 512]; scratch[lid] = sum;} barrier(CLK_LOCAL_MEM_FENCE);}\n" <<
         "if(TPB >= 512){\n" <<
            "if(lid < 256) { sum += scratch[lid + 256]; scratch[lid] = sum;} barrier(CLK_LOCAL_MEM_FENCE);}\n" <<
         "if(TPB >= 256){\n" <<
            "if(lid < 128) { sum += scratch[lid + 128]; scratch[lid] = sum;} barrier(CLK_LOCAL_MEM_FENCE);}\n" <<
         "if(TPB >= 128){\n" <<
            "if(lid < 64) { sum += scratch[lid + 64]; scratch[lid] = sum;} barrier(CLK_LOCAL_MEM_FENCE);} \n" <<

        // warp reduction
        "if(lid < 32){\n" <<
            // volatile this way we don't need any barrier
            "volatile __local " << type_name<T>() << " *lmem = scratch;\n" <<
            "if(TPB >= 64) { lmem[lid] = sum = sum + lmem[lid+32];} \n" <<
            "if(TPB >= 32) { lmem[lid] = sum = sum + lmem[lid+16];} \n" <<
            "if(TPB >= 16) { lmem[lid] = sum = sum + lmem[lid+ 8];} \n" <<
            "if(TPB >=  8) { lmem[lid] = sum = sum + lmem[lid+ 4];} \n" <<
            "if(TPB >=  4) { lmem[lid] = sum = sum + lmem[lid+ 2];} \n" <<
            "if(TPB >=  2) { lmem[lid] = sum = sum + lmem[lid+ 1];} \n" <<
        "}\n";
        return k.str();
    }
};

template<class InputIterator, class Function>
inline void initial_reduce(InputIterator first,
                           InputIterator last,
                           buffer result,
                           const Function &function,
                           kernel &reduce_kernel,
                           const uint_ vpt,
                           const uint_ tpb,
                           command_queue &queue)
{
    (void) function;
    (void) reduce_kernel;

    typedef typename std::iterator_traits<InputIterator>::value_type Arg;
    typedef typename ::boost::compute::result_of<Function(Arg, Arg)>::type T;

    size_t count = std::distance(first, last);
    detail::meta_kernel k("initial_reduce");
    k.add_set_arg<const uint_>("count", uint_(count));
    size_t output_arg = k.add_arg<T *>(memory_object::global_memory, "output");

    k <<
        k.decl<const uint_>("offset") << " = get_group_id(0) * VPT * TPB;\n" <<
        k.decl<const uint_>("lid") << " = get_local_id(0);\n" <<

        "__local " << type_name<T>() << " scratch[TPB];\n" <<

        // private reduction
        k.decl<T>("sum") << " = 0;\n" <<
        "for(uint i = 0; i < VPT; i++){\n" <<
        "    if(offset + lid + i*TPB < count){\n" <<
        "        sum = sum + " << first[k.var<uint_>("offset+lid+i*TPB")] << ";\n" <<
        "    }\n" <<
        "}\n" <<

        "scratch[lid] = sum;\n" <<

        // local reduction
        ReduceBody<T,false>::body() <<

        // write sum to output
        "if(lid == 0){\n" <<
        "    output[get_group_id(0)] = scratch[0];\n" <<
        "}\n";

    const context &context = queue.get_context();
    std::stringstream options;
    options << "-DVPT=" << vpt << " -DTPB=" << tpb;
    kernel generic_reduce_kernel = k.compile(context, options.str());
    generic_reduce_kernel.set_arg(output_arg, result);

    size_t work_size = calculate_work_size(count, vpt, tpb);

    queue.enqueue_1d_range_kernel(generic_reduce_kernel, 0, work_size, tpb);
}

template<class T>
inline void initial_reduce(const buffer_iterator<T> &first,
                           const buffer_iterator<T> &last,
                           const buffer &result,
                           const plus<T> &function,
                           kernel &reduce_kernel,
                           const uint_ vpt,
                           const uint_ tpb,
                           command_queue &queue)
{
    (void) function;

    size_t count = std::distance(first, last);

    reduce_kernel.set_arg(0, first.get_buffer());
    reduce_kernel.set_arg(1, uint_(first.get_index()));
    reduce_kernel.set_arg(2, uint_(count));
    reduce_kernel.set_arg(3, result);
    reduce_kernel.set_arg(4, uint_(0));

    size_t work_size = calculate_work_size(count, vpt, tpb);

    queue.enqueue_1d_range_kernel(reduce_kernel, 0, work_size, tpb);
}

template<class InputIterator, class T, class Function>
inline void reduce_on_gpu(InputIterator first,
                          InputIterator last,
                          buffer_iterator<T> result,
                          Function function,
                          command_queue &queue)
{
    const device &device = queue.get_device();

    detail::meta_kernel k("reduce");
    k.add_arg<const T*>(memory_object::global_memory, "input");
    k.add_arg<const uint_>("offset");
    k.add_arg<const uint_>("count");
    k.add_arg<T*>(memory_object::global_memory, "output");
    k.add_arg<const uint_>("output_offset");

    k <<
        k.decl<const uint_>("block_offset") << " = get_group_id(0) * VPT * TPB;\n" <<
        "__global const " << type_name<T>() << " *block = input + offset + block_offset;\n" <<
        k.decl<const uint_>("lid") << " = get_local_id(0);\n" <<

        "__local " << type_name<T>() << " scratch[TPB];\n" <<
        // private reduction
        k.decl<T>("sum") << " = 0;\n" <<
        "for(uint i = 0; i < VPT; i++){\n" <<
        "    if(block_offset + lid + i*TPB < count){\n" <<
        "        sum = sum + block[lid+i*TPB]; \n" <<
        "    }\n" <<
        "}\n" <<

        "scratch[lid] = sum;\n";

    // discrimination on vendor name
    if(is_nvidia_device(device))
        k << ReduceBody<T,true>::body();
    else
        k << ReduceBody<T,false>::body();

    k <<
        // write sum to output
         "if(lid == 0){\n" <<
         "    output[output_offset + get_group_id(0)] = scratch[0];\n" <<
         "}\n";

    uint_ vpt = 8;
    uint_ tpb = 128;

    size_t count = std::distance(first, last);

    const context &context = queue.get_context();

    // load (or create) reduce program
    boost::shared_ptr<program_cache> cache =
        program_cache::get_global_cache(context);

    std::string cache_key = std::string("__boost_reduce_on_gpu_") + type_name<T>();

    std::stringstream options;
    options << "-DT=" << type_name<T>() << " -DVPT=" << vpt << " -DTPB=" << tpb;

    program reduce_program =
        cache->get_or_build(cache_key, options.str(), k.source(), context);

    // create reduce kernel
    kernel reduce_kernel(reduce_program, "reduce");

    // first pass, reduce from input to ping
    buffer ping(context, std::ceil(float(count) / vpt / tpb) * sizeof(T));
    initial_reduce(first, last, ping, function, reduce_kernel, vpt, tpb, queue);

    // update count after initial reduce
    count = std::ceil(float(count) / vpt / tpb);

    // middle pass(es), reduce between ping and pong
    const buffer *input_buffer = &ping;
    buffer pong(context, count / vpt / tpb * sizeof(T));
    const buffer *output_buffer = &pong;
    if(count > vpt * tpb){
        while(count > vpt * tpb){
            reduce_kernel.set_arg(0, *input_buffer);
            reduce_kernel.set_arg(1, uint_(0));
            reduce_kernel.set_arg(2, uint_(count));
            reduce_kernel.set_arg(3, *output_buffer);
            reduce_kernel.set_arg(4, uint_(0));

            size_t work_size = std::ceil(float(count) / vpt);
            if(work_size % tpb != 0){
                work_size += tpb - work_size % tpb;
            }
            queue.enqueue_1d_range_kernel(reduce_kernel, 0, work_size, tpb);

            std::swap(input_buffer, output_buffer);
            count = std::ceil(float(count) / vpt / tpb);
        }
    }

    // final pass, reduce from ping/pong to result
    reduce_kernel.set_arg(0, *input_buffer);
    reduce_kernel.set_arg(1, uint_(0));
    reduce_kernel.set_arg(2, uint_(count));
    reduce_kernel.set_arg(3, result.get_buffer());
    reduce_kernel.set_arg(4, uint_(result.get_index()));

    queue.enqueue_1d_range_kernel(reduce_kernel, 0, tpb, tpb);
}

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_REDUCE_ON_GPU_HPP


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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 boolean_operator.cpp
/// \brief Test for boolean operators.

#define BOOST_TEST_MODULE boolean_operator_test
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/data/optimized_boolean_operators.h"
#include "mcrl2/pbes/parse.h"

using namespace mcrl2;

const std::string VARIABLE_SPECIFICATION =
  "datavar         \n"
  "  b:  Bool;     \n"
  "  b1: Bool;     \n"
  "  b2: Bool;     \n"
  "  b3: Bool;     \n"
  "                \n"
  "  n:  Nat;      \n"
  "  n1: Nat;      \n"
  "  n2: Nat;      \n"
  "  n3: Nat;      \n"
  "                \n"
  "  p:  Pos;      \n"
  "  p1: Pos;      \n"
  "  p2: Pos;      \n"
  "  p3: Pos;      \n"
  "                \n"
  "predvar         \n"
  "  X;            \n"
  "  Y: Nat;       \n"
  "  Z: Bool, Pos; \n"
  ;

inline
pbes_system::pbes_expression expr(const std::string& text)
{
  return pbes_system::parse_pbes_expression(text, VARIABLE_SPECIFICATION);
}

BOOST_AUTO_TEST_CASE(test_boolean_operators)
{
  BOOST_CHECK(data::optimized_or(expr("false"), expr("X")) == expr("X"));
  BOOST_CHECK(data::optimized_and(expr("false"), expr("X")) == expr("false"));
}


#include <ndt_fuser/ndt_fuser_hmt.h>
#include <ndt_offline/VelodyneBagReader.h>
#include <ndt_generic/eigen_utils.h>
// PCL specific includes
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/ndt_cell.h>
#include <ndt_map/pointcloud_utils.h>
#include <tf_conversions/tf_eigen.h>
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <dirent.h>
#include <algorithm>
#include <boost/program_options.hpp>
#include "graph_map/graph_map_fuser.h"
#include "graph_map/ndt/ndt_map_param.h"
#include "graph_map/ndt/ndtd2d_reg_type.h"
#include "graph_map/ndt/ndt_map_type.h"
#include "graph_map/graphfactory.h"
#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "ros/publisher.h"
#include "tf/transform_broadcaster.h"
#include "ndt_generic/eigen_utils.h"
//#include "ndt_mcl/3d_ndt_mcl.h"
#include "ndt_mcl/3d_ndt_mcl.h"
#include "ndt_generic/io.h"

using namespace perception_oru;
using namespace libgraphMap;

namespace po = boost::program_options;
using namespace std;

std::string map_dir_name="";
std::string output_dir_name="";
std::string base_name="";
std::string dataset="";
//map parameters
int itrs=0;
int nb_neighbours=0;
int nb_scan_msgs=0;
bool use_odometry=true;
bool visualize=true;
bool filter_fov=false;
bool step_control=false;
bool registration2d=true;
bool alive=false;
bool disable_reg=false, do_soft_constraints=false;
perception_oru::MotionModel2d::Params motion_params;
std::string base_link_id="", gt_base_link_id="", tf_world_frame="";
std::string velodyne_config_file="";
std::string velodyne_packets_topic="";
std::string velodyne_frame_id="";
std::string map_file_name="";
std::string tf_topic="";
tf::Transform tf_sensor_pose;
Eigen::Affine3d sensor_offset,fuser_pose;//Mapping from base frame to sensor frame
ros::NodeHandle *n_=NULL;

MapParamPtr mapParPtr=NULL;
GraphParamPtr graphParPtr=NULL;
double sensor_time_offset=0;
double resolution_local_factor=0;
double max_range=0, min_range=0;
double maxRotationNorm_=0;
double interchange_radius_=0;
double maxTranslationNorm_=0;
double rotationRegistrationDelta_=0;
double sensorRange_=30;
double translationRegistrationDelta_=0;
double resolution=0;
double hori_min=0, hori_max=0;
double min_dist=0, min_rot_in_deg=0;
ros::Publisher *gt_pub,*fuser_pub,*cloud_pub;
nav_msgs::Odometry gt_pose_msg,fuser_pose_msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr msg_cloud;
NDTMCL3D *mcl;

template<class T> std::string toString (const T& x)
{
  std::ostringstream o;

  if (!(o << x))
    throw std::runtime_error ("::toString()");

  return o.str ();
}

void filter_fov_fun(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointCloud<pcl::PointXYZ> &cloud_nofilter, double hori_min, double hori_max) {
  for(int i=0; i<cloud_nofilter.points.size(); ++i) {
    double ang = atan2(cloud_nofilter.points[i].y, cloud_nofilter.points[i].x);
    if(ang < hori_min || ang > hori_max) continue;
    cloud.points.push_back(cloud_nofilter.points[i]);
  }
  cloud.width = cloud.points.size();
  cloud.height = 1;
  std::cout << "nb clouds : " << cloud.points.size() << std::endl;
}

std::string transformToEvalString(const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T) {
  std::ostringstream stream;
  stream << std::setprecision(std::numeric_limits<double>::digits10);
  Eigen::Quaternion<double> tmp(T.rotation());
  stream << T.translation().transpose() << " " << tmp.x() << " " << tmp.y() << " " << tmp.z() << " " << tmp.w() << std::endl;
  return stream.str();
}
bool GetSensorPose(const std::string &dataset,  Eigen::Vector3d & transl,  Eigen::Vector3d &euler,tf::Transform &tf_sensor){

  tf::Quaternion quat;
  bool found_sensor_pose=false;
  if(dataset.compare("oru-basement")){
    transl[0]=0.3;
    transl[1]=0;
    transl[2]=0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  else if(dataset.compare("default")){
    transl[0]=0.3;
    transl[1]=0;
    transl[2]=0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  quat.setRPY(euler[0], euler[1], euler[2]);
  tf::Vector3 trans(transl[0], transl[1], transl[2]);
  tf_sensor  = tf::Transform(quat,trans);
  tf::poseTFToEigen(tf_sensor,sensor_offset);
  return found_sensor_pose;
}

bool LocateRosBagFilePaths(const std::string &folder_name,std::vector<std::string> &scanfiles){
  DIR *dir;
  struct dirent *ent;
  if ((dir = opendir (folder_name.c_str())) != NULL) {
    while ((ent = readdir (dir)) != NULL) {
      if(ent->d_name[0] == '.') continue;
      char tmpcname[400];
      snprintf(tmpcname,399,"%s/%s",folder_name.c_str(),ent->d_name);
      std::string tmpfname = tmpcname;
      scanfiles.push_back(tmpfname);
    }
    closedir (dir);
  } else {
    std::cerr<<"Could not parse dir name\n";
    return false;
  }
  sort(scanfiles.begin(),scanfiles.end());
  {
    std::cout << "files to be loaded : " << std::endl;
    for (size_t i = 0; i < scanfiles.size(); i++) {
      std::cout << " " << scanfiles[i] << std::flush;
    }
    std::cout << std::endl;
  }
  return true;
}
bool ReadAllParameters(po::options_description &desc,int &argc, char ***argv){

  Eigen::Vector3d transl;
  Eigen::Vector3d euler;
  // First of all, make sure to advertise all program options
  desc.add_options()
      ("help", "produce help message")
      ("map-file-path", po::value<std::string>(&map_file_name)->default_value(std::string("graph_map.MAP")), "name of file to load containing graphMap")
      ("visualize", "visualize the output")
      ("base-name", po::value<string>(&base_name)->default_value(std::string("mcl")), "prefix for all generated files")
      ("output-dir-name", po::value<string>(&output_dir_name)->default_value("/home/daniel/.ros/maps"), "where to save the pieces of the map (default it ./map)")
      ("data-set", po::value<string>(&dataset)->default_value(""), "where to save the pieces of the map (default it ./map)")
      ("filter-fov", "cutoff part of the field of view")
      ("dir-name", po::value<string>(&map_dir_name), "where to look for ros bags")
      ("hori-max", po::value<double>(&hori_max)->default_value(2*M_PI), "the maximum field of view angle horizontal")
      ("hori-min", po::value<double>(&hori_min)->default_value(-hori_max), "the minimum field of view angle horizontal")
      ("Dd", po::value<double>(&motion_params.Dd)->default_value(1.), "forward uncertainty on distance traveled")
      ("Dt", po::value<double>(&motion_params.Dt)->default_value(1.), "forward uncertainty on rotation")
      ("Cd", po::value<double>(&motion_params.Cd)->default_value(1.), "side uncertainty on distance traveled")
      ("Ct", po::value<double>(&motion_params.Ct)->default_value(1.), "side uncertainty on rotation")
      ("Td", po::value<double>(&motion_params.Td)->default_value(1.), "rotation uncertainty on distance traveled")
      ("Tt", po::value<double>(&motion_params.Tt)->default_value(1.), "rotation uncertainty on rotation")
      ("min_dist", po::value<double>(&min_dist)->default_value(0.2), "minimum distance traveled before adding cloud")
      ("min_rot_in_deg", po::value<double>(&min_rot_in_deg)->default_value(5), "minimum rotation before adding cloud")
      ("tf_base_link", po::value<std::string>(&base_link_id)->default_value(std::string("/odom_base_link")), "tf_base_link")
      ("tf_gt_link", po::value<std::string>(&gt_base_link_id)->default_value(std::string("/state_base_link")), "tf ground truth link")
      ("velodyne_config_file", po::value<std::string>(&velodyne_config_file)->default_value(std::string("../config/velo32.yaml")), "configuration file for the scanner")
      ("tf_world_frame", po::value<std::string>(&tf_world_frame)->default_value(std::string("/world")), "tf world frame")
      ("velodyne_packets_topic", po::value<std::string>(&velodyne_packets_topic)->default_value(std::string("/velodyne_packets")), "velodyne packets topic used")
      ("velodyne_frame_id", po::value<std::string>(&velodyne_frame_id)->default_value(std::string("/velodyne")), "frame_id of the velodyne")
      ("min_range", po::value<double>(&min_range)->default_value(0.6), "minimum range used from scanner")
      ("max_range", po::value<double>(&max_range)->default_value(30), "minimum range used from scanner")
      ("save-map", "saves the graph map at the end of execution")
      ("nb_scan_msgs", po::value<int>(&nb_scan_msgs)->default_value(1), "number of scan messages that should be loaded at once from the bag")
      ("tf_topic", po::value<std::string>(&tf_topic)->default_value(std::string("/tf")), "tf topic to listen to")
      ("x", po::value<double>(&transl[0])->default_value(0.), "sensor pose - translation vector x")
      ("y", po::value<double>(&transl[1])->default_value(0.), "sensor pose - translation vector y")
      ("z", po::value<double>(&transl[2])->default_value(0.), "sensor pose - translation vector z")
      ("ex", po::value<double>(&euler[0])->default_value(0.), "sensor pose - euler angle vector x")
      ("ey", po::value<double>(&euler[1])->default_value(0.), "sensor pose - euler angle vector y")
      ("ez", po::value<double>(&euler[2])->default_value(0.), "sensor pose - euler angle vector z")
      ("sensor_time_offset", po::value<double>(&sensor_time_offset)->default_value(0.), "timeoffset of the scanner data")
      ("registration2d","registration2d")
      ("resolution", po::value<double>(&resolution)->default_value(0.4), "resolution of the map")
      ("resolution_local_factor", po::value<double>(&resolution_local_factor)->default_value(1.), "resolution factor of the local map used in the match and fusing step")
      ("use-submap", "Adopt the sub-mapping technique which represent the global map as a set of local submaps");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, *argv, desc), vm);
  po::notify(vm);
  cout<<"sensor pose"<<endl;
  if(!GetSensorPose(dataset,transl,euler,tf_sensor_pose))
    exit(0);

  visualize = vm.count("visualize");
  filter_fov = vm.count("filter-fov");
  step_control = (vm.count("no-step-control") == 0);
  alive = vm.count("alive");
  //Check if all iputs are assigned
  if (!vm.count("base-name") || !vm.count("dir-name")){
    cout << "Missing base or dir names.\n";
    cout << desc << "\n";
    return false;
  }
  if (vm.count("help")){
    cout << desc << "\n";
    return false;
  }
  cout<<"base-name:"<<base_name<<endl;
  cout<<"dir-name:"<<map_dir_name<<endl;
  return true;

}
void initializeRosPublishers(){
  gt_pub=new ros::Publisher();
  fuser_pub=new ros::Publisher();
  cloud_pub=new ros::Publisher();
  *gt_pub    =n_->advertise<nav_msgs::Odometry>("/GT", 50);
  *fuser_pub =n_->advertise<nav_msgs::Odometry>("/fuser", 50);
  *cloud_pub = n_->advertise<pcl::PointCloud<pcl::PointXYZ>>("/points2", 1);
}
void printParameters(){
  cout<<"Output directory: "<<output_dir_name<<endl;
  if(filter_fov)
    cout << "Filtering FOV of sensor to min/max "<<hori_min<<" "<<hori_max<<endl;
  else
    cout<<"No FOV filter."<<endl;

}

/////////////////////////////////////////////////////////////////////////////////7
/////////////////////////////////////////////////////////////////////////////////7
/// *!!MAIN!!*
/////////////////////////////////////////////////////////////////////////////////7
/////////////////////////////////////////////////////////////////////////////////7
///

int main(int argc, char **argv){
  cout<<"start"<<endl;
  ros::init(argc, argv, "graph_fuser3d_offline");
  cout<<"po options"<<endl;
  po::options_description desc("Allowed options");
  cout<<"node handle"<<endl;
  n_=new ros::NodeHandle("~");
  cout<<"test"<<endl;

  bool succesfull=ReadAllParameters(desc,argc,&argv);
  if(!succesfull)
    exit(0);
  cout<<"load map from file"<<endl;
  GraphMapNavigatorPtr graph_map;
  //LoadGraphMap(map_file_name,graphMap);
  std::ifstream ifs(map_file_name);
  boost::archive::text_iarchive ia(ifs);
  ia >> graph_map;
  cout<<"map succesfully loaded"<<endl;
  cout<<graph_map->ToString()<<endl;
  ndt_generic::CreateEvalFiles eval_files(output_dir_name,base_name);
  printParameters();
  initializeRosPublishers();
  tf::TransformBroadcaster br;
  gt_pose_msg.header.frame_id="/world";
  fuser_pose_msg.header.frame_id="/world";

  base_name += motion_params.getDescString() + std::string("_res") + toString(resolution) + std::string("_SC") + toString(do_soft_constraints) + std::string("_mindist") + toString(min_dist) + std::string("_sensorcutoff") + toString(max_range) + std::string("_stepcontrol") + toString(step_control) + std::string("_neighbours") + toString(nb_neighbours) + std::string("_rlf") + toString(resolution_local_factor);

  ros::Time::init();
  srand(time(NULL));

  /// Set up the sensor link
  tf::StampedTransform sensor_link; ///Link from /odom_base_link -> velodyne
  sensor_link.child_frame_id_ = velodyne_frame_id;
  sensor_link.frame_id_ = base_link_id;//tf_base_link; //"/odom_base_link";
  sensor_link.setData(tf_sensor_pose);

  std::vector<std::string> ros_bag_paths;
  if(!LocateRosBagFilePaths(map_dir_name,ros_bag_paths)){
    cout<<"couldnt locate ros bags"<<endl;
    exit(0);
  }

  int counter = 0;

  cout<<"opening bag files"<<endl;
  for(int i=0; i<ros_bag_paths.size(); i++) {
    std::string bagfilename = ros_bag_paths[i];
    fprintf(stderr,"Opening %s\n",bagfilename.c_str());
    cout<<velodyne_config_file<<","<<bagfilename<<","<<velodyne_packets_topic<<","<<velodyne_frame_id<<","<<tf_world_frame<<","<<tf_topic<<endl;
    VelodyneBagReader<pcl::PointXYZ> vreader(velodyne_config_file,
                                             bagfilename,
                                             velodyne_packets_topic,  //"/velodyne_packets"
                                             velodyne_frame_id,
                                             tf_world_frame,
                                             tf_topic,
                                             ros::Duration(3600),
                                             &sensor_link, max_range, min_range,
                                             sensor_time_offset);

    pcl::PointCloud<pcl::PointXYZ> cloud, cloud_nofilter;
    tf::Transform tf_scan_source;
    tf::Transform tf_gt_base;
    Eigen::Affine3d Todom_base_prev,Tgt_base_prev;

    while(vreader.readMultipleMeasurements(nb_scan_msgs,cloud_nofilter,tf_scan_source,tf_gt_base,base_link_id)){
      if(!n_->ok())
        exit(0);

      if(cloud_nofilter.size()==0) continue;

      if(filter_fov) {
        filter_fov_fun(cloud,cloud_nofilter,hori_min,hori_max);
      } else {
        cloud = cloud_nofilter;
      }

      if (cloud.size() == 0) continue; // Check that we have something to work with depending on the FOV filter here...

      tf::Transform tf_odom_base;
      vreader.getPoseFor(tf_odom_base, base_link_id);
      vreader.getPoseFor(tf_gt_base, gt_base_link_id);
      Eigen::Affine3d Todom_base,Tgt_base;
      tf::transformTFToEigen(tf_gt_base,Tgt_base);
      tf::transformTFToEigen(tf_odom_base,Todom_base);

      if(counter == 0){
        counter ++;
        cloud.clear();
        cloud_nofilter.clear();
        continue;
      }
      if(counter == 1){
        Tgt_base_prev = Tgt_base;
        Todom_base_prev = Todom_base;
        Eigen::Affine3d init_pose=graph_map->GetCurrentNodePose().inverse()*Tgt_base;
        Eigen::Vector3d tgt_euler = init_pose.rotation().eulerAngles(0,1,2);
        Eigen::Vector3d tgt_pos = init_pose.translation();
        NDTMapPtr curr_node = boost::dynamic_pointer_cast< NDTMapType >(graph_map->GetCurrentNode()->GetMap());
        mcl=new NDTMCL3D(resolution, *curr_node->GetNDTMap(),0);
        //mcl->initializeFilter(tgt_pos(0),tgt_pos(1),tgt_pos(2),tgt_euler(0),tgt_euler(1),tgt_euler(2),0.1,0.1,0.0001,0.00001,0.00001,0.001,1000);
        mcl->initializeFilter(tgt_pos(0),tgt_pos(1),tgt_pos(2),tgt_euler(0),tgt_euler(1),tgt_euler(2)  ,0.1,0.1,0.0001,0.00001,0.00001,0.001,600);
        counter ++;
        cloud.clear();
        cloud_nofilter.clear();
        continue;
      }

      Eigen::Affine3d Tmotion = Todom_base_prev.inverse()*Todom_base;
      Eigen::Vector3d Tmotion_euler = Tmotion.rotation().eulerAngles(0,1,2);
      ndt_generic::normalizeEulerAngles(Tmotion_euler);

      counter++;

      //pfilter.predict(Tmotion,0.01,0.01,0.01,0.00001,0.00001,0.001);
      //fuser_pose=fuser_pose*Tmotion;

      if(visualize){
        br.sendTransform(tf::StampedTransform(tf_gt_base,ros::Time::now(), "/world", "/state_base_link"));
        if(counter%10==0){
          cloud.header.frame_id="/velodyne";
          pcl_conversions::toPCL(ros::Time::now(), cloud.header.stamp);
          cloud_pub->publish(cloud);
          GraphPlot::plotParticleCloud(graph_map->GetCurrentNodePose(),mcl->pf.pcloud);
        }
      }
        perception_oru::transformPointCloudInPlace(sensor_offset, cloud);
        mcl->updateAndPredictEff(Tmotion,cloud,1.0);
        fuser_pose=graph_map->GetCurrentNodePose()*mcl->getMean();
        cout<<"mean="<<mcl->getMean().translation().transpose()<<endl;
        if(visualize){
          gt_pose_msg.header.stamp=ros::Time::now();
          tf::poseEigenToMsg(Tgt_base, gt_pose_msg.pose.pose);
          gt_pub->publish(gt_pose_msg);
          fuser_pose_msg.header.stamp=ros::Time::now();
          tf::poseEigenToMsg(fuser_pose, fuser_pose_msg.pose.pose);
          fuser_pub->publish(fuser_pose_msg);
        }

        //  fuser_->ProcessFrame(cloud,fuser_pose,Tmotion);

     //   graph_map->SwitchToClosestMapNode(fuser_pose,unit_covar,T,std::numeric_limits<double>::max());
        if(visualize && counter%10==0){
          GraphPlot::PlotPoseGraph(graph_map);
          NDTMapPtr curr_node = boost::dynamic_pointer_cast< NDTMapType >(graph_map->GetCurrentNode()->GetMap());
          GraphPlot::SendGlobalMapToRviz(curr_node->GetNDTMap(),1,graph_map->GetCurrentNodePose());
        }
        double diff = (fuser_pose.inverse() * Tgt_base).translation().norm();
        cout<<"norm between estimated and actual pose="<<diff<<endl;
        Tgt_base_prev = Tgt_base;
        Todom_base_prev = Todom_base;
        cloud.clear();
        cloud_nofilter.clear();

        eval_files.Write( vreader.getTimeStampOfLastSensorMsg(),Tgt_base,Todom_base,fuser_pose,sensor_offset);

      }
    }
    eval_files.Close();

    if (alive) {
      while (1) {
        usleep(1000);
      }
    }
    usleep(1000*1000);
    std::cout << "Done." << std::endl;
  }


/**
 * \file OversamplingFilter.cpp
 */

#include <ATK/Tools/OversamplingFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

constexpr gsl::index PROCESSSIZE = 1024;

BOOST_AUTO_TEST_CASE( OversamplingFilter_sinus_2_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_2<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*128);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*128);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(50, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( OversamplingFilter_sinus_4_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_4<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64*4);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64*4);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(50, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( OversamplingFilter_sinus_8_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_8<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64*8);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64*8);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(50, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( OversamplingFilter_sinus_16_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_16<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64*16);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64*16);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(50, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( OversamplingFilter_sinus_32_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_32<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64*32);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64*32);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(50, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}


#include <boost/range/adaptor/map.hpp>
#include <boost/assign.hpp>
#include <utility>

#include "q_slim.h"

double garland::QSlim::ERROR_LEVEL = 0;

garland::QSlim::QSlim(garland::Mesh &mesh,
                      std::vector<garland::FaceId> keys,
                      boost::program_options::variables_map vm,
                      int iteration)
        : heap(Heap()), mesh(mesh), keys(std::move(keys)), options(std::move(vm)), currentIteration(iteration) {
}

garland::QSlim::~QSlim() = default;

void garland::QSlim::buildHeap() {
    for (auto &edge : edges)
        updateHeap(edge.second);
}

bool garland::QSlim::converged(garland::Edge &edge) {
    return edge.cost > ERROR_LEVEL;
}

void garland::QSlim::contract(garland::Edge &edge) {
    if (Geometry::flipped(mesh, edge)) return;
    if (!Geometry::movedToTarget(mesh, edge)) return;

    updateEdge(edge);
}

void garland::QSlim::applyContraction() {
    while (!heap.empty()) {
        auto edge = heap.top().get();

        heap.pop();
        edges.erase(edge.getKey());

        if (!mesh.isValid(edge))
            continue;

        if (!mesh.sameCluster(edge))
            continue;

        if (converged(edge))
            break;

        contract(edge);
    }
}

void garland::QSlim::updateEdge(garland::Edge &edge) {
    for (auto &ref : getEdgesForVertex(edge.v)) {
        auto &edgeToUpdate = ref.get();

        edgeToUpdate.Q->reset();
        edgeToUpdate.Q->add(mesh.getVertices()[edgeToUpdate.v]->getQuadric());
        edgeToUpdate.Q->add(mesh.getVertices()[edgeToUpdate.u]->getQuadric());

        updateHeap(edgeToUpdate);

        Geometry::moveToCluster(mesh, edgeToUpdate, clustersAABBs);
    }
}

void garland::QSlim::updateHeap(garland::Edge &edge) {
    auto v = mesh.getVertices()[edge.v];
    auto u = mesh.getVertices()[edge.u];

    if (!mesh.isValid(edge))
        return;

    edge.computeOptimum(v->attributes(), u->attributes());

    if (!edge.inHeap) {
        edge.inHeap = true;
        edge.handle = heap.push(std::ref(edge));
        return;
    }

    heap.erase(edge.handle);
    edge.handle = heap.push(std::ref(edge));
}

garland::EdgeRefsVector garland::QSlim::getEdgesForVertex(garland::VertexId id) {
    auto neighbourEdges = std::vector<std::reference_wrapper<garland::Edge>>();

    if (!mesh.isValidVertex(id))
        return neighbourEdges;

    auto faces = mesh.getFacesForVertex(id);
    auto edgeKeys = std::set<EdgeKey>();

    // Find valid keys for edges connected with the vertex
    for (const auto &face: faces) {
        for (const auto &edge: face->getEdges()) {
            edgeKeys.insert(edge.getKey());
        }
    }

    for (auto key: edgeKeys) {
        if (edges.find(key) == edges.end()) continue;

        auto &edge = edges[key];
        if (mesh.isValid(edge))
            neighbourEdges.emplace_back(std::ref(edges[key]));
    }

    return neighbourEdges;
}


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

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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP

#include <algorithm>
#include <vector>

#include <boost/range.hpp>

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

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

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

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/expand.hpp>

#include <boost/geometry/algorithms/detail/check_iterator_range.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>
#include <boost/geometry/algorithms/detail/disjoint/multirange_geometry.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_geometry.hpp>
#include <boost/geometry/algorithms/detail/relate/less.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

template <typename MultiPoint1, typename MultiPoint2>
class multipoint_multipoint
{
private:
    template <typename Iterator>
    class unary_disjoint_predicate
        : detail::relate::less
    {
    private:
        typedef detail::relate::less base_type;

    public:
        unary_disjoint_predicate(Iterator first, Iterator last)
            : base_type(), m_first(first), m_last(last)
        {}

        template <typename Point>
        inline bool apply(Point const& point) const
        {
            return !std::binary_search(m_first,
                                       m_last,
                                       point,
                                       static_cast<base_type const&>(*this));
        }

    private:
        Iterator m_first, m_last;
    };

public:
    static inline bool apply(MultiPoint1 const& multipoint1,
                             MultiPoint2 const& multipoint2)
    {
        BOOST_GEOMETRY_ASSERT( boost::size(multipoint1) <= boost::size(multipoint2) );

        typedef typename boost::range_value<MultiPoint1>::type point1_type;

        std::vector<point1_type> points1(boost::begin(multipoint1),
                                         boost::end(multipoint1));

        std::sort(points1.begin(), points1.end(), detail::relate::less());

        typedef unary_disjoint_predicate
            <
                typename std::vector<point1_type>::const_iterator
            > predicate_type;

        return check_iterator_range
            <
                predicate_type
            >::apply(boost::begin(multipoint2),
                     boost::end(multipoint2),
                     predicate_type(points1.begin(), points1.end()));
    }
};

template <typename MultiPoint, typename Linear>
class multipoint_linear
{
private:
    // structs for partition -- start
    struct expand_box
    {
        template <typename Box, typename Geometry>
        static inline void apply(Box& total, Geometry const& geometry)
        {
            geometry::expand(total, geometry::return_envelope<Box>(geometry));
        }

    };

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

    class item_visitor_type
    {
    public:
        item_visitor_type() : m_intersection_found(false) {}

        template <typename Item1, typename Item2>
        inline void apply(Item1 const& item1, Item2 const& item2)
        {
            if (! m_intersection_found
                && ! dispatch::disjoint<Item1, Item2>::apply(item1, item2))
            {
                m_intersection_found = true;
            }
        }

        inline bool intersection_found() const { return m_intersection_found; }

    private:
        bool m_intersection_found;
    };
    // structs for partition -- end

    class segment_range
    {
    public:
        typedef geometry::segment_iterator<Linear const> const_iterator;
        typedef const_iterator iterator;

        segment_range(Linear const& linear)
            : m_linear(linear)
        {}

        const_iterator begin() const
        {
            return geometry::segments_begin(m_linear);
        }

        const_iterator end() const
        {
            return geometry::segments_end(m_linear);
        }

    private:
        Linear const& m_linear;
    };

public:
    static inline bool apply(MultiPoint const& multipoint, Linear const& linear)
    {
        item_visitor_type visitor;

        geometry::partition
            <
                geometry::model::box<typename point_type<MultiPoint>::type>,
                expand_box,
                overlaps_box
            >::apply(multipoint, segment_range(linear), visitor);

        return ! visitor.intersection_found();
    }

    static inline bool apply(Linear const& linear, MultiPoint const& multipoint)
    {
        return apply(multipoint, linear);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename MultiPoint, std::size_t DimensionCount>
struct disjoint
    <
        Point, MultiPoint, DimensionCount, point_tag, multi_point_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Point>
{};

template <typename MultiPoint, typename Segment, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Segment, DimensionCount, multi_point_tag, segment_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Segment>
{};

template <typename MultiPoint, typename Box, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Box, DimensionCount, multi_point_tag, box_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Box>
{};

template
<
    typename MultiPoint1,
    typename MultiPoint2,
    std::size_t DimensionCount
>
struct disjoint
    <
        MultiPoint1, MultiPoint2, DimensionCount,
        multi_point_tag, multi_point_tag, false
    >
{
    static inline bool apply(MultiPoint1 const& multipoint1,
                             MultiPoint2 const& multipoint2)
    {
        if ( boost::size(multipoint2) < boost::size(multipoint1) )
        {
            return detail::disjoint::multipoint_multipoint
                <
                    MultiPoint2, MultiPoint1
                >::apply(multipoint2, multipoint1);
        }

        return detail::disjoint::multipoint_multipoint
            <
                MultiPoint1, MultiPoint2
            >::apply(multipoint1, multipoint2);
   }
};

template <typename Linear, typename MultiPoint, std::size_t DimensionCount>
struct disjoint
    <
        Linear, MultiPoint, DimensionCount, linear_tag, multi_point_tag, false
    > : detail::disjoint::multipoint_linear<MultiPoint, Linear>
{};

template <typename MultiPoint, typename Linear, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Linear, DimensionCount, multi_point_tag, linear_tag, false
    > : detail::disjoint::multipoint_linear<MultiPoint, Linear>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <limits>
#include <thread>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
#include <boost/make_shared.hpp>

#include "rttbBoostMask.h"
#include "rttbNullPointerException.h"
#include "rttbInvalidParameterException.h"
#include "rttbBoostMaskGenerateMaskVoxelListThread.h"
#include "rttbBoostMaskVoxelizationThread.h"

namespace rttb
{
	namespace masks
	{
		namespace boost
		{

			BoostMask::BoostMask(core::GeometricInfo::Pointer aDoseGeoInfo,
        core::Structure::Pointer aStructure, bool strict, unsigned int numberOfThreads)
				: _geometricInfo(aDoseGeoInfo), _structure(aStructure),
                _strict(strict), _numberOfThreads(numberOfThreads), _voxelizationThickness(0.0),
				  _voxelInStructure(::boost::make_shared<MaskVoxelList>())
			{

				_isUpToDate = false;

				if (_geometricInfo == nullptr)
				{
					throw rttb::core::NullPointerException("Error: Geometric info is nullptr!");
				}
				else if (_structure == nullptr)
				{
					throw rttb::core::NullPointerException("Error: Structure is nullptr!");
				}

				if (_numberOfThreads == 0)
				{
					_numberOfThreads = std::thread::hardware_concurrency();
                    if (_numberOfThreads == 0)
                    {
                        throw rttb::core::InvalidParameterException("Error: detection of the number of hardware threads is not possible. Please specify number of threads for voxelization explicitly as parameter in BoostMask.");
                    }
				}

			}

			BoostMask::MaskVoxelListPointer BoostMask::getRelevantVoxelVector()
			{
				if (!_isUpToDate)
				{
					calcMask();
				}

				return _voxelInStructure;
			}

			void BoostMask::calcMask()
			{
				preprocessing();
				voxelization();
				generateMaskVoxelList();
				_isUpToDate = true;
			}

			void BoostMask::preprocessing()
			{
				rttb::PolygonSequenceType polygonSequence = _structure->getStructureVector();

				//Convert world coordinate polygons to the polygons with geometry coordinate
				rttb::PolygonSequenceType geometryCoordinatePolygonVector;
				rttb::PolygonSequenceType::iterator it;
				rttb::ContinuousVoxelGridIndex3D globalMaxGridIndex(std::numeric_limits<double>::min(),
				        std::numeric_limits<double>::min(), std::numeric_limits<double>::min());
				rttb::ContinuousVoxelGridIndex3D globalMinGridIndex(_geometricInfo->getNumColumns(),
				        _geometricInfo->getNumRows(), 0);

				for (it = polygonSequence.begin(); it != polygonSequence.end(); ++it)
				{
					PolygonType rttbPolygon = *it;
					PolygonType geometryCoordinatePolygon;

					//1. convert polygon to geometry coordinate polygons
					//2. calculate global min/max
					//3. check if polygon is planar
					if (!preprocessingPolygon(rttbPolygon, geometryCoordinatePolygon, globalMinGridIndex,
					                          globalMaxGridIndex, errorConstant))
					{
						throw rttb::core::Exception("TiltedMaskPlaneException");
					}

					geometryCoordinatePolygonVector.push_back(geometryCoordinatePolygon);
				}

				rttb::VoxelGridIndex3D minIndex = VoxelGridIndex3D(GridIndexType(globalMinGridIndex(0) ),
				                                  GridIndexType(globalMinGridIndex(1) ), GridIndexType(globalMinGridIndex(2) ));
				rttb::VoxelGridIndex3D maxIndex = VoxelGridIndex3D(GridIndexType(globalMaxGridIndex(0) ),
				                                  GridIndexType(globalMaxGridIndex(1) ), GridIndexType(globalMaxGridIndex(2) ));

				_globalBoundingBox.push_back(minIndex);
				_globalBoundingBox.push_back(maxIndex);

				//convert rttb polygon sequence to a map of z index and a vector of boost ring 2d (without holes)
				_ringMap = convertRTTBPolygonSequenceToBoostRingMap(geometryCoordinatePolygonVector);

			}

			void BoostMask::voxelization()
			{

				if (_globalBoundingBox.size() < 2)
				{
					throw rttb::core::InvalidParameterException("Bounding box calculation failed! ");
				}

				BoostRingMap::iterator itMap;

				size_t mapSizeInAThread = _ringMap.size() / _numberOfThreads;
				unsigned int count = 0;
				unsigned int countThread = 0;
				BoostPolygonMap polygonMap;
				std::vector<BoostPolygonMap> polygonMapVector;

				//check donut and convert to a map of z index and a vector of boost polygon 2d (with or without holes)
				for (itMap = _ringMap.begin(); itMap != _ringMap.end(); ++itMap)
				{
					//the vector of all boost 2d polygons with the same z grid index(donut polygon is accepted).
					BoostPolygonVector polygonVector = checkDonutAndConvert((*itMap).second);

					if (count == mapSizeInAThread && countThread < (_numberOfThreads - 1))
					{
						polygonMapVector.push_back(polygonMap);
						polygonMap.clear();
						count = 0;
						countThread++;
					}

					polygonMap.insert(std::pair<double, BoostPolygonVector>((*itMap).first,
					                  polygonVector));
					count++;

				}

                _voxelizationMap = ::boost::make_shared<std::map<double, BoostArray2DPointer> >();

				polygonMapVector.push_back(polygonMap); //insert the last one

				//generate voxelization map, multi-threading
				std::vector<std::thread> threads;

        auto aMutex = ::boost::make_shared<std::mutex>();

				for (const auto & i : polygonMapVector)
				{
          BoostMaskVoxelizationThread t(i, _globalBoundingBox,
            _voxelizationMap, aMutex, _strict);
					threads.emplace_back(t);
				}

        for (auto& thread : threads)
        {
          if (thread.joinable())
          {
            thread.join();
          }
        }
			}

			void BoostMask::generateMaskVoxelList()
			{
				if (_globalBoundingBox.size() < 2)
				{
					throw rttb::core::InvalidParameterException("Bounding box calculation failed! ");
				}

				//check homogeneous of the voxelization plane (the contours plane)
				if (!calcVoxelizationThickness(_voxelizationThickness))
				{
					throw rttb::core::InvalidParameterException("Error: The contour plane should be homogeneous!");
				}

        std::vector<std::thread> threads;
        auto aMutex = ::boost::make_shared<std::mutex>();

				unsigned int sliceNumberInAThread = _geometricInfo->getNumSlices() / _numberOfThreads;

				//generate mask voxel list, multi-threading
				for (unsigned int i = 0; i < _numberOfThreads; ++i)
				{
					unsigned int beginSlice = i * sliceNumberInAThread;
					unsigned int endSlice;

					if (i < _numberOfThreads - 1)
					{
						endSlice = (i + 1) * sliceNumberInAThread;
					}
					else
					{
						endSlice = _geometricInfo->getNumSlices();
					}

					BoostMaskGenerateMaskVoxelListThread t(_globalBoundingBox, _geometricInfo, _voxelizationMap,
					                                       _voxelizationThickness, beginSlice, endSlice,
					                                       _voxelInStructure, _strict, aMutex);

          threads.emplace_back(t);

				}

        for (auto& thread : threads)
        {
          if (thread.joinable())
          {
            thread.join();
          }
        }

			}

			bool BoostMask::preprocessingPolygon(const rttb::PolygonType& aRTTBPolygon,
			                                     rttb::PolygonType& geometryCoordinatePolygon, rttb::ContinuousVoxelGridIndex3D& minimum,
			                                     rttb::ContinuousVoxelGridIndex3D& maximum, double aErrorConstant) const
			{

				double minZ = _geometricInfo->getNumSlices();
				double maxZ =  0.0;

				for (auto worldCoordinatePoint : aRTTBPolygon)
				{
						//convert to geometry coordinate polygon
					rttb::ContinuousVoxelGridIndex3D geometryCoordinatePoint;
					_geometricInfo->worldCoordinateToContinuousIndex(worldCoordinatePoint, geometryCoordinatePoint);

					geometryCoordinatePolygon.push_back(geometryCoordinatePoint);

					//calculate the current global min/max
					//min and max for x
					if (geometryCoordinatePoint(0) < minimum(0))
					{
						minimum(0) = geometryCoordinatePoint(0);
					}

					if (geometryCoordinatePoint(0) > maximum(0))
					{
						maximum(0) = geometryCoordinatePoint(0);
					}

					//min and max for y
					if (geometryCoordinatePoint(1) < minimum(1))
					{
						minimum(1) = geometryCoordinatePoint(1);
					}

					if (geometryCoordinatePoint(1) > maximum(1))
					{
						maximum(1) = geometryCoordinatePoint(1);
					}

					//min and max for z
					if (geometryCoordinatePoint(2) < minimum(2))
					{
						minimum(2) = geometryCoordinatePoint(2);
					}

					if (geometryCoordinatePoint(2) > maximum(2))
					{
						maximum(2) = geometryCoordinatePoint(2);
					}

					//check planar
					if (geometryCoordinatePoint(2) < minZ)
					{
						minZ = geometryCoordinatePoint(2);
					}

					if (geometryCoordinatePoint(2) > maxZ)
					{
						maxZ = geometryCoordinatePoint(2);
					}

				}

				return (std::abs(maxZ - minZ) <= aErrorConstant);
			}

			BoostMask::BoostRing2D BoostMask::convertRTTBPolygonToBoostRing(const rttb::PolygonType&
			        aRTTBPolygon) const
			{
				BoostMask::BoostRing2D polygon2D;
				BoostPoint2D firstPoint;

				for (unsigned int i = 0; i < aRTTBPolygon.size(); i++)
				{
					rttb::WorldCoordinate3D rttbPoint = aRTTBPolygon.at(i);
					BoostPoint2D boostPoint(rttbPoint[0], rttbPoint[1]);

					if (i == 0)
					{
						firstPoint = boostPoint;
					}

					::boost::geometry::append(polygon2D, boostPoint);
				}

				::boost::geometry::append(polygon2D, firstPoint);
				return polygon2D;
			}

			BoostMask::BoostRingMap BoostMask::convertRTTBPolygonSequenceToBoostRingMap(
			    const rttb::PolygonSequenceType& aRTTBPolygonVector) const
			{
				rttb::PolygonSequenceType::const_iterator it;
				BoostMask::BoostRingMap aRingMap;

				for (it = aRTTBPolygonVector.begin(); it != aRTTBPolygonVector.end(); ++it)
				{
					rttb::PolygonType rttbPolygon = *it;
					double zIndex = rttbPolygon.at(0)[2];//get the first z index of the polygon
					bool isFirstZ = true;

					if (!aRingMap.empty())
					{
						auto findIt = findNearestKey(aRingMap, zIndex, errorConstant);

						//if the z index is found (same slice), add the polygon to vector
						if (findIt != aRingMap.end())
						{
							//BoostRingVector ringVector = ;
							(*findIt).second.push_back(convertRTTBPolygonToBoostRing(rttbPolygon));
							isFirstZ = false;
						}
					}

					//if it is the first z index in the map, insert vector with the polygon
					if (isFirstZ)
					{
						BoostRingVector ringVector;
						ringVector.push_back(convertRTTBPolygonToBoostRing(rttbPolygon));
						aRingMap.insert(std::pair<double, BoostRingVector>(zIndex, ringVector));
					}

				}

				return aRingMap;
			}

			BoostMask::BoostRingMap::iterator BoostMask::findNearestKey(BoostMask::BoostRingMap&
			        aBoostRingMap, double aIndex, double aErrorConstant) const
			{
				auto find = aBoostRingMap.find(aIndex);

				//if find a key equivalent to aIndex, found
				if (find != aBoostRingMap.end())
				{
					return find;
				}
				else
				{
					auto lowerBound = aBoostRingMap.lower_bound(aIndex);

					//if all keys go before aIndex, check the last key
					if (lowerBound == aBoostRingMap.end())
					{
						lowerBound = --aBoostRingMap.end();
					}

					//if the lower bound very close to aIndex, found
					if (std::abs((*lowerBound).first - aIndex) <= aErrorConstant)
					{
						return lowerBound;
					}
					else
					{
						//if the lower bound is the beginning, not found
						if (lowerBound == aBoostRingMap.begin())
						{
							return aBoostRingMap.end();
						}
						else
						{
							auto lowerBound1 = --lowerBound;//the key before the lower bound

							//if the key before the lower bound very close to a Index, found
							if (std::abs((*lowerBound1).first - aIndex) <= aErrorConstant)
							{
								return lowerBound1;
							}
							//else, not found
							else
							{
								return aBoostRingMap.end();
							}
						}
					}

				}
			}

			BoostMask::BoostPolygonVector BoostMask::checkDonutAndConvert(const BoostMask::BoostRingVector&
			        aRingVector) const
			{
				//check donut
				BoostMask::BoostRingVector::const_iterator it1;
				BoostMask::BoostRingVector::const_iterator it2;
				BoostMask::BoostPolygonVector boostPolygonVector;
				std::vector<unsigned int> donutIndexVector;//store the outer and inner ring index
				BoostMask::BoostPolygonVector donutVector;//store new generated donut polygon

				//Get donut index and donut polygon
				unsigned int index1 = 0;

				for (it1 = aRingVector.begin(); it1 != aRingVector.end(); ++it1, index1++)
				{
					bool it1IsDonut = false;

					//check if the ring is already determined as a donut
					for (unsigned int i : donutIndexVector)
					{
						if (i == index1)
						{
							it1IsDonut = true;
							break;
						}
					}

					//if not jet, check now
					if (!it1IsDonut)
					{
						bool it2IsDonut = false;
						unsigned int index2 = 0;

						for (it2 = aRingVector.begin(); it2 != aRingVector.end(); ++it2, index2++)
						{
							if (it2 != it1)
							{
								BoostMask::BoostPolygon2D polygon2D;

								if (::boost::geometry::within(*it1, *it2))
								{
									::boost::geometry::append(polygon2D, *it2);//append an outer ring to the polygon
									::boost::geometry::interior_rings(polygon2D).resize(1);//create an interior ring
									::boost::geometry::append(polygon2D, *it1, 0);//append a ring to the interior ring
									it2IsDonut = true;
								}
								//if donut
								else if (::boost::geometry::within(*it2, *it1))
								{
									::boost::geometry::append(polygon2D, *it1);//append an outer ring to the polygon
									::boost::geometry::interior_rings(polygon2D).resize(1);//create an interior ring
									::boost::geometry::append(polygon2D, *it2, 0);//append a ring to the interior ring
									it2IsDonut = true;
								}

								if (it2IsDonut)
								{
									donutIndexVector.push_back(index1);
									donutIndexVector.push_back(index2);
									donutVector.push_back(polygon2D);//store donut polygon
									break;//Only store the first donut!
								}
							}
						}
					}
				}

				//Store no donut polygon to boostPolygonVector
				index1 = 0;

				for (it1 = aRingVector.begin(); it1 != aRingVector.end(); ++it1, index1++)
				{
					bool it1IsDonut = false;

					//check if the ring is the outer or inner of a donut
					for (unsigned int i : donutIndexVector)
					{
						if (i == index1)
						{
							it1IsDonut = true;
							break;
						}
					}

					if (!it1IsDonut)
					{
						BoostMask::BoostPolygon2D polygon2D;
						::boost::geometry::append(polygon2D, *it1);
						boostPolygonVector.push_back(polygon2D);//insert the ring, which is not a part of donut
					}
				}

				//Append donut polygon to boostPolygonVector
				BoostMask::BoostPolygonVector::iterator itDonut;

				for (itDonut = donutVector.begin(); itDonut != donutVector.end(); ++itDonut)
				{
					boostPolygonVector.push_back(*itDonut);//append donuts
				}

				return boostPolygonVector;
			}

			bool BoostMask::calcVoxelizationThickness(double& aThickness) const
			{

				if (_voxelizationMap->size() <= 1)
				{
					aThickness = 1;
					return true;
				}

				double thickness = 0;

                auto it = _voxelizationMap->cbegin();
                auto it2 = ++_voxelizationMap->cbegin();
				for (;
				     it != _voxelizationMap->cend() && it2 != _voxelizationMap->cend(); ++it, ++it2)
				{
					if (thickness == 0)
					{
						thickness = it2->first - it->first;
					}
					else
					{
                        double curThickness = it2->first - it->first;
						//if not homogeneous (leave out double imprecisions), return false
						if (std::abs(thickness-curThickness)>errorConstant)
						{
							//return false;
							std::cout << "Two polygons are far from each other?" << std::endl;
						}
					}

				}

				if (thickness != 0)
				{
					aThickness = thickness;
				}
				else
				{
					aThickness = 1;
				}

				return true;
			}
		}
	}
}


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_GEOMETRY_ADAPTERS_HPP
#define MAPNIK_GEOMETRY_ADAPTERS_HPP

#include <mapnik/config.hpp>

// undef B0 to workaround https://svn.boost.org/trac/boost/ticket/10467
#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#undef B0
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
// NOTE: ideally we would not include all of boost/geometry here to save on compile time
// however we need to pull in <boost/geometry/multi/multi.hpp> for things to work
// and once we do that the compile time is == to just including boost/geometry.hpp
#include <boost/geometry.hpp>
#pragma GCC diagnostic pop

#include <mapnik/geometry.hpp>
#include <mapnik/coord.hpp>
#include <mapnik/box2d.hpp>

#include <cstdint>

// register point
BOOST_GEOMETRY_REGISTER_POINT_2D (mapnik::geometry::point<double>, double, boost::geometry::cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_POINT_2D (mapnik::geometry::point<std::int64_t>, std::int64_t, boost::geometry::cs::cartesian, x, y)
// ring
BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(mapnik::geometry::linear_ring)
// needed by box2d<double>
BOOST_GEOMETRY_REGISTER_POINT_2D(mapnik::coord2d, double, boost::geometry::cs::cartesian, x, y)

namespace boost {

template <typename CoordinateType>
struct range_iterator<mapnik::geometry::line_string<CoordinateType> >
{
    using type = typename mapnik::geometry::line_string<CoordinateType>::iterator;
};

template <typename CoordinateType>
struct range_const_iterator<mapnik::geometry::line_string<CoordinateType> >
{
    using type = typename mapnik::geometry::line_string<CoordinateType>::const_iterator;
};

template <typename CoordinateType>
inline typename mapnik::geometry::line_string<CoordinateType>::iterator
range_begin(mapnik::geometry::line_string<CoordinateType> & line) {return line.begin();}

template <typename CoordinateType>
inline typename mapnik::geometry::line_string<CoordinateType>::iterator
range_end(mapnik::geometry::line_string<CoordinateType> & line) {return line.end();}

template <typename CoordinateType>
inline typename mapnik::geometry::line_string<CoordinateType>::const_iterator
range_begin(mapnik::geometry::line_string<CoordinateType> const& line) {return line.begin();}

template <typename CoordinateType>
inline typename mapnik::geometry::line_string<CoordinateType>::const_iterator
range_end(mapnik::geometry::line_string<CoordinateType> const& line) {return line.end();}

namespace geometry { namespace traits {

// register mapnik::box2d<double>
template<> struct tag<mapnik::box2d<double> > { using type = box_tag; };
template<> struct point_type<mapnik::box2d<double> > { using type = mapnik::coord2d; };

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 0>
{
    using ct = coordinate_type<mapnik::coord2d>::type;
    static inline ct get(mapnik::box2d<double> const& b) { return b.minx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_minx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 1>
{
    using ct = coordinate_type<mapnik::coord2d>::type;
    static inline ct get(mapnik::box2d<double> const& b) { return b.miny();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_miny(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 0>
{
    using ct = coordinate_type<mapnik::coord2d>::type;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_maxx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 1>
{
    using ct = coordinate_type<mapnik::coord2d>::type;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxy();}
    static inline void set(mapnik::box2d<double> &b , ct const& value) { b.set_maxy(value); }
};

// mapnik::geometry::line_string
template<typename CoordinateType>
struct tag<mapnik::geometry::line_string<CoordinateType> >
{
    using type = linestring_tag;
};

// mapnik::geometry::polygon
template<typename CoordinateType>
struct tag<mapnik::geometry::polygon<CoordinateType> >
{
    using type = polygon_tag;
};

template <typename CoordinateType>
struct point_order<mapnik::geometry::linear_ring<CoordinateType> >
{
    static const order_selector value = counterclockwise;
};

template<typename CoordinateType>
struct tag<mapnik::geometry::multi_point<CoordinateType> >
{
    using type = multi_point_tag;
};

template<typename CoordinateType>
struct tag<mapnik::geometry::multi_line_string<CoordinateType> >
{
    using type = multi_linestring_tag;
};

template<typename CoordinateType>
struct tag<mapnik::geometry::multi_polygon<CoordinateType> >
{
    using type = multi_polygon_tag;
};

// ring
template <typename CoordinateType>
struct ring_const_type<mapnik::geometry::polygon<CoordinateType> >
{
    using type = typename mapnik::geometry::linear_ring<CoordinateType> const&;
};

template <typename CoordinateType>
struct ring_mutable_type<mapnik::geometry::polygon<CoordinateType> >
{
    using type = typename mapnik::geometry::linear_ring<CoordinateType>&;
};

// interior
template <typename CoordinateType>
struct interior_const_type<mapnik::geometry::polygon<CoordinateType> >
{
    using type = typename mapnik::geometry::polygon<CoordinateType>::rings_container const&;
};

template <typename CoordinateType>
struct interior_mutable_type<mapnik::geometry::polygon<CoordinateType> >
{
    using type = typename mapnik::geometry::polygon<CoordinateType>::rings_container&;
};

// exterior
template <typename CoordinateType>
struct exterior_ring<mapnik::geometry::polygon<CoordinateType> >
{
    static mapnik::geometry::linear_ring<CoordinateType> & get(mapnik::geometry::polygon<CoordinateType> & p)
    {
        return p.exterior_ring;
    }

    static mapnik::geometry::linear_ring<CoordinateType> const& get(mapnik::geometry::polygon<CoordinateType> const& p)
    {
        return p.exterior_ring;
    }
};

template <typename CoordinateType>
struct interior_rings<mapnik::geometry::polygon<CoordinateType> >
{
    using holes_type = typename mapnik::geometry::polygon<CoordinateType>::rings_container;
    static holes_type&  get(mapnik::geometry::polygon<CoordinateType> & p)
    {
        return p.interior_rings;
    }

    static holes_type const& get(mapnik::geometry::polygon<CoordinateType> const& p)
    {
        return p.interior_rings;
    }
};

}}}

#endif //MAPNIK_GEOMETRY_ADAPTERS_HPP