/**
 * \file NearestNeighbor.hpp
 * \brief Header for GeographicLib::NearestNeighbor class
 *
 * Copyright (c) Charles Karney (2016-2019) <charles@karney.com> and licensed
 * under the MIT/X11 License.  For more information, see
 * https://geographiclib.sourceforge.io/
 **********************************************************************/

#if !defined(GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP)
#define GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP 1

#include <algorithm>            // for nth_element, max_element, etc.
#include <vector>
#include <queue>                // for priority_queue
#include <utility>              // for swap + pair
#include <cstring>
#include <limits>
#include <cmath>
#include <iostream>
#include <sstream>
// Only for GeographicLib::GeographicErr
#include <GeographicLib/Constants.hpp>

#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>
#endif

#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
#  pragma warning (push)
#  pragma warning (disable: 4127)
#endif

namespace GeographicLib {

  /**
   * \brief Nearest-neighbor calculations
   *
   * This class solves the nearest-neighbor problm using a vantage-point tree
   * as described in \ref nearest.
   *
   * This class is templated so that it can handle arbitrary metric spaces as
   * follows:
   *
   * @tparam dist_t the type used for measuring distances; it can be a real or
   *   signed integer type; in typical geodetic applications, \e dist_t might
   *   be double.
   * @tparam pos_t the type for specifying the positions of points; geodetic
   *   application might bundled the latitude and longitude into a
   *   std::pair<dist_t, dist_t>.
   * @tparam distfun_t the type of a function object which takes takes two
   *   positions (of type \e pos_t) and returns the distance (of type \e
   *   dist_t); in geodetic applications, this might be a class which is
   *   constructed with a Geodesic object and which implements a member
   *   function with a signature dist_t operator() (const pos_t&, const
   *   pos_t&) const, which returns the geodesic distance between two
   *   points.
   *
   * \note The distance measure must satisfy the triangle inequality, \f$
   * d(a,c) \le d(a,b) + d(b,c) \f$ for all points \e a, \e b, \e c.  The
   * geodesic distance (given by Geodesic::Inverse) does, while the great
   * ellipse distance and the rhumb line distance do not.  If you use
   * the ordinary Euclidean distance, i.e., \f$ \sqrt{(x_a-x_b)^2 +
   * (y_a-y_b)^2} \f$ for two dimensions, don't be tempted to leave out the
   * square root in the interests of "efficiency"; the squared distance does
   * not satisfy the triangle inequality!
   *
   * \note This is a "header-only" implementation and, as such, depends in a
   * minimal way on the rest of GeographicLib (the only dependency is through
   * the use of GeographicLib::GeographicErr for handling compile-time and
   * run-time exceptions).  Therefore, it is easy to extract this class from
   * the rest of GeographicLib and use it as a stand-alone facility.
   *
   * The \e dist_t type must support numeric_limits queries (specifically:
   * is_signed, is_integer, max(), digits).
   *
   * The NearestNeighbor object is constructed with a vector of points (type \e
   * pos_t) and a distance function (type \e distfun_t).  However the object
   * does \e not store the points.  When querying the object with Search(),
   * it's necessary to supply the same vector of points and the same distance
   * function.
   *
   * There's no capability in this implementation to add or remove points from
   * the set.  Instead Initialize() should be called to re-initialize the
   * object with the modified vector of points.
   *
   * Because of the overhead in constructing a NearestNeighbor object for a
   * large set of points, functions Save() and Load() are provided to save the
   * object to an external file.  operator<<(), operator>>() and  Boost
   * serialization can also be used to save and restore a NearestNeighbor
   * object.  This is illustrated in the example.
   *
   * Example of use:
   * \include example-NearestNeighbor.cpp
   **********************************************************************/
  template <typename dist_t, typename pos_t, class distfun_t>
  class NearestNeighbor {
    // For tracking changes to the I/O format
    static const int version = 1;
    // This is what we get "free"; but if sizeof(dist_t) = 1 (unlikely), allow
    // 4 slots (and this accommodates the default value bucket = 4).
    static const int maxbucket =
      (2 + ((4 * sizeof(dist_t)) / sizeof(int) >= 2 ?
            (4 * sizeof(dist_t)) / sizeof(int) : 2));
  public:

    /**
     * Default constructor for NearestNeighbor.
     *
     * This is equivalent to specifying an empty set of points.
     **********************************************************************/
    NearestNeighbor() : _numpoints(0), _bucket(0), _cost(0) {}

    /**
     * Constructor for NearestNeighbor.
     *
     * @param[in] pts a vector of points to include in the set.
     * @param[in] dist the distance function object.
     * @param[in] bucket the size of the buckets at the leaf nodes; this must
     *   lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)] (default 4).
     * @exception GeographicErr if the value of \e bucket is out of bounds or
     *   the size of \e pts is too big for an int.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * \e pts may contain coincident points (i.e., the distance between them
     * vanishes); these are treated as distinct.
     *
     * The choice of \e bucket is a tradeoff between space and efficiency.  A
     * larger \e bucket decreases the size of the NearestNeighbor object which
     * scales as pts.size() / max(1, bucket) and reduces the number of distance
     * calculations to construct the object by log2(bucket) * pts.size().
     * However each search then requires about bucket additional distance
     * calculations.
     *
     * \warning The distances computed by \e dist must satisfy the standard
     * metric conditions.  If not, the results are undefined.  Neither the data
     * in \e pts nor the query points should contain NaNs or infinities because
     * such data violates the metric conditions.
     *
     * \warning The same arguments \e pts and \e dist must be provided
     * to the Search() function.
     **********************************************************************/
    NearestNeighbor(const std::vector<pos_t>& pts, const distfun_t& dist,
                    int bucket = 4) {
      Initialize(pts, dist, bucket);
    }

    /**
     * Initialize or re-initialize NearestNeighbor.
     *
     * @param[in] pts a vector of points to include in the tree.
     * @param[in] dist the distance function object.
     * @param[in] bucket the size of the buckets at the leaf nodes; this must
     *   lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)] (default 4).
     * @exception GeographicErr if the value of \e bucket is out of bounds or
     *   the size of \e pts is too big for an int.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * See also the documentation on the constructor.
     *
     * If an exception is thrown, the state of the NearestNeighbor is
     * unchanged.
     **********************************************************************/
    void Initialize(const std::vector<pos_t>& pts, const distfun_t& dist,
                    int bucket = 4) {
      static_assert(std::numeric_limits<dist_t>::is_signed,
                    "dist_t must be a signed type");
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr
          ("bucket must lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)]");
      if (pts.size() > size_t(std::numeric_limits<int>::max()))
        throw GeographicLib::GeographicErr("pts array too big");
      // the pair contains distance+id
      std::vector<item> ids(pts.size());
      for (int k = int(ids.size()); k--;)
        ids[k] = std::make_pair(dist_t(0), k);
      int cost = 0;
      std::vector<Node> tree;
      init(pts, dist, bucket, tree, ids, cost,
           0, int(ids.size()), int(ids.size()/2));
      _tree.swap(tree);
      _numpoints = int(pts.size());
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

    /**
     * Search the NearestNeighbor.
     *
     * @param[in] pts the vector of points used for initialization.
     * @param[in] dist the distance function object used for initialization.
     * @param[in] query the query point.
     * @param[out] ind a vector of indices to the closest points found.
     * @param[in] k the number of points to search for (default = 1).
     * @param[in] maxdist only return points with distances of \e maxdist or
     *   less from \e query (default is the maximum \e dist_t).
     * @param[in] mindist only return points with distances of more than
     *   \e mindist from \e query (default = −1).
     * @param[in] exhaustive whether to do an exhaustive search (default true).
     * @param[in] tol the tolerance on the results (default 0).
     * @return the distance to the closest point found (−1 if no points
     *   are found).
     * @exception GeographicErr if \e pts has a different size from that used
     *   to construct the object.
     *
     * The indices returned in \e ind are sorted by distance from \e query
     * (closest first).
     *
     * The simplest invocation is with just the 4 non-optional arguments.  This
     * returns the closest distance and the index to the closest point in
     * ind0.  If there are several points equally close, then
     * ind0 gives the index of an arbirary one of them.  If
     * there's no closest point (because the set of points is empty), then \e
     * ind is empty and −1 is returned.
     *
     * With \e exhaustive = true and \e tol = 0 (their default values), this
     * finds the indices of \e k closest neighbors to \e query whose distances
     * to \e query are in (\e mindist, \e maxdist].  If \e mindist and \e
     * maxdist have their default values, then these bounds have no effect.  If
     * \e query is one of the points in the tree, then set \e mindist = 0 to
     * prevent this point (and other coincident points) from being returned.
     *
     * If \e exhaustive = false, exit as soon as \e k results satisfying the
     * distance criteria are found.  If less than \e k results are returned
     * then the search was exhaustive even if \e exhaustive = false.
     *
     * If \e tol is positive, do an approximate search; in this case the
     * results are to be interpreted as follows: if the k'th distance is
     * \e dk, then all results with distances less than or equal \e dk −
     * \e tol are correct; all others are suspect — there may be other
     * closer results with distances greater or equal to \e dk − \e tol.
     * If less than \e k results are found, then the search is exact.
     *
     * \e mindist should be used to exclude a "small" neighborhood of the query
     * point (relative to the average spacing of the data).  If \e mindist is
     * large, the efficiency of the search deteriorates.
     *
     * \note Only the shortest distance is returned (as as the function value).
     * The distances to other points (indexed by indj
     * for \e j > 0) can be found by invoking \e dist again.
     *
     * \warning The arguments \e pts and \e dist must be identical to those
     * used to initialize the NearestNeighbor; if not, this function will
     * return some meaningless result (however, if the size of \e pts is wrong,
     * this function throw an exception).
     *
     * \warning The query point cannot be a NaN or infinite because then the
     * metric conditions are violated.
     **********************************************************************/
    dist_t Search(const std::vector<pos_t>& pts, const distfun_t& dist,
                  const pos_t& query,
                  std::vector<int>& ind,
                  int k = 1,
                  dist_t maxdist = std::numeric_limits<dist_t>::max(),
                  dist_t mindist = -1,
                  bool exhaustive = true,
                  dist_t tol = 0) const {
      if (_numpoints != int(pts.size()))
          throw GeographicLib::GeographicErr("pts array has wrong size");
      std::priority_queue<item> results;
      if (_numpoints > 0 && k > 0 && maxdist > mindist) {
        // distance to the kth closest point so far
        dist_t tau = maxdist;
        // first is negative of how far query is outside boundary of node
        // +1 if on boundary or inside
        // second is node index
        std::priority_queue<item> todo;
        todo.push(std::make_pair(dist_t(1), int(_tree.size()) - 1));
        int c = 0;
        while (!todo.empty()) {
          int n = todo.top().second;
          dist_t d = -todo.top().first;
          todo.pop();
          dist_t tau1 = tau - tol;
          // compare tau and d again since tau may have become smaller.
          if (!( n >= 0 && tau1 >= d )) continue;
          const Node& current = _tree[n];
          dist_t dst = 0;   // to suppress warning about uninitialized variable
          bool exitflag = false, leaf = current.index < 0;
          for (int i = 0; i < (leaf ? _bucket : 1); ++i) {
            int index = leaf ? current.leaves[i] : current.index;
            if (index < 0) break;
            dst = dist(pts[index], query);
            ++c;

            if (dst > mindist && dst <= tau) {
              if (int(results.size()) == k) results.pop();
              results.push(std::make_pair(dst, index));
              if (int(results.size()) == k) {
                if (exhaustive)
                  tau = results.top().first;
                else {
                  exitflag = true;
                  break;
                }
                if (tau <= tol) {
                  exitflag = true;
                  break;
                }
              }
            }
          }
          if (exitflag) break;

          if (current.index < 0) continue;
          tau1 = tau - tol;
          for (int l = 0; l < 2; ++l) {
            if (current.data.child[l] >= 0 &&
                dst + current.data.upper[l] >= mindist) {
              if (dst < current.data.lower[l]) {
                d = current.data.lower[l] - dst;
                if (tau1 >= d)
                  todo.push(std::make_pair(-d, current.data.child[l]));
              } else if (dst > current.data.upper[l]) {
                d = dst - current.data.upper[l];
                if (tau1 >= d)
                  todo.push(std::make_pair(-d, current.data.child[l]));
              } else
                todo.push(std::make_pair(dist_t(1), current.data.child[l]));
            }
          }
        }
        ++_k;
        _c1 += c;
        double omc = _mc;
        _mc += (c - omc) / _k;
        _sc += (c - omc) * (c - _mc);
        if (c > _cmax) _cmax = c;
        if (c < _cmin) _cmin = c;
      }

      dist_t d = -1;
      ind.resize(results.size());

      for (int i = int(ind.size()); i--;) {
        ind[i] = int(results.top().second);
        if (i == 0) d = results.top().first;
        results.pop();
      }
      return d;

    }

    /**
     * @return the total number of points in the set.
     **********************************************************************/
    int NumPoints() const { return _numpoints; }

    /**
     * Write the object to an I/O stream.
     *
     * @param[in,out] os the stream to write to.
     * @param[in] bin if true (the default) save in binary mode.
     * @exception std::bad_alloc if memory for the string representation of the
     *   object can't be allocated.
     *
     * The counters tracking the statistics of searches are not saved; however
     * the initializtion cost is saved.  The format of the binary saves is \e
     * not portable.
     *
     * \note
     * Boost serialization can also be used to save and restore a
     * NearestNeighbor object.  This requires that the
     * GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION macro be defined.
     **********************************************************************/
    void Save(std::ostream& os, bool bin = true) const {
      int realspec = std::numeric_limits<dist_t>::digits *
        (std::numeric_limits<dist_t>::is_integer ? -1 : 1);
      if (bin) {
        char id[] = "NearestNeighbor_";
        os.write(id, 16);
        int buf[6];
        buf[0] = version;
        buf[1] = realspec;
        buf[2] = _bucket;
        buf[3] = _numpoints;
        buf[4] = int(_tree.size());
        buf[5] = _cost;
        os.write(reinterpret_cast<const char *>(buf), 6 * sizeof(int));
        for (int i = 0; i < int(_tree.size()); ++i) {
          const Node& node = _tree[i];
          os.write(reinterpret_cast<const char *>(&node.index), sizeof(int));
          if (node.index >= 0) {
            os.write(reinterpret_cast<const char *>(node.data.lower),
                     2 * sizeof(dist_t));
            os.write(reinterpret_cast<const char *>(node.data.upper),
                     2 * sizeof(dist_t));
            os.write(reinterpret_cast<const char *>(node.data.child),
                     2 * sizeof(int));
          } else {
            os.write(reinterpret_cast<const char *>(node.leaves),
                     _bucket * sizeof(int));
          }
        }
      } else {
        std::stringstream ostring;
          // Ensure enough precision for type dist_t.  With C++11, max_digits10
          // can be used instead.
        if (!std::numeric_limits<dist_t>::is_integer) {
          static const int prec
            = int(std::ceil(std::numeric_limits<dist_t>::digits *
                            std::log10(2.0) + 1));
          ostring.precision(prec);
        }
        ostring << version << " " << realspec << " " << _bucket << " "
                << _numpoints << " " << _tree.size() << " " << _cost;
        for (int i = 0; i < int(_tree.size()); ++i) {
          const Node& node = _tree[i];
          ostring << "\n" << node.index;
          if (node.index >= 0) {
            for (int l = 0; l < 2; ++l)
              ostring << " " << node.data.lower[l] << " " << node.data.upper[l]
                      << " " << node.data.child[l];
          } else {
            for (int l = 0; l < _bucket; ++l)
              ostring << " " << node.leaves[l];
          }
        }
        os << ostring.str();
      }
    }

    /**
     * Read the object from an I/O stream.
     *
     * @param[in,out] is the stream to read from
     * @param[in] bin if true (the default) load in binary mode.
     * @exception GeographicErr if the state read from \e is is illegal.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * The counters tracking the statistics of searches are reset by this
     * operation.  Binary data must have been saved on a machine with the same
     * architecture.  If an exception is thrown, the state of the
     * NearestNeighbor is unchanged.
     *
     * \note
     * Boost serialization can also be used to save and restore a
     * NearestNeighbor object.  This requires that the
     * GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION macro be defined.
     *
     * \warning The same arguments \e pts and \e dist used for
     * initialization must be provided to the Search() function.
     **********************************************************************/
    void Load(std::istream& is, bool bin = true) {
      int version1, realspec, bucket, numpoints, treesize, cost;
      if (bin) {
        char id[17];
        is.read(id, 16);
        id[16] = '\0';
        if (!(std::strcmp(id, "NearestNeighbor_") == 0))
          throw GeographicLib::GeographicErr("Bad ID");
        is.read(reinterpret_cast<char *>(&version1), sizeof(int));
        is.read(reinterpret_cast<char *>(&realspec), sizeof(int));
        is.read(reinterpret_cast<char *>(&bucket), sizeof(int));
        is.read(reinterpret_cast<char *>(&numpoints), sizeof(int));
        is.read(reinterpret_cast<char *>(&treesize), sizeof(int));
        is.read(reinterpret_cast<char *>(&cost), sizeof(int));
      } else {
        if (!( is >> version1 >> realspec >> bucket >> numpoints >> treesize
               >> cost ))
          throw GeographicLib::GeographicErr("Bad header");
      }
      if (!( version1 == version ))
        throw GeographicLib::GeographicErr("Incompatible version");
      if (!( realspec == std::numeric_limits<dist_t>::digits *
             (std::numeric_limits<dist_t>::is_integer ? -1 : 1) ))
        throw GeographicLib::GeographicErr("Different dist_t types");
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr("Bad bucket size");
      if (!( 0 <= treesize && treesize <= numpoints ))
        throw
          GeographicLib::GeographicErr("Bad number of points or tree size");
      if (!( 0 <= cost ))
        throw GeographicLib::GeographicErr("Bad value for cost");
      std::vector<Node> tree;
      tree.reserve(treesize);
      for (int i = 0; i < treesize; ++i) {
        Node node;
        if (bin) {
          is.read(reinterpret_cast<char *>(&node.index), sizeof(int));
          if (node.index >= 0) {
            is.read(reinterpret_cast<char *>(node.data.lower),
                    2 * sizeof(dist_t));
            is.read(reinterpret_cast<char *>(node.data.upper),
                    2 * sizeof(dist_t));
            is.read(reinterpret_cast<char *>(node.data.child),
                    2 * sizeof(int));
          } else {
            is.read(reinterpret_cast<char *>(node.leaves),
                    bucket * sizeof(int));
            for (int l = bucket; l < maxbucket; ++l)
              node.leaves[l] = 0;
          }
        } else {
          if (!( is >> node.index ))
            throw GeographicLib::GeographicErr("Bad index");
          if (node.index >= 0) {
            for (int l = 0; l < 2; ++l) {
              if (!( is >> node.data.lower[l] >> node.data.upper[l]
                     >> node.data.child[l] ))
                throw GeographicLib::GeographicErr("Bad node data");
            }
          } else {
            // Must be at least one valid leaf followed by a sequence end
            // markers (-1).
            for (int l = 0; l < bucket; ++l) {
              if (!( is >> node.leaves[l] ))
                throw GeographicLib::GeographicErr("Bad leaf data");
            }
            for (int l = bucket; l < maxbucket; ++l)
              node.leaves[l] = 0;
          }
        }
        node.Check(numpoints, treesize, bucket);
        tree.push_back(node);
      }
      _tree.swap(tree);
      _numpoints = numpoints;
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

    /**
     * Write the object to stream \e os as text.
     *
     * @param[in,out] os the output stream.
     * @param[in] t the NearestNeighbor object to be saved.
     * @exception std::bad_alloc if memory for the string representation of the
     *   object can't be allocated.
     **********************************************************************/
    friend std::ostream& operator<<(std::ostream& os, const NearestNeighbor& t)
    { t.Save(os, false); return os; }

    /**
     * Read the object from stream \e is as text.
     *
     * @param[in,out] is the input stream.
     * @param[out] t the NearestNeighbor object to be loaded.
     * @exception GeographicErr if the state read from \e is is illegal.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     **********************************************************************/
    friend std::istream& operator>>(std::istream& is, NearestNeighbor& t)
    { t.Load(is, false); return is; }

    /**
     * Swap with another NearestNeighbor object.
     *
     * @param[in,out] t the NearestNeighbor object to swap with.
     **********************************************************************/
    void swap(NearestNeighbor& t) {
      std::swap(_numpoints, t._numpoints);
      std::swap(_bucket, t._bucket);
      std::swap(_cost, t._cost);
      _tree.swap(t._tree);
      std::swap(_mc, t._mc);
      std::swap(_sc, t._sc);
      std::swap(_c1, t._c1);
      std::swap(_k, t._k);
      std::swap(_cmin, t._cmin);
      std::swap(_cmax, t._cmax);
    }

    /**
     * The accumulated statistics on the searches so far.
     *
     * @param[out] setupcost the cost of initializing the NearestNeighbor.
     * @param[out] numsearches the number of calls to Search().
     * @param[out] searchcost the total cost of the calls to Search().
     * @param[out] mincost the minimum cost of a Search().
     * @param[out] maxcost the maximum cost of a Search().
     * @param[out] mean the mean cost of a Search().
     * @param[out] sd the standard deviation in the cost of a Search().
     *
     * Here "cost" measures the number of distance calculations needed.  Note
     * that the accumulation of statistics is \e not thread safe.
     **********************************************************************/
    void Statistics(int& setupcost, int& numsearches, int& searchcost,
                    int& mincost, int& maxcost,
                    double& mean, double& sd) const {
      setupcost = _cost; numsearches = _k; searchcost = _c1;
      mincost = _cmin; maxcost = _cmax;
      mean = _mc; sd = std::sqrt(_sc / (_k - 1));
    }

    /**
     * Reset the counters for the accumulated statistics on the searches so
     * far.
     **********************************************************************/
    void ResetStatistics() const {
      _mc = _sc = 0;
      _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

  private:
    // Package up a dist_t and an int.  We will want to sort on the dist_t so
    // put it first.
    typedef std::pair<dist_t, int> item;
    // \cond SKIP
    class Node {
    public:
      struct bounds {
        dist_t lower[2], upper[2]; // bounds on inner/outer distances
        int child[2];
      };
      union {
        bounds data;
        int leaves[maxbucket];
      };
      int index;

      Node()
        : index(-1)
      {
        for (int i = 0; i < 2; ++i) {
          data.lower[i] = data.upper[i] = 0;
          data.child[i] = -1;
        }
      }

      // Sanity check on a Node
      void Check(int numpoints, int treesize, int bucket) const {
        if (!( -1 <= index && index < numpoints ))
          throw GeographicLib::GeographicErr("Bad index");
        if (index >= 0) {
          if (!( -1 <= data.child[0] && data.child[0] < treesize &&
                 -1 <= data.child[1] && data.child[1] < treesize ))
            throw GeographicLib::GeographicErr("Bad child pointers");
          if (!( 0 <= data.lower[0] && data.lower[0] <= data.upper[0] &&
                 data.upper[0] <= data.lower[1] &&
                 data.lower[1] <= data.upper[1] ))
            throw GeographicLib::GeographicErr("Bad bounds");
        } else {
          // Must be at least one valid leaf followed by a sequence end markers
          // (-1).
          bool start = true;
          for (int l = 0; l < bucket; ++l) {
            if (!( (start ?
                    ((l == 0 ? 0 : -1) <= leaves[l] && leaves[l] < numpoints) :
                    leaves[l] == -1) ))
              throw GeographicLib::GeographicErr("Bad leaf data");
            start = leaves[l] >= 0;
          }
          for (int l = bucket; l < maxbucket; ++l) {
            if (leaves[l] != 0)
              throw GeographicLib::GeographicErr("Bad leaf data");
          }
        }
      }

#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      friend class boost::serialization::access;
      template<class Archive>
      void save(Archive& ar, const unsigned int) const {
        ar & boost::serialization::make_nvp("index", index);
        if (index < 0)
          ar & boost::serialization::make_nvp("leaves", leaves);
        else
          ar & boost::serialization::make_nvp("lower", data.lower)
            & boost::serialization::make_nvp("upper", data.upper)
            & boost::serialization::make_nvp("child", data.child);
      }
      template<class Archive>
      void load(Archive& ar, const unsigned int) {
        ar & boost::serialization::make_nvp("index", index);
        if (index < 0)
          ar & boost::serialization::make_nvp("leaves", leaves);
        else
          ar & boost::serialization::make_nvp("lower", data.lower)
            & boost::serialization::make_nvp("upper", data.upper)
            & boost::serialization::make_nvp("child", data.child);
      }
      template<class Archive>
      void serialize(Archive& ar, const unsigned int file_version)
      { boost::serialization::split_member(ar, *this, file_version); }
#endif
    };
    // \endcond
#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class Archive> void save(Archive& ar, const unsigned) const {
      int realspec = std::numeric_limits<dist_t>::digits *
        (std::numeric_limits<dist_t>::is_integer ? -1 : 1);
      // Need to use version1, otherwise load error in debug mode on Linux:
      // undefined reference to GeographicLib::NearestNeighbor<...>::version.
      int version1 = version;
      ar & boost::serialization::make_nvp("version", version1)
        & boost::serialization::make_nvp("realspec", realspec)
        & boost::serialization::make_nvp("bucket", _bucket)
        & boost::serialization::make_nvp("numpoints", _numpoints)
        & boost::serialization::make_nvp("cost", _cost)
        & boost::serialization::make_nvp("tree", _tree);
    }
    template<class Archive> void load(Archive& ar, const unsigned) {
      int version1, realspec, bucket, numpoints, cost;
      ar & boost::serialization::make_nvp("version", version1);
      if (version1 != version)
        throw GeographicLib::GeographicErr("Incompatible version");
      std::vector<Node> tree;
      ar & boost::serialization::make_nvp("realspec", realspec);
      if (!( realspec == std::numeric_limits<dist_t>::digits *
             (std::numeric_limits<dist_t>::is_integer ? -1 : 1) ))
        throw GeographicLib::GeographicErr("Different dist_t types");
      ar & boost::serialization::make_nvp("bucket", bucket);
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr("Bad bucket size");
      ar & boost::serialization::make_nvp("numpoints", numpoints)
        & boost::serialization::make_nvp("cost", cost)
        & boost::serialization::make_nvp("tree", tree);
      if (!( 0 <= int(tree.size()) && int(tree.size()) <= numpoints ))
        throw
          GeographicLib::GeographicErr("Bad number of points or tree size");
      for (int i = 0; i < int(tree.size()); ++i)
        tree[i].Check(numpoints, int(tree.size()), bucket);
      _tree.swap(tree);
      _numpoints = numpoints;
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }
    template<class Archive>
    void serialize(Archive& ar, const unsigned int file_version)
    { boost::serialization::split_member(ar, *this, file_version); }
#endif

    int _numpoints, _bucket, _cost;
    std::vector<Node> _tree;
    // Counters to track stastistics on the cost of searches
    mutable double _mc, _sc;
    mutable int _c1, _k, _cmin, _cmax;

    int init(const std::vector<pos_t>& pts, const distfun_t& dist, int bucket,
             std::vector<Node>& tree, std::vector<item>& ids, int& cost,
             int l, int u, int vp) {

      if (u == l)
        return -1;
      Node node;

      if (u - l > (bucket == 0 ? 1 : bucket)) {

        // choose a vantage point and move it to the start
        int i = vp;
        std::swap(ids[l], ids[i]);

        int m = (u + l + 1) / 2;

        for (int k = l + 1; k  l + 1) {        // node.child[0] is possibly empty
          typename std::vector<item>::iterator
            t = std::min_element(ids.begin() + l + 1, ids.begin() + m);
          node.data.lower[0] = t->first;
          t = std::max_element(ids.begin() + l + 1, ids.begin() + m);
          node.data.upper[0] = t->first;
          // Use point with max distance as vantage point; this point act as a
          // "corner" point and leads to a good partition.
          node.data.child[0] = init(pts, dist, bucket, tree, ids, cost,
                                    l + 1, m, int(t - ids.begin()));
        }
        typename std::vector<item>::iterator
          t = std::max_element(ids.begin() + m, ids.begin() + u);
        node.data.lower[1] = ids[m].first;
        node.data.upper[1] = t->first;
        // Use point with max distance as vantage point here too
        node.data.child[1] = init(pts, dist, bucket, tree, ids, cost,
                                  m, u, int(t - ids.begin()));
      } else {
        if (bucket == 0)
          node.index = ids[l].second;
        else {
          node.index = -1;
          // Sort the bucket entries so that the tree is independent of the
          // implementation of nth_element.
          std::sort(ids.begin() + l, ids.begin() + u);
          for (int i = l; i < u; ++i)
            node.leaves[i-l] = ids[i].second;
          for (int i = u - l; i < bucket; ++i)
            node.leaves[i] = -1;
          for (int i = bucket; i < maxbucket; ++i)
            node.leaves[i] = 0;
        }
      }

      tree.push_back(node);
      return int(tree.size()) - 1;
    }

  };

} // namespace GeographicLib

namespace std {

  /**
   * Swap two GeographicLib::NearestNeighbor objects.
   *
   * @tparam dist_t the type used for measuring distances.
   * @tparam pos_t the type for specifying the positions of points.
   * @tparam distfun_t the type for a function object which calculates
   *   distances between points.
   * @param[in,out] a the first GeographicLib::NearestNeighbor to swap.
   * @param[in,out] b the second GeographicLib::NearestNeighbor to swap.
   **********************************************************************/
  template <typename dist_t, typename pos_t, class distfun_t>
  void swap(GeographicLib::NearestNeighbor<dist_t, pos_t, distfun_t>& a,
            GeographicLib::NearestNeighbor<dist_t, pos_t, distfun_t>& b) {
    a.swap(b);
  }

} // namespace std

#if defined(_MSC_VER)
#  pragma warning (pop)
#endif

#endif  // GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core
*/

#ifndef openOR_core_math_create_hpp
#define openOR_core_math_create_hpp

#include <openOR/Math/vector.hpp>
#include <openOR/Math/matrix.hpp>
#include <openOR/Math/matrixsetaxis.hpp>

#include <boost/mpl/assert.hpp>

namespace openOR {
	namespace Math {

		namespace Impl {
			/**
			* Functor for Impl::VectorCompileIterator in order to copy the content of source vector into destination vector
			* \internal
			*/
			template <class Vec, class VecSrc, int I>
			struct VectorCopy {
				void operator()(Vec& vec, const VecSrc& vecSrc) {
					get<I>(vec) = get<I>(vecSrc);
				}
			};

			template <class Mat, class MatSrc, int I, int J>
			struct MatrixCopy {
				void operator()(Mat& mat, const MatSrc& matSrc) {
					get<I, J>(mat) = get<I, J>(matSrc);
				}
			};

			/**
			* v0 is an vector because the destination vector is larger than Dimension 2.
			* \internal
			*/
			template<class Vec_type, class Type2, int Dim>
			struct CreateVec2 {
				static Vec_type create(const Type2& v0,
					typename VectorTraits<Vec_type>::ValueType v1)
				{
					BOOST_MPL_ASSERT((typename VectorTraits<Vec_type>::IsVector));
					Vec_type t;
					Impl::VectorCompileTimeIterator < Vec_type, 0, VectorTraits<Vec_type>::Dimension::value - 1 > ()
						.template apply<Impl::VectorCopy, Type2>(t, v0);
					get < VectorTraits<Vec_type>::Dimension::value - 1 > (t) = v1;
					return t;
				}
			};

			/**
			* v0 is a ValueType because the vector to create has dimension 2.
			* \internal
			*/
			template<class Type, class Type2>
			struct CreateVec2<Type, Type2, 2> {
				static Type create(const Type2& v0,
					typename VectorTraits<Type>::ValueType v1) {
						Type t;
						get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
						get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
						return t;
				}
			};

			template<class Mat, class MatSrc, class Vec>
			struct CreateMat2 {
				static Mat create(
					const MatSrc& matSrc,
					const Vec& v)
				{
					BOOST_MPL_ASSERT((typename MatrixTraits<Mat>::IsMatrix));
					BOOST_MPL_ASSERT((typename VectorTraits<Vec>::IsVector));
					Mat mat = MatrixTraits<Mat>::IDENTITY;
					Impl::MatrixCompileTimeIterator <Mat, 0, MatrixTraits<Mat>::RowDimension::value - 1, 0, MatrixTraits<Mat>::ColDimension::value - 1> ()
						.template apply<Impl::MatrixCopy, MatSrc>(mat, matSrc);

					for (int i = 0; i < VectorTraits<Vec>::Dimension::value; ++i)
					{
						mat(i, MatrixTraits<Mat>::ColDimension::value - 1) = v(i);
					}
					return mat;
				}
			};

			/**
			*
			* \internal
			*/
			template<class Type, class Type2, class Type3, class IsVector>
			struct Create2 {
				static Type create(const Type2& v0, const Type3& v1)
				{
					return CreateVec2<Type, Type2, VectorTraits<Type>::Dimension::value>::create(v0, v1);
				}
			};

			/**
			*
			* \internal
			*/
			template<class Type, class Type2, class Type3>
			struct Create2<Type, Type2, Type3, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1)
				{
					return CreateMat2<Type, Type2, Type3>::create(v0, v1);
				}
			};

			/**
			* Type is a vector
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class IsVector>
			struct Create3 {
				static Type create(const Type2& v0,
					const Type3& v1,
					const Type4& v2) {
						BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
						Type t;
						get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
						get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
						get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
						return t;
				}
			};

			/**
			* Type is matrix
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4>
			struct Create3<Type, Type2, Type3, Type4, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2) {
					BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));
					Type t = MatrixTraits<Type>::IDENTITY;
					setXAxis(t, v0);
					setYAxis(t, v1);
					setZAxis(t, v2);
					return t;
				}
			};

			/**
			* Type is a vector
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class Type5, class IsVector>
			struct Create4 {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
					BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
					Type t;
					get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
					get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
					get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
					get<3>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v3);
					return t;
				}
			};

			/**
			* Type is matrix
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class Type5>
			struct Create4<Type, Type2, Type3, Type4, Type5, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
					Type t = MatrixTraits<Type>::IDENTITY;
					setXAxis(t, v0);
					setYAxis(t, v1);
					setZAxis(t, v2);
					setTranslation(t, v3);
					return t;
				}
			};

			template<class Type, class Type2, class Type3, class Type4, class Type5, class Type6, class Type7, class IsVector>
			struct Create6 {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3, const Type6& v4, const Type7& v5) {
					BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
					Type t;
					get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
					get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
					get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
					get<3>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v3);
					get<4>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v4);
					get<5>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v5);
					return t;
				}
			};
		}

		/**
		* \brief create
		* \ingroup openOR_core
		*/
		template <class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstVector<Vec>)),
			(Type))
			create(const Vec& vec) {
				Type mat = MatrixTraits<Type>::IDENTITY;
				setTranslation(mat, vec);
				return mat;
		}

		/**
		* @brief Creation of a vector with two parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3>
		inline Type create(const Type2& v0,
			const Type3& v1) {
				return Impl::Create2<Type, Type2, Type3, typename VectorTraits<Type>::IsVector>::create(v0, v1);
		}

		/**
		* @brief Creation of a vector with three parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4>
		inline Type create(const Type2& v0,
			const Type3& v1,
			const Type4& v2) {
				return Impl::Create3<Type, Type2, Type3, Type4, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2);
		}

		/**
		* @brief Creation of a vector with four parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4, class Type5>
		inline Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
			return Impl::Create4<Type, Type2, Type3, Type4, Type5, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2, v3);
		}

		/**
		* @brief Creation of a vector with six parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4, class Type5, class Type6, class Type7>
		inline Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3, const Type6& v4, const Type7& v5) {
			return Impl::Create6<Type, Type2, Type3, Type4, Type5, Type6, Type7, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2, v3, v4, v5);
		}

	}
}

#endif


// Copyright 2004, 2005 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_RMAT_GENERATOR_HPP
#define BOOST_GRAPH_RMAT_GENERATOR_HPP

#include <math.h>
#include <iterator>
#include <utility>
#include <vector>
#include <queue>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/assert.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/detail/mpi_include.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
// #include <boost/test/floating_point_comparison.hpp>

using boost::shared_ptr;
using boost::uniform_01;

// Returns floor(log_2(n)), and -1 when n is 0
template <typename IntegerType>
inline int int_log2(IntegerType n) {
  int l = 0;
  while (n > 0) {++l; n >>= 1;}
  return l - 1;
}

struct keep_all_edges {
  template <typename T>
  bool operator()(const T&, const T&) { return true; }
};

template <typename Distribution, typename ProcessId>
struct keep_local_edges {

  keep_local_edges(const Distribution& distrib, const ProcessId& id)
    : distrib(distrib), id(id)
  { }

  template <typename T>
  bool operator()(const T& x, const T& y)
  { return distrib(x) == id || distrib(y) == id; }

private:
  const Distribution& distrib;
  const ProcessId&    id;
};

template <typename RandomGenerator, typename T>
void
generate_permutation_vector(RandomGenerator& gen, std::vector<T>& vertexPermutation, T n)
{
  using boost::uniform_int;

  vertexPermutation.resize(n);

  // Generate permutation map of vertex numbers
  uniform_int<T> rand_vertex(0, n-1);
  for (T i = 0; i < n; ++i)
    vertexPermutation[i] = i;

  // Can't use std::random_shuffle unless we create another (synchronized) PRNG
  for (T i = 0; i < n; ++i)
    std::swap(vertexPermutation[i], vertexPermutation[rand_vertex(gen)]);
}

template <typename RandomGenerator, typename T>
std::pair<T,T>
generate_edge(shared_ptr<uniform_01<RandomGenerator> > prob, T n,
              unsigned int SCALE, double a, double b, double c, double d)
{
  T u = 0, v = 0;
  T step = n/2;
  for (unsigned int j = 0; j < SCALE; ++j) {
    double p = (*prob)();

    if (p < a)
      ;
    else if (p >= a && p < a + b)
      v += step;
    else if (p >= a + b && p < a + b + c)
      u += step;
    else { // p > a + b + c && p < a + b + c + d
      u += step;
      v += step;
    }

    step /= 2;

    // 0.2 and 0.9 are hardcoded in the reference SSCA implementation.
    // The maximum change in any given value should be less than 10%
    a *= 0.9 + 0.2 * (*prob)();
    b *= 0.9 + 0.2 * (*prob)();
    c *= 0.9 + 0.2 * (*prob)();
    d *= 0.9 + 0.2 * (*prob)();

    double S = a + b + c + d;

    a /= S; b /= S; c /= S;
    // d /= S;
    // Ensure all values add up to 1, regardless of floating point errors
    d = 1. - a - b - c;
  }

  return std::make_pair(u, v);
}

namespace boost {

  /*
    Chakrabarti's R-MAT scale free generator.

    For all flavors of the R-MAT iterator a+b+c+d must equal 1 and for the
    unique_rmat_iterator 'm' << 'n^2'.  If 'm' is too close to 'n^2' the
    generator may be unable to generate sufficient unique edges

    To get a true scale free distribution {a, b, c, d : a > b, a > c, a > d}
  */

  template<typename RandomGenerator, typename Graph>
  class rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    rmat_iterator()
      : gen(), edge(0) { }

    // Initialize for edge generation
    rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                  edges_size_type m, double a, double b, double c,
                  double d, bool permute_vertices = true)
      : gen(), n(n), a(a), b(b), c(c), d(d), edge(m),
        permute_vertices(permute_vertices),
        SCALE(int_log2(n))

    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: Generate the entire adjacency matrix then "Clip and flip" if undirected graph

      // Generate the first edge
      vertices_size_type u, v;
      boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    rmat_iterator& operator++()
    {
      vertices_size_type u, v;
      boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;

      return *this;
    }

    rmat_iterator operator++(int)
    {
      rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const rmat_iterator& other) const
    {
      return edge == other.edge;
    }

    bool operator!=(const rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    vertices_size_type n;
    double a, b, c, d;
    int edge;
    bool permute_vertices;
    int SCALE;

    // Internal data structures
    std::vector<vertices_size_type> vertexPermutation;
    value_type current;
  };

  // Sorted version for CSR
  template <typename T>
  struct sort_pair {
    bool operator() (const std::pair<T,T>& x, const std::pair<T,T>& y)
    {
      if (x.first == y.first)
        return x.second > y.second;
      else
        return x.first > y.first;
    }
  };

  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    sorted_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true)
    { }

    // Initialize for edge generation
    sorted_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), permute_vertices(permute_vertices),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: "Clip and flip" if undirected graph
      int SCALE = int_log2(n);

      for (edges_size_type i = 0; i < m; ++i) {

        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (permute_vertices) {
          if (ep(vertexPermutation[u], vertexPermutation[v]))
            values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
        } else {
          if (ep(u, v))
            values.push(std::make_pair(u, v));
        }

      }

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_rmat_iterator operator++(int)
    {
      sorted_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool permute_vertices;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>, sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    unique_rmat_iterator()
        : gen(), done(true)
    { }

    // Initialize for edge generation
    unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {
        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        // Lowest vertex number always comes first
        // (this means we don't have to worry about i->j and j->i being in the edge list)
        if (u > v && is_same<directed_category, undirected_tag>::value)
          std::swap(u, v);

        if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
          edge_map[std::make_pair(u, v)] = true;

          if (permute_vertices) {
            if (ep(vertexPermutation[u], vertexPermutation[v]))
              values.push_back(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
          } else {
            if (ep(u, v))
              values.push_back(std::make_pair(u, v));
          }

          edges++;
        }
      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.back();
      values.pop_back();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.back();
        values.pop_back();
      } else
        done = true;

      return *this;
    }

    unique_rmat_iterator operator++(int)
    {
      unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;

    // Internal data structures
    std::vector<value_type> values;
    value_type              current;
    bool                    done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    sorted_unique_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true) { }

    // Initialize for edge generation
    sorted_unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                                edges_size_type m, double a, double b, double c,
                                double d, bool bidirectional = false,
                                bool permute_vertices = true,
                                EdgePredicate ep = keep_all_edges())
      : gen(), bidirectional(bidirectional),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {

        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (bidirectional) {
          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;
            edge_map[std::make_pair(v, u)] = true;

            if (ep(u, v)) {
              if (permute_vertices) {
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
                values.push(std::make_pair(vertexPermutation[v], vertexPermutation[u]));
              } else {
                values.push(std::make_pair(u, v));
                values.push(std::make_pair(v, u));
              }
           }

            ++edges;
          }
        } else {
          // Lowest vertex number always comes first
          // (this means we don't have to worry about i->j and j->i being in the edge list)
          if (u > v && is_same<directed_category, undirected_tag>::value)
            std::swap(u, v);

          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;

            if (permute_vertices) {
              if (ep(vertexPermutation[u], vertexPermutation[v]))
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
            } else {
              if (ep(u, v))
                values.push(std::make_pair(u, v));
            }

            ++edges;
          }
        }

      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_unique_rmat_iterator operator++(int)
    {
      sorted_unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool             bidirectional;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>,
                        sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

} // end namespace boost

#include BOOST_GRAPH_MPI_INCLUDE(<boost/graph/distributed/rmat_graph_generator.hpp>)

#endif // BOOST_GRAPH_RMAT_GENERATOR_HPP


#include <NTL/ZZ_p.h>
#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

NTL_THREAD_LOCAL SmartPtr<ZZ_pInfoT> ZZ_pInfo = 0;
NTL_THREAD_LOCAL SmartPtr<ZZ_pTmpSpaceT> ZZ_pTmpSpace = 0;
NTL_THREAD_LOCAL bool ZZ_pInstalled = false;

ZZ_pInfoT::ZZ_pInfoT(const ZZ& NewP)
{
   if (NewP <= 1) LogicError("ZZ_pContext: p must be > 1");

   p = NewP;
   size = p.size();

   ExtendedModulusSize = 2*size +
                 (NTL_BITS_PER_LONG + NTL_ZZ_NBITS - 1)/NTL_ZZ_NBITS;

}

// we use a lazy strategy for initializing and installing
// FFTInfo and TmpSpace related to a ZZ_p modulus.
// The routines GetFFTInfo and GetTmpSpace make sure this process
// is complete.

void ZZ_p::DoInstall()
{
   SmartPtr<ZZ_pTmpSpaceT> tmps = 0;

   do { // NOTE: thread safe lazy init
      Lazy<ZZ_pFFTInfoT>::Builder builder(ZZ_pInfo->FFTInfo);
      if (!builder()) break;

      UniquePtr<ZZ_pFFTInfoT> FFTInfo;
      FFTInfo.make();

      ZZ B, M, M1, M2, M3;
      long n, i;
      long q, t;

      sqr(B, ZZ_pInfo->p);

      LeftShift(B, B, NTL_FFTMaxRoot+NTL_FFTFudge);

      // FIXME: the following is quadratic time...would
      // be nice to get a faster solution...
      // One could estimate the # of primes by summing logs,
      // then multiply using a tree-based multiply, then
      // adjust up or down...

      // Assuming IEEE floating point, the worst case estimate
      // for error guarantees a correct answer +/- 1 for
      // numprimes up to 2^25...for sure we won't be
      // using that many primes...we can certainly put in
      // a sanity check, though.

      // If I want a more accuaruate summation (with using Kahan,
      // which has some portability issues), I could represent
      // numbers as x = a + f, where a is integer and f is the fractional
      // part.  Summing in this representation introduces an *absolute*
      // error of 2 epsilon n, which is just as good as Kahan
      // for this application.

      // same strategy could also be used in the ZZX HomMul routine,
      // if we ever want to make that subquadratic

      set(M);
      n = 0;
      while (M <= B) {
         UseFFTPrime(n);
         q = GetFFTPrime(n);
         n++;
         mul(M, M, q);
      }

      FFTInfo->NumPrimes = n;
      FFTInfo->MaxRoot = CalcMaxRoot(q);

      double fn = double(n);

      if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION)
         ResourceError("modulus too big");

      if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION/double(NTL_SP_BOUND))
         FFTInfo->QuickCRT = false;
      else
         FFTInfo->QuickCRT = true;

      FFTInfo->x.SetLength(n);
      FFTInfo->u.SetLength(n);

      FFTInfo->rem_struct.init(n, ZZ_pInfo->p, GetFFTPrime);

      FFTInfo->crt_struct.init(n, ZZ_pInfo->p, GetFFTPrime);

      if (!FFTInfo->crt_struct.special()) {
         ZZ qq, rr;

         DivRem(qq, rr, M, ZZ_pInfo->p);

         NegateMod(FFTInfo->MinusMModP, rr, ZZ_pInfo->p);

         for (i = 0; i < n; i++) {
            q = GetFFTPrime(i);

            long tt = rem(qq, q);

            mul(M2, ZZ_pInfo->p, tt);
            add(M2, M2, rr);
            div(M2, M2, q);  // = (M/q) rem p

            div(M1, M, q);
            t = rem(M1, q);
            t = InvMod(t, q);

            mul(M3, M2, t);
            rem(M3, M3, ZZ_pInfo->p);

            FFTInfo->crt_struct.insert(i, M3);

            FFTInfo->x[i] = ((double) t)/((double) q);
            FFTInfo->u[i] = t;
         }
      }

      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);

      builder.move(FFTInfo);
   } while (0);

   if (!tmps) {
      const ZZ_pFFTInfoT *FFTInfo = ZZ_pInfo->FFTInfo.get();
      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);
   }

   ZZ_pTmpSpace = tmps;
}

void ZZ_p::init(const ZZ& p)
{
   ZZ_pContext c(p);
   c.restore();
}

void ZZ_pContext::restore() const
{
   ZZ_pInfo = ptr;
   ZZ_pTmpSpace = 0;
   ZZ_pInstalled = false;
}

ZZ_pBak::~ZZ_pBak()
{
   if (MustRestore) c.restore();
}

void ZZ_pBak::save()
{
   c.save();
   MustRestore = true;
}

void ZZ_pBak::restore()
{
   c.restore();
   MustRestore = false;
}

const ZZ_p& ZZ_p::zero()
{
   NTL_THREAD_LOCAL static ZZ_p z(INIT_NO_ALLOC);
   return z;
}

NTL_THREAD_LOCAL
ZZ_p::DivHandlerPtr ZZ_p::DivHandler = 0;

ZZ_p::ZZ_p(INIT_VAL_TYPE, const ZZ& a)  // NO_ALLOC
{
   conv(*this, a);
}

ZZ_p::ZZ_p(INIT_VAL_TYPE, long a) // NO_ALLOC
{
   conv(*this, a);
}

void conv(ZZ_p& x, long a)
{
   if (a == 0)
      clear(x);
   else if (a == 1)
      set(x);
   else {
      NTL_ZZRegister(y);

      conv(y, a);
      conv(x, y);
   }
}

istream& operator>>(istream& s, ZZ_p& x)
{
   NTL_ZZRegister(y);

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(ZZ_p& x, const ZZ_p& a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);

   inv(T, b);
   mul(x, a, T);
}

void inv(ZZ_p& x, const ZZ_p& a)
{
   NTL_ZZRegister(T);

   if (InvModStatus(T, a._ZZ_p__rep, ZZ_p::modulus())) {
      if (!IsZero(a._ZZ_p__rep) && ZZ_p::DivHandler)
         (*ZZ_p::DivHandler)(a);

      InvModError("ZZ_p: division by non-invertible element",
                   a._ZZ_p__rep, ZZ_p::modulus());
   }

   x._ZZ_p__rep = T;
}

long operator==(const ZZ_p& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (b == 1)
      return IsOne(a);

   NTL_ZZ_pRegister(T);
   conv(T, b);
   return a == T;
}

void add(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   add(x, a, T);
}

void sub(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   sub(x, a, T);
}

void sub(ZZ_p& x, long a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);
   conv(T, a);
   sub(x, T, b);
}

void mul(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   mul(x, a, T);
}

void div(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   div(x, a, T);
}

void div(ZZ_p& x, long a, const ZZ_p& b)
{
   if (a == 1) {
      inv(x, b);
   }
   else {
      NTL_ZZ_pRegister(T);
      conv(T, a);
      div(x, T, b);
   }
}

NTL_END_IMPL


/**
 * @file SimPolygon2D.cpp
 * @author Alex Cunningham
 */

#include <iostream>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <gtsam_unstable/geometry/SimPolygon2D.h>

namespace gtsam {

using namespace std;

const size_t max_it = 100000;
boost::minstd_rand SimPolygon2D::rng(42u);

/* ************************************************************************* */
void SimPolygon2D::seedGenerator(unsigned long seed) {
  rng = boost::minstd_rand(seed);
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::createTriangle(const Point2& pA, const Point2& pB, const Point2& pC) {
  SimPolygon2D result;
  result.landmarks_.push_back(pA);
  result.landmarks_.push_back(pB);
  result.landmarks_.push_back(pC);
  return result;
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
  SimPolygon2D result;
  result.landmarks_.push_back(p);
  result.landmarks_.push_back(p + Point2(width, 0.0));
  result.landmarks_.push_back(p + Point2(width, height));
  result.landmarks_.push_back(p + Point2(0.0, height));
  return result;
}

/* ************************************************************************* */
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
  if (p.size() != size()) return false;
  for (size_t i=0; i<size(); ++i)
    if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
      return false;
  return true;
}

/* ************************************************************************* */
void SimPolygon2D::print(const string& s) const {
  cout << "SimPolygon " << s << ": " << endl;
  for(const Point2& p: landmarks_)
    traits<Point2>::Print(p, "   ");
}

/* ************************************************************************* */
vector<SimWall2D> SimPolygon2D::walls() const {
  vector<SimWall2D> result;
  for (size_t i=0; i<size()-1; ++i)
    result.push_back(SimWall2D(landmarks_[i], landmarks_[i+1]));
  result.push_back(SimWall2D(landmarks_[size()-1], landmarks_[0]));
  return result;
}

/* ************************************************************************* */
bool SimPolygon2D::contains(const Point2& c) const {
  vector<SimWall2D> edges = walls();
  bool initialized = false;
  bool lastSide = false;
  for(const SimWall2D& ab: edges) {
    // compute cross product of ab and ac
    Point2 dab = ab.b() - ab.a();
    Point2 dac = c - ab.a();
    double cross = dab.x() * dac.y() - dab.y() * dac.x();
    if (fabs(cross) < 1e-6) // check for on one of the edges
      return true;
    bool side = cross > 0;
    // save the first side found
    if (!initialized) {
      lastSide = side;
      initialized = true;
      continue;
    }

    // to be inside the polygon, point must be on the same side of all lines
    if (lastSide != side)
      return false;
  }
  return true;
}

/* ************************************************************************* */
bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
  for(const Point2& a: landmarks_)
    if (p.contains(a))
      return true;
  for(const Point2& a: p.landmarks_)
    if (contains(a))
      return true;
  return false;
}

/* ***************************************************************** */
bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
  for(const SimPolygon2D& poly: obstacles)
    if (poly.contains(p))
      return true;
  return false;
}

/* ************************************************************************* */
bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
  for(const SimPolygon2D& poly: obstacles)
    if (poly.overlaps(p))
      return true;
  return false;
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::randomTriangle(
    double side_len, double mean_side_len, double sigma_side_len,
    double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
  // get the current set of landmarks
  std::vector<Point2> lms;
  double d2 = side_len/2.0;
  lms.push_back(Point2( d2, d2));
  lms.push_back(Point2(-d2, d2));
  lms.push_back(Point2(-d2,-d2));
  lms.push_back(Point2( d2,-d2));

  for(const SimPolygon2D& poly: existing_polys)
    lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());

  for (size_t i=0; i<max_it; ++i) {

    // find a random pose for line AB
    Pose2 xA(randomAngle(), randomBoundedPoint2(side_len, lms, existing_polys, min_vertex_dist));

    // extend line by random dist and angle to get BC
    double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    double tABC = randomAngle().theta();
    Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC).finished());

    // extend from B to find C
    double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);

    // use triangle equality to verify non-degenerate triangle
    double dAC = distance2(xA.t(), xC.t());

    // form a triangle and test if it meets requirements
    SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());

    // check inside walls, long enough edges, far away from landmarks
    const double thresh = mean_side_len / 2.0;
    if ((dAB + dBC + thresh > dAC) &&  // triangle inequality
        (dAB + dAC + thresh > dBC) &&
        (dAC + dBC + thresh > dAB) &&
        insideBox(side_len, test_tri.landmark(0)) &&
        insideBox(side_len, test_tri.landmark(1)) &&
        insideBox(side_len, test_tri.landmark(2)) &&
        distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
        !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
        !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
        !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
        !anyOverlaps(test_tri, existing_polys)) {
      return test_tri;
    }
  }
  throw runtime_error("Could not find space for a triangle");
  return SimPolygon2D::createTriangle(Point2(99,99), Point2(99,99), Point2(99,99));
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::randomRectangle(
    double side_len, double mean_side_len, double sigma_side_len,
    double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
  // get the current set of landmarks
  std::vector<Point2> lms;
  double d2 = side_len/2.0;
  lms.push_back(Point2( d2, d2));
  lms.push_back(Point2(-d2, d2));
  lms.push_back(Point2(-d2,-d2));
  lms.push_back(Point2( d2,-d2));
  for(const SimPolygon2D& poly: existing_polys)
    lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());

  const Point2 lower_corner(-side_len,-side_len);
  const Point2 upper_corner( side_len, side_len);

  for (size_t i=0; i<max_it; ++i) {

    // pick height and width to be viable distances
    double height = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    double width = randomDistance(mean_side_len, sigma_side_len, min_side_len);

    // find a starting point - limited to region viable for this height/width
    Point2 pA = randomBoundedPoint2(lower_corner, upper_corner - Point2(width, height),
        lms, existing_polys, min_vertex_dist);

    // verify
    SimPolygon2D rect = SimPolygon2D::createRectangle(pA, height, width);

    // check inside walls, long enough edges, far away from landmarks
    if (insideBox(side_len, rect.landmark(0)) &&
        insideBox(side_len, rect.landmark(1)) &&
        insideBox(side_len, rect.landmark(2)) &&
        insideBox(side_len, rect.landmark(3)) &&
        !nearExisting(lms, rect.landmark(0), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(1), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(2), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(3), min_vertex_dist) &&
        !anyOverlaps(rect, existing_polys)) {
      return rect;
    }
  }
  throw runtime_error("Could not find space for a rectangle");
  return SimPolygon2D::createRectangle(Point2(99,99), 100, 100);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomPoint2(double s) {
  boost::uniform_real<>  gen_t(-s/2.0, s/2.0);
  return Point2(gen_t(rng), gen_t(rng));
}

/* ***************************************************************** */
Rot2 SimPolygon2D::randomAngle() {
  boost::uniform_real<>  gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
  return Rot2::fromAngle(gen_r(rng));
}

/* ***************************************************************** */
double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
  boost::normal_distribution<double> norm_dist(mu, sigma);
  boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
  double d = -10.0;
  for (size_t i=0; i<max_it; ++i) {
    d = fabs(gen_d());
    if (d > min_dist)
      return d;
  }
  cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
       << " min_dist = " << min_dist << endl;
  throw runtime_error("Failed to find a viable distance");
  return fabs(norm_dist(rng));
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
      const vector<SimPolygon2D>& obstacles) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
    const std::vector<Point2>& landmarks, double min_landmark_dist) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!nearExisting(landmarks, p, min_landmark_dist))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
    const std::vector<Point2>& landmarks,
    const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(
    const Point2& LL_corner, const Point2& UR_corner,
    const std::vector<Point2>& landmarks,
    const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {

  boost::uniform_real<>  gen_x(0.0, UR_corner.x() - LL_corner.x());
  boost::uniform_real<>  gen_y(0.0, UR_corner.y() - LL_corner.y());

  for (size_t i=0; i<max_it; ++i) {
    Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
    if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
  return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
}

/* ***************************************************************** */
bool SimPolygon2D::insideBox(double s, const Point2& p) {
  return fabs(p.x()) < s/2.0 && fabs(p.y()) < s/2.0;
}

/* ***************************************************************** */
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
    const Point2& p, double threshold) {
  for(const Point2& Sp: S)
    if (distance2(Sp, p) < threshold)
      return true;
  return false;
}

} //\namespace gtsam


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_FUNCTION_DERIVATIVE_NUMERICAL_DERIVATIVE_HPP__
#define __OPTIMISATION2_FUNCTION_DERIVATIVE_NUMERICAL_DERIVATIVE_HPP__

#include "detail_derivative.hpp"
#include <libv/lma/ttt/traits/unroll1.hpp>
#include <boost/fusion/include/vector_tie.hpp>
#include <libv/lma/lm/trait/accessor.hpp>

template<class T >
struct Container;

namespace lma
{
  //! calcul de dérivée numérique avec déroulage static sur les blocks, puis sur les paramètres de chaque block, puis sur chaque résidu

  namespace detail
  {
    template<class Float, std::size_t I, std::size_t Fin> struct TupleDerivator;
    template<class Float, std::size_t Fin> struct TupleDerivator<Float,Fin,Fin>
    {
      template<class A, class B, class C, class D> static void compute(const A&, const B&, const C&, const D&){}
      template<class A, class B, class C> static void compute(const A&, const B&, const C&){}
    };

    template<class Float, std::size_t K, size_t I, std::size_t F> struct TupleDerivatorInternal;
    template<class Float, std::size_t K, size_t F> struct TupleDerivatorInternal<Float,K,F,F>
    {
      template<class Fonction, class Result, class Tuple, class R1> static void compute(const Fonction&, Result&, Tuple&, const R1&){}
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction&, Result&, Tuple&){}
    };

    template<class Float, size_t I, class TIE> struct FinalUnroll
    {
      TIE& tie;
      FinalUnroll(TIE& tie_):tie(tie_){}

      template<size_t K, class A, class D> inline void compute(A& a, const Float& b, const Float& c, const D& d) const
      {
      	static_assert( K==0, "K==0" );
      	std::get<K,I>(a) = ( b - c ) * d;
        //a(K,I) = ( b - c ) * d;
      }

      template<size_t K, class A, class B, class C, class D> inline void compute(A& a, const B& b, const C& c, const D& d) const
      {
	       //a(K,I) = ( std::get<K>(b) - std::get<K>(c) ) * d;
         std::get<K,I>(a) = ( std::get<K>(b) - std::get<K>(c) ) * d;
      }

      template<size_t K> inline void operator()(Int<K> const &) const
      {
	       compute<K>(bf::at_c<0>(tie),bf::at_c<1>(tie),bf::at_c<2>(tie),bf::at_c<3>(tie));
      }
    };

    template<class Float, std::size_t K, size_t I, std::size_t Fin> struct TupleDerivatorInternal
    {
      // forward derivative
      template<class Fonction, class Result, class Tuple, class R> static void compute(const Fonction& fonction, Result& result, Tuple& tuple, const R& r1)
      {
        static const Float h = Float(2.0)*std::sqrt( std::numeric_limits<Float>::epsilon() );
        static const Float _h = Float(1.0) / h ;
        //! to_ref : on crée une référence vers l'objet contenu dans le tuple (uniquement pour simplifier l'écriture)
        //! -> to_ref renvoie T& que l'objet contenu soit T ou T*
        //! at -> renvoie at_c<I>(tuple) ou at_c<I>(map).second

        auto& ref_objet = ttt::to_ref(bf::at_c<K>(tuple));
        auto& jacob = bf::at_c<K>(result).second;

        BOOST_MPL_ASSERT((boost::is_reference<decltype(ref_objet)>));
        assert( (&ref_objet == &ttt::to_ref(bf::at_c<K>(tuple))) );

        auto backup = back_up<I>(ref_objet);
        detail::internal_apply_small_increment(ref_objet,h,v::numeric_tag<I>());
        typedef typename Fonction::ErreurType Residu;
        Residu r2;
        bool b2 = fonction(tuple,r2);
        backup.restore();

        if(b2) // functor evaluation didn't failed
        {
          auto tie = bf::vector_tie(jacob,r2,r1,_h);
          ttt::unroll<0,Size<R>::value>(FinalUnroll<Float,I,decltype(tie)>(tie));
          TupleDerivatorInternal<Float,K,I+1,Fin>::template compute(fonction,result,tuple,r1);
          return;
        }

        set_zero(jacob);
      }

      // central derivative
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction& fonction, Result& result, Tuple& tuple)
      {
        static const Float h = Float(2.0)*std::sqrt( std::numeric_limits<Float>::epsilon() );
        static const Float _h = Float(1.0) / (2.0*h) ;
        //! to_ref : on crée une référence vers l'objet contenu dans le tuple (uniquement pour simplifier l'écriture)
        //! -> to_ref renvoie T& que l'objet contenu soit T ou T*
        //! at -> renvoie at_c<I>(tuple) ou at_c<I>(map).second

        auto& ref_objet = ttt::to_ref(bf::at_c<K>(tuple));
        auto& jacob = bf::at_c<K>(result).second;

        BOOST_MPL_ASSERT((boost::is_reference<decltype(ref_objet)>));
        assert( (&ref_objet == &ttt::to_ref(bf::at_c<K>(tuple))) );

        typedef typename Fonction::ErreurType Residu;
        Residu r2;

        auto backup = back_up<I>(ref_objet);
        detail::internal_apply_small_increment(ref_objet,h,v::numeric_tag<I>());
        bool b2 = fonction(tuple,r2);
        backup.restore();
        if (b2)
        {
          detail::internal_apply_small_increment(ref_objet,-h,v::numeric_tag<I>());
          Residu r1;
          bool b1 = fonction(tuple,r1);
          backup.restore();

          if(b1)
          {
            auto tie = bf::vector_tie(jacob,r2,r1,_h);
            ttt::unroll<0,Size<Residu>::value>(FinalUnroll<Float,I,decltype(tie)>(tie));
            TupleDerivatorInternal<Float,K,I+1,Fin>::template compute(fonction,result,tuple);
            return;
          }
        }
        set_zero(jacob);
      }
    };

    template<class Float, std::size_t I, std::size_t Fin> struct TupleDerivator
    {
      // numerical central derivative
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction& fonction, Result& result, Tuple& tuple)
      {
        static const size_t F = Size<decltype(ttt::to_ref(bf::at_c<I>(tuple)))>::value;
        TupleDerivatorInternal<Float,I,0,F>::template compute(fonction,result,tuple);
        TupleDerivator<Float,I+1,Fin>::template compute(fonction,result,tuple);
      }

      // numerical foward derivative
      template<class Fonction, class Result, class Tuple, class R1> static void compute(const Fonction& fonction, Result& result, Tuple& tuple, const R1& r1)
      {
        static const size_t F = Size<decltype(ttt::to_ref(bf::at_c<I>(tuple)))>::value;
        TupleDerivatorInternal<Float,I,0,F>::template compute(fonction,result,tuple,r1);
        TupleDerivator<Float,I+1,Fin>::template compute(fonction,result,tuple,r1);
      }
    };
  }// eon detail

  template<class Tag> struct NumericalDerivator
  {
    //! le tuple en entrée ne doit pas être constant car il sera modifié, et remis à l'état d'origine
    template<class Fonctor, class Tuple, class Jacob>
    static void derive(const Function<Fonctor>& fonctor, Tuple tuple, Jacob& result)
    {
      detail::TupleDerivator<typename Tag::second_type, 0,br::size<Jacob>::value>::template compute(fonctor,result,tuple);
    }

    template<class Fonctor, class Tuple, class Jacob, class Residual>
    static void derive(const Function<Fonctor>& fonctor, Tuple tuple, Jacob& result, const Residual& residual)
    {
      detail::TupleDerivator<typename Tag::second_type, 0,br::size<Jacob>::value>::template compute(fonctor,result,tuple,residual);
    }
  };

}// eon

namespace ttt
{
  template<class T> struct Name<lma::NumericalDerivator<T>> { static std::string name(){ return "NumericalDerivator<" + ttt::name<T>() + ">"; } };
}

#endif