// This file is part of the dune-grid-multiscale project:
//   http://users.dune-project.org/projects/dune-grid-multiscale
// Copyright holders: Felix Albrecht
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GRID_MULTISCALE_GLUED_HH
#define DUNE_GRID_MULTISCALE_GLUED_HH

#include <algorithm>
#include <memory>
#include <map>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/version.hh>

#include <dune/grid/common/gridfactory.hh>
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
# include <dune/grid/common/rangegenerators.hh>
#else
# include <dune/stuff/common/ranges.hh>
#endif
#include <dune/grid/io/file/vtk/vtkwriter.hh>

#include <dune/grid-glue/extractors/codim1extractor.hh>
#include <dune/grid-glue/extractors/extractorpredicate.hh>
#include <dune/grid-glue/gridglue.hh>
#include <dune/grid-glue/merging/contactmerge.hh>

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/float_cmp.hh>
#include <dune/stuff/common/memory.hh>
#include <dune/stuff/common/timedlogging.hh>
#include <dune/stuff/common/ranges.hh>
#include <dune/stuff/common/vector.hh>
#include <dune/stuff/grid/intersection.hh>
#include <dune/stuff/grid/provider/cube.hh>
#include <dune/stuff/grid/provider/interface.hh>
#include <dune/stuff/grid/search.hh>

namespace Dune {
namespace grid {
namespace Multiscale {
namespace Exceptions {

class intersection_orientation_is_broken : public Dune::InvalidStateException {};

} // namespace Exceptions

template<typename P0, typename P1>
size_t check_for_broken_coupling_intersections(const GridGlue::GridGlue<P0, P1>& glue,
                                               const typename P0::ctype& tolerance = 10*Stuff::Common::FloatCmp::DefaultEpsilon<typename P0::ctype>::value())
{
  const auto inside_grid_view = glue.template gridView<0>();
  size_t failures = 0;
  // walk the coupling
  const auto coupling_intersection_it_end = glue.template iend<0>();
  for (auto coupling_intersection_it = glue.template ibegin<0>();
       coupling_intersection_it != coupling_intersection_it_end;
       ++coupling_intersection_it) {
    const auto& coupling_intersection = *coupling_intersection_it;
    const auto coupling_intersection_normal = coupling_intersection.centerUnitOuterNormal();
    const auto local_entity_ptr = coupling_intersection.inside();
    const auto& local_entity = *local_entity_ptr;
    typename std::remove_const<decltype(coupling_intersection_normal)>::type local_intersection_normal(0.);
    // find the intersection of the local inside entity that corresponds to the coupling intersection
    size_t found = 0;
    for (auto&& local_intersection : intersection_range(inside_grid_view, local_entity)) {
      // the coupling intersection may be smaller than the local intersection, so check if all of the corners of the
      // coupling intersection lie within this local intersection
      int corners_inside = 0;
      for (auto ii : DSC::valueRange(coupling_intersection.geometry().corners()))
        if (DSG::contains(local_intersection, coupling_intersection.geometry().corner(ii)))
          ++corners_inside;
      if (corners_inside == coupling_intersection.geometry().corners()) {
        // this is the one
        ++found;
        local_intersection_normal = local_intersection.centerUnitOuterNormal();
      }
    }
    if (found != 1)
      DUNE_THROW(InvalidStateException,
                 "This should not happen!\n"
                 << "There were " << found << " local intersections which contain the coupling intersection, "
                 << "and there must not be more than one!");
    // now the expected normal is local_intersection_normal
    // and we would like coupling_intersection_normal to point in the same direction
    // since they have unit length, they should be identical
    if ((local_intersection_normal - coupling_intersection_normal).infinity_norm() > tolerance)
      ++failures;
  }
  return failures;
} // ... check_for_broken_coupling_intersections(...)

// forward
template <class MacroGridType, class LocalGridType>
class GluedVTKWriter;

template <class MacroGridImp, class LocalGridImp>
class Glued
{
  template< class G, bool anything = true>
  struct allowed_macro_grid
  {
    static const bool value = true;
  };

  template< class G, bool anything = true>
  struct allowed_local_grid
  {
    static const bool value = true;
  };

#if HAVE_ALUGRID
  template< class Comm, bool anything>
  struct allowed_local_grid<ALUGrid<3, 3, simplex, conforming, Comm>, anything>
  {
    static const bool value = false;
  };

  template< class Comm, bool anything>
  struct allowed_local_grid<ALUGrid<3, 3, cube, conforming, Comm>, anything>
  {
    static const bool value = false;
  };
#endif

  static_assert(allowed_macro_grid<MacroGridImp>::value,
                "This macro grid is known to fail, enable on your onw risk by disabling this check!");
  static_assert(allowed_local_grid<LocalGridImp>::value,
                "This local grid is known to fail, enable on your onw risk by disabling this check!");
public:
  typedef MacroGridImp                                      MacroGridType;
  typedef Stuff::Grid::ProviderInterface<MacroGridType>     MacroGridProviderType;
  typedef typename MacroGridProviderType::LeafGridViewType  MacroGridViewType;
  typedef typename MacroGridType::template Codim<0>::Entity MacroEntityType;

  typedef LocalGridImp                                             LocalGridType;
  typedef Stuff::Grid::ProviderInterface<LocalGridType>            LocalGridProviderType;

#if HAVE_DUNE_FEM
  typedef typename LocalGridProviderType::LevelGridPartType            MicroGridPartType;
#endif
  typedef typename LocalGridType::LevelGridView                        MicroGridViewType;
  typedef typename MicroGridViewType::template Codim<0>::Entity        MicroEntityType;
  typedef typename MicroGridViewType::template Codim<0>::EntityPointer MicroEntityPointerType;

  typedef typename MacroGridType::ctype ctype;
  static const size_t dimDomain = MacroGridType::dimension;
  static const size_t dimWorld  = MacroGridType::dimensionworld;

private:
  typedef typename LocalGridProviderType::LevelGridViewType LocalViewType;
  typedef GridGlue::Codim1Extractor<LocalViewType> LocalExtractorType;
  typedef GridGlue::GridGlue<LocalExtractorType, LocalExtractorType> GlueType;

  template <class GridView, class MacroIntersectionType>
  class CouplingFaceDescriptor : public GridGlue::ExtractorPredicate<GridView, 1>
  {
    typedef typename GridView::Traits::template Codim<0>::Entity LocalEntityType;
    typedef typename GridView::ctype ctype;

  public:
    CouplingFaceDescriptor(const MacroIntersectionType& macro_intersection) : macro_intersection_(macro_intersection) {}

    virtual bool contains(const LocalEntityType& element, unsigned int face) const override final
    {
      const auto local_intersection_ptr       = element.template subEntity<1>(face);
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
      const auto& local_intersection          = local_intersection_ptr;
#else
      const auto& local_intersection          = *local_intersection_ptr;
#endif
      const auto& local_intersection_geometry = local_intersection.geometry();
      // Check if all corners of the local intersection lie within the macro intersection.
      for (auto ii : DSC::valueRange(local_intersection_geometry.corners()))
        if (!DSG::contains(macro_intersection_, local_intersection_geometry.corner(ii)))
          return false;
      return true;
    } // ... contains(...)

  private:
    const MacroIntersectionType& macro_intersection_;
  }; // CouplingFaceDescriptor

  template <class GV, class MI>
  static CouplingFaceDescriptor<GV, MI> create_descriptor(const GV& /*gv*/, const MI& mi)
  {
    return CouplingFaceDescriptor<GV, MI>(mi);
  }

public:
  Glued(MacroGridProviderType& macro_grid_provider,
        const size_t num_local_refinements = 0,
        const bool prepare_glues = false,
        const bool allow_for_broken_orientation_of_coupling_intersections = false,
        const ctype& allowed_overlap = 10 * Stuff::Common::FloatCmp::DefaultEpsilon<ctype>::value())
    : macro_grid_(macro_grid_provider)
    , allowed_overlap_(allowed_overlap)
    , macro_leaf_view_(macro_grid_.leaf_view())
    , local_grids_(macro_leaf_view_.indexSet().size(0), nullptr)
    , glues_(macro_leaf_view_.indexSet().size(0))
  {
    setup_local_grids();
    if (num_local_refinements > 0)
      for (auto& local_grid_provider : local_grids_) {
        assert(local_grid_provider);
        local_grid_provider->grid().globalRefine(boost::numeric_cast<int>(num_local_refinements));
      }
    if (prepare_glues)
      setup_glues(allow_for_broken_orientation_of_coupling_intersections);
  } // Glued(...)

  const MacroGridViewType& macro_grid_view() const { return macro_leaf_view_; }

  size_t num_subdomains() const
  {
    return macro_grid_view().indexSet().size(0);
  }

  size_t subdomain(const MacroEntityType& macro_entity) const
  {
    assert(macro_leaf_view_.indexSet().contains(macro_entity));
    return macro_leaf_view_.indexSet().index(macro_entity);
  }

  bool boundary(const MacroEntityType& macro_entity) const
  {
    assert(macro_leaf_view_.indexSet().contains(macro_entity));
    return macro_entity.hasBoundaryIntersections();
  }

  MicroGridViewType global_grid_view()
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.global_grid_view");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    return global_grid_->level_view(global_grid_->grid().maxLevel());
  }

#if HAVE_DUNE_FEM
  MicroGridPartType global_grid_part()
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.global_grid_part");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    return MicroGridPartType(global_grid_->grid(), global_grid_->grid().maxLevel());
  }
#endif // HAVE_DUNE_FEM

  const std::vector<std::vector<size_t>>& local_to_global_indices()
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.local_to_global_indices");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    return *local_to_global_indices_;
  }

  MicroEntityPointerType local_to_global_entity(const size_t subdomain, const MicroEntityType& local_entity)
  {
    return local_to_global_entity(subdomain, local_grids_[subdomain]->level_view(max_local_level(subdomain)).indexSet().index(local_entity));
  }

  MicroEntityPointerType local_to_global_entity(const size_t subdomain, const size_t local_entity_index)
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.local_to_global_entity");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    const size_t global_index_of_local_entity = local_to_global_indices_->operator[](subdomain)[local_entity_index];
    const auto global_micro_grid_view = global_grid_view();
    const auto entity_it_end = global_micro_grid_view.template end<0>();
    for (auto entity_it = global_micro_grid_view.template begin<0>();
         entity_it != entity_it_end;
         ++entity_it) {
      const auto& entity = *entity_it;
      if (global_micro_grid_view.indexSet().index(entity) == global_index_of_local_entity)
        return entity_it;
    }
    DUNE_THROW(Stuff::Exceptions::wrong_input_given,
               "subdomain: " << subdomain << "\n"
               << "local_entity_index: " << local_entity_index << "\n"
               << "global_index_of_local_entity: " << global_index_of_local_entity);
    return global_micro_grid_view.template begin<0>();
  } // ... local_to_global_entity(...)

  MicroEntityPointerType global_to_local_entity(const MicroEntityType& micro_entity)
  {
    prepare_global_grid();
    return global_to_local_entity(global_grid_view().indexSet().index(micro_entity));
  }

  MicroEntityPointerType global_to_local_entity(const size_t micro_entity_index)
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.global_to_local_entity");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    auto subdomain_and_local_entity_index = global_to_local_indices_->operator[](micro_entity_index);
    const auto subdomain = subdomain_and_local_entity_index.first;
    const auto local_index_of_global_entity = subdomain_and_local_entity_index.second;
    const auto local_grid_view = local_grids_[subdomain]->level_view(max_local_level(subdomain));
    const auto entity_it_end = local_grid_view.template end<0>();
    for (auto entity_it = local_grid_view.template begin<0>();
         entity_it != entity_it_end;
         ++entity_it) {
      const auto& entity = *entity_it;
      if (local_grid_view.indexSet().index(entity) == local_index_of_global_entity)
        return MicroEntityPointerType(entity_it);
    }
    DUNE_THROW(Stuff::Exceptions::wrong_input_given,
               "micro_entity_index: " << micro_entity_index << "\n"
               "subdomain: " << subdomain << "\n"
               << "local_index_of_global_entity: " << local_index_of_global_entity);
    return MicroEntityPointerType(local_grid_view.template begin<0>());
  } // ... global_to_local_entity(...)

  const std::vector<std::pair<size_t, size_t>>& global_to_local_indices()
  {
    auto logger = DSC::TimedLogger().get("grid-multiscale.glued.global_to_local_indices");
    logger.warn() << "Requiring access to global micro grid!" << std::endl;
    prepare_global_grid();
    assert(global_to_local_indices_);
    return *global_to_local_indices_;
  }

  const LocalGridProviderType& local_grid(const MacroEntityType& macro_entity) const
  {
    assert(macro_leaf_view_.indexSet().contains(macro_entity));
    return local_grid(macro_leaf_view_.indexSet().index(macro_entity));
  }

  LocalGridProviderType& local_grid(const MacroEntityType& macro_entity)
  {
    assert(macro_leaf_view_.indexSet().contains(macro_entity));
    return local_grid(macro_leaf_view_.indexSet().index(macro_entity));
  }

  LocalGridProviderType& local_grid(const size_t macro_entity_index)
  {
    return *(local_grids_.at(macro_entity_index));
  }

  const LocalGridProviderType& local_grid(const size_t macro_entity_index) const
  {
    return *(local_grids_.at(macro_entity_index));
  }

  /**
   * \brief Returns (and creates, if it does not exist) the coupling glue between the local grid view of level
   *        local_level_macro_entity on macro_entity and the local grid view of level local_level_macro_neighbor on macro_neighbor.
   * \note  Access is not implemented efficiently. This could be improved by using std::map::find instead of
   *        std::map::operator[].
   */
  const GlueType& coupling(const MacroEntityType& macro_entity, const int local_level_macro_entity,
                           const MacroEntityType& macro_neighbor, const int local_level_macro_neighbor,
                           const bool allow_for_broken_orientation_of_coupling_intersections = false)
  {
    const auto& macro_index_set = macro_leaf_view_.indexSet();
    assert(macro_index_set.contains(macro_entity));
    assert(macro_index_set.contains(macro_neighbor));
    if (local_level_macro_entity > max_local_level(macro_entity))
      DUNE_THROW(Stuff::Exceptions::you_are_using_this_wrong,
                    "max_local_level(macro_entity): " << max_local_level(macro_entity) << "\n"
                 << "   local_level_macro_entity:      " << local_level_macro_entity);
    if (local_level_macro_neighbor > max_local_level(macro_neighbor))
      DUNE_THROW(Stuff::Exceptions::you_are_using_this_wrong,
                    "max_local_level(macro_neighbor): " << max_local_level(macro_neighbor) << "\n"
                 << "   local_level_macro_neighbor:      " << local_level_macro_neighbor);
    const auto entity_index   = macro_index_set.index(macro_entity);
    const auto neighbor_index = macro_index_set.index(macro_neighbor);
    if (glues_[entity_index][neighbor_index][local_level_macro_entity][local_level_macro_neighbor] == nullptr) {
      // find the corresponding macro intersection ...
      const auto macro_intersection_it_end = macro_leaf_view_.iend(macro_entity);
      for (auto macro_intersection_it = macro_leaf_view_.ibegin(macro_entity);
           macro_intersection_it != macro_intersection_it_end;
           ++macro_intersection_it) {
        const auto& macro_intersection = *macro_intersection_it;
        if (macro_intersection.neighbor() && !macro_intersection.boundary()) {
          const auto real_neighbor_ptr = macro_intersection.outside();
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
          const auto& real_neighbor = real_neighbor_ptr;
#else
          const auto& real_neighbor = *real_neighbor_ptr;
#endif
          if (macro_index_set.index(real_neighbor) == neighbor_index)
            glues_[entity_index][neighbor_index][local_level_macro_entity][local_level_macro_neighbor] =
                create_glue(macro_entity, macro_neighbor,
                            macro_intersection,
                            local_level_macro_entity, local_level_macro_neighbor);
        }
      } // ... find the corresponding macro intersection
    }

    const auto& glue = *(glues_[entity_index][neighbor_index][local_level_macro_entity][local_level_macro_neighbor]);
    if (!allow_for_broken_orientation_of_coupling_intersections) {
      const size_t brocken_intersections = check_for_broken_coupling_intersections(glue);
      if (brocken_intersections > 0)
        DUNE_THROW(Exceptions::intersection_orientation_is_broken,
                   "The coupling glue between the grid views of\n"
                   << "     level " << local_level_macro_entity << " on macro entity   "
                   << macro_leaf_view_.indexSet().index(macro_entity) << " and\n"
                   << "     level " << local_level_macro_neighbor << " on macro neighbor "
                   << macro_leaf_view_.indexSet().index(macro_neighbor) << "\n"
                   << "   contains\n"
                   << "     " << brocken_intersections << "/" << glue.size()
                   << " intersections with wrong orientation!");
    }
    return glue;
  } // ... coupling(...)

  const GlueType& coupling(const MacroEntityType& macro_entity,
                           const MacroEntityType& macro_neighbor,
                           const bool allow_for_broken_orientation_of_coupling_intersections = false)
  {
    return coupling(macro_entity, max_local_level(macro_entity),
                    macro_neighbor, max_local_level(macro_neighbor),
                    allow_for_broken_orientation_of_coupling_intersections);
  }

  int max_local_level(const MacroEntityType& macro_entity) const
  {
    return local_grid(macro_entity).grid().maxLevel();
  }

  int max_local_level(const size_t macro_entity_index) const
  {
    return local_grid(macro_entity_index).grid().maxLevel();
  }

      const std::vector<std::pair<MicroEntityPointerType, std::vector<int>>>&
  local_boundary_entities(const MacroEntityType& macro_entity, const int local_level)
  {
//    auto logger = Stuff::Common::TimedLogger().get("grid-multiscale.glued.local_boundary_entities");
    assert(macro_leaf_view_.indexSet().contains(macro_entity));
    const size_t macro_entity_index = macro_leaf_view_.indexSet().index(macro_entity);
    if (local_level > max_local_level(macro_entity))
      DUNE_THROW(Stuff::Exceptions::you_are_using_this_wrong,
                 "macro_entity_index: " << macro_entity_index << "\n"
                 << "local_level: " << local_level << "\n"
                 << "max_local_level(macro_entity): " << max_local_level(macro_entity));
    auto& local_level_to_boundary_entity_ptrs_with_local_intersections = macro_entity_to_local_level_to_boundary_entity_ptrs_with_local_intersections_[macro_entity_index];
    auto& boundary_entity_ptrs_with_local_intersections = local_level_to_boundary_entity_ptrs_with_local_intersections[local_level];
//    logger.debug() << "macro_entity: " << macro_entity_index << std::endl;
    if (boundary(macro_entity) && boundary_entity_ptrs_with_local_intersections.empty()) {
      // create the container, therefore
      const auto local_leaf_view = local_grids_[macro_entity_index]->leaf_view();
      // * walk the local grid (manually, to have access to the entity pointer)
      const auto local_entity_it_end = local_leaf_view.template end<0>();
      for (auto local_entity_it = local_leaf_view.template begin<0>(); local_entity_it != local_entity_it_end; ++local_entity_it) {
        const auto& local_entity = *local_entity_it;
//        logger.debug() << "local_entity: " << local_leaf_view.indexSet().index(local_entity) << " ";
        if (local_entity.hasBoundaryIntersections()) {
//          logger.debug() << "(boundary entity)" << std::endl;
          std::vector<int> local_boundary_intersections;
          // This entity has intersections on the local grid boundary, those could either be the domain boundary (which
          // we are looking for) or a boundary to another local grid (which we are not looking for). To find out
          // * walk the intersections
          for (auto&& local_intersection : DSC::intersectionRange(local_leaf_view, local_entity)) {
            if (local_intersection.boundary() && !local_intersection.neighbor()) {
//              logger.debug() << "local_intersection: " << local_intersection.indexInInside() << ":" << std::endl;
              const auto local_intersection_geometry = local_intersection.geometry();
              const size_t num_corners = boost::numeric_cast<size_t>(local_intersection_geometry.corners());
              // ** Check if all corners of the intersection lie on the domain boundary (aka the boundary intersection of
              //    the macro entity this local grid belongs to. Therefore
              //    *** walk the intersections of the macro entity
              for (auto&& macro_intersection : DSC::intersectionRange(macro_leaf_view_, macro_entity)) {
                if (macro_intersection.boundary() && !macro_intersection.neighbor()) {
                  // This macro intersection lies on the domain boundary, check if the local intersection is contained.
                  size_t corners_lie_on_boundary = 0;
                  for (size_t ii = 0; ii < num_corners; ++ii) {
                    if (DSG::contains(macro_intersection, local_intersection_geometry.corner(boost::numeric_cast<int>(ii))))
                      ++corners_lie_on_boundary;
                  }
                  if (corners_lie_on_boundary == num_corners) {
                    // add the information to the container
                    local_boundary_intersections.push_back(local_intersection.indexInInside());
                  } // add the information to the container
                }
              } //    *** walk the intersections of the macro entity
            }
          } // * walk the intersections
          if (!local_boundary_intersections.empty()) {
            // add this local entity and its local intersections to the container
            boundary_entity_ptrs_with_local_intersections.emplace_back(local_entity_it, local_boundary_intersections);
          }
        } /*else
          logger.debug() << "(inner entity)" << std::endl;*/
      } // * walk the local grid
    } // create the container
    return boundary_entity_ptrs_with_local_intersections;
  } // ... local_boundary_entities(...)

  template< class... Args >
  void visualize(const std::string& filename = "grid.multiscale.glued",
                 Args&& ...args)
  {
    auto logger = Stuff::Common::TimedLogger().get("grid-multiscale.glued.visualize");
    macro_grid_.visualize(filename + ".macro", std::forward< Args >(args)...);
    GluedVTKWriter<MacroGridType, LocalGridType> vtk_writer(*this);
    const auto& macro_index_set = macro_leaf_view_.indexSet();
    std::vector<std::vector<double>> subdomain_visualization(macro_index_set.size(0));
    std::vector<std::vector<double>> boundary_visualization(macro_index_set.size(0));
    std::vector<std::vector<double>> inside_outside_coupling_visualization(macro_index_set.size(0));
    std::vector<std::vector<double>> outside_inside_coupling_visualization(macro_index_set.size(0));
    // walk the macro grid
    for (auto&& macro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                               elements
#else
                               DSC::entityRange
#endif
                                               (macro_leaf_view_)) {
      const size_t macro_entity_index = macro_index_set.index(macro_entity);
      logger.debug() << "macro_entity: " << macro_entity_index << " ";
      const auto local_level = max_local_level(macro_entity);
      const auto local_grid_view = local_grids_[macro_entity_index]->level_view(local_level);
      subdomain_visualization[macro_entity_index] = std::vector<double>(local_grid_view.indexSet().size(0),
                                                                        macro_entity_index);
      boundary_visualization[macro_entity_index] = std::vector<double>(local_grid_view.indexSet().size(0), -1);
      if (inside_outside_coupling_visualization[macro_entity_index].empty())
        inside_outside_coupling_visualization[macro_entity_index] = std::vector<double>(local_grid_view.indexSet().size(0), -1);
      if (outside_inside_coupling_visualization[macro_entity_index].empty())
        outside_inside_coupling_visualization[macro_entity_index] = std::vector<double>(local_grid_view.indexSet().size(0), -1);
      // local boundary entities
      if (boundary(macro_entity)) {
        logger.debug() << "(boundary entity)" << std::endl;
        const auto& boundary_entity_ptrs_with_local_intersections
            = local_boundary_entities(macro_entity, local_level);
        logger.debug() << "  " << boundary_entity_ptrs_with_local_intersections.size() << "/"
                       << local_grid_view.indexSet().size(0) << " boundary entities" << std::endl;
        for (const auto& element : boundary_entity_ptrs_with_local_intersections) {
          const auto& local_entity_ptr = element.first;
          const auto& local_intersections = element.second;
          if (!local_intersections.empty()) {
            const auto& local_entity = *local_entity_ptr;
            const size_t local_entity_index = local_grid_view.indexSet().index(local_entity);
            boundary_visualization[macro_entity_index][local_entity_index] = macro_entity_index;
          }
        }
      } else
        logger.debug() << "(inner entity)" << std::endl;
      for (auto&& macro_intersection : DSC::intersectionRange(macro_leaf_view_, macro_entity)) {
        if (!macro_intersection.boundary() && macro_intersection.neighbor()) {
          const auto macro_neighbor_ptr = macro_intersection.outside();
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
          const auto& macro_neighbor = macro_neighbor_ptr;
#else
          const auto& macro_neighbor = *macro_neighbor_ptr;
#endif
          const size_t macro_neighbor_index = macro_leaf_view_.indexSet().index(macro_neighbor);
          const auto local_neighbor_level = max_local_level(macro_neighbor);
          const auto local_neighbor_grid_view = local_grids_[macro_neighbor_index]->level_view(local_neighbor_level);
          if (inside_outside_coupling_visualization[macro_neighbor_index].empty())
            inside_outside_coupling_visualization[macro_neighbor_index] = std::vector<double>(local_neighbor_grid_view.indexSet().size(0), -1);
          if (outside_inside_coupling_visualization[macro_neighbor_index].empty())
            outside_inside_coupling_visualization[macro_neighbor_index] = std::vector<double>(local_neighbor_grid_view.indexSet().size(0), -1);
          // walk the coupling, where this is the inside
          size_t num_coupling_intersections = 0;
          const auto& in_out_coupling_glue = coupling(macro_entity, local_level, macro_neighbor, local_neighbor_level,
                                                      /*allow_for_broken_orientation_of_coupling_intersections=*/true);
          const auto in_out_coupling_intersection_it_end = in_out_coupling_glue.template iend<0>();
          for (auto in_out_coupling_intersection_it = in_out_coupling_glue.template ibegin<0>();
               in_out_coupling_intersection_it != in_out_coupling_intersection_it_end;
               ++in_out_coupling_intersection_it) {
            ++num_coupling_intersections;
            const auto& coupling_intersection = *in_out_coupling_intersection_it;
            const auto local_entity_ptr = coupling_intersection.inside();
            const auto& local_entity = *local_entity_ptr;
            const size_t local_entity_index = local_grid_view.indexSet().index(local_entity);
            inside_outside_coupling_visualization[macro_entity_index][local_entity_index] = macro_entity_index;
            const auto local_neighbor_ptr = coupling_intersection.outside();
            const auto& local_neighbor = *local_neighbor_ptr;
            const size_t local_neighbor_index = local_neighbor_grid_view.indexSet().index(local_neighbor);
            inside_outside_coupling_visualization[macro_neighbor_index][local_neighbor_index] = macro_neighbor_index;
          }
          // walk the coupling, where this is the outside
          size_t out_in_num_coupling_intersections = 0;
          const auto& out_in_coupling_glue = coupling(macro_neighbor, local_neighbor_level, macro_entity, local_level,
                                                      /*allow_for_broken_orientation_of_coupling_intersections=*/true);
          const auto out_in_coupling_intersection_it_end = out_in_coupling_glue.template iend<0>();
          for (auto out_in_coupling_intersection_it = out_in_coupling_glue.template ibegin<0>();
               out_in_coupling_intersection_it != out_in_coupling_intersection_it_end;
               ++out_in_coupling_intersection_it) {
            ++out_in_num_coupling_intersections;
            const auto& coupling_intersection = *out_in_coupling_intersection_it;
            const auto local_entity_ptr = coupling_intersection.inside();
            const auto& local_entity = *local_entity_ptr;
            const size_t local_entity_index = local_grid_view.indexSet().index(local_entity);
            outside_inside_coupling_visualization[macro_neighbor_index][local_entity_index] = macro_neighbor_index;
            const auto local_neighbor_ptr = coupling_intersection.outside();
            const auto& local_neighbor = *local_neighbor_ptr;
            const size_t local_neighbor_index = local_neighbor_grid_view.indexSet().index(local_neighbor);
            outside_inside_coupling_visualization[macro_entity_index][local_neighbor_index] = macro_entity_index;
          }
          if (num_coupling_intersections != out_in_num_coupling_intersections)
            DUNE_THROW(Stuff::Exceptions::internal_error,
                       "The coupling glue is broken!\n"
                       << "macro entity (local level):   " << macro_entity_index << " (" << local_level << ")\n"
                       << "macro neighbor (local level): " << macro_neighbor_index << " (" << local_neighbor_level
                       << ")");
          logger.debug() << "  " << num_coupling_intersections << " coupling intersections with neighbor " << macro_neighbor_index << std::endl;
        }
      }
    } // walk the macro grid
    vtk_writer.addCellData(subdomain_visualization, "subdomains");
    vtk_writer.addCellData(boundary_visualization, "local boundary entities");
    vtk_writer.addCellData(inside_outside_coupling_visualization, "local coupling entities (inside/outside)");
    vtk_writer.addCellData(outside_inside_coupling_visualization, "local coupling entities (outside/inside)");
    vtk_writer.write(filename, VTK::appendedraw);
  } // ... visualize(...)

private:
  template <class MacroEntityType>
  static std::shared_ptr<LocalGridProviderType> create_grid_of_simplex(const MacroEntityType& macro_entity)
  {
    try {
      GridFactory<LocalGridType> subdomain_factory;
      const auto num_vertices = macro_entity.
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)

                                             subEntities(dimDomain);
#else
                                             template count<dimDomain>();
#endif
      std::vector<unsigned int> vertex_ids(num_vertices, 0);
      for (int local_vertex_id = 0; local_vertex_id < num_vertices; ++local_vertex_id) {
        const auto vertex = macro_entity.template subEntity<dimDomain>(local_vertex_id)
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                                                                                .
#else
                                                                                ->
#endif
                                                                                geometry().center();
        subdomain_factory.insertVertex(vertex);
        vertex_ids[local_vertex_id] = local_vertex_id;
      }
      subdomain_factory.insertElement(macro_entity.geometry().type(), vertex_ids);
      return std::make_shared<Stuff::Grid::Providers::Default<LocalGridType>>(subdomain_factory.createGrid());
    } catch (GridError& ee) {
      DUNE_THROW(GridError,
                 "It was not possible to create a grid for this simplex with the given GridType!\n\n"
                     << "GridType: "
                     << Stuff::Common::Typename<LocalGridType>::value()
                     << "\n\n"
                     << "This was the original error: "
                     << ee.what());
    }
  } // ... create_grid_of_simplex(...)

  template <class MacroEntityType>
  static std::shared_ptr<LocalGridProviderType> create_grid_of_cube(const MacroEntityType& macro_entity)
  {
    const auto num_vertices = macro_entity.
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)

                                           subEntities(dimDomain);
#else
                                           template count<dimDomain>();
#endif
    FieldVector<ctype, dimDomain> lower_left(std::numeric_limits<ctype>::max());
    FieldVector<ctype, dimDomain> upper_right(std::numeric_limits<ctype>::min());
    for (int local_vertex_id = 0; local_vertex_id < num_vertices; ++local_vertex_id) {
      const auto vertex = macro_entity.template subEntity<dimDomain>(local_vertex_id)
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                                                                                .
#else
                                                                                ->
#endif
                                                                                geometry().center();
      for (size_t dd = 0; dd < dimDomain; ++dd) {
        lower_left[dd]  = std::min(lower_left[dd], vertex[dd]);
        upper_right[dd] = std::max(upper_right[dd], vertex[dd]);
      }
    }
    return std::make_shared<Stuff::Grid::Providers::Cube<LocalGridType>>(lower_left, upper_right, 1);
  } // ... create_grid_of_cube(...)

  void setup_local_grids()
  {
    const auto& macro_index_set = macro_leaf_view_.indexSet();
    for (auto&& macro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                               elements
#else
                               DSC::entityRange
#endif
                                               (macro_leaf_view_)) {
      auto macro_entity_index = macro_index_set.index(macro_entity);
      if (macro_entity.type().isSimplex())
        local_grids_[macro_entity_index] = create_grid_of_simplex(macro_entity);
      else if (macro_entity.type().isCube())
        local_grids_[macro_entity_index] = create_grid_of_cube(macro_entity);
      else
        DUNE_THROW(GridError, "Unknown entity.type() encountered: " << macro_entity.type());
    }
  } // ... setup_local_grids()

  template <class MacroIntersectionType>
  std::shared_ptr<GlueType> create_glue(const MacroEntityType& macro_entity, const MacroEntityType& macro_neighbor,
                                        const MacroIntersectionType& macro_intersection, const int local_entity_level,
                                        const int local_neighbor_level) const
  {
    assert(local_entity_level >= 0);
    assert(local_neighbor_level >= 0);
    const auto& local_entity_grid   = local_grid(macro_entity);
    const auto& local_neighbor_grid = local_grid(macro_neighbor);
    assert(local_entity_level <= local_entity_grid.grid().maxLevel());
    assert(local_neighbor_level <= local_neighbor_grid.grid().maxLevel());
    auto local_entity_view   = local_entity_grid.level_view(local_entity_level);
    auto local_neighbor_view = local_neighbor_grid.level_view(local_neighbor_level);
    // create descriptors, these can be discarded after creating the extractors
    auto entity_descriptor   = create_descriptor(local_entity_view, macro_intersection);
    auto neighbor_descriptor = create_descriptor(local_neighbor_view, macro_intersection);
    // create extractors and merger as shared_ptr, so glue will handle memory
    auto entity_extractor   = std::make_shared<LocalExtractorType>(local_entity_view, entity_descriptor);
    auto neighbor_extractor = std::make_shared<LocalExtractorType>(local_neighbor_view, neighbor_descriptor);
    auto contact_merger     = std::make_shared<GridGlue::ContactMerge<dimWorld, ctype>>(allowed_overlap_);
    // create glue
    auto glue = std::make_shared<GlueType>(entity_extractor, neighbor_extractor, contact_merger);
    glue->build();
    if (glue->size() == 0)
      DUNE_THROW(GridError, // clang-format off
                 "Something went wrong, the coupling glue is empty!\n"
                     << "   macro_entity "         << macro_leaf_view_.indexSet().index(macro_entity) << "\n"
                     << "   local_entity_level "   << local_entity_level << "\n"
                     << "   macro_neighbor "       << macro_leaf_view_.indexSet().index(macro_neighbor) << "\n"
                     << "   local_neighbor_level " << local_neighbor_level << "\n"); // clang-format on
    return glue;
  } // ... create_glue(...)

  void setup_glues(const bool allow_for_broken_orientation_of_coupling_intersections = false)
  {
    const auto& macro_index_set = macro_leaf_view_.indexSet();
    for (auto&& macro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                               elements
#else
                               DSC::entityRange
#endif
                                               (macro_leaf_view_)) {
      const auto macro_entity_index = macro_index_set.index(macro_entity);
      auto& entity_glues            = glues_[macro_entity_index];
      // walk the neighbors ...
      const auto macro_intersection_it_end = macro_leaf_view_.iend(macro_entity);
      for (auto macro_intersection_it = macro_leaf_view_.ibegin(macro_entity);
           macro_intersection_it != macro_intersection_it_end;
           ++macro_intersection_it) {
        const auto& macro_intersection = *macro_intersection_it;
        if (macro_intersection.neighbor() && !macro_intersection.boundary()) {
          const auto macro_neighbor_ptr   = macro_intersection.outside();
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
          const auto& macro_neighbor      = macro_neighbor_ptr;
#else
          const auto& macro_neighbor      = *macro_neighbor_ptr;
#endif
          const auto macro_neighbor_index = macro_index_set.index(macro_neighbor);
          for (auto local_entity_level : DSC::valueRange(local_grids_[macro_entity_index]->grid().maxLevel() + 1))
            for (auto local_neighbor_level : DSC::valueRange(local_grids_[macro_neighbor_index]->grid().maxLevel() + 1)) {
              auto glue = create_glue(macro_entity, macro_neighbor,
                                      macro_intersection,
                                      local_entity_level, local_neighbor_level);
              if (!allow_for_broken_orientation_of_coupling_intersections) {
                const size_t brocken_intersections = check_for_broken_coupling_intersections(*glue);
                if (brocken_intersections > 0)
                  DUNE_THROW(Exceptions::intersection_orientation_is_broken,
                             "The coupling glue between the grid views of\n"
                             << "  level " << local_entity_level << " on macro entity   "
                             << macro_leaf_view_.indexSet().index(macro_entity) << " and\n"
                             << "  level " << local_neighbor_level << " on macro neighbor "
                             << macro_leaf_view_.indexSet().index(macro_neighbor) << "\n"
                             << "contains\n"
                             << "  " << brocken_intersections << "/" << glue->size() << " intersections with wrong"
                             << "orientation!");
              }
              entity_glues[macro_neighbor_index][local_entity_level][local_neighbor_level] = glue;
            }
        }
      } // ... walk the neighbors
    }
  } // ... setup_glues(...)

  void prepare_global_grid()
  {
    if (global_grid_)
      return;
    const auto& macro_index_set = macro_leaf_view_.indexSet();
    std::vector<FieldVector<ctype, dimDomain>> vertices;
    std::vector<std::vector<std::vector<unsigned int>>> entity_to_vertex_ids(local_grids_.size());
    std::vector<std::vector<GeometryType>> geometry_types(local_grids_.size());
    // walk the grid for the first time
    for (auto&& macro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                               elements
#else
                               DSC::entityRange
#endif
                                               (macro_leaf_view_)) {
      const auto local_leaf_view = local_grid(macro_entity).leaf_view();
      const auto& local_index_set = local_leaf_view.indexSet();
      const size_t macro_index = macro_index_set.index(macro_entity);
      entity_to_vertex_ids[macro_index] = std::vector<std::vector<unsigned int>>(local_index_set.size(0));
      geometry_types[macro_index] = std::vector<GeometryType>(local_index_set.size(0));
      for (auto&& micro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                                 elements
#else
                                 DSC::entityRange
#endif
                                                 (local_leaf_view)) {
        const size_t micro_index = local_index_set.index(micro_entity);
        const auto num_vertices = micro_entity.
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)

                                                subEntities(dimDomain);
#else
                                                template count<dimDomain>();
#endif
        entity_to_vertex_ids[macro_index][micro_index] = std::vector<unsigned int>(num_vertices);
        geometry_types[macro_index][micro_index] = micro_entity.geometry().type();
        for (unsigned int local_vertex_id = 0; local_vertex_id < num_vertices; ++local_vertex_id) {
          const unsigned int global_vertex_id
              = find_insert_vertex(vertices,
                                   micro_entity.template subEntity<dimDomain>(local_vertex_id)
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                                                                                .
#else
                                                                                ->
#endif
                                                                                geometry().center());
          entity_to_vertex_ids[macro_index][micro_index][local_vertex_id] = global_vertex_id;
        }
      } // walk the local grid
    } // walk the macro grid
    GridFactory<LocalGridType> global_factory;
    for (const auto& vertex : vertices)
      global_factory.insertVertex(vertex);
    size_t II = 0;
    size_t JJ = 0;
    try {
      for (size_t ii = 0; ii < local_grids_.size(); ++ii, II = ii)
        for (size_t jj = 0; jj < entity_to_vertex_ids[ii].size(); ++jj, JJ = jj)
            global_factory.insertElement(geometry_types[ii][jj], entity_to_vertex_ids[ii][jj]);
    } catch (GridError& ee) {
      DUNE_THROW(GridError,
                 "It was not possible to insert an element into the grid factory!\n\n"
                 << "GridType: " << Stuff::Common::Typename<LocalGridType>::value() << "\n"
                 << "GeometryType: " << geometry_types[II][JJ] << "\n"
                 << "\n"
                 << "This was the original error: " << ee.what());
    } // try
    global_grid_ = DSC::make_unique<Stuff::Grid::Providers::Default<LocalGridType>>(global_factory.createGrid());

    // build maps of indices, relating the local to the global grid
    const auto global_grid_view = global_grid_->leaf_view();
    local_to_global_indices_ = DSC::make_unique<std::vector<std::vector<size_t>>>(macro_leaf_view_.indexSet().size(0));
    auto& local_to_global_indices = *local_to_global_indices_;
    global_to_local_indices_ = DSC::make_unique<std::vector<std::pair<size_t, size_t>>>(global_grid_view.indexSet().size(0));
    auto& global_to_local_indices = *global_to_local_indices_;
    // therefore
    // * create a search on the global view: if all is fine, we only need to walk that one once
    std::vector<FieldVector<ctype, dimDomain>> local_entity_center{FieldVector<ctype, dimDomain>(0.0)};
    auto global_search = DSG::make_entity_in_level_search(global_grid_view);
    // * walk the macro grid
    for (auto&& macro_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                               elements
#else
                               DSC::entityRange
#endif
                                               (macro_leaf_view_)) {
      const size_t subdomain = macro_leaf_view_.indexSet().index(macro_entity);
      const auto local_leaf_view = local_grid(macro_entity).leaf_view();
      const auto& local_index_set = local_leaf_view.indexSet();
      local_to_global_indices[subdomain] = std::vector<size_t>(local_index_set.size(0));
      // * walk the local grid
      for (auto&& local_entity :
#if DUNE_VERSION_NEWER(DUNE_GRID, 2, 4)
                                 elements
#else
                                 DSC::entityRange
#endif
                                                 (local_leaf_view)) {
        const size_t local_entity_index = local_index_set.index(local_entity);
        local_entity_center[0] = local_entity.geometry().center();
        const auto global_entity_ptr_unique_ptrs = global_search(local_entity_center);
        // the search has to be successfull, since the global grid has been constructed to exactly contain each local entity
        assert(global_entity_ptr_unique_ptrs.size() == 1);
        const auto& global_entity_ptr_unique_ptr = global_entity_ptr_unique_ptrs.at(0);
        assert(global_entity_ptr_unique_ptr);
        const auto& global_entity_ptr = *global_entity_ptr_unique_ptr;
        const auto& global_entity = *global_entity_ptr;
        const size_t global_entity_index = global_grid_view.indexSet().index(global_entity);
        // store information
        local_to_global_indices[subdomain][local_entity_index] = global_entity_index;
        global_to_local_indices[global_entity_index] = {subdomain, local_entity_index};
      } // * walk the local grid
    } // * walk the macro grid

  } // ... prepare_global_grid(...)

  size_t find_insert_vertex(std::vector<FieldVector<ctype, dimDomain>>& vertices,
                            FieldVector<ctype, dimDomain>&& vertex) const
  {
    // check if vertex is already contained
    for (size_t ii = 0; ii < vertices.size(); ++ii)
      if (DSC::FloatCmp::eq(vertex, vertices[ii]))
        return ii;
    // if not, add it
    vertices.emplace_back(std::move(vertex));
    return vertices.size() - 1;
  } // ... find_insert_vertex(...)

  MacroGridProviderType& macro_grid_;
  const ctype allowed_overlap_;
  MacroGridViewType macro_leaf_view_;
  std::vector<std::shared_ptr<LocalGridProviderType>> local_grids_;
  std::vector<std::map<size_t, std::map<int, std::map<int, std::shared_ptr<GlueType>>>>> glues_;
  std::map<size_t, std::map<int, std::vector<std::pair<MicroEntityPointerType, std::vector<int>>>>>
      macro_entity_to_local_level_to_boundary_entity_ptrs_with_local_intersections_;
  std::unique_ptr<LocalGridProviderType> global_grid_;
  std::unique_ptr<std::vector<std::vector<size_t>>> local_to_global_indices_;
  std::unique_ptr<std::vector<std::pair<size_t, size_t>>> global_to_local_indices_;
}; // class Glued

template <class MacroGridType, class LocalGridType>
class GluedVTKWriter
{
  typedef typename LocalGridType::LevelGridView LocalGridViewType;

  // we only need this class to access a protected pwrite method of VTKWriter
  class LocalVTKWriter
    : public VTKWriter<LocalGridViewType>
  {
    typedef VTKWriter<LocalGridViewType> BaseType;
  public:
    LocalVTKWriter(const LocalGridViewType& local_grid_view, const size_t subdomain, const size_t num_subdomains)
      : BaseType(local_grid_view)
      , commRank_(boost::numeric_cast<int>(subdomain))
      , commSize_(boost::numeric_cast<int>(num_subdomains))
    {}

    void write_locally(const std::string& name, VTK::OutputType ot)
    {
      BaseType::pwrite(name, "", "", ot, commRank_, commSize_);
    }

  private:
    const int commRank_;
    const int commSize_;
  }; // class LocalVTKWriter

public:
  typedef Glued<MacroGridType, LocalGridType> GluedGridType;

  GluedVTKWriter(const GluedGridType& glued_grid, const int local_level = -1)
    : glued_grid_(glued_grid)
    , local_levels_(glued_grid_.num_subdomains(), -1)
  {
    // set each local level to its respective max
    if (local_level < 0)
      for (auto&& macro_entity : DSC::entityRange(glued_grid_.macro_grid_view()))
        local_levels_[glued_grid_.subdomain(macro_entity)] = glued_grid_.max_local_level(macro_entity);
    prepare_local_vtk_writers();
  } // GluedVTKWriter(...)

  GluedVTKWriter(const GluedGridType& glued_grid, const std::vector<int>& local_levels)
    : glued_grid_(glued_grid)
    , local_levels_(local_levels)
  {
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      if (local_levels_[ss] < 0)
        local_levels_[ss] = glued_grid_.max_local_level(ss);
    prepare_local_vtk_writers();
  }

  template <class V>
  void addCellData(const std::vector<std::vector<V>>& vectors, const std::string& name, const int ncomps = 1)
  {
    if (vectors.size() != glued_grid_.num_subdomains())
      DUNE_THROW(Stuff::Exceptions::shapes_do_not_match,
                 "vectors.size(): " << vectors.size() << "\n"
                 << "glued_grid_.num_subdomains(): " << glued_grid_.num_subdomains());
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      local_vtk_writers_[ss]->addCellData(vectors[ss], name, ncomps);
  } // ... addCellData(...)

  template <class VTKFunctionType>
  void addCellData(const std::vector<std::shared_ptr<VTKFunctionType>>& functions)
  {
    if (functions.size() != glued_grid_.num_subdomains())
      DUNE_THROW(Stuff::Exceptions::shapes_do_not_match,
                 "funcitons.size(): " << functions.size() << "\n"
                 << "glued_grid_.num_subdomains(): " << glued_grid_.num_subdomains());
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      local_vtk_writers_[ss]->addCellData(functions[ss]);
  } // ... addCellData(...)

  template <class V>
  void addVertexData(const std::vector<std::vector<V>>& vectors, const std::string& name, const int ncomps = 1)
  {
    if (vectors.size() != glued_grid_.num_subdomains())
      DUNE_THROW(Stuff::Exceptions::shapes_do_not_match,
                 "vectors.size(): " << vectors.size() << "\n"
                 << "glued_grid_.num_subdomains(): " << glued_grid_.num_subdomains());
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      local_vtk_writers_[ss]->addVertexData(vectors[ss], name, ncomps);
  } // ... addVertexData(...)

  template <class VTKFunctionType>
  void addVertexData(const std::vector<std::shared_ptr<VTKFunctionType>>& functions)
  {
    if (functions.size() != glued_grid_.num_subdomains())
      DUNE_THROW(Stuff::Exceptions::shapes_do_not_match,
                 "funcitons.size(): " << functions.size() << "\n"
                 << "glued_grid_.num_subdomains(): " << glued_grid_.num_subdomains());
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      local_vtk_writers_[ss]->addVertexData(functions[ss]);
  } // ... addVertexData(...)

  void clear()
  {
    for (auto& local_vtk_writer : local_vtk_writers_)
      local_vtk_writer.clear();
  }

  void write(const std::string& name, VTK::OutputType type = VTK::ascii)
  {
    for (size_t ss = 0; ss < glued_grid_.num_subdomains(); ++ss)
      local_vtk_writers_[ss]->write_locally(name, type);
  }

private:
  void prepare_local_vtk_writers()
  {
    for (auto&& macro_entity : DSC::entityRange(glued_grid_.macro_grid_view())) {
      const size_t subdomain = glued_grid_.subdomain(macro_entity);
      local_vtk_writers_.emplace_back(
            new LocalVTKWriter(glued_grid_.local_grid(macro_entity).level_view(local_levels_[subdomain]),
                               subdomain,
                               glued_grid_.num_subdomains()));
    }
  } // ... prepare_local_vtk_writers(...)

  const GluedGridType& glued_grid_;
  std::vector<int> local_levels_;
  std::vector<std::unique_ptr<LocalVTKWriter>> local_vtk_writers_;
}; // class GluedVTKWriter

} // namespace Multiscale
} // namespace grid
} // namespace Dune

#endif // DUNE_GRID_MULTISCALE_GLUED_HH


/* Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Ross Linscott (rossklin@gmail.com)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *     Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in
 *     the documentation and/or other materials provided with the
 *     distribution.
 *
 *     The names of its contributors may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#if defined(NDEBUG)
#undef NDEBUG
#endif

#include <cstdlib>
#include <cmath>
#include <cstring>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#ifdef STANDALONE
#include <RInside.h>
#endif

#include "../inst/include/SimpleSDESampler.h"

//' LPoly system constructor
//'
//' Returns an XPtr to an lpoly_system_type object
//' @param cm Model coefficient matrix
//' @param trm Model term specification, as generated by lpoly_model_spec (each row represents the powers of the different factors in a term)
//' @export
// [[Rcpp::export]]
XPtr<lpoly_system_type> lpoly_make_system_xptr(NumericMatrix cm, NumericMatrix trm){

  if (cm.ncol() != trm.nrow()){
    Rcpp::Rcout << "lpoly_make_system: dimension mismatch: " << cm.ncol() << " != " << trm.nrow() << endl;
    exit(-1);
  }

  return XPtr<lpoly_system_type>(new lpoly_system_type(lpoly_evaluator(cm,trm), lpoly_jacobian(cm, trm)), true);
}

//' LPoly model matrix generator
//'
//' Builds the model matrix from the data using the lpoly_system_type object
//' @param lps XPtr to an lpoly_system_type object created by a call to lpoly_make_system
//' @param data One column per measurement variable
//' @export
// [[Rcpp::export]]
NumericMatrix lpoly_model_matrix(XPtr<lpoly_system_type> lps, NumericMatrix data){
  return lps -> first.build(data);
}

//' LPoly Jacobian
//'
//' Builds the model matrix from the data using the lpoly_system_type object
//' @param lps XPtr to an lpoly_system_type object created by a call to lpoly_make_system
//' @param state Single row matrix
//' @export
// [[Rcpp::export]]
NumericMatrix lpoly_compute_jacobian(XPtr<lpoly_system_type> lps, NumericMatrix state){
  uvector q(state.ncol());
  umatrix res;
  memcpy(&q(0), &state[0], q.size() * sizeof(double));
  lps -> second(q, res, 0);
  return as_r_matrix(res);
}

//' LPoly System Implicit SDE Simulator using NLOPT
//'
//' Simulates a trajectory to the SDE specified by *sys* and the noise level sigma, starting at the point start and integrating over times [*from*,*to*] on *steps + 1* time points.
//' @param sys lpoly_system_type XPtr object created with lpoly_make_system
//' @param sigma Amplitude of noise: scalar
//' @param start Initial position: n vector
//' @param from Initial time: scalar
//' @param to Final time: scalar
//' @param steps Number of points to take, s.t. dt = (from - to) / (steps + 1): integer
//' @export
// [[Rcpp::export]]
NumericMatrix lpoly_sde(XPtr<lpoly_system_type> sys
			, double sigma
			, NumericVector start
			, double from, double to, int steps
			, double x_tol = 0
			, const char* algorithm = "LBFGS") {

  const double dt = (to - from)/steps;
  lpoly_system s(sys);
  nlopt_stepper stepper(&s, dt, start.size(), sigma, steps, x_tol, algorithm);

  return lpoly_sde(stepper, start, from, to, steps, x_tol, algorithm);
}

//' LPoly System Implicit SDE Simulator using NLOPT, with precached noise data
//'
//' Simulates a trajectory to the SDE specified by *sys* and the noise level sigma, starting at the point start and integrating over times [*from*,*to*] on *steps + 1* time points.
//' @param sys lpoly_system_type XPtr object created with lpoly_make_system
//' @param noise Noise buffer (brownian motion normalised steps ~N(0,sigma^2))
//' @param start Initial position: n vector
//' @param from Initial time: scalar
//' @param to Final time: scalar
//' @param steps Number of points to take, s.t. dt = (from - to) / (steps + 1): integer
//' @export
// [[Rcpp::export]]
NumericMatrix lpoly_sde_precached(XPtr<lpoly_system_type> sys
				  , NumericMatrix noise
				  , NumericVector start
				  , double from
				  , double to
				  , int steps
				  , double x_tol = 0
				  , const char* algorithm = "LBFGS") {

  const double dt = (to - from)/steps;
  lpoly_system s(sys);
  nlopt_stepper stepper(&s, dt, start.size(), as_ublas_matrix(noise), x_tol, algorithm);

  return lpoly_sde(stepper, start, from, to, steps, x_tol, algorithm);
}

//' LPoly System Implicit SDE Simulator: time-wise averages
//'
//' Samples multiple times from lpoly_sde and returns the time-wise average
//' @param nrep Number of repetitions to average over: integer
//' @param sys lpoly_system_type XPtr object created with lpoly_make_system
//' @param sigma Amplitude of noise: scalar
//' @param start Initial position: n vector
//' @param from Initial time: scalar
//' @param to Final time: scalar
//' @param steps Number of points to take, s.t. dt = (from - to) / (steps + 1): integer
//' @export
// [[Rcpp::export]]
NumericMatrix lpoly_sde_averages( int nrep
				  , XPtr<lpoly_system_type> sys
				  , double sigma
				  , NumericVector start
				  , double from, double to, int steps){

  NumericMatrix result(steps+1, start.size());
  NumericMatrix buf;
  int i,j;
  double *p, *q;
  int smax = result.nrow() * result.ncol();

  p = &result(0,0);

  memset(p, 0, smax * sizeof(double));

  for (i = 0; i < nrep; i++){
    buf = lpoly_sde( sys
		     , sigma
		     , start
		     , from
		     , to
		     , steps);

    q = &buf(0,0);
    for (j = 0; j < smax; j++){
      p[j] += q[j];
    }
  }

  for (j = 0; j < smax; j++){
    p[j] /= nrep;
  }

  return result;
}


#ifndef STATIC_RTREE_HPP
#define STATIC_RTREE_HPP

#include "storage/io.hpp"
#include "util/bearing.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/deallocating_vector.hpp"
#include "util/exception.hpp"
#include "util/hilbert_value.hpp"
#include "util/integer_range.hpp"
#include "util/mmap_file.hpp"
#include "util/rectangle.hpp"
#include "util/typedefs.hpp"
#include "util/vector_view.hpp"
#include "util/web_mercator.hpp"

#include "osrm/coordinate.hpp"

#include "storage/shared_memory_ownership.hpp"

#include <boost/assert.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/iostreams/device/mapped_file.hpp>

#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_sort.h>

#include <algorithm>
#include <array>
#include <limits>
#include <memory>
#include <queue>
#include <string>
#include <vector>

// An extended alignment is implementation-defined, so use compiler attributes
// until alignas(LEAF_PAGE_SIZE) is compiler-independent.
#if defined(_MSC_VER)
#define ALIGNED(x) __declspec(align(x))
#elif defined(__GNUC__)
#define ALIGNED(x) __attribute__((aligned(x)))
#else
#define ALIGNED(x)
#endif

namespace osrm
{
namespace util
{

/***
 * Static RTree for serving nearest neighbour queries
 * // All coordinates are pojected first to Web Mercator before the bounding boxes
 * // are computed, this means the internal distance metric doesn not represent meters!
 */

template <class EdgeDataT,
          storage::Ownership Ownership = storage::Ownership::Container,
          std::uint32_t BRANCHING_FACTOR = 64,
          std::uint32_t LEAF_PAGE_SIZE = 4096>
class StaticRTree
{
    /**********************************************************
     * Example RTree construction:
     *
     * 30 elements (EdgeDataT objects)
     * LEAF_NODE_SIZE = 3
     * BRANCHING_FACTOR = 2
     *
     * 012 345 678 901 234 567 890 123 456 789  <- EdgeDataT objects in .fileIndex data, sorted by
     * \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/     Hilbert Code of the centroid coordinate
     * A   B   C   D   E   F   G   H   I   J   <- Everything from here down is a Rectangle in
     * \ /     \ /     \ /     \ /     \ /        .ramIndex
     *  K       L       M       N       O
     *   \     /         \     /       /
     *    \   /           \   /       /
     *     \ /             \ /       /
     *      P               Q       R
     *       \             /       /
     *        \           /       /
     *         \         /       /
     *          \       /       /
     *           \     /       /
     *            \   /       /
     *             \ /       /
     *              U       V
     *               \     /
     *                \   /
     *                 \ /
     *                  W
     *
     * Step 1 - objects 01234567... are sorted by Hilbert code (these are the line
     *          segments of the OSM roads)
     * Step 2 - we grab LEAF_NODE_SIZE of them at a time and create TreeNode A with a
     *          bounding-box that surrounds the first LEAF_NODE_SIZE objects
     * Step 2a- continue grabbing LEAF_NODE_SIZE objects, creating TreeNodes B,C,D,E...J
     *          until we run out of objects.  The last TreeNode J may not have
     *          LEAF_NODE_SIZE entries.  Our math later on caters for this.
     * Step 3 - Now start grabbing nodes from A..J in groups of BRANCHING_FACTOR,
     *          and create K..O with bounding boxes surrounding the groups of
     *          BRANCHING_FACTOR.  Again, O, the last entry, may have fewer than
     *          BRANCHING_FACTOR entries.
     * Step 3a- Repeat this process for each level, until you only create 1 TreeNode
     *          to contain its children (in this case, W).
     *
     * As we create TreeNodes, we append them to the m_search_tree vector.
     *
     * After this part of the building process, m_search_tree will contain TreeNode
     * objects in this order:
     *
     * ABCDEFGHIJ KLMNO PQR UV W
     * 10         5     3   2  1  <- number of nodes in the level
     *
     * In order to make our math easy later on, we reverse the whole array,
     * then reverse the nodes within each level:
     *
     *   Reversed:        W VU RQP ONMKL JIHGFEDCBA
     *   Levels reversed: W UV PQR KLMNO ABCDEFGHIJ
     *
     * We also now have the following information:
     *
     *   level sizes = {1,2,3,5,10}
     *
     * and we can calculate the array position the nodes for each level
     * start (based on the sum of the previous level sizes):
     *
     *   level starts = {0,1,3,6,11}
     *
     * Now, some basic math can be used to navigate around the tree.  See
     * the body of the `child_indexes` function for the details.
     *
     ***********************************************/
    template <typename T> using Vector = ViewOrVector<T, Ownership>;

  public:
    using Rectangle = RectangleInt2D;
    using EdgeData = EdgeDataT;
    using CoordinateList = Vector<util::Coordinate>;

    static_assert(LEAF_PAGE_SIZE >= sizeof(EdgeDataT), "page size is too small");
    static_assert(((LEAF_PAGE_SIZE - 1) & LEAF_PAGE_SIZE) == 0, "page size is not a power of 2");
    static constexpr std::uint32_t LEAF_NODE_SIZE = (LEAF_PAGE_SIZE / sizeof(EdgeDataT));

    struct CandidateSegment
    {
        Coordinate fixed_projected_coordinate;
        EdgeDataT data;
    };

    /**
     * Represents a node position somewhere in our tree.  This is purely a navigation
     * class used to find children of each node - the actual data for each node
     * is in the m_search_tree vector of TreeNode objects.
     */
    struct TreeIndex
    {
        TreeIndex() : level(0), offset(0) {}
        TreeIndex(std::uint32_t level_, std::uint32_t offset_) : level(level_), offset(offset_) {}
        std::uint32_t level;  // Which level of the tree is this node in
        std::uint32_t offset; // Which node on this level is this (0=leftmost)
    };

    /**
     * An actual node in the tree.  It's pretty minimal, we use the TreeIndex
     * classes to navigate around.  The TreeNode is packed into m_search_tree
     * in a specific order so we can calculate positions of children
     * (see the children_indexes function)
     */
    struct TreeNode
    {
        Rectangle minimum_bounding_rectangle;
    };

  private:
    /**
     * A lightweight wrapper for the Hilbert Code for each EdgeDataT object
     * A vector of these is used to sort the EdgeDataT input onto the
     * Hilbert Curve.
     * The sorting doesn't modify the original array, so this struct
     * maintains a pointer to the original index position (m_original_index)
     * so we can fetch the original data from the sorted position.
     */
    struct WrappedInputElement
    {
        explicit WrappedInputElement(const uint64_t _hilbert_value,
                                     const std::uint32_t _original_index)
            : m_hilbert_value(_hilbert_value), m_original_index(_original_index)
        {
        }

        WrappedInputElement() : m_hilbert_value(0), m_original_index(UINT_MAX) {}

        uint64_t m_hilbert_value;
        std::uint32_t m_original_index;

        inline bool operator<(const WrappedInputElement &other) const
        {
            return m_hilbert_value < other.m_hilbert_value;
        }
    };

    struct QueryCandidate
    {
        QueryCandidate(std::uint64_t squared_min_dist, TreeIndex tree_index)
            : squared_min_dist(squared_min_dist), tree_index(tree_index),
              segment_index(std::numeric_limits<std::uint32_t>::max())
        {
        }

        QueryCandidate(std::uint64_t squared_min_dist,
                       TreeIndex tree_index,
                       std::uint32_t segment_index,
                       const Coordinate &coordinate)
            : squared_min_dist(squared_min_dist), tree_index(tree_index),
              fixed_projected_coordinate(coordinate), segment_index(segment_index)
        {
        }

        inline bool is_segment() const
        {
            return segment_index != std::numeric_limits<std::uint32_t>::max();
        }

        inline bool operator<(const QueryCandidate &other) const
        {
            // Attn: this is reversed order. std::priority_queue is a
            // max pq (biggest item at the front)!
            return other.squared_min_dist < squared_min_dist;
        }

        std::uint64_t squared_min_dist;
        TreeIndex tree_index;
        Coordinate fixed_projected_coordinate;
        std::uint32_t segment_index;
    };

    // We use a const view type when we don't own the data, otherwise
    // we use a mutable type (usually becase we're building the tree)
    using TreeViewType = typename std::conditional<Ownership == storage::Ownership::View,
                                                   const Vector<const TreeNode>,
                                                   Vector<TreeNode>>::type;
    TreeViewType m_search_tree;

    // Reference to the actual lon/lat data we need for doing math
    const Vector<Coordinate> &m_coordinate_list;

    // Holds the number of TreeNodes in each level.
    // We always start with the root node, so
    // m_tree_level_sizes[0] should always be 1
    std::vector<std::uint64_t> m_tree_level_sizes;

    // Holds the start indexes of each level in m_search_tree
    std::vector<std::uint64_t> m_tree_level_starts;

    // mmap'd .fileIndex file
    boost::iostreams::mapped_file_source m_objects_region;
    // This is a view of the EdgeDataT data mmap'd from the .fileIndex file
    util::vector_view<const EdgeDataT> m_objects;

  public:
    StaticRTree(const StaticRTree &) = delete;
    StaticRTree &operator=(const StaticRTree &) = delete;

    // Construct a packed Hilbert-R-Tree with Kamel-Faloutsos algorithm [1]
    explicit StaticRTree(const std::vector<EdgeDataT> &input_data_vector,
                         const std::string &tree_node_filename,
                         const std::string &leaf_node_filename,
                         const Vector<Coordinate> &coordinate_list)
        : m_coordinate_list(coordinate_list)
    {
        const auto element_count = input_data_vector.size();
        std::vector<WrappedInputElement> input_wrapper_vector(element_count);

        // Step 1 - create a vector of Hilbert Code/original position pairs
        tbb::parallel_for(
            tbb::blocked_range<uint64_t>(0, element_count),
            [&input_data_vector, &input_wrapper_vector, this](
                const tbb::blocked_range<uint64_t> &range) {
                for (uint64_t element_counter = range.begin(), end = range.end();
                     element_counter != end;
                     ++element_counter)
                {
                    WrappedInputElement &current_wrapper = input_wrapper_vector[element_counter];
                    current_wrapper.m_original_index = element_counter;

                    EdgeDataT const &current_element = input_data_vector[element_counter];

                    // Get Hilbert-Value for centroid in mercartor projection
                    BOOST_ASSERT(current_element.u < m_coordinate_list.size());
                    BOOST_ASSERT(current_element.v < m_coordinate_list.size());

                    Coordinate current_centroid = coordinate_calculation::centroid(
                        m_coordinate_list[current_element.u], m_coordinate_list[current_element.v]);
                    current_centroid.lat = FixedLatitude{static_cast<std::int32_t>(
                        COORDINATE_PRECISION *
                        web_mercator::latToY(toFloating(current_centroid.lat)))};

                    current_wrapper.m_hilbert_value = GetHilbertCode(current_centroid);
                }
            });

        // sort the hilbert-value representatives
        tbb::parallel_sort(input_wrapper_vector.begin(), input_wrapper_vector.end());
        {
            storage::io::FileWriter leaf_node_file(leaf_node_filename,
                                                   storage::io::FileWriter::HasNoFingerprint);
            // Note, we can't just write everything in one go, because the input_data_vector
            // is not sorted by hilbert code, only the input_wrapper_vector is in the correct
            // order.  Instead, we iterate over input_wrapper_vector, copy the hilbert-indexed
            // entries from input_data_vector into a temporary contiguous array, then write
            // that array to disk.

            // Create the first level of TreeNodes - each bounding LEAF_NODE_COUNT EdgeDataT
            // objects.
            std::size_t wrapped_element_index = 0;
            while (wrapped_element_index < element_count)
            {
                TreeNode current_node;

                std::array<EdgeDataT, LEAF_NODE_SIZE> objects;
                std::uint32_t object_count = 0;

                // Loop over the next block of EdgeDataT, calculate the bounding box
                // for the block, and save the data to write to disk in the correct
                // order.
                for (std::uint32_t object_index = 0;
                     object_index < LEAF_NODE_SIZE && wrapped_element_index < element_count;
                     ++object_index, ++wrapped_element_index)
                {
                    const std::uint32_t input_object_index =
                        input_wrapper_vector[wrapped_element_index].m_original_index;
                    const EdgeDataT &object = input_data_vector[input_object_index];

                    object_count += 1;
                    objects[object_index] = object;

                    Coordinate projected_u{
                        web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.u]})};
                    Coordinate projected_v{
                        web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.v]})};

                    BOOST_ASSERT(std::abs(toFloating(projected_u.lon).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_u.lat).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_v.lon).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_v.lat).operator double()) <= 180.);

                    Rectangle rectangle;
                    rectangle.min_lon =
                        std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon));
                    rectangle.max_lon =
                        std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon));

                    rectangle.min_lat =
                        std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat));
                    rectangle.max_lat =
                        std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat));

                    BOOST_ASSERT(rectangle.IsValid());
                    current_node.minimum_bounding_rectangle.MergeBoundingBoxes(rectangle);
                }

                // Write out our EdgeDataT block to the leaf node file
                leaf_node_file.WriteFrom(objects.data(), object_count);

                m_search_tree.emplace_back(current_node);
            }

            // leaf_node_file wil be RAII closed at this point
        }

        // Should hold the number of nodes at the lowest level of the graph (closest
        // to the data)
        std::uint32_t nodes_in_previous_level = m_search_tree.size();
        m_tree_level_sizes.push_back(nodes_in_previous_level);

        // Now, repeatedly create levels of nodes that contain BRANCHING_FACTOR
        // nodes from the previous level.
        while (nodes_in_previous_level > 1)
        {
            auto previous_level_start_pos = m_search_tree.size() - nodes_in_previous_level;

            // We can calculate how many nodes will be in this level, we divide by
            // BRANCHING_FACTOR
            // and round up
            std::uint32_t nodes_in_current_level =
                std::ceil(static_cast<double>(nodes_in_previous_level) / BRANCHING_FACTOR);

            for (auto current_node_idx : irange<std::size_t>(0, nodes_in_current_level))
            {
                TreeNode parent_node;
                auto first_child_index =
                    current_node_idx * BRANCHING_FACTOR + previous_level_start_pos;
                auto last_child_index =
                    first_child_index +
                    std::min<std::size_t>(BRANCHING_FACTOR,
                                          nodes_in_previous_level -
                                              current_node_idx * BRANCHING_FACTOR);

                // Calculate the bounding box for BRANCHING_FACTOR nodes in the previous
                // level, then save that box as a new TreeNode in the new level.
                for (auto child_node_idx : irange<std::size_t>(first_child_index, last_child_index))
                {
                    parent_node.minimum_bounding_rectangle.MergeBoundingBoxes(
                        m_search_tree[child_node_idx].minimum_bounding_rectangle);
                }
                m_search_tree.emplace_back(parent_node);
            }
            nodes_in_previous_level = nodes_in_current_level;
            m_tree_level_sizes.push_back(nodes_in_previous_level);
        }
        // At this point, we've got our tree built, but the nodes are in a weird order.
        // Next thing we'll do is flip it around so that we don't end up with a lot of
        // `size - n` math later on.

        // Flip the tree so that the root node is at 0.
        // This just makes our math during search a bit more intuitive
        std::reverse(m_search_tree.begin(), m_search_tree.end());

        // Same for the level sizes - root node / base level is at 0
        std::reverse(m_tree_level_sizes.begin(), m_tree_level_sizes.end());

        // The first level starts at 0
        m_tree_level_starts = {0};
        // The remaining levels start at the partial sum of the preceeding level sizes
        std::partial_sum(m_tree_level_sizes.begin(),
                         m_tree_level_sizes.end() - 1,
                         std::back_inserter(m_tree_level_starts));

        // Now we have to flip the coordinates within each level so that math is easier
        // later on.  The workflow here is:
        // The initial order of tree nodes in the m_search_tree array is roughly:
        // 6789 345 12 0   (each block here is a level of the tree)
        // Then we reverse it and get:
        // 0 21 543 9876
        // Now the loop below reverses each level to give us the final result
        // 0 12 345 6789
        // This ordering keeps the position math easy to understand during later
        // searches
        for (auto i : irange<std::size_t>(0, m_tree_level_sizes.size()))
        {
            std::reverse(m_search_tree.begin() + m_tree_level_starts[i],
                         m_search_tree.begin() + m_tree_level_starts[i] + m_tree_level_sizes[i]);
        }

        // Write all the TreeNode data to disk
        {
            storage::io::FileWriter tree_node_file(tree_node_filename,
                                                   storage::io::FileWriter::GenerateFingerprint);

            std::uint64_t size_of_tree = m_search_tree.size();
            BOOST_ASSERT_MSG(0 < size_of_tree, "tree empty");

            tree_node_file.WriteOne(size_of_tree);
            tree_node_file.WriteFrom(m_search_tree);

            tree_node_file.WriteOne(static_cast<std::uint64_t>(m_tree_level_sizes.size()));
            tree_node_file.WriteFrom(m_tree_level_sizes);
        }

        m_objects = mmapFile<EdgeDataT>(leaf_node_filename, m_objects_region);
    }

    /**
     * Constructs an r-tree from already prepared files on disk (generated by the previous
     * constructor)
     */
    explicit StaticRTree(const boost::filesystem::path &node_file,
                         const boost::filesystem::path &leaf_file,
                         const Vector<Coordinate> &coordinate_list)
        : m_coordinate_list(coordinate_list)
    {
        storage::io::FileReader tree_node_file(node_file,
                                               storage::io::FileReader::VerifyFingerprint);

        const auto tree_size = tree_node_file.ReadElementCount64();
        m_search_tree.resize(tree_size);
        tree_node_file.ReadInto(m_search_tree);

        const auto levels_array_size = tree_node_file.ReadElementCount64();
        m_tree_level_sizes.resize(levels_array_size);
        tree_node_file.ReadInto(m_tree_level_sizes);

        // The first level always starts at 0
        m_tree_level_starts = {0};
        // The remaining levels start at the partial sum of the preceeding level sizes
        std::partial_sum(m_tree_level_sizes.begin(),
                         m_tree_level_sizes.end() - 1,
                         std::back_inserter(m_tree_level_starts));

        m_objects = mmapFile<EdgeDataT>(leaf_file, m_objects_region);
    }

    /**
     * Constructs an r-tree from blocks of memory loaded by someone else
     * (usually a shared memory block created by osrm-datastore)
     * These memory blocks basically just contain the files read into RAM,
     * excep the .fileIndex file always stays on disk, and we mmap() it as usual
     */
    explicit StaticRTree(const TreeNode *tree_node_ptr,
                         const uint64_t number_of_nodes,
                         const std::uint64_t *level_sizes_ptr,
                         const std::size_t number_of_levels,
                         const boost::filesystem::path &leaf_file,
                         const Vector<Coordinate> &coordinate_list)
        : m_search_tree(tree_node_ptr, number_of_nodes), m_coordinate_list(coordinate_list),
          m_tree_level_sizes(level_sizes_ptr, level_sizes_ptr + number_of_levels)
    {
        // The first level starts at 0
        m_tree_level_starts = {0};
        // The remaining levels start at the partial sum of the preceeding level sizes
        std::partial_sum(m_tree_level_sizes.begin(),
                         m_tree_level_sizes.end() - 1,
                         std::back_inserter(m_tree_level_starts));
        m_objects = mmapFile<EdgeDataT>(leaf_file, m_objects_region);
    }

    /* Returns all features inside the bounding box.
       Rectangle needs to be projected!*/
    std::vector<EdgeDataT> SearchInBox(const Rectangle &search_rectangle) const
    {
        const Rectangle projected_rectangle{
            search_rectangle.min_lon,
            search_rectangle.max_lon,
            toFixed(FloatLatitude{
                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.min_lat)))}),
            toFixed(FloatLatitude{
                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.max_lat)))})};
        std::vector<EdgeDataT> results;

        std::queue<TreeIndex> traversal_queue;
        traversal_queue.push(TreeIndex{});

        while (!traversal_queue.empty())
        {
            auto const current_tree_index = traversal_queue.front();
            traversal_queue.pop();

            // If we're at the bottom of the tree, we need to explore the
            // element array
            if (is_leaf(current_tree_index))
            {

                // Note: irange is [start,finish), so we need to +1 to make sure we visit the
                // last
                for (const auto current_child_index : child_indexes(current_tree_index))
                {
                    const auto &current_edge = m_objects[current_child_index];

                    // we don't need to project the coordinates here,
                    // because we use the unprojected rectangle to test against
                    const Rectangle bbox{std::min(m_coordinate_list[current_edge.u].lon,
                                                  m_coordinate_list[current_edge.v].lon),
                                         std::max(m_coordinate_list[current_edge.u].lon,
                                                  m_coordinate_list[current_edge.v].lon),
                                         std::min(m_coordinate_list[current_edge.u].lat,
                                                  m_coordinate_list[current_edge.v].lat),
                                         std::max(m_coordinate_list[current_edge.u].lat,
                                                  m_coordinate_list[current_edge.v].lat)};

                    // use the _unprojected_ input rectangle here
                    if (bbox.Intersects(search_rectangle))
                    {
                        results.push_back(current_edge);
                    }
                }
            }
            else
            {
                BOOST_ASSERT(current_tree_index.level + 1 < m_tree_level_starts.size());

                for (const auto child_index : child_indexes(current_tree_index))
                {
                    const auto &child_rectangle =
                        m_search_tree[child_index].minimum_bounding_rectangle;

                    if (child_rectangle.Intersects(projected_rectangle))
                    {
                        traversal_queue.push(TreeIndex(
                            current_tree_index.level + 1,
                            child_index - m_tree_level_starts[current_tree_index.level + 1]));
                    }
                }
            }
        }
        return results;
    }

    // Override filter and terminator for the desired behaviour.
    std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate,
                                   const std::size_t max_results) const
    {
        return Nearest(input_coordinate,
                       [](const CandidateSegment &) { return std::make_pair(true, true); },
                       [max_results](const std::size_t num_results, const CandidateSegment &) {
                           return num_results >= max_results;
                       });
    }

    // Override filter and terminator for the desired behaviour.
    template <typename FilterT, typename TerminationT>
    std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate,
                                   const FilterT filter,
                                   const TerminationT terminate) const
    {
        std::vector<EdgeDataT> results;
        auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
        Coordinate fixed_projected_coordinate{projected_coordinate};
        // initialize queue with root element
        std::priority_queue<QueryCandidate> traversal_queue;
        traversal_queue.push(QueryCandidate{0, TreeIndex{}});

        while (!traversal_queue.empty())
        {
            QueryCandidate current_query_node = traversal_queue.top();
            traversal_queue.pop();

            const TreeIndex &current_tree_index = current_query_node.tree_index;
            if (!current_query_node.is_segment())
            { // current object is a tree node
                if (is_leaf(current_tree_index))
                {
                    ExploreLeafNode(current_tree_index,
                                    fixed_projected_coordinate,
                                    projected_coordinate,
                                    traversal_queue);
                }
                else
                {
                    ExploreTreeNode(
                        current_tree_index, fixed_projected_coordinate, traversal_queue);
                }
            }
            else
            { // current candidate is an actual road segment
                // We deliberatly make a copy here, we mutate the value below
                auto edge_data = m_objects[current_query_node.segment_index];
                const auto &current_candidate =
                    CandidateSegment{current_query_node.fixed_projected_coordinate, edge_data};

                // to allow returns of no-results if too restrictive filtering, this needs to be
                // done here even though performance would indicate that we want to stop after
                // adding the first candidate
                if (terminate(results.size(), current_candidate))
                {
                    break;
                }

                auto use_segment = filter(current_candidate);
                if (!use_segment.first && !use_segment.second)
                {
                    continue;
                }
                edge_data.forward_segment_id.enabled &= use_segment.first;
                edge_data.reverse_segment_id.enabled &= use_segment.second;

                // store phantom node in result vector
                results.push_back(std::move(edge_data));
            }
        }

        return results;
    }

  private:
    /**
     * Iterates over all the objects in a leaf node and inserts them into our
     * search priority queue.  The speed of this function is very much governed
     * by the value of LEAF_NODE_SIZE, as we'll calculate the euclidean distance
     * for every child of each leaf node visited.
     */
    template <typename QueueT>
    void ExploreLeafNode(const TreeIndex &leaf_id,
                         const Coordinate &projected_input_coordinate_fixed,
                         const FloatCoordinate &projected_input_coordinate,
                         QueueT &traversal_queue) const
    {
        // Check that we're actually looking at the bottom level of the tree
        BOOST_ASSERT(is_leaf(leaf_id));

        for (const auto i : child_indexes(leaf_id))
        {
            const auto &current_edge = m_objects[i];

            const auto projected_u = web_mercator::fromWGS84(m_coordinate_list[current_edge.u]);
            const auto projected_v = web_mercator::fromWGS84(m_coordinate_list[current_edge.v]);

            FloatCoordinate projected_nearest;
            std::tie(std::ignore, projected_nearest) =
                coordinate_calculation::projectPointOnSegment(
                    projected_u, projected_v, projected_input_coordinate);

            const auto squared_distance = coordinate_calculation::squaredEuclideanDistance(
                projected_input_coordinate_fixed, projected_nearest);
            // distance must be non-negative
            BOOST_ASSERT(0. <= squared_distance);
            BOOST_ASSERT(i < std::numeric_limits<std::uint32_t>::max());
            traversal_queue.push(QueryCandidate{squared_distance,
                                                leaf_id,
                                                static_cast<std::uint32_t>(i),
                                                Coordinate{projected_nearest}});
        }
    }

    /**
     * Iterates over all the children of a TreeNode and inserts them into the search
     * priority queue using their distance from the search coordinate as the
     * priority metric.
     * The closests distance to a box from our point is also the closest distance
     * to the closest line in that box (assuming the boxes hug their contents).
     */
    template <class QueueT>
    void ExploreTreeNode(const TreeIndex &parent,
                         const Coordinate &fixed_projected_input_coordinate,
                         QueueT &traversal_queue) const
    {
        // Figure out which_id level the parent is on, and it's offset
        // in that level.
        // Check that we're actually looking at the bottom level of the tree
        BOOST_ASSERT(!is_leaf(parent));

        for (const auto child_index : child_indexes(parent))
        {
            const auto &child = m_search_tree[child_index];

            const auto squared_lower_bound_to_element =
                child.minimum_bounding_rectangle.GetMinSquaredDist(
                    fixed_projected_input_coordinate);

            traversal_queue.push(QueryCandidate{
                squared_lower_bound_to_element,
                TreeIndex(parent.level + 1, child_index - m_tree_level_starts[parent.level + 1])});
        }
    }

    /**
     * Calculates the absolute position of child data in our packed data
     * vectors.
     *
     * when given a TreeIndex that is a leaf node (i.e. at the bottom of the tree),
     * this function returns indexes valid for `m_objects`
     *
     * otherwise, the indexes are to be used with m_search_tree to iterate over
     * the children of `parent`
     *
     * This function assumes we pack nodes as described in the big comment
     * at the top of this class.  All nodes are fully filled except for the last
     * one in each level.
     */
    range<std::size_t> child_indexes(const TreeIndex &parent) const
    {
        // If we're looking at a leaf node, the index is from 0 to m_objects.size(),
        // there is only 1 level of object data in the m_objects array
        if (is_leaf(parent))
        {
            const std::uint64_t first_child_index = parent.offset * LEAF_NODE_SIZE;
            const std::uint64_t end_child_index = std::min(
                first_child_index + LEAF_NODE_SIZE, static_cast<std::uint64_t>(m_objects.size()));

            BOOST_ASSERT(first_child_index < std::numeric_limits<std::uint32_t>::max());
            BOOST_ASSERT(end_child_index < std::numeric_limits<std::uint32_t>::max());
            BOOST_ASSERT(end_child_index <= m_objects.size());

            return irange<std::size_t>(first_child_index, end_child_index);
        }
        else
        {
            const std::uint64_t first_child_index =
                m_tree_level_starts[parent.level + 1] + parent.offset * BRANCHING_FACTOR;

            const std::uint64_t end_child_index = std::min(
                first_child_index + BRANCHING_FACTOR,
                m_tree_level_starts[parent.level + 1] + m_tree_level_sizes[parent.level + 1]);
            BOOST_ASSERT(first_child_index < std::numeric_limits<std::uint32_t>::max());
            BOOST_ASSERT(end_child_index < std::numeric_limits<std::uint32_t>::max());
            BOOST_ASSERT(end_child_index <= m_search_tree.size());
            BOOST_ASSERT(end_child_index <= m_tree_level_starts[parent.level + 1] +
                                                m_tree_level_sizes[parent.level + 1]);
            return irange<std::size_t>(first_child_index, end_child_index);
        }
    }

    bool is_leaf(const TreeIndex &treeindex) const
    {
        return treeindex.level == m_tree_level_starts.size() - 1;
    }
};

//[1] "On Packing R-Trees"; I. Kamel, C. Faloutsos; 1993; DOI: 10.1145/170088.170403
//[2] "Nearest Neighbor Queries", N. Roussopulos et al; 1995; DOI: 10.1145/223784.223794
//[3] "Distance Browsing in Spatial Databases"; G. Hjaltason, H. Samet; 1999; ACM Trans. DB Sys
// Vol.24 No.2, pp.265-318
}
}

#endif // STATIC_RTREE_HPP