// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef KDBVH_H_INCLUDED
#define KDBVH_H_INCLUDED

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <algorithm>
#include <queue>

#include "../BVHLeaf.hpp"

namespace Eigen {

namespace internal {

//internal pair class for the BVH--used instead of std::pair because of alignment
template<typename Scalar, int Dim>
struct vector_int_pair
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
  typedef Eigen::Matrix<Scalar, Dim, 1> VectorType;

  vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}

  VectorType first;
  int second;
};

//these templates help the tree initializer get the bounding boxes either from a provided
//iterator range or using bounding_box in a unified way
template<typename ObjectList, typename VolumeList, typename BoxIter>
struct get_boxes_helper {
  void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
  {
    outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
    eigen_assert(outBoxes.size() == objects.size());
  }
};

template<typename ObjectList, typename VolumeList>
struct get_boxes_helper<ObjectList, VolumeList, int> {
  void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
  {
    outBoxes.reserve(objects.size());
    for(int i = 0; i < (int)objects.size(); ++i)
      outBoxes.push_back(bounding_box(objects[i]));
  }
};

} // end namespace internal

/** \class KdBVH
 *  \brief A simple bounding volume hierarchy based on AlignedBox
 *
 *  \param _Scalar The underlying scalar type of the bounding boxes
 *  \param _Dim The dimension of the space in which the hierarchy lives
 *  \param _Object The object type that lives in the hierarchy.  It must have value semantics.  Either bounding_box(_Object) must
 *                 be defined and return an AlignedBox<_Scalar, _Dim> or bounding boxes must be provided to the tree initializer.
 *
 *  This class provides a simple (as opposed to optimized) implementation of a bounding volume hierarchy analogous to a Kd-tree.
 *  Given a sequence of objects, it computes their bounding boxes, constructs a Kd-tree of their centers
 *  and builds a BVH with the structure of that Kd-tree.  When the elements of the tree are too expensive to be copied around,
 *  it is useful for _Object to be a pointer.
 */
template<typename _Scalar, int _Dim, typename _Object> class KdBVH
{
public:
  enum { Dim = _Dim };
  typedef _Object Object;
  typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
  typedef _Scalar Scalar;
  //typedef AlignedBox<Scalar, Dim> Volume;
  typedef mcl::BoundingBox<Scalar, Dim> Volume;
  typedef std::vector<Volume, Eigen::aligned_allocator<Volume> > VolumeList;
  typedef int Index;
  typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors
  typedef const Object *ObjectIterator;

  KdBVH() {}

  /** Given an iterator range over \a Object references, constructs the BVH.  Requires that bounding_box(Object) return a Volume. */
  template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type

  /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, constructs the BVH */
  template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }

  /** Given an iterator range over \a Object references, constructs the BVH, overwriting whatever is in there currently.
    * Requires that bounding_box(Object) return a Volume. */
  template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }

  /** Given an iterator range over \a Object references and an iterator range over their bounding boxes,
    * constructs the BVH, overwriting whatever is in there currently. */
  template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
  {
    objects.clear();
    boxes.clear();
    children.clear();

    objects.insert(objects.end(), begin, end);
    int n = static_cast<int>(objects.size());

    if(n < 2)
      return; //if we have at most one object, we don't need any internal nodes

    VolumeList objBoxes;
    VIPairList objCenters;

    //compute the bounding boxes depending on BIter type
    internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);

    objCenters.reserve(n);
    boxes.reserve(n - 1);
    children.reserve(2 * n - 2);

    for(int i = 0; i < n; ++i)
      objCenters.push_back(VIPair(objBoxes[i].center(), i));

    build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm

    ObjectList tmp(n);
    tmp.swap(objects);
    for(int i = 0; i < n; ++i)
      objects[i] = tmp[objCenters[i].second];
  }

  /** \returns the index of the root of the hierarchy */
  inline Index getRootIndex() const { return (int)boxes.size() - 1; }

  /** Given an \a index of a node, on exit, \a outVBegin and \a outVEnd range over the indices of the volume children of the node
    * and \a outOBegin and \a outOEnd range over the object children of the node */
  EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
                                       ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
  { //inlining this function should open lots of optimization opportunities to the compiler
    if(index < 0) {
      outVBegin = outVEnd;
      if(!objects.empty())
        outOBegin = &(objects[0]);
      outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object
      return;
    }

    int numBoxes = static_cast<int>(boxes.size());

    int idx = index * 2;
    if(children[idx + 1] < numBoxes) { //second index is always bigger
      outVBegin = &(children[idx]);
      outVEnd = outVBegin + 2;
      outOBegin = outOEnd;
    }
    else if(children[idx] >= numBoxes) { //if both children are objects
      outVBegin = outVEnd;
      outOBegin = &(objects[children[idx] - numBoxes]);
      outOEnd = outOBegin + 2;
    } else { //if the first child is a volume and the second is an object
      outVBegin = &(children[idx]);
      outVEnd = outVBegin + 1;
      outOBegin = &(objects[children[idx + 1] - numBoxes]);
      outOEnd = outOBegin + 1;
    }
  }

  /** \returns the bounding box of the node at \a index */
  inline const Volume &getVolume(Index index) const
  {
    return boxes[index];
  }

private:
  typedef internal::vector_int_pair<Scalar, Dim> VIPair;
  typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
  typedef Matrix<Scalar, Dim, 1> VectorType;
  struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension
  {
    VectorComparator(int inDim) : dim(inDim) {}
    inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
    int dim;
  };

  //Build the part of the tree between objects[from] and objects[to] (not including objects[to]).
  //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs
  //the two halves, and adds their parent node.  TODO: a cache-friendlier layout
  void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
  {
    eigen_assert(to - from > 1);
    if(to - from == 2) {
      boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
      children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes
      children.push_back(from + (int)objects.size());
    }
    else if(to - from == 3) {
      int mid = from + 2;
      std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
                        objCenters.begin() + to, VectorComparator(dim)); //partition
      build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
      int idx1 = (int)boxes.size() - 1;
      boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
      children.push_back(idx1);
      children.push_back(mid + (int)objects.size() - 1);
    }
    else {
      int mid = from + (to - from) / 2;
      nth_element(objCenters.begin() + from, objCenters.begin() + mid,
                  objCenters.begin() + to, VectorComparator(dim)); //partition
      build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
      int idx1 = (int)boxes.size() - 1;
      build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
      int idx2 = (int)boxes.size() - 1;
      boxes.push_back(boxes[idx1].merged(boxes[idx2]));
      children.push_back(idx1);
      children.push_back(idx2);
    }
  }

  std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects.
  VolumeList boxes;
  ObjectList objects;
};

} // end namespace Eigen

#endif //KDBVH_H_INCLUDED


#ifndef GRAPHS_H_
#define GRAPHS_H_

#include <cstddef>
#include <vector>
#include <Eigen/Sparse>
#include "multi_array.hpp"

namespace graphs {

typedef std::size_t NodeID;
typedef std::size_t Index;

template<typename TReal=void>
struct EdgeTail {

  EdgeTail() {
    source = 0;
    weight = 1;
  }

  EdgeTail(NodeID source_node, TReal source_weight) :
      source(source_node), weight(source_weight) {}

  NodeID source;
  TReal weight;
};

template<>
struct EdgeTail<void> {

  EdgeTail() {
    source = 0;
  }

  EdgeTail(NodeID source_node) :
      source(source_node) {}

  NodeID source;
};

template<typename TReal=void>
class PredecessorGraph {
  public:
    typedef std::size_t NodeID;
    typedef std::size_t Index;

    /*
     * When building from an edge list it is assumed nodes are labelled
     * contiguously starting from 0 to num_nodes-1
     */
    PredecessorGraph() : PredecessorGraph(0) {
    }

    PredecessorGraph(NodeID num_nodes)
        : num_nodes_(num_nodes) {
      graph_.resize(num_nodes);
      num_edges_ = 0;
      for (NodeID iii = 0; iii < num_nodes; ++iii) {
        graph_[iii].resize(0);
      }
    }

    PredecessorGraph(const std::vector< std::vector<EdgeTail<TReal>> >& graph) : graph_(graph) {
      num_nodes_ = graph_.size();
      num_edges_ = CalcNumEdges();
    }

    PredecessorGraph(const PredecessorGraph<TReal>& other) : graph_(other.graph_),
        num_nodes_(other.num_nodes_), num_edges_(other.num_edges_) {}

    PredecessorGraph(PredecessorGraph<TReal>&& other) : graph_(std::move(other.graph_)),
        num_nodes_(std::move(other.num_nodes_)),
        num_edges_(std::move(other.num_edges_)) {}

    PredecessorGraph<TReal>& operator=(const PredecessorGraph<TReal>& other) {
      graph_ = other.graph_;
      num_nodes_ = other.num_nodes_;
      num_edges_ = other.num_edges_;
      return *this;
    }

    PredecessorGraph<TReal>& operator=(PredecessorGraph<TReal>&& other) {
      graph_ = std::move(other.graph_);
      num_nodes_ = std::move(other.num_nodes_);
      num_edges_ = std::move(other.num_edges_);
      return *this;
    }

    std::size_t CalcNumEdges() const {
      std::size_t sum = 0;
      for (NodeID iii = 0; iii < graph_.size(); iii++) {
        sum += graph_[iii].size();
      }

      return sum;
    }

    std::size_t NumEdges() const {
      return num_edges_;
    }

    std::size_t NumNodes() const {
      return num_nodes_;
    }

    const std::vector<EdgeTail<TReal>>& Predecessors(NodeID node) const {
      return graph_[node];
    }

    void AddEdge(NodeID source, NodeID target, TReal weight=1) {
      // It is assumed the Node should exist, so new nodes are added
      // up to the NodeID of the source or target
      if (source >= num_nodes_) {
        std::size_t num_new_nodes = source - num_nodes_ + 1;
        for (Index iii = 0; iii < num_new_nodes; ++iii) {
          graph_.push_back(std::vector<EdgeTail<TReal>>(0));
          ++num_nodes_;
        }
      }

      if (target >= num_nodes_) {
        std::size_t num_new_nodes = target - num_nodes_ + 1;
        for (Index iii = 0; iii < num_new_nodes; ++iii) {
          graph_.push_back(std::vector<EdgeTail<TReal>>(0));
          ++num_nodes_;
        }
      }

      graph_[target].push_back(EdgeTail<TReal>(source, weight));
      ++num_edges_;
    }

  protected:
    std::vector< std::vector<EdgeTail<TReal>> > graph_;
    std::size_t num_nodes_;
    std::size_t num_edges_;
};

template<>
class PredecessorGraph<void> {
  public:
    typedef std::size_t NodeID;
    typedef std::size_t Index;

    /*
     * When building from an edge list it is assumed nodes are labelled
     * contiguously starting from 0 to num_nodes-1
     */
    PredecessorGraph() : PredecessorGraph(0) {
    }

    PredecessorGraph(NodeID num_nodes)
        : num_nodes_(num_nodes) {
      graph_.resize(num_nodes);
      num_edges_ = 0;
      for (NodeID iii = 0; iii < num_nodes; ++iii) {
        graph_[iii].resize(0);
      }
    }

    PredecessorGraph(const std::vector< std::vector<EdgeTail<>> >& graph) : graph_(graph) {
      num_nodes_ = graph_.size();
      num_edges_ = CalcNumEdges();
    }

    PredecessorGraph(const PredecessorGraph& other) : graph_(other.graph_),
        num_nodes_(other.num_nodes_), num_edges_(other.num_edges_) {}

    PredecessorGraph(PredecessorGraph&& other) : graph_(std::move(other.graph_)),
        num_nodes_(std::move(other.num_nodes_)),
        num_edges_(std::move(other.num_edges_)) {}

    PredecessorGraph& operator=(const PredecessorGraph& other) {
      graph_ = other.graph_;
      num_nodes_ = other.num_nodes_;
      num_edges_ = other.num_edges_;
      return *this;
    }

    PredecessorGraph& operator=(PredecessorGraph&& other) {
      graph_ = std::move(other.graph_);
      num_nodes_ = std::move(other.num_nodes_);
      num_edges_ = std::move(other.num_edges_);
      return *this;
    }

    std::size_t CalcNumEdges() const {
      std::size_t sum = 0;
      for (NodeID iii = 0; iii < graph_.size(); iii++) {
        sum += graph_[iii].size();
      }

      return sum;
    }

    std::size_t NumEdges() const {
      return num_edges_;
    }

    std::size_t NumNodes() const {
      return num_nodes_;
    }

    const std::vector<EdgeTail<>>& Predecessors(NodeID node) const {
      return graph_[node];
    }

    // Automatically resizes graph if new nodes are introduced
    void AddEdge(NodeID source, NodeID target) {
      // It is assumed the Node should exist, so new nodes are added
      // up to the NodeID of the source or target
      if (source >= num_nodes_) {
        std::size_t num_new_nodes = source - num_nodes_ + 1;
        for (Index iii = 0; iii < num_new_nodes; ++iii) {
          graph_.push_back(std::vector<EdgeTail<>>(0));
          ++num_nodes_;
        }
      }

      if (target >= num_nodes_) {
        std::size_t num_new_nodes = target - num_nodes_ + 1;
        for (Index iii = 0; iii < num_new_nodes; ++iii) {
          graph_.push_back(std::vector<EdgeTail<>>(0));
          ++num_nodes_;
        }
      }

      graph_[target].push_back(EdgeTail<>(source));
      ++num_edges_;
    }

  protected:
    std::vector< std::vector<EdgeTail<>> > graph_;
    std::size_t num_nodes_;
    std::size_t num_edges_;
};

/*
 * edge list may exclude end nodes if they aren't connected, so it is
 * insufficient to just pass a node list.
 * First dimension is NumEdges, second dimension is [source, target]
 * If there are weights then pass weight array (must be same size as edges)
 */

template<typename Integer, template<typename, Index> class multi>
PredecessorGraph<> ConvertEdgeListToPredecessorGraph(const multi<Integer, 2>& edge_list) {
  PredecessorGraph<> graph = PredecessorGraph<>();
  const multi_array::ArrayView<Integer, 2> edge_view = edge_list.accessor();
  for (Index iii = 0; iii < edge_view.extent(0); ++iii) {
    graph.AddEdge(edge_view[iii][0], edge_view[iii][1]);
  }

  return graph;
}

template<typename Integer, typename TReal, template<typename, Index> class multi>
PredecessorGraph<TReal> ConvertEdgeListToPredecessorGraph(const multi<Integer, 2>& edge_list,
    const multi<TReal, 1>& weights) {
  PredecessorGraph<TReal> graph = PredecessorGraph<TReal>();
  const multi_array::ArrayView<Integer, 2> edge_view = edge_list.accessor();
  const multi_array::ArrayView<TReal, 1> weight_view = weights.accessor();
  for (Index iii = 0; iii < edge_view.extent(0); ++iii) {
    graph.AddEdge(edge_view[iii][0], edge_view[iii][1], weight_view[iii]);
  }

  return graph;
}

/*
 * Converts an edge list into a sparse matrix. The tail_size, is the number
 * of nodes that act as the source of the links, and head_size is the number
 * of nodes that act as the sink of the links. For a standard graph this is
 * just the total number of nodes in the graph, else if the graph is
 * bipartite, then one will be larger than the other.
 * For edge list, the order of the list should be row-major
 * with shape Ex2 with major axis as the edge with (tail, head) so that
 * X[edge#][0]=tail, X[edge#][1]=head
 *
 * Because Eigen uses Fortran's column major format, the ordering will be
 * reversed in order to preserve the proper direction of the links in the
 * sparse matrix.
 *
 * The resulting sparse matrix shape is: Head x Tail
 * Where Tail == # cols, and Head == # rows
 */
template<typename TReal, template<typename, Index> class multi, typename Integer>
Eigen::SparseMatrix<TReal> ConvertEdgeListToSparseMatrix(const multi<Integer, 2>& edge_list,
                                                         const int num_tail_nodes,
                                                         const int num_head_nodes) {

  const auto edge_view = edge_list.accessor();
  std::vector<Eigen::Triplet<TReal>> matrix_elements(edge_view.extent(0));
  for (std::size_t i = 0; i < edge_view.extent(0); ++i) {
    matrix_elements[i] = Eigen::Triplet<TReal>(static_cast<int>(edge_view[i][1]),
                                               static_cast<int>(edge_view[i][0]), 1);
  }
  Eigen::SparseMatrix<TReal> graph(num_head_nodes, num_tail_nodes);
  graph.setFromTriplets(matrix_elements.begin(), matrix_elements.end());

  return graph;
};

template<typename TReal, template<typename, Index> class MultiArray2D,
         template<typename, Index> class MultiArray1D, typename Integer>
Eigen::SparseMatrix<TReal> ConvertEdgeListToSparseMatrix(const MultiArray2D<Integer, 2>& edge_list,
                                                         const int num_tail_nodes,
                                                         const int num_head_nodes,
                                                         const MultiArray1D<TReal, 1>& weights) {

  const auto edge_view = edge_list.accessor();
  const auto weight_view = weights.accessor();
  std::vector<Eigen::Triplet<TReal>> matrix_elements(edge_view.extent(0));
  for (std::size_t i = 0; i < edge_view.extent(0); ++i) {
    matrix_elements[i] = Eigen::Triplet<TReal>(static_cast<int>(edge_view[i][1]),
                                               static_cast<int>(edge_view[i][0]),
                                               weight_view[i]);
  }
  Eigen::SparseMatrix<TReal> graph(num_head_nodes, num_tail_nodes);
  graph.setFromTriplets(matrix_elements.begin(), matrix_elements.end());

  return graph;
};

} // End graphs namespace

#endif /* GRAPHS_H_ */


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <algorithm>
#include <map>
#include <boost/pending/stringtok.hpp>
#include <boost/utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/depth_first_search.hpp>

template <class Distance>
class calc_distance_visitor : public boost::bfs_visitor<>
{
public:
  calc_distance_visitor(Distance d) : distance(d) { }

  template <class Graph>
  void tree_edge(typename boost::graph_traits<Graph>::edge_descriptor e,
                 Graph& g)
  {
    typename boost::graph_traits<Graph>::vertex_descriptor u, v;
    u = boost::source(e, g);
    v = boost::target(e, g);
    distance[v] = distance[u] + 1;
  }
private:
  Distance distance;
};

template <class VertexNameMap, class DistanceMap>
class print_tree_visitor : public boost::dfs_visitor<>
{
public:
  print_tree_visitor(VertexNameMap n, DistanceMap d) : name(n), distance(d) { }
  template <class Graph>
  void
  discover_vertex(typename boost::graph_traits<Graph>::vertex_descriptor v,
            Graph&)
  {
    typedef typename boost::property_traits<DistanceMap>::value_type Dist;
    // indentation based on depth
    for (Dist i = 0; i < distance[v]; ++i)
      std::cout << "  ";
    std::cout << name[v] << std::endl;
  }

  template <class Graph>
  void tree_edge(typename boost::graph_traits<Graph>::edge_descriptor e,
                 Graph& g)
  {
    distance[boost::target(e, g)] = distance[boost::source(e, g)] + 1;
  }

private:
  VertexNameMap name;
  DistanceMap distance;
};

int
main()
{
  using namespace boost;

  std::ifstream datafile("./boost_web.dat");
  if (!datafile) {
    std::cerr << "No ./boost_web.dat file" << std::endl;
    return -1;
  }

  //===========================================================================
  // Declare the graph type and object, and some property maps.

  typedef adjacency_list<vecS, vecS, directedS,
    property<vertex_name_t, std::string,
      property<vertex_color_t, default_color_type> >,
    property<edge_name_t, std::string, property<edge_weight_t, int> >
  > Graph;

  typedef graph_traits<Graph> Traits;
  typedef Traits::vertex_descriptor Vertex;
  typedef Traits::edge_descriptor Edge;

  typedef std::map<std::string, Vertex> NameVertexMap;
  NameVertexMap name2vertex;
  Graph g;

  typedef property_map<Graph, vertex_name_t>::type NameMap;
  NameMap node_name  = get(vertex_name, g);
  property_map<Graph, edge_name_t>::type link_name = get(edge_name, g);

  //===========================================================================
  // Read the data file and construct the graph.

  std::string line;
  while (std::getline(datafile,line)) {

    std::list<std::string> line_toks;
    boost::stringtok(line_toks, line, "|");

    NameVertexMap::iterator pos;
    bool inserted;
    Vertex u, v;

    std::list<std::string>::iterator i = line_toks.begin();

    boost::tie(pos, inserted) = name2vertex.insert(std::make_pair(*i, Vertex()));
    if (inserted) {
      u = add_vertex(g);
      put(node_name, u, *i);
      pos->second = u;
    } else
      u = pos->second;
    ++i;

    std::string hyperlink_name = *i++;

    boost::tie(pos, inserted) = name2vertex.insert(std::make_pair(*i, Vertex()));
    if (inserted) {
      v = add_vertex(g);
      put(node_name, v, *i);
      pos->second = v;
    } else
      v = pos->second;

    Edge e;
    boost::tie(e, inserted) = add_edge(u, v, g);
    if (inserted) {
      put(link_name, e, hyperlink_name);
    }
  }

  //===========================================================================
  // Calculate the diameter of the graph.

  typedef Traits::vertices_size_type size_type;
  typedef std::vector<size_type> IntVector;
  // Create N x N matrix for storing the shortest distances
  // between each vertex. Initialize all distances to zero.
  std::vector<IntVector> d_matrix(num_vertices(g),
                                  IntVector(num_vertices(g), 0));

  size_type i;
  for (i = 0; i < num_vertices(g); ++i) {
    calc_distance_visitor<size_type*> vis(&d_matrix[i][0]);
    Traits::vertex_descriptor src = vertices(g).first[i];
    breadth_first_search(g, src, boost::visitor(vis));
  }

  size_type diameter = 0;
  BOOST_USING_STD_MAX();
  for (i = 0; i < num_vertices(g); ++i)
    diameter = max BOOST_PREVENT_MACRO_SUBSTITUTION(diameter, *std::max_element(d_matrix[i].begin(),
                                                    d_matrix[i].end()));

  std::cout << "The diameter of the boost web-site graph is " << diameter
            << std::endl << std::endl;

  std::cout << "Number of clicks from the home page: " << std::endl;
  Traits::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    std::cout << d_matrix[0][*vi] << "\t" << node_name[*vi] << std::endl;
  std::cout << std::endl;

  //===========================================================================
  // Print out the breadth-first search tree starting at the home page

  // Create storage for a mapping from vertices to their parents
  std::vector<Traits::vertex_descriptor> parent(num_vertices(g));
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    parent[*vi] = *vi;

  // Do a BFS starting at the home page, recording the parent of each
  // vertex (where parent is with respect to the search tree).
  Traits::vertex_descriptor src = vertices(g).first[0];
  breadth_first_search
    (g, src,
     boost::visitor(make_bfs_visitor(record_predecessors(&parent[0],
                                                         on_tree_edge()))));

  // Add all the search tree edges into a new graph
  Graph search_tree(num_vertices(g));
  boost::tie(vi, vi_end) = vertices(g);
  ++vi;
  for (; vi != vi_end; ++vi)
    add_edge(parent[*vi], *vi, search_tree);

  std::cout << "The breadth-first search tree:" << std::endl;

  // Print out the search tree. We use DFS because it visits
  // the tree nodes in the order that we want to print out:
  // a directory-structure like format.
  std::vector<size_type> dfs_distances(num_vertices(g), 0);
  print_tree_visitor<NameMap, size_type*>
    tree_printer(node_name, &dfs_distances[0]);
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    get(vertex_color, g)[*vi] = white_color;
  depth_first_visit(search_tree, src, tree_printer, get(vertex_color, g));

  return EXIT_SUCCESS;
}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <unsupported/Eigen/AutoDiff>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <sdf_tools/SDF.h>

#ifndef SDF_HPP
#define SDF_HPP

namespace sdf_tools
{
using VoxelGrid::GRID_INDEX;

class SignedDistanceField : public VoxelGrid::VoxelGrid<float>
{
protected:

  std::string frame_;
  bool locked_;

  /*
   * You *MUST* provide valid indices to this function, hence why it is
   * protected (there are safe wrappers available - use them!)
   */
  void FollowGradientsToLocalExtremaUnsafe(
      VoxelGrid<Eigen::Vector3d>& watershed_map,
      const int64_t x_index,
      const int64_t y_index,
      const int64_t z_index) const;

  bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const;

  GRID_INDEX GetNextFromGradient(const GRID_INDEX& index,
                                 const Eigen::Vector3d& gradient) const;

public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  inline SignedDistanceField(const std::string& frame,
                             double resolution,
                             double x_size,
                             double y_size,
                             double z_size,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(resolution,
                                  x_size, y_size, z_size,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const Eigen::Isometry3d& origin_transform,
                             const std::string& frame,
                             double resolution,
                             double x_size,
                             double y_size,
                             double z_size,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(origin_transform, resolution,
                                  x_size, y_size, z_size,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const std::string& frame,
                             double resolution,
                             int64_t x_cells,
                             int64_t y_cells,
                             int64_t z_cells,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(resolution,
                                  x_cells, y_cells, z_cells,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField(const Eigen::Isometry3d& origin_transform,
                             const std::string& frame,
                             double resolution,
                             int64_t x_cells,
                             int64_t y_cells,
                             int64_t z_cells,
                             float OOB_value)
    : VoxelGrid::VoxelGrid<float>(origin_transform, resolution,
                                  x_cells, y_cells, z_cells,
                                  OOB_value),
      frame_(frame), locked_(false) {}

  inline SignedDistanceField()
    : VoxelGrid::VoxelGrid<float>(), frame_(""), locked_(false) {}

  virtual VoxelGrid<float>* Clone() const
  {
    return new SignedDistanceField(
          static_cast<const SignedDistanceField&>(*this));
  }

  inline bool IsLocked() const
  {
      return locked_;
  }

  inline void Lock()
  {
      locked_ = true;
  }

  inline void Unlock()
  {
      locked_ = false;
  }

  virtual uint64_t SerializeSelf(
      std::vector<uint8_t>& buffer,
      const std::function<uint64_t(
        const float&, std::vector<uint8_t>&)>& value_serializer
      =arc_helpers::SerializeFixedSizePOD<float>) const;

  virtual uint64_t DeserializeSelf(
      const std::vector<uint8_t>& buffer, const uint64_t current,
      const std::function<std::pair<float, uint64_t>(
        const std::vector<uint8_t>&, const uint64_t)>& value_deserializer
      =arc_helpers::DeserializeFixedSizePOD<float>);

  /*
   * Mutable access and setter functions MUST be used carefully!
   * If you arbitrarily change SDF values, it is not a proper SDF any more!
   *
   * Use of these functions can be prevented by calling
   * SignedDistanceField::Lock() on the SDF
   */

  virtual std::pair<float&, bool> GetMutable3d(
      const Eigen::Vector3d& location)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return GetMutable(index);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable4d(
      const Eigen::Vector4d& location)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return GetMutable(index);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable(const double x,
                                             const double y,
                                             const double z)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return GetMutable4d(location);
  }

  virtual std::pair<float&, bool> GetMutable(const GRID_INDEX& index)
  {
    if (IndexInBounds(index) && !locked_)
    {
      return std::pair<float&, bool>(
            AccessIndex(GetDataIndex(index)), true);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual std::pair<float&, bool> GetMutable(const int64_t x_index,
                                             const int64_t y_index,
                                             const int64_t z_index)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      return std::pair<float&, bool>(
            AccessIndex(GetDataIndex(x_index, y_index, z_index)), true);
    }
    else
    {
      return std::pair<float&, bool>(oob_value_, false);
    }
  }

  virtual bool SetValue3d(const Eigen::Vector3d& location,
                          const float& value)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue4d(const Eigen::Vector4d& location,
                          const float& value)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const double x,
                        const double y,
                        const double z,
                        const float& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  virtual bool SetValue(const GRID_INDEX& index,
                        const float& value)
  {
    if (IndexInBounds(index) && !locked_)
    {
      AccessIndex(GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const int64_t x_index,
                        const int64_t y_index,
                        const int64_t z_index,
                        const float& value)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      AccessIndex(GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue3d(const Eigen::Vector3d& location,
                          float&& value)
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue4d(const Eigen::Vector4d& location,
                          float&& value)
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return SetValue(index, value);
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const double x,
                        const double y,
                        const double z,
                        float&& value)
  {
    const Eigen::Vector4d location(x, y, z, 1.0);
    return SetValue4d(location, value);
  }

  virtual bool SetValue(const GRID_INDEX& index,
                        float&& value)
  {
    if (IndexInBounds(index) && !locked_)
    {
      AccessIndex(GetDataIndex(index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  virtual bool SetValue(const int64_t x_index,
                        const int64_t y_index,
                        const int64_t z_index,
                        float&& value)
  {
    if (IndexInBounds(x_index, y_index, z_index) && !locked_)
    {
      AccessIndex(GetDataIndex(x_index, y_index, z_index)) = value;
      return true;
    }
    else
    {
      return false;
    }
  }

  inline double GetResolution() const { return GetCellSizes().x(); }

  inline std::string GetFrame() const
  {
    return frame_;
  }

  inline std::vector<double> GetGradient(
      const double x, const double y, const double z,
      const bool enable_edge_gradients=false) const
  {
    return GetGradient4d(Eigen::Vector4d(x, y, z, 1.0), enable_edge_gradients);
  }

  inline std::vector<double> GetGradient3d(
      const Eigen::Vector3d& location,
      const bool enable_edge_gradients=false) const
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return GetGradient(index, enable_edge_gradients);
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGradient4d(
      const Eigen::Vector4d& location,
      const bool enable_edge_gradients=false) const
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return GetGradient(index, enable_edge_gradients);
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGradient(
      const GRID_INDEX& index,
      const bool enable_edge_gradients=false) const
  {
    return GetGradient(index.x, index.y, index.z, enable_edge_gradients);
  }

  inline std::vector<double> GetGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const bool enable_edge_gradients=false) const
  {
    const std::vector<double> grid_aligned_gradient
        = GetGridAlignedGradient(x_index, y_index, z_index,
                                 enable_edge_gradients);
    if (grid_aligned_gradient.size() == 3)
    {
      const Eigen::Quaterniond grid_rotation(origin_transform_.rotation());
      // Derived from EigenHelpers::RotateVector, but without extra copies
      const Eigen::Quaterniond temp(0.0,
                                    grid_aligned_gradient[0],
                                    grid_aligned_gradient[1],
                                    grid_aligned_gradient[2]);
      const Eigen::Quaterniond result
          = grid_rotation * (temp * grid_rotation.inverse());
      return std::vector<double>{result.x(),
                                 result.y(),
                                 result.z()};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetGridAlignedGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const bool enable_edge_gradients=false) const
  {
    // Make sure the index is inside bounds
    if (IndexInBounds(x_index, y_index, z_index))
    {
      // See if the index we're trying to query is one cell in from the edge
      if ((x_index > 0) && (y_index > 0) && (z_index > 0)
          && (x_index < (GetNumXCells() - 1))
          && (y_index < (GetNumYCells() - 1))
          && (z_index < (GetNumZCells() - 1)))
      {
        const double inv_twice_resolution = 1.0 / (2.0 * GetResolution());
        const double gx = (GetImmutable(x_index + 1, y_index, z_index).first
                           - GetImmutable(x_index - 1, y_index, z_index).first)
                          * inv_twice_resolution;
        const double gy = (GetImmutable(x_index, y_index + 1, z_index).first
                           - GetImmutable(x_index, y_index - 1, z_index).first)
                          * inv_twice_resolution;
        const double gz = (GetImmutable(x_index, y_index, z_index + 1).first
                           - GetImmutable(x_index, y_index, z_index - 1).first)
                          * inv_twice_resolution;
        return std::vector<double>{gx, gy, gz};
      }
      // If we're on the edge, handle it specially
      // TODO: we actually need to handle corners even more carefully,
      // since if the SDF is build with a virtual border, these cells will
      // get zero gradient from this approach!
      else if (enable_edge_gradients)
      {
        // Get the "best" indices we can use
        const int64_t low_x_index = std::max((int64_t)0, x_index - 1);
        const int64_t high_x_index = std::min(GetNumXCells() - 1, x_index + 1);
        const int64_t low_y_index = std::max((int64_t)0, y_index - 1);
        const int64_t high_y_index = std::min(GetNumYCells() - 1, y_index + 1);
        const int64_t low_z_index = std::max((int64_t)0, z_index - 1);
        const int64_t high_z_index = std::min(GetNumZCells() - 1, z_index + 1);
        // Compute the axis increments
        const double x_increment
            = (high_x_index - low_x_index) * GetResolution();
        const double y_increment
            = (high_y_index - low_y_index) * GetResolution();
        const double z_increment
            = (high_z_index - low_z_index) * GetResolution();
        // Compute the gradients for each axis - by default these are zero
        double gx = 0.0;
        double gy = 0.0;
        double gz = 0.0;
        // Only if the increments are non-zero do we compute the axis gradient
        if (x_increment > 0.0)
        {
          const double inv_x_increment = 1.0 / x_increment;
          const double high_x_value
              = GetImmutable(high_x_index, y_index, z_index).first;
          const double low_x_value
              = GetImmutable(low_x_index, y_index, z_index).first;
          // Compute the gradient
          gx = (high_x_value - low_x_value) * inv_x_increment;
        }
        if (y_increment > 0.0)
        {
          const double inv_y_increment = 1.0 / y_increment;
          const double high_y_value
              = GetImmutable(x_index, high_y_index, z_index).first;
          const double low_y_value
              = GetImmutable(x_index, low_y_index, z_index).first;
          // Compute the gradient
          gy = (high_y_value - low_y_value) * inv_y_increment;
        }
        if (z_increment > 0.0)
        {
          const double inv_z_increment = 1.0 / z_increment;
          const double high_z_value
              = GetImmutable(x_index, y_index, high_z_index).first;
          const double low_z_value
              = GetImmutable(x_index, y_index, low_z_index).first;
          // Compute the gradient
          gz = (high_z_value - low_z_value) * inv_z_increment;
        }
        // Assemble and return the computed gradient
        return std::vector<double>{gx, gy, gz};
      }
      // Edge gradients disabled, return no gradient
      else
      {
        return std::vector<double>();
      }
    }
    // If we're out of bounds, return no gradient
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetSmoothGradient3d(
      const Eigen::Vector3d& location,
      const double nominal_window_size) const
  {
    return GetSmoothGradient(location.x(), location.y(), location.z(),
                             nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient4d(
      const Eigen::Vector4d& location,
      const double nominal_window_size) const
  {
    return GetSmoothGradient(location(0), location(1), location(2),
                             nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient(
      const double x, const double y, const double z,
      const double nominal_window_size) const
  {
    const double ideal_window_size = std::abs(nominal_window_size);
    if (LocationInBounds(x, y, z))
    {
      const double min_x = x - ideal_window_size;
      const double max_x = x + ideal_window_size;
      const double min_y = y - ideal_window_size;
      const double max_y = y + ideal_window_size;
      const double min_z = z - ideal_window_size;
      const double max_z = z + ideal_window_size;
      // Retrieve distance estimates
      const std::pair<double, bool> point_distance = EstimateDistance(x, y, z);
      const std::pair<double, bool> mx_distance = EstimateDistance(min_x, y, z);
      const std::pair<double, bool> px_distance = EstimateDistance(max_x, y, z);
      const std::pair<double, bool> my_distance = EstimateDistance(x, min_y, z);
      const std::pair<double, bool> py_distance = EstimateDistance(x, max_y, z);
      const std::pair<double, bool> mz_distance = EstimateDistance(x, y, min_z);
      const std::pair<double, bool> pz_distance = EstimateDistance(x, y, max_z);
      // Compute gradient for each axis
      const double gx = ComputeAxisSmoothGradient(point_distance,
                                                  mx_distance,
                                                  px_distance,
                                                  x, min_x, max_x);
      const double gy = ComputeAxisSmoothGradient(point_distance,
                                                  my_distance,
                                                  py_distance,
                                                  y, min_y, max_y);
      const double gz = ComputeAxisSmoothGradient(point_distance,
                                                  mz_distance,
                                                  pz_distance,
                                                  z, min_z, max_z);
      return std::vector<double>{gx, gy, gz};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetSmoothGradient(
      const GRID_INDEX& index, const double nominal_window_size) const
  {
    return GetSmoothGradient4d(GridIndexToLocation(index), nominal_window_size);
  }

  inline std::vector<double> GetSmoothGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index,
      const double nominal_window_size) const
  {
    return GetSmoothGradient4d(
          GridIndexToLocation(x_index, y_index, z_index), nominal_window_size);
  }

  inline std::vector<double> GetAutoDiffGradient3d(
      const Eigen::Vector3d& location) const
  {
    return GetAutoDiffGradient(location.x(), location.y(), location.z());
  }

  inline std::vector<double> GetAutoDiffGradient4d(
      const Eigen::Vector4d& location) const
  {
    return GetAutoDiffGradient(location(0), location(1), location(2));
  }

  // TODO: this does not work if you query at cell centers!
  inline std::vector<double> GetAutoDiffGradient(
      const double x, const double y, const double z) const
  {
    const GRID_INDEX index = LocationToGridIndex(x, y, z);
    if (IndexInBounds(index))
    {
      // Use with AutoDiffScalar
      typedef Eigen::AutoDiffScalar<Eigen::Vector4d> AScalar;
      typedef Eigen::Matrix<AScalar, 4, 1> APosition;
      APosition Alocation;
      Alocation(0) = x;
      Alocation(1) = y;
      Alocation(2) = z;
      Alocation(3) = 1.0;
      Alocation(0).derivatives() = Eigen::Vector4d::Unit(0);
      Alocation(1).derivatives() = Eigen::Vector4d::Unit(1);
      Alocation(2).derivatives() = Eigen::Vector4d::Unit(2);
      Alocation(3).derivatives() = Eigen::Vector4d::Unit(3);
      AScalar Adist = EstimateDistanceInterpolateFromNeighbors<AScalar>(
                        Alocation, index.x, index.y, index.z);
      return std::vector<double>{
        Adist.derivatives()(0), Adist.derivatives()(1), Adist.derivatives()(2)};
    }
    else
    {
      return std::vector<double>();
    }
  }

  inline std::vector<double> GetAutoDiffGradient(const GRID_INDEX& index) const
  {
    return GetAutoDiffGradient4d(GridIndexToLocation(index));
  }

  inline std::vector<double> GetAutoDiffGradient(
      const int64_t x_index, const int64_t y_index, const int64_t z_index) const
  {
    return GetAutoDiffGradient4d(
          GridIndexToLocation(x_index, y_index, z_index));
  }

protected:

  inline double ComputeAxisSmoothGradient(
      const std::pair<double, bool>& query_point_distance_estimate,
      const std::pair<double, bool>& minus_axis_distance_estimate,
      const std::pair<double, bool>& plus_axis_distance_estimate,
      const double query_point_axis_value,
      const double minus_point_axis_value,
      const double plus_point_axis_value) const
  {
    if (query_point_distance_estimate.second
        && minus_axis_distance_estimate.second
        && plus_axis_distance_estimate.second)
    {
      const double window_size = plus_point_axis_value
                                 - minus_point_axis_value;
      const double distance_delta = plus_axis_distance_estimate.first
                                    - minus_axis_distance_estimate.first;
      return distance_delta / window_size;
    }
    else if (query_point_distance_estimate.second
             && minus_axis_distance_estimate.second)
    {
      const double window_size = query_point_axis_value
                                 - minus_point_axis_value;
      const double distance_delta = query_point_distance_estimate.first
                                    - minus_axis_distance_estimate.first;
      return distance_delta / window_size;
    }
    else if (query_point_distance_estimate.second
             && plus_axis_distance_estimate.second)
    {
      const double window_size = plus_point_axis_value
                                 - query_point_axis_value;
      const double distance_delta = plus_axis_distance_estimate.first
                                    - query_point_distance_estimate.first;
      return distance_delta / window_size;
    }
    else
    {
      throw std::runtime_error(
            "Window size for GetSmoothGradient is too large for SDF");
    }
  }

  template<typename T>
  inline T BilinearInterpolate(const double low_d1,
                               const double high_d1,
                               const double low_d2,
                               const double high_d2,
                               const T query_d1,
                               const T query_d2,
                               const double l1l2_val,
                               const double l1h2_val,
                               const double h1l2_val,
                               const double h1h2_val) const
  {
    Eigen::Matrix<T, 1, 2> d1_offsets;
    d1_offsets(0, 0) = high_d1 - query_d1;
    d1_offsets(0, 1) = query_d1 - low_d1;
    Eigen::Matrix<T, 2, 2> values;
    values(0, 0) = l1l2_val;
    values(0, 1) = l1h2_val;
    values(1, 0) = h1l2_val;
    values(1, 1) = h1h2_val;
    Eigen::Matrix<T, 2, 1> d2_offsets;
    d2_offsets(0, 0) = high_d2 - query_d2;
    d2_offsets(1, 0) = query_d2 - low_d2;
    const T multiplier = 1.0 / ((high_d1 - low_d1) * (high_d2 - low_d2));
    const T bilinear_interpolated
        = multiplier * d1_offsets * values * d2_offsets;
    return bilinear_interpolated;
  }

  template<typename T>
  inline T BilinearInterpolateDistanceXY(
      const Eigen::Vector4d& corner_location,
      const Eigen::Matrix<T, 4, 1>& query_location,
      const double mxmy_dist, const double mxpy_dist,
      const double pxmy_dist, const double pxpy_dist) const
  {
    return BilinearInterpolate(corner_location(0),
                               corner_location(0) + GetResolution(),
                               corner_location(1),
                               corner_location(1) + GetResolution(),
                               query_location(0),
                               query_location(1),
                               mxmy_dist, mxpy_dist,
                               pxmy_dist, pxpy_dist);
  }

  template<typename T>
  inline T TrilinearInterpolateDistance(
      const Eigen::Vector4d& corner_location,
      const Eigen::Matrix<T, 4, 1>& query_location,
      const double mxmymz_dist, const double mxmypz_dist,
      const double mxpymz_dist, const double mxpypz_dist,
      const double pxmymz_dist, const double pxmypz_dist,
      const double pxpymz_dist, const double pxpypz_dist) const
  {
    // Do bilinear interpolation in the lower XY plane
    const T mz_bilinear_interpolated
        = BilinearInterpolateDistanceXY(corner_location, query_location,
                                        mxmymz_dist, mxpymz_dist,
                                        pxmymz_dist, pxpymz_dist);
    // Do bilinear interpolation in the upper XY plane
    const T pz_bilinear_interpolated
        = BilinearInterpolateDistanceXY(corner_location, query_location,
                                        mxmypz_dist, mxpypz_dist,
                                        pxmypz_dist, pxpypz_dist);
    // Perform linear interpolation/extrapolation between lower and upper planes
    const double inv_resolution = 1.0 / GetResolution();
    const T distance_delta
        = pz_bilinear_interpolated - mz_bilinear_interpolated;
    const T distance_slope = distance_delta * inv_resolution;
    const T query_z_delta = query_location(2) - T(corner_location(2));
    return mz_bilinear_interpolated + (query_z_delta * distance_slope);
  }

  inline double GetCorrectedCenterDistance(const int64_t x_idx,
                                           const int64_t y_idx,
                                           const int64_t z_idx) const
  {
    const std::pair<const float&, bool> query
        = GetImmutable(x_idx, y_idx, z_idx);
    if (query.second)
    {
      const double nominal_sdf_distance = (double)query.first;
      const double cell_center_distance_offset = GetResolution() * 0.5;
      if (nominal_sdf_distance >= 0.0)
      {
        return nominal_sdf_distance - cell_center_distance_offset;
      }
      else
      {
        return nominal_sdf_distance + cell_center_distance_offset;
      }
    }
    else
    {
        throw std::invalid_argument("Index out of bounds");
    }
  }

  template<typename T>
  std::pair<int64_t, int64_t> GetAxisInterpolationIndices(
      const int64_t initial_index,
      const int64_t axis_size,
      const T axis_offset) const
  {
    int64_t lower = initial_index;
    int64_t upper = initial_index;
    if (axis_offset >= 0.0)
    {
      upper = initial_index + 1;
      if (upper >= axis_size)
      {
        upper = initial_index;
        lower = initial_index -1;
        if (lower < 0)
        {
          lower = initial_index;
        }
      }
    }
    else
    {
      lower = initial_index - 1;
      if (lower < 0)
      {
        upper = initial_index + 1;
        lower = initial_index;
        if (upper >= axis_size)
        {
          upper = initial_index;
        }
      }
    }
    return std::make_pair(lower, upper);
  }

  template<typename T>
  inline T EstimateDistanceInterpolateFromNeighbors(
      const Eigen::Matrix<T, 4, 1>& query_location,
      const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const
  {
    // Get the query location in grid frame
    const Eigen::Matrix<T, 4, 1> grid_frame_query_location
        = GetInverseOriginTransform() * query_location;
    // Switch between all the possible options of where we are
    const Eigen::Vector4d cell_center_location
        = GridIndexToLocationGridFrame(x_idx, y_idx, z_idx);
    const Eigen::Matrix<T, 4, 1> query_offset
        = grid_frame_query_location - cell_center_location.cast<T>();
    // Catch the easiest case
//    if ((query_offset(0) == 0.0)
//        && (query_offset(1) == 0.0)
//        && (query_offset(2) == 0.0))
//    {
//      return GetCorrectedCenterDistance(x_idx, y_idx, z_idx);
//    }
    // Find the best-matching 8 surrounding cell centers
    const std::pair<int64_t, int64_t> x_axis_indices
        = GetAxisInterpolationIndices(x_idx, GetNumXCells(), query_offset(0));
    const std::pair<int64_t, int64_t> y_axis_indices
        = GetAxisInterpolationIndices(y_idx, GetNumYCells(), query_offset(1));
    const std::pair<int64_t, int64_t> z_axis_indices
        = GetAxisInterpolationIndices(z_idx, GetNumZCells(), query_offset(2));
    const Eigen::Vector4d lower_corner_location
        = GridIndexToLocationGridFrame(x_axis_indices.first,
                                       y_axis_indices.first,
                                       z_axis_indices.first);
    const double mxmymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.first,
                                     z_axis_indices.first);
    const double mxmypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.first,
                                     z_axis_indices.second);
    const double mxpymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.second,
                                     z_axis_indices.first);
    const double mxpypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.first,
                                     y_axis_indices.second,
                                     z_axis_indices.second);
    const double pxmymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.first,
                                     z_axis_indices.first);
    const double pxmypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.first,
                                     z_axis_indices.second);
    const double pxpymz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.second,
                                     z_axis_indices.first);
    const double pxpypz_distance
        = GetCorrectedCenterDistance(x_axis_indices.second,
                                     y_axis_indices.second,
                                     z_axis_indices.second);
    return TrilinearInterpolateDistance(lower_corner_location,
                                        grid_frame_query_location,
                                        mxmymz_distance, mxmypz_distance,
                                        mxpymz_distance, mxpypz_distance,
                                        pxmymz_distance, pxmypz_distance,
                                        pxpymz_distance, pxpypz_distance);
  }

public:

  inline std::pair<double, bool> EstimateDistance(const double x,
                                                  const double y,
                                                  const double z) const
  {
      return EstimateDistance4d(Eigen::Vector4d(x, y, z, 1.0));
  }

  inline std::pair<double, bool> EstimateDistance3d(
      const Eigen::Vector3d& location) const
  {
    const GRID_INDEX index = LocationToGridIndex3d(location);
    if (IndexInBounds(index))
    {
      return std::make_pair(
            EstimateDistanceInterpolateFromNeighbors<double>(
              Eigen::Vector4d(location.x(), location.y(), location.z(), 1.0),
              index.x, index.y, index.z),
            true);
    }
    else
    {
      return std::make_pair((double)GetOOBValue(), false);
    }
  }

  inline std::pair<double, bool> EstimateDistance4d(
      const Eigen::Vector4d& location) const
  {
    const GRID_INDEX index = LocationToGridIndex4d(location);
    if (IndexInBounds(index))
    {
      return std::make_pair(EstimateDistanceInterpolateFromNeighbors<double>(
                              location, index.x, index.y, index.z),
                            true);
    }
    else
    {
      return std::make_pair((double)GetOOBValue(), false);
    }
  }

  inline Eigen::Vector3d ProjectOutOfCollision(
      const double x, const double y, const double z,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    const Eigen::Vector4d result
        = ProjectOutOfCollision4d(Eigen::Vector4d(x, y, z, 1.0),
                                  stepsize_multiplier);
    return result.head<3>();
  }

  inline Eigen::Vector3d ProjectOutOfCollision3d(
      const Eigen::Vector3d& location,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollision(location.x(), location.y(), location.z(),
                                 stepsize_multiplier);
  }

  inline Eigen::Vector4d ProjectOutOfCollision4d(
      const Eigen::Vector4d& location,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance4d(location, 0.0,
                                                    stepsize_multiplier);
  }

  inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(
      const double x, const double y, const double z,
      const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance4d(
          Eigen::Vector4d(x, y, z, 1.0),
          minimum_distance, stepsize_multiplier).head<3>();
  }

  inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(
      const Eigen::Vector3d& location, const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    return ProjectOutOfCollisionToMinimumDistance(
          location.x(), location.y(), location.z(),
          minimum_distance, stepsize_multiplier);
  }

  inline Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(
      const Eigen::Vector4d& location,
      const double minimum_distance,
      const double stepsize_multiplier = 1.0 / 10.0) const
  {
    // To avoid potential problems with alignment, we need to pass location
    // by reference, so we make a local copy here that we can change.
    // See https://eigen.tuxfamily.org/dox/group__TopicPassingByValue.html
    Eigen::Vector4d mutable_location = location;
    // If we are in bounds, start the projection process,
    // otherwise return the location unchanged
    if (LocationInBounds4d(mutable_location))
    {
      // Add a small collision margin to account for rounding and similar
      const double minimum_distance_with_margin
          = minimum_distance + GetResolution() * stepsize_multiplier * 1e-3;
      const double max_stepsize = GetResolution() * stepsize_multiplier;
      const bool enable_edge_gradients = true;
      double sdf_dist = EstimateDistance4d(mutable_location).first;
      while (sdf_dist <= minimum_distance)
      {
        const std::vector<double> gradient
            = GetGradient4d(mutable_location, enable_edge_gradients);
        if (gradient.size() == 3)
        {
          const Eigen::Vector4d grad_vector(
                gradient[0], gradient[1], gradient[2], 0.0);
          if (grad_vector.norm() > GetResolution() * 0.25) // Sanity check
          {
            // Don't step any farther than is needed
            const double step_distance
                = std::min(max_stepsize,
                           minimum_distance_with_margin - sdf_dist);
            mutable_location += grad_vector.normalized() * step_distance;
            sdf_dist = EstimateDistance4d(mutable_location).first;
          }
          else
          {
            throw std::runtime_error("Encountered flat gradient - stuck");
          }
        }
        else
        {
          throw std::runtime_error("Failed to compute gradient - out of SDF?");
        }
      }
    }
    return mutable_location;
  }

  static void SaveToFile(const SignedDistanceField& sdf,
                         const std::string& filepath,
                         const bool compress);

  static SignedDistanceField LoadFromFile(const std::string& filepath);

  static sdf_tools::SDF GetMessageRepresentation(
      const SignedDistanceField& sdf);

  static SignedDistanceField LoadFromMessageRepresentation(
      const sdf_tools::SDF& message);

  visualization_msgs::Marker ExportForDisplay(const float alpha = 0.01f) const;

  visualization_msgs::Marker ExportForDisplayCollisionOnly(
      const float alpha = 0.01f) const;

  /*
   * The following function can be *VERY EXPENSIVE* to compute, since it
   * performs gradient ascent/descent across the SDF
   */
  VoxelGrid<Eigen::Vector3d> ComputeLocalExtremaMap() const;
};
}

#endif // SDF_HPP


#ifndef ALEPH_PERSISTENCE_DIAGRAMS_DISTANCES_BOTTLENECK_HH__
#define ALEPH_PERSISTENCE_DIAGRAMS_DISTANCES_BOTTLENECK_HH__

#include <aleph/geometry/distances/Infinity.hh>

#include <aleph/persistenceDiagrams/PersistenceDiagram.hh>
#include <aleph/persistenceDiagrams/distances/detail/Orthogonal.hh>

#include <boost/iterator/counting_iterator.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

namespace aleph
{

namespace distances
{

namespace detail
{

template <class T> struct Edge
{
  std::size_t source;
  std::size_t target;

  T weight;

  Edge( std::size_t s, std::size_t t, T w )
    : source( s )
    , target( t )
    , weight( w )
  {
  }

  bool operator<( const Edge& other ) const
  {
    return weight < other.weight;
  }
};

template <class T> struct CheckMatchingCardinality
{
  using GraphType          = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  using MatchingVectorType = std::vector<boost::graph_traits<GraphType>::vertex_descriptor>;

  CheckMatchingCardinality( std::size_t size, typename std::vector<Edge<T> >::const_iterator begin )
    : _maximumSize( size )
    , _last( begin )
    , _graph( 2 * size )
    , _mates( 2 * size )
  {
    boost::add_edge( begin->source, begin->target, _graph );
  }

  bool operator()( typename std::vector<Edge<T> >::const_iterator /* it1 */, typename std::vector<Edge<T> >::const_iterator it2 )
  {
    // The new edge lies beyond the edges that are already known, so the
    // edges between the last edge and the new iterator position need to
    // be added.
    if( it2 > _last )
    {
      do
      {
        ++_last;
        boost::add_edge( _last->source, _last->target, _graph );
      }
      while( _last != it2 );
    }

    // The new edge lies behind the edges that are already known, so the
    // surplus edges need to be removed.
    else
    {
      do
      {
        boost::remove_edge( _last->source, _last->target, _graph );
        --_last;
      }
      while( _last != it2 );
    }

    boost::edmonds_maximum_cardinality_matching( _graph,
                                                 &_mates[0] );

    // Look out for _perfect matchings_ in the bipartite graph. Any other
    // maximum cardinality matching does not qualify for the Bottleneck
    // distance.
    return boost::matching_size( _graph, &_mates[0] ) == _maximumSize;
  }

  std::size_t _maximumSize;

  typename std::vector<Edge<T> >::const_iterator _last;

  GraphType          _graph; // Input data
  MatchingVectorType _mates; // Edges of the matching
};

} // namespace detail

/**
  Calculates the Bottleneck distance between two persistence diagrams.
  The algorithm used for this involves checking a (complete) bipartite
  graph for perfect matchings.

  A brief description of the algoritmh is given in

    Computational Topology
    Herbert Edelsbrunner and John Harer

  on page 191.

  The implementation has been inspired by Dmitriy Morozov's "Dionysus"
  framework.

  @param D1 First persistence diagram
  @param D2 Second persistence diagram

  @returns Bottleneck distance between the two persistence diagrams
*/

template <
  class DataType,
  class Distance = aleph::geometry::distances::InfinityDistance<DataType>
> DataType bottleneckDistance( const PersistenceDiagram<DataType>& D1,
                               const PersistenceDiagram<DataType>& D2 )
{
  auto n           = D1.size();
  auto m           = D2.size();
  auto maximumSize = n + m;

  using SizeType = decltype(n);
  using Edge     = detail::Edge<DataType>;

  std::vector<Edge> edges;

  // Diagonal edges ----------------------------------------------------

  for( SizeType i = n; i < maximumSize; i++ )
    for( SizeType j = maximumSize + i; j < 2 * maximumSize; j++ )
      edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(j), DataType() ) );

  SizeType i = 0;

  // Edges between regular points --------------------------------------

  Distance distance;

  for( auto it1 = D1.begin(); it1 != D1.end(); ++it1 )
  {
    auto j = maximumSize;

    for( auto it2 = D2.begin(); it2 != D2.end(); ++it2 )
    {
      auto weight = distance( *it1, *it2 );

      edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(j),
                             weight ) );

      ++j;
    }

    ++i;
  }

  // Edges between points and their projections ------------------------

  i = 0;

  for( auto it1 = D1.begin(); it1 != D1.end(); ++it1 )
  {
    edges.push_back( Edge( static_cast<std::size_t>(i), static_cast<std::size_t>(maximumSize + m + i),
                           aleph::distances::detail::orthogonalDistance<Distance>( *it1 ) ) );

    ++i;
  }

  i = maximumSize;

  for( auto it2 = D2.begin(); it2 != D2.end(); ++it2 )
  {
    edges.push_back( Edge( static_cast<std::size_t>(n + i - maximumSize), static_cast<std::size_t>(i),
                           aleph::distances::detail::orthogonalDistance<Distance>( *it2 ) ) );
  }

  // Identify matchings ------------------------------------------------

  std::sort( edges.begin(), edges.end() );

  // Perform binary search over edge sets. Starting from the empty graph, use
  // more and more edges to find out the first graph that permits a maximum
  // cardinality matching.

  using EdgeIteratorType     = typename std::vector<Edge>::const_iterator;
  using CountingIteratorType = boost::counting_iterator<EdgeIteratorType>;

  auto itEdge = std::upper_bound( CountingIteratorType( edges.begin() ),
                                  CountingIteratorType( edges.end() ),
                                  edges.begin(),
                                  detail::CheckMatchingCardinality<DataType>( maximumSize, edges.begin() ) );

  return (*itEdge)->weight;
}

} // namespace distances

} // namespace aleph

#endif


#include "geom/Vector2D.hpp"
#include "geom/Transform.hpp"
#include "geom/BoundingBox.hpp"
#include "geom/Math.hpp"
#include "geom/CollisionCheck.hpp"

#include <boost/python.hpp>

void export_geom()
{
    namespace bp = boost::python;
    namespace ag = avs::geom;

    // Make from avsimpy.geometrics import ... work
    bp::object utilModule(bp::handle<>(bp::borrowed(PyImport_AddModule("avsimpy.geometrics"))));
    // Make from avsimpy import geometrics work
    bp::scope().attr("geometrics") = utilModule;
    bp::scope util_scope = utilModule;

    bp::class_<ag::Vector2D>("Vector2D")
        .def(bp::init<const float, const float>((bp::arg("x")=0.0f, bp::arg("y")=0.0f)))
        .def(bp::init<const ag::Vector2D>((bp::arg("vector"))))
        .def_readwrite("x", &ag::Vector2D::x)
        .def_readwrite("y", &ag::Vector2D::y)
        .def("get_length", &ag::Vector2D::get_length)
        .def("get_unit_vector", &ag::Vector2D::get_unit_vector)
        .def("__eq__", &ag::Vector2D::operator==)
        .def("__ne__", &ag::Vector2D::operator!=)
        .def(bp::self += bp::self)
        .def(bp::self + bp::self)
        .def(bp::self -= bp::self)
        .def(bp::self - bp::self)
        .def(bp::self *= double())
        .def(bp::self * double())
        .def(double() * bp::self)
        .def(bp::self /= double())
        .def(bp::self / double())
        .def(double() / bp::self)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::Transform>("Transform")
        .def(bp::init<const ag::Vector2D, const float>(
            (bp::arg("location")=ag::Vector2D(), bp::arg("rotation")=0.0f)))
        .def(bp::init<const ag::Transform&>((bp::arg("transform"))))
        .def_readwrite("location", &ag::Transform::location)
        .def_readwrite("rotation", &ag::Transform::rotation)
        .def("get_forward_vector", &ag::Transform::get_forward_vector)
        .def("calculate_distance", &ag::Transform::calculate_distance)
        .def("__eq__", &ag::Transform::operator==)
        .def("__ne__", &ag::Transform::operator!=)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::BoundingBox>("BoundingBox")
        .def(bp::init<const ag::Vector2D, const ag::Vector2D>(
            (bp::arg("location")=ag::Vector2D(), bp::arg("extent")=ag::Vector2D())))
        .def(bp::init<const float, const float, const float, const float>(
            (bp::arg("x")=0.0f, bp::arg("y")=0.0f, bp::arg("extent_x")=1.0f, bp::arg("extent_y")=1.0f)))
        .def(bp::init<const ag::BoundingBox&>((bp::arg("bounding_box"))))
        .def_readwrite("location", &ag::BoundingBox::center)
        .def_readwrite("extent", &ag::BoundingBox::extent)
        .def("contains", &ag::BoundingBox::contains, bp::arg("point"))
        .def("intersects", &ag::BoundingBox::intersects, bp::arg("bounding_box"))
        .def("__eq__", &ag::BoundingBox::operator==)
        .def("__ne__", &ag::BoundingBox::operator!=)
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

    bp::class_<ag::Math>("math")
        .def("to_radian", &ag::Math::to_radian, bp::arg("deg"))
        .def("to_degree", &ag::Math::to_degree, bp::arg("rad"))
        .def("dot_product", &ag::Math::dot_product, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("calculate_distance", &ag::Math::calculate_distance, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("calculate_angle", &ag::Math::calculate_angle, (bp::arg("vector_A"), bp::arg("vector_B")))
        .def("get_forward_vector", &ag::Math::get_forward_vector, bp::arg("deg"))
        .def("rotate_vector_on_origin", &ag::Math::rotate_vector_on_origin, (bp::arg("vector"), bp::arg("deg")))
        .def("rotate_vector_on_point", &ag::Math::rotate_vector_on_point, (bp::arg("vector"), bp::arg("deg"), bp::arg("point")))
    ;

    bp::class_<ag::CollisionChecker>("CollisionChecker")
        .def("check_collision", &ag::CollisionChecker::check_collision, (bp::arg("entity_A"), bp::arg("entity_B")))
        .def(bp::self_ns::str(bp::self_ns::self))
    ;

}