/**
 * @file tests/rectangle_tree_test.cpp
 * @author Andrew Wells
 *
 * Tests for the RectangleTree class.  This should ensure that the class works
 * correctly and that subsequent changes don't break anything.  Because it's
 * only used to test the trees, it is slow.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/tree/tree_traits.hpp>
#include <mlpack/core/tree/rectangle_tree.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::neighbor;
using namespace mlpack::tree;
using namespace mlpack::metric;

BOOST_AUTO_TEST_SUITE(RectangleTreeTest);

// Test the traits on RectangleTrees.

BOOST_AUTO_TEST_CASE(RectangleTreeTraitsTest)
{
  // Children may be overlapping.
  bool b = TreeTraits<RTree<EuclideanDistance, EmptyStatistic,
      arma::mat>>::HasOverlappingChildren;
  BOOST_REQUIRE_EQUAL(b, true);

  // Points are not contained in multiple levels.
  b = TreeTraits<RTree<EuclideanDistance, EmptyStatistic,
      arma::mat>>::HasSelfChildren;
  BOOST_REQUIRE_EQUAL(b, false);
}

// Test to make sure the tree can be contains the correct number of points after
// it is constructed.

BOOST_AUTO_TEST_CASE(RectangleTreeConstructionCountTest)
{
  arma::mat dataset;
  dataset.randu(3, 1000); // 1000 points in 3 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;

  TreeType tree(dataset, 20, 6, 5, 2, 0);
  TreeType tree2 = tree;

  BOOST_REQUIRE_EQUAL(tree.NumDescendants(), 1000);
  BOOST_REQUIRE_EQUAL(tree2.NumDescendants(), 1000);
}

/**
 * A function to return a std::vector containing pointers to each point in the
 * tree.
 *
 * @param tree The tree that we want to extract all of the points from.
 * @return A vector containing pointers to each point in this tree.
 */
template<typename TreeType>
std::vector<arma::vec*> GetAllPointsInTree(const TreeType& tree)
{
  std::vector<arma::vec*> vec;
  if (tree.NumChildren() > 0)
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      std::vector<arma::vec*> tmp = GetAllPointsInTree(tree.Child(i));
      vec.insert(vec.begin(), tmp.begin(), tmp.end());
    }
  }
  else
  {
    for (size_t i = 0; i < tree.Count(); i++)
    {
      arma::vec* c = new arma::vec(tree.Dataset().col(tree.Point(i)));
      vec.push_back(c);
    }
  }
  return vec;
}

// Test to ensure that none of the points in the tree are duplicates.  This,
// combined with the above test to see how many points are in the tree, should
// ensure that we inserted all points.
BOOST_AUTO_TEST_CASE(RectangleTreeConstructionRepeatTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;

  TreeType tree(dataset, 20, 6, 5, 2, 0);

  std::vector<arma::vec*> allPoints = GetAllPointsInTree(tree);
  for (size_t i = 0; i < allPoints.size(); i++)
  {
    for (size_t j = i + 1; j < allPoints.size(); j++)
    {
      arma::vec v1 = *(allPoints[i]);
      arma::vec v2 = *(allPoints[j]);
      bool same = true;
      for (size_t k = 0; k < v1.n_rows; k++)
        same &= (v1[k] == v2[k]);

      BOOST_REQUIRE_NE(same, true);
    }
  }

  for (size_t i = 0; i < allPoints.size(); i++)
    delete allPoints[i];
}

/**
 * A function to check that each non-leaf node fully encloses its child nodes
 * and that each leaf node encloses its points.  It recurses so that it checks
 * each node under (and including) this one.
 *
 * @param tree The tree to check.
 */
template<typename TreeType>
void CheckContainment(const TreeType& tree)
{
  if (tree.NumChildren() == 0)
  {
    for (size_t i = 0; i < tree.Count(); i++)
      BOOST_REQUIRE(tree.Bound().Contains(
          tree.Dataset().unsafe_col(tree.Point(i))));
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      for (size_t j = 0; j < tree.Bound().Dim(); j++)
      {
        //  All children should be covered by the parent node.
        //  Some children can be empty (only in case of the R++ tree)
        bool success = (tree.Child(i).Bound()[j].Hi() ==
                std::numeric_limits<typename TreeType::ElemType>::lowest() &&
                tree.Child(i).Bound()[j].Lo() ==
                std::numeric_limits<typename TreeType::ElemType>::max()) ||
            tree.Bound()[j].Contains(tree.Child(i).Bound()[j]);

        BOOST_REQUIRE(success);
      }

      CheckContainment(tree.Child(i));
    }
  }
}

/**
 * A function to check that containment is as tight as possible.
 */
template<typename TreeType>
void CheckExactContainment(const TreeType& tree)
{
  if (tree.NumChildren() == 0)
  {
    for (size_t i = 0; i < tree.Bound().Dim(); i++)
    {
      double min = DBL_MAX;
      double max = -1.0 * DBL_MAX;
      for (size_t j = 0; j < tree.Count(); j++)
      {
        if (tree.Dataset().col(tree.Point(j))[i] < min)
          min = tree.Dataset().col(tree.Point(j))[i];
        if (tree.Dataset().col(tree.Point(j))[i] > max)
          max = tree.Dataset().col(tree.Point(j))[i];
      }
      BOOST_REQUIRE_EQUAL(max, tree.Bound()[i].Hi());
      BOOST_REQUIRE_EQUAL(min, tree.Bound()[i].Lo());
    }
  }
  else
  {
    for (size_t i = 0; i < tree.Bound().Dim(); i++)
    {
      double min = DBL_MAX;
      double max = -1.0 * DBL_MAX;
      for (size_t j = 0; j < tree.NumChildren(); j++)
      {
        if (tree.Child(j).Bound()[i].Lo() < min)
          min = tree.Child(j).Bound()[i].Lo();
        if (tree.Child(j).Bound()[i].Hi() > max)
          max = tree.Child(j).Bound()[i].Hi();
      }

      BOOST_REQUIRE_EQUAL(max, tree.Bound()[i].Hi());
      BOOST_REQUIRE_EQUAL(min, tree.Bound()[i].Lo());
    }

    for (size_t i = 0; i < tree.NumChildren(); i++)
      CheckExactContainment(tree.Child(i));
  }
}

/**
 * A function to check that parents and children are set correctly.
 */
template<typename TreeType>
void CheckHierarchy(const TreeType& tree)
{
  for (size_t i = 0; i < tree.NumChildren(); i++)
  {
    BOOST_REQUIRE_EQUAL(&tree, tree.Child(i).Parent());
    CheckHierarchy(tree.Child(i));
  }
}

// Test to see if the bounds of the tree are correct. (Cover all bounds and
// points beneath this node of the tree).
BOOST_AUTO_TEST_CASE(RectangleTreeContainmentTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;

  TreeType tree(dataset, 20, 6, 5, 2, 0);
  CheckContainment(tree);
  CheckExactContainment(tree);
}

/**
 * A function to check that each of the fill requirements is met.  For a
 * non-leaf node:
 *
 * MinNumChildren() <= NumChildren() <= MaxNumChildren()
 * For a leaf node:
 * MinLeafSize() <= Count() <= MaxLeafSize
 *
 * It recurses so that it checks each node under (and including) this one.
 * @param tree The tree to check.
 */
template<typename TreeType>
void CheckFills(const TreeType& tree)
{
  if (tree.IsLeaf())
  {
    BOOST_REQUIRE(tree.Count() >= tree.MinLeafSize() || tree.Parent() == NULL);
    BOOST_REQUIRE(tree.Count() <= tree.MaxLeafSize());
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      BOOST_REQUIRE(tree.NumChildren() >= tree.MinNumChildren() ||
                    tree.Parent() == NULL);
      BOOST_REQUIRE(tree.NumChildren() <= tree.MaxNumChildren());
      CheckFills(tree.Child(i));
    }
  }
}

// Test to ensure that the minimum and maximum fills are satisfied.
BOOST_AUTO_TEST_CASE(CheckMinAndMaxFills)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;

  TreeType tree(dataset, 20, 6, 5, 2, 0);
  CheckFills(tree);
}

/**
 * A function to get the height of this tree.  Though it should equal
 * tree.TreeDepth(), we ensure that every leaf node is on the same level by
 * doing it this way.
 *
 * @param tree The tree for which we want the height.
 * @return The height of this tree.
 */
template<typename TreeType>
int GetMaxLevel(const TreeType& tree)
{
  int max = 1;
  if (!tree.IsLeaf())
  {
    int m = 0;
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      int n = GetMaxLevel(tree.Child(i));
      if (n > m)
        m = n;
    }
    max += m;
  }

  return max;
}

/**
 * A function to get the "shortest height" of this tree.  Though it should equal
 * tree.TreeDepth(), we ensure that every leaf node is on the same level by
 * doing it this way.
 *
 * @param tree The tree for which we want the height.
 * @return The "shortest height" of the tree.
 */
template<typename TreeType>
int GetMinLevel(const TreeType& tree)
{
  int min = 1;
  if (!tree.IsLeaf())
  {
    int m = INT_MAX;
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      int n = GetMinLevel(tree.Child(i));
      if (n < m)
        m = n;
    }
    min += m;
  }

  return min;
}

/**
 * A function to check that numDescendants values are set correctly.
 */
template<typename TreeType>
size_t CheckNumDescendants(const TreeType& tree)
{
  if (tree.IsLeaf())
  {
    BOOST_REQUIRE_EQUAL(tree.NumDescendants(), tree.Count());
    return tree.Count();
  }

  size_t numDescendants = 0;

  for (size_t i = 0; i < tree.NumChildren(); i++)
    numDescendants += CheckNumDescendants(tree.Child(i));

  BOOST_REQUIRE_EQUAL(tree.NumDescendants(), numDescendants);

  return numDescendants;
}

// A test to ensure that all leaf nodes are stored on the same level of the
// tree.
BOOST_AUTO_TEST_CASE(TreeBalance)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;

  TreeType tree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(GetMinLevel(tree), GetMaxLevel(tree));
  BOOST_REQUIRE_EQUAL(tree.TreeDepth(), GetMinLevel(tree));
}

// A test to see if point deletion is working correctly.  We build a tree, then
// delete numIter points and test that the query gives correct results.  It is
// remotely possible that this test will give a false negative if it should
// happen that two points are the same distance from a third point.
BOOST_AUTO_TEST_CASE(PointDeletion)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  arma::mat querySet;
  querySet.randu(8, 500);

  const int numIter = 50;

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType tree(dataset, 20, 6, 5, 2, 0);

  for (int i = 0; i < numIter; i++)
    tree.DeletePoint(999 - i);

  // Do a few sanity checks.  Ensure each point is unique, the tree has the
  // correct number of points, the tree has legal containment, and the tree's
  // data is in sync.
  std::vector<arma::vec*> allPoints = GetAllPointsInTree(tree);
  for (size_t i = 0; i < allPoints.size(); i++)
  {
    for (size_t j = i + 1; j < allPoints.size(); j++)
    {
      arma::vec v1 = *(allPoints[i]);
      arma::vec v2 = *(allPoints[j]);
      bool same = true;
      for (size_t k = 0; k < v1.n_rows; k++)
        same &= (v1[k] == v2[k]);

      BOOST_REQUIRE(!same);
    }
  }

  for (size_t i = 0; i < allPoints.size(); i++)
    delete allPoints[i];

  BOOST_REQUIRE_EQUAL(tree.NumDescendants(), 1000 - numIter);

  CheckContainment(tree);
  CheckExactContainment(tree);
  CheckNumDescendants(tree);

  // Single-tree search.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      RTree> knn1(std::move(tree), SINGLE_TREE_MODE);

  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  knn1.Search(querySet, 5, neighbors1, distances1);

  arma::mat newDataset;
  newDataset = dataset;
  newDataset.resize(8, 1000-numIter);

  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search the naive way.
  KNN knn2(newDataset, NAIVE_MODE);

  knn2.Search(querySet, 5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
  }
}

// A test to see if dynamic point insertion is working correctly.
// We build a tree, then add numIter points and test that the query gives
// correct results.  It is remotely possible that this test will give a false
// negative if it should happen that two points are the same distance from a
// third point.  Note that this is extremely inefficient.  You should not use
// dynamic insertion until a better solution for resizing matrices is available.
BOOST_AUTO_TEST_CASE(PointDynamicAdd)
{
  const int numIter = 50;
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType tree(dataset, 20, 6, 5, 2, 0);

  // Add numIter new points to the dataset.  The tree copies the dataset, so we
  // must modify both the original dataset and the one that the tree holds.
  // (This API is clunky.  It should be redone sometime.)
  tree.Dataset().reshape(8, 1000 + numIter);
  dataset.reshape(8, 1000 + numIter);
  arma::mat tmpData;
  tmpData.randu(8, numIter);
  for (int i = 0; i < numIter; i++)
  {
    tree.Dataset().col(1000 + i) = tmpData.col(i);
    dataset.col(1000 + i) = tmpData.col(i);
    tree.InsertPoint(1000 + i);
  }

  // Do a few sanity checks.  Ensure each point is unique, the tree has the
  // correct number of points, the tree has legal containment, and the tree's
  // data is in sync.
  std::vector<arma::vec*> allPoints = GetAllPointsInTree(tree);
  for (size_t i = 0; i < allPoints.size(); i++)
  {
    for (size_t j = i + 1; j < allPoints.size(); j++)
    {
      arma::vec v1 = *(allPoints[i]);
      arma::vec v2 = *(allPoints[j]);
      bool same = true;
      for (size_t k = 0; k < v1.n_rows; k++)
        same &= (v1[k] == v2[k]);

      BOOST_REQUIRE(!same);
    }
  }

  for (size_t i = 0; i < allPoints.size(); i++)
    delete allPoints[i];

  BOOST_REQUIRE_EQUAL(tree.NumDescendants(), 1000 + numIter);
  CheckContainment(tree);
  CheckExactContainment(tree);
  CheckNumDescendants(tree);

  // Now we will compare the output of the R Tree vs the output of a naive
  // search.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search with the R tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      RTree> knn1(std::move(tree), SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
  }
}

// A test to ensure that the SingleTreeTraverser is working correctly by
// comparing its results to the results of a naive search.
BOOST_AUTO_TEST_CASE(SingleTreeTraverserTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  typedef RStarTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType rTree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(rTree.NumDescendants(), 1000);

  CheckContainment(rTree);
  CheckExactContainment(rTree);
  CheckHierarchy(rTree);
  CheckNumDescendants(rTree);

  // Nearest neighbor search with the R tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      RStarTree> knn1(std::move(rTree), SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

// A test to ensure that the SingleTreeTraverser is working correctly by
// comparing its results to the results of a naive search.
BOOST_AUTO_TEST_CASE(XTreeTraverserTest)
{
  arma::mat dataset;

  const int numP = 1000;

  dataset.randu(8, numP); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  typedef XTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType xTree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(xTree.NumDescendants(), numP);

  CheckContainment(xTree);
  CheckExactContainment(xTree);
  CheckHierarchy(xTree);
  CheckNumDescendants(xTree);

  // Nearest neighbor search with the X tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      XTree> knn1(std::move(xTree), SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

BOOST_AUTO_TEST_CASE(HilbertRTreeTraverserTest)
{
  arma::mat dataset;

  const int numP = 1000;

  dataset.randu(8, numP); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  typedef HilbertRTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;
  TreeType hilbertRTree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(hilbertRTree.NumDescendants(), numP);

  CheckContainment(hilbertRTree);
  CheckExactContainment(hilbertRTree);
  CheckHierarchy(hilbertRTree);
  CheckNumDescendants(hilbertRTree);

  // Nearest neighbor search with the Hilbert R tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      HilbertRTree> knn1(std::move(hilbertRTree), SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

template<typename TreeType>
void CheckHilbertOrdering(const TreeType& tree)
{
  if (tree.IsLeaf())
  {
    for (size_t i = 0; i < tree.NumPoints() - 1; i++)
      BOOST_REQUIRE_LE(tree.AuxiliaryInfo().HilbertValue().ComparePoints(
          tree.Dataset().col(tree.Point(i)),
          tree.Dataset().col(tree.Point(i + 1))),
          0);

    BOOST_REQUIRE_EQUAL(tree.AuxiliaryInfo().HilbertValue().CompareWith(
        tree.Dataset().col(tree.Point(tree.NumPoints() - 1))),
        0);
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren() - 1; i++)
      BOOST_REQUIRE_LE(tree.AuxiliaryInfo().HilbertValue().CompareValues(
          tree.Child(i).AuxiliaryInfo().HilbertValue(),
          tree.Child(i + 1).AuxiliaryInfo().HilbertValue()),
          0);

    BOOST_REQUIRE_EQUAL(tree.AuxiliaryInfo().HilbertValue().CompareWith(
        tree.Child(tree.NumChildren() - 1).AuxiliaryInfo().HilbertValue()),
        0);

    for (size_t i = 0; i < tree.NumChildren(); i++)
      CheckHilbertOrdering(tree.Child(i));
  }
}

BOOST_AUTO_TEST_CASE(HilbertRTreeOrderingTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef HilbertRTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;
  TreeType hilbertRTree(dataset, 20, 6, 5, 2, 0);

  CheckHilbertOrdering(hilbertRTree);
}

template<typename TreeType>
void CheckDiscreteHilbertValueSync(const TreeType& tree)
{
  typedef DiscreteHilbertValue<typename TreeType::ElemType>
      HilbertValue;
  typedef typename HilbertValue::HilbertElemType HilbertElemType;

  if (tree.IsLeaf())
  {
    const HilbertValue& value = tree.AuxiliaryInfo().HilbertValue();

    for (size_t i = 0; i < tree.NumPoints(); i++)
    {
      arma::Col<HilbertElemType> pointValue =
          HilbertValue::CalculateValue(tree.Dataset().col(tree.Point(i)));

      const int equal = HilbertValue::CompareValues(
          value.LocalHilbertValues()->col(i), pointValue);

      BOOST_REQUIRE_EQUAL(equal, 0);
    }
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
      CheckDiscreteHilbertValueSync(tree.Child(i));
  }
}

BOOST_AUTO_TEST_CASE(DiscreteHilbertValueSyncTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef HilbertRTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;
  TreeType hilbertRTree(dataset, 20, 6, 5, 2, 0);

  CheckDiscreteHilbertValueSync(hilbertRTree);
}

BOOST_AUTO_TEST_CASE(DiscreteHilbertValueTest)
{
  arma::vec point01(1);
  arma::vec point02(1);

  point01[0] = -DBL_MAX;
  point02[0] = DBL_MAX;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = -DBL_MAX;
  point02[0] = -100;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = -100;
  point02[0] = -1;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = -1;
  point02[0] = -std::numeric_limits<double>::min();

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = -std::numeric_limits<double>::min();
  point02[0] = 0;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = 0;
  point02[0] = std::numeric_limits<double>::min();

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = std::numeric_limits<double>::min();
  point02[0] = 1;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = 1;
  point02[0] = 100;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  point01[0] = 100;
  point02[0] = DBL_MAX;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point01,
                                                                  point02), -1);

  arma::vec point1(2);
  arma::vec point2(2);

  point1[0] = -DBL_MAX;
  point1[1] = -DBL_MAX;

  point2[0] = 0;
  point2[1] = 0;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point1,
                                                                  point2), -1);

  point1[0] = -1;
  point1[1] = -1;

  point2[0] = 1;
  point2[1] = -1;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point1,
                                                                  point2), -1);

  point1[0] = -1;
  point1[1] = -1;

  point2[0] = -1;
  point2[1] = 1;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point1,
                                                                  point2), -1);

  point1[0] = -DBL_MAX + 1;
  point1[1] = -DBL_MAX + 1;

  point2[0] = -1;
  point2[1] = -1;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point1,
                                                                  point2), -1);

  point1[0] = DBL_MAX * 0.75;
  point1[1] = DBL_MAX * 0.75;

  point2[0] = DBL_MAX * 0.25;
  point2[1] = DBL_MAX * 0.25;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point1,
                                                                  point2), 1);

  arma::vec point3(4);
  arma::vec point4(4);

  point3[0] = -DBL_MAX;
  point3[1] = -DBL_MAX;
  point3[2] = -DBL_MAX;
  point3[3] = -DBL_MAX;

  point4[0] = 1.0;
  point4[1] = 1.0;
  point4[2] = 1.0;
  point4[3] = 1.0;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point3,
                                                                  point4), -1);

  point3[0] = -DBL_MAX;
  point3[1] = DBL_MAX;
  point3[2] = DBL_MAX;
  point3[3] = DBL_MAX;

  point4[0] = DBL_MAX;
  point4[1] = DBL_MAX;
  point4[2] = DBL_MAX;
  point4[3] = DBL_MAX;

  BOOST_REQUIRE_EQUAL(DiscreteHilbertValue<double>::ComparePoints(point3,
                                                                  point4), -1);
}

template<typename TreeType>
void CheckHilbertValue(const TreeType& tree)
{
  typedef DiscreteHilbertValue<typename TreeType::ElemType>
      HilbertValue;

  const HilbertValue& value = tree.AuxiliaryInfo().HilbertValue();

  if (tree.IsLeaf())
  {
    BOOST_REQUIRE_EQUAL(value.OwnsLocalHilbertValues(), true);
    return;
  }

  for (size_t i = 0; i < tree.NumChildren(); i++)
  {
    const HilbertValue& childValue =
        tree.Child(i).AuxiliaryInfo().HilbertValue();
    BOOST_REQUIRE_EQUAL(value.ValueToInsert(), childValue.ValueToInsert());
  }

  const HilbertValue& childValue =
      tree.Child(tree.NumChildren() - 1).AuxiliaryInfo().HilbertValue();
  BOOST_REQUIRE_EQUAL(value.LocalHilbertValues(),
      childValue.LocalHilbertValues());

  if (!tree.Parent())
    BOOST_REQUIRE_EQUAL(value.OwnsValueToInsert(), true);
  else
    BOOST_REQUIRE_EQUAL(value.OwnsValueToInsert(), false);

  BOOST_REQUIRE_EQUAL(value.OwnsLocalHilbertValues(), false);

  for (size_t i = 0; i < tree.NumChildren(); i++)
    CheckHilbertValue(tree.Child(i));
}

BOOST_AUTO_TEST_CASE(HilbertRTeeCopyConstructorTest)
{
  typedef HilbertRTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;

  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  TreeType tree(dataset, 20, 6, 5, 2, 0);
  TreeType copy(tree);

  CheckHilbertValue(copy);
  CheckDiscreteHilbertValueSync(copy);
  CheckHilbertOrdering(copy);
  CheckContainment(copy);
  CheckExactContainment(copy);
  CheckHierarchy(copy);
  CheckNumDescendants(copy);
}

BOOST_AUTO_TEST_CASE(HilbertRTeeMoveConstructorTest)
{
  typedef HilbertRTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;

  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  TreeType tree(dataset, 20, 6, 5, 2, 0);
  TreeType copy(std::move(tree));

  CheckHilbertValue(copy);
  CheckDiscreteHilbertValueSync(copy);
  CheckHilbertOrdering(copy);
  CheckContainment(copy);
  CheckExactContainment(copy);
  CheckHierarchy(copy);
  CheckNumDescendants(copy);
}

template<typename TreeType>
void CheckOverlap(const TreeType& tree)
{
  bool success = true;

  // Check if two nodes overlap each other.
  for (size_t i = 0; i < tree.NumChildren(); i++)
  {
    success = true;

    for (size_t j = 0; j < tree.NumChildren(); j++)
    {
      if (j == i)
        continue;

      success = !tree.Child(i).Bound().Contains(tree.Child(j).Bound());

      if (!success)
        break;
    }
    if (!success)
      break;
  }
  BOOST_REQUIRE_EQUAL(success, true);

  for (size_t i = 0; i < tree.NumChildren(); i++)
    CheckOverlap(tree.Child(i));
}

BOOST_AUTO_TEST_CASE(RPlusTreeOverlapTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  typedef RPlusTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;
  TreeType rPlusTree(dataset, 20, 6, 5, 2, 0);

  CheckOverlap(rPlusTree);

  // Children can not be overlapping.
  bool b = TreeTraits<TreeType>::HasOverlappingChildren;
  BOOST_REQUIRE_EQUAL(b, false);

  // Ensure that all leaf nodes are at the same level.
  BOOST_REQUIRE_EQUAL(GetMinLevel(rPlusTree), GetMaxLevel(rPlusTree));
  BOOST_REQUIRE_EQUAL(rPlusTree.TreeDepth(), GetMinLevel(rPlusTree));
}

BOOST_AUTO_TEST_CASE(RPlusTreeTraverserTest)
{
  arma::mat dataset;

  const int numP = 1000;

  dataset.randu(8, numP); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  typedef RPlusTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat > TreeType;
  TreeType rPlusTree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(rPlusTree.NumDescendants(), numP);

  CheckContainment(rPlusTree);
  CheckExactContainment(rPlusTree);
  CheckHierarchy(rPlusTree);
  CheckOverlap(rPlusTree);
  CheckNumDescendants(rPlusTree);

  // Nearest neighbor search with the R+ tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      RPlusTree > knn1(std::move(rPlusTree), SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

template<typename TreeType>
void CheckRPlusPlusTreeBound(const TreeType& tree)
{
  typedef bound::HRectBound<metric::EuclideanDistance,
      typename TreeType::ElemType> Bound;

  bool success = true;

  // Ensure that the maximum bounding rectangle contains all children.
  for (size_t k = 0; k < tree.Bound().Dim(); k++)
  {
    BOOST_REQUIRE_LE(tree.Bound()[k].Hi(),
        tree.AuxiliaryInfo().OuterBound()[k].Hi());
    BOOST_REQUIRE_LE(tree.AuxiliaryInfo().OuterBound()[k].Lo(),
        tree.Bound()[k].Lo());
  }

  if (tree.IsLeaf())
  {
    // Ensure that the maximum bounding rectangle contains all points.
    for (size_t i = 0; i < tree.Count(); i++)
      BOOST_REQUIRE_EQUAL(true,
          tree.Bound().Contains(tree.Dataset().col(tree.Point(i))));

    return;
  }

  // Ensure that two children's maximum bounding rectangles do not overlap
  // each other.
  for (size_t i = 0; i < tree.NumChildren(); i++)
  {
    const Bound& bound1 = tree.Child(i).AuxiliaryInfo().OuterBound();
    success = true;

    for (size_t j = 0; j < tree.NumChildren(); j++)
    {
      if (j == i)
        continue;
      const Bound& bound2 = tree.Child(j).AuxiliaryInfo().OuterBound();

      success = !bound1.Contains(bound2);

      if (!success)
        break;
    }
    if (!success)
      break;
  }
  BOOST_REQUIRE_EQUAL(success, true);

  for (size_t i = 0; i < tree.NumChildren(); i++)
    CheckRPlusPlusTreeBound(tree.Child(i));
}

BOOST_AUTO_TEST_CASE(RPlusPlusTreeBoundTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.

  // Check the MinimalCoverageSweep.
  typedef RPlusPlusTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat> TreeType;
  TreeType rPlusPlusTree(dataset, 20, 6, 5, 2, 0);

  CheckRPlusPlusTreeBound(rPlusPlusTree);

  // Children can not be overlapping.
  bool b = TreeTraits<TreeType>::HasOverlappingChildren;
  BOOST_REQUIRE_EQUAL(b, false);

  BOOST_REQUIRE_EQUAL(GetMinLevel(rPlusPlusTree), GetMaxLevel(rPlusPlusTree));
  BOOST_REQUIRE_EQUAL(rPlusPlusTree.TreeDepth(), GetMinLevel(rPlusPlusTree));

  // Check the MinimalSplitsNumberSweep.
  typedef RectangleTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat,
      RPlusTreeSplit<RPlusPlusTreeSplitPolicy, MinimalCoverageSweep>,
      RPlusPlusTreeDescentHeuristic, RPlusPlusTreeAuxiliaryInformation>
          RPlusPlusTreeMinimalSplits;

  RPlusPlusTreeMinimalSplits rPlusPlusTree2(dataset, 20, 6, 5, 2, 0);

  CheckRPlusPlusTreeBound(rPlusPlusTree2);

  BOOST_REQUIRE_EQUAL(GetMinLevel(rPlusPlusTree2), GetMaxLevel(rPlusPlusTree2));
  BOOST_REQUIRE_EQUAL(rPlusPlusTree2.TreeDepth(), GetMinLevel(rPlusPlusTree2));
}

BOOST_AUTO_TEST_CASE(RPlusPlusTreeTraverserTest)
{
  arma::mat dataset;

  const int numP = 1000;

  dataset.randu(8, numP); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  typedef RPlusPlusTree<EuclideanDistance,
      NeighborSearchStat<NearestNeighborSort>, arma::mat > TreeType;
  TreeType rPlusPlusTree(dataset, 20, 6, 5, 2, 0);

  BOOST_REQUIRE_EQUAL(rPlusPlusTree.NumDescendants(), numP);

  CheckContainment(rPlusPlusTree);
  CheckExactContainment(rPlusPlusTree);
  CheckHierarchy(rPlusPlusTree);
  CheckRPlusPlusTreeBound(rPlusPlusTree);
  CheckNumDescendants(rPlusPlusTree);

  // Nearest neighbor search with the R++ tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>,
      arma::mat, RPlusPlusTree > knn1(std::move(rPlusPlusTree),
      SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

// Test the tree splitting.  We set MaxLeafSize and MaxNumChildren rather low
// to allow us to test by hand without adding hundreds of points.
BOOST_AUTO_TEST_CASE(RTreeSplitTest)
{
  arma::mat data = arma::trans(arma::mat("0.0 0.0;"
                                         "0.0 1.0;"
                                         "1.0 0.1;"
                                         "1.0 0.5;"
                                         "0.7 0.3;"
                                         "0.9 0.9;"
                                         "0.5 0.6;"
                                         "0.6 0.3;"
                                         "0.1 0.5;"
                                         "0.3 0.7;"));

  typedef RTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType rTree(data, 5, 2, 2, 1, 0);

  // There's technically no reason they have to be in a certain order, so we
  // use firstChild etc. to arbitrarily name them.
  BOOST_REQUIRE_EQUAL(rTree.NumChildren(), 2);
  BOOST_REQUIRE_EQUAL(rTree.NumDescendants(), 10);
  BOOST_REQUIRE_EQUAL(rTree.TreeDepth(), 3);

  int firstChild = 0, secondChild = 1;
  if (rTree.Child(firstChild).NumChildren() == 2)
  {
    firstChild = 1;
    secondChild = 0;
  }

  BOOST_REQUIRE_SMALL(rTree.Child(firstChild).Bound()[0].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(firstChild).Bound()[0].Hi(), 0.1,
      1e-15);
  BOOST_REQUIRE_SMALL(rTree.Child(firstChild).Bound()[1].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(firstChild).Bound()[1].Hi(), 1.0,
      1e-15);

  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[0].Lo(), 0.3,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[0].Hi(), 1.0,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[1].Lo(), 0.1,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[1].Hi(), 0.9,
      1e-15);

  BOOST_REQUIRE_EQUAL(rTree.Child(firstChild).NumChildren(), 1);
  BOOST_REQUIRE_SMALL(
      rTree.Child(firstChild).Child(0).Bound()[0].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(firstChild).Child(0).Bound()[0].Hi(), 0.1,
      1e-15);
  BOOST_REQUIRE_SMALL(
      rTree.Child(firstChild).Child(0).Bound()[1].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(firstChild).Child(0).Bound()[1].Hi(), 1.0,
      1e-15);
  BOOST_REQUIRE_EQUAL(rTree.Child(firstChild).Child(0).Count(), 3);

  int firstPrime = 0, secondPrime = 1;
  if (rTree.Child(secondChild).Child(firstPrime).Count() == 3)
  {
    firstPrime = 1;
    secondPrime = 0;
  }

  BOOST_REQUIRE_EQUAL(rTree.Child(secondChild).NumChildren(), 2);
  BOOST_REQUIRE_EQUAL(
      rTree.Child(secondChild).Child(firstPrime).Count(), 4);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[0].Lo(),
      0.3, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[0].Hi(),
      0.7, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[1].Lo(),
      0.3, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[1].Hi(),
      0.7, 1e-15);

  BOOST_REQUIRE_EQUAL(
      rTree.Child(secondChild).Child(secondPrime).Count(), 3);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[0].Lo(),
      0.9, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[0].Hi(),
      1.0, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[1].Lo(),
      0.1, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[1].Hi(),
      0.9, 1e-15);
}

// Test the tree splitting.  We set MaxLeafSize and MaxNumChildren rather low
// to allow us to test by hand without adding hundreds of points.
BOOST_AUTO_TEST_CASE(RStarTreeSplitTest)
{
  arma::mat data = arma::trans(arma::mat("0.0 0.0;"
                                         "0.0 1.0;"
                                         "1.0 0.1;"
                                         "1.0 0.5;"
                                         "0.7 0.3;"
                                         "0.9 0.9;"
                                         "0.5 0.6;"
                                         "0.6 0.3;"
                                         "0.1 0.5;"
                                         "0.3 0.7;"));

  typedef RStarTree<EuclideanDistance, NeighborSearchStat<NearestNeighborSort>,
    arma::mat> TreeType;

  TreeType rTree(data, 5, 2, 2, 1, 0);

  // There's technically no reason they have to be in a certain order, so we
  // use firstChild etc. to arbitrarily name them.
  BOOST_REQUIRE_EQUAL(rTree.NumChildren(), 2);
  BOOST_REQUIRE_EQUAL(rTree.NumDescendants(), 10);
  BOOST_REQUIRE_EQUAL(rTree.TreeDepth(), 3);

  int firstChild = 0, secondChild = 1;
  if (rTree.Child(firstChild).NumChildren() == 2)
  {
    firstChild = 1;
    secondChild = 0;
  }

  BOOST_REQUIRE_SMALL(rTree.Child(firstChild).Bound()[0].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(firstChild).Bound()[0].Hi(), 0.1,
      1e-15);
  BOOST_REQUIRE_SMALL(rTree.Child(firstChild).Bound()[1].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(firstChild).Bound()[1].Hi(), 1.0,
      1e-15);

  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[0].Lo(), 0.3,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[0].Hi(), 1.0,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[1].Lo(), 0.1,
      1e-15);
  BOOST_REQUIRE_CLOSE(rTree.Child(secondChild).Bound()[1].Hi(), 0.9,
      1e-15);

  BOOST_REQUIRE_EQUAL(rTree.Child(firstChild).NumChildren(), 1);
  BOOST_REQUIRE_SMALL(
      rTree.Child(firstChild).Child(0).Bound()[0].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(firstChild).Child(0).Bound()[0].Hi(), 0.1, 1e-15);
  BOOST_REQUIRE_SMALL(
      rTree.Child(firstChild).Child(0).Bound()[1].Lo(), 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(firstChild).Child(0).Bound()[1].Hi(), 1.0, 1e-15);
  BOOST_REQUIRE_EQUAL(rTree.Child(firstChild).Child(0).Count(), 3);

  int firstPrime = 0, secondPrime = 1;
  if (rTree.Child(secondChild).Child(firstPrime).Count() == 3)
  {
    firstPrime = 1;
    secondPrime = 0;
  }

  BOOST_REQUIRE_EQUAL(rTree.Child(secondChild).NumChildren(), 2);
  BOOST_REQUIRE_EQUAL(
      rTree.Child(secondChild).Child(firstPrime).Count(), 4);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[0].Lo(),
      0.3, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[0].Hi(),
      0.7, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[1].Lo(),
      0.3, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(firstPrime).Bound()[1].Hi(),
      0.7, 1e-15);

  BOOST_REQUIRE_EQUAL(
      rTree.Child(secondChild).Child(secondPrime).Count(), 3);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[0].Lo(),
      0.9, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[0].Hi(),
      1.0, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[1].Lo(),
      0.1, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Child(secondChild).Child(secondPrime).Bound()[1].Hi(),
      0.9, 1e-15);
}

BOOST_AUTO_TEST_CASE(RectangleTreeMoveDatasetTest)
{
  arma::mat dataset = arma::randu<arma::mat>(3, 1000);
  typedef RTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;

  TreeType tree(std::move(dataset));

  BOOST_REQUIRE_EQUAL(dataset.n_elem, 0);
  BOOST_REQUIRE_EQUAL(tree.Dataset().n_rows, 3);
  BOOST_REQUIRE_EQUAL(tree.Dataset().n_cols, 1000);
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef DIJKSTRAS_HPP
#define DIJKSTRAS_HPP

#include <random>
#include <cstdint>
#include <functional>
#include <limits>
#include <queue>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/serialization.hpp>

namespace arc_dijkstras
{
    class GraphEdge
    {
    protected:

        int64_t from_index_;
        int64_t to_index_;
        double weight_;

    public:

        static uint64_t Serialize(const GraphEdge& edge, std::vector<uint8_t>& buffer)
        {
            return edge.SerializeSelf(buffer);
        }

        static std::pair<GraphEdge, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            GraphEdge temp_edge;
            const uint64_t bytes_read = temp_edge.DeserializeSelf(buffer, current);
            return std::make_pair(temp_edge, bytes_read);
        }

        GraphEdge(const int64_t from_index, const int64_t to_index, const double weight)
            : from_index_(from_index), to_index_(to_index), weight_(weight)
        {}

        GraphEdge()
            : from_index_(-1), to_index_(-1), weight_(0.0)
        {}

        uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
        {
            const uint64_t start_buffer_size = buffer.size();
            arc_utilities::SerializeFixedSizePOD<int64_t>(from_index_, buffer);
            arc_utilities::SerializeFixedSizePOD<int64_t>(to_index_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(weight_, buffer);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            assert(current < buffer.size());
            uint64_t current_position = current;
            const std::pair<int64_t, uint64_t> deserialized_from_index = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            from_index_ = deserialized_from_index.first;
            current_position += deserialized_from_index.second;
            const std::pair<int64_t, uint64_t> deserialized_to_index = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            to_index_ = deserialized_to_index.first;
            current_position += deserialized_to_index.second;
            const std::pair<double, uint64_t> deserialized_weight = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            weight_ = deserialized_weight.first;
            current_position += deserialized_weight.second;
            // Figure out how many bytes were read
            const uint64_t bytes_read = current_position - current;
            return bytes_read;
        }

        bool operator==(const GraphEdge& other) const
        {
            return (from_index_ == other.GetFromIndex() && to_index_ == other.GetToIndex() && weight_ == other.GetWeight());
        }

        std::string Print() const
        {
            return std::string("(" + std::to_string(from_index_) + "->" + std::to_string(to_index_) + ") : " + std::to_string(weight_));
        }

        int64_t GetFromIndex() const
        {
            return from_index_;
        }

        int64_t GetToIndex() const
        {
            return to_index_;
        }

        double GetWeight() const
        {
            return weight_;
        }

        void SetFromIndex(const int64_t new_from_index)
        {
            from_index_ = new_from_index;
        }

        void SetToIndex(const int64_t new_to_index)
        {
            to_index_ = new_to_index;
        }

        void SetWeight(const double new_weight)
        {
            weight_ = new_weight;
        }
    };

    inline std::ostream& operator<< (std::ostream& stream, const GraphEdge& edge)
    {
        stream << edge.Print();
        return stream;
    }

    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
    class GraphNode
    {
    protected:

        NodeValueType value_;
        double distance_;
        std::vector<GraphEdge> in_edges_;
        std::vector<GraphEdge> out_edges_;

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        static uint64_t Serialize(
                const GraphNode<NodeValueType, Allocator>& node,
                std::vector<uint8_t>& buffer,
                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
        {
            return node.SerializeSelf(buffer, value_serializer);
        }

        static std::pair<GraphNode<NodeValueType, Allocator>, uint64_t> Deserialize(
                const std::vector<uint8_t>& buffer,
                const uint64_t current,
                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            GraphNode<NodeValueType, Allocator> temp_node;
            const uint64_t bytes_read = temp_node.DeserializeSelf(buffer, current, value_deserializer);
            return std::make_pair(temp_node, bytes_read);
        }

        GraphNode(
                const NodeValueType& value,
                const double distance,
                const std::vector<GraphEdge>& new_in_edges,
                const std::vector<GraphEdge>& new_out_edges)
            : value_(value)
            , distance_(distance)
            , in_edges_(new_in_edges)
            , out_edges_(new_out_edges)
        {}

        GraphNode(const NodeValueType& value)
            : value_(value)
            , distance_(std::numeric_limits<double>::infinity())
        {}

        GraphNode()
            : distance_(std::numeric_limits<double>::infinity())
        {}

        uint64_t SerializeSelf(
                std::vector<uint8_t>& buffer,
                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
        {
            const uint64_t start_buffer_size = buffer.size();
            // Serialize the value
            value_serializer(value_, buffer);
            // Serialize the distance
            arc_utilities::SerializeFixedSizePOD<double>(distance_, buffer);
            // Serialize the in edges
            arc_utilities::SerializeVector<GraphEdge>(in_edges_, buffer, GraphEdge::Serialize);
            // Serialize the in edges
            arc_utilities::SerializeVector<GraphEdge>(out_edges_, buffer, GraphEdge::Serialize);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        uint64_t DeserializeSelf(
                const std::vector<uint8_t>& buffer,
                const uint64_t current,
                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            assert(current < buffer.size());
            uint64_t current_position = current;
            // Deserialize the value
            const std::pair<NodeValueType, uint64_t> value_deserialized = value_deserializer(buffer, current_position);
            value_ = value_deserialized.first;
            current_position += value_deserialized.second;
            // Deserialize the distace
            const std::pair<double, uint64_t> distance_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            distance_ = distance_deserialized.first;
            current_position += distance_deserialized.second;
            // Deserialize the in edges
            const std::pair<std::vector<GraphEdge>, uint64_t> in_edges_deserialized = arc_utilities::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
            in_edges_ = in_edges_deserialized.first;
            current_position += in_edges_deserialized.second;
            // Deserialize the out edges
            const std::pair<std::vector<GraphEdge>, uint64_t> out_edges_deserialized = arc_utilities::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
            out_edges_ = out_edges_deserialized.first;
            current_position += out_edges_deserialized.second;
            // Figure out how many bytes were read
            const uint64_t bytes_read = current_position - current;
            return bytes_read;
        }

        std::string Print() const
        {
            std::ostringstream strm;
            strm << "Node : " << distance_ << " In Edges : ";
            if (in_edges_.size() > 0)
            {
                strm << in_edges_[0].Print();
                for (size_t idx = 1; idx < in_edges_.size(); idx++)
                {
                    strm << ", " << in_edges_[idx].Print();
                }
            }
            strm << " Out Edges : ";
            if (out_edges_.size() > 0)
            {
                strm << out_edges_[0].Print();
                for (size_t idx = 1; idx < out_edges_.size(); idx++)
                {
                    strm << ", " << out_edges_[idx].Print();
                }
            }
            return strm.str();
        }

        const NodeValueType& GetValueImmutable() const
        {
            return value_;
        }

        NodeValueType& GetValueMutable()
        {
            return value_;
        }

        void AddInEdge(const GraphEdge& new_in_edge)
        {
            in_edges_.push_back(new_in_edge);
        }

        void AddOutEdge(const GraphEdge& new_out_edge)
        {
            out_edges_.push_back(new_out_edge);
        }

        void AddEdgePair(const GraphEdge& new_in_edge, const GraphEdge& new_out_edge)
        {
            AddInEdge(new_in_edge);
            AddOutEdge(new_out_edge);
        }

        double GetDistance() const
        {
            return distance_;
        }

        void SetDistance(const double distance)
        {
            distance_ = distance;
        }

        const std::vector<GraphEdge>& GetInEdgesImmutable() const
        {
            return in_edges_;
        }

        std::vector<GraphEdge>& GetInEdgesMutable()
        {
            return in_edges_;
        }

        const std::vector<GraphEdge>& GetOutEdgesImmutable() const
        {
            return out_edges_;
        }

        std::vector<GraphEdge>& GetOutEdgesMutable()
        {
            return out_edges_;
        }

        void SetInEdges(const std::vector<GraphEdge>& new_in_edges)
        {
            in_edges_ = new_in_edges;
        }

        void SetOutEdges(const std::vector<GraphEdge>& new_out_edges)
        {
            out_edges_ = new_out_edges;
        }
    };

    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
    class Graph
    {
    protected:

        std::vector<GraphNode<NodeValueType, Allocator>> nodes_;

        size_t MarkConnectedComponentUndirected(
                const int64_t starting_node_idx,
                std::vector<uint32_t>& components,
                const uint32_t component_id) const
        {
            // When we push into the queue, we mark as connected, so we don't need a separate "queued" tracker to
            // avoid duplication, unlike what is used in CollisionMapGrid::MarkConnectedComponent
            std::queue<int64_t> working_queue;
            working_queue.push(starting_node_idx);
            size_t num_marked = 1;

            while (working_queue.size() > 0)
            {
                const auto next_node_idx = working_queue.front();
                working_queue.pop();

                const auto& out_edges = nodes_[next_node_idx].GetOutEdgesImmutable();
                for (const auto& edge : out_edges)
                {
                    const auto neighbour_idx = edge.GetToIndex();
                    if (components[neighbour_idx] == 0)
                    {
                        components[neighbour_idx] = component_id;
                        ++num_marked;
                        working_queue.push(neighbour_idx);
                    }
                }
            }

            return num_marked;
        }

    public:

        static uint64_t Serialize(
                const Graph<NodeValueType,
                Allocator>& graph, std::vector<uint8_t>& buffer,
                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
        {
            return graph.SerializeSelf(buffer, value_serializer);
        }

        static std::pair<Graph<NodeValueType, Allocator>, uint64_t> Deserialize(
                const std::vector<uint8_t>& buffer, const uint64_t current,
                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            Graph<NodeValueType, Allocator> temp_graph;
            const uint64_t bytes_read = temp_graph.DeserializeSelf(buffer, current, value_deserializer);
            return std::make_pair(temp_graph, bytes_read);
        }

        Graph(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
        {
            if (CheckGraphLinkage(nodes))
            {
                nodes_ = nodes;
            }
            else
            {
                throw std::invalid_argument("Invalid graph linkage");
            }
        }

        Graph(const size_t expected_size)
        {
            nodes_.reserve(expected_size);
        }

        Graph()
        {}

        uint64_t SerializeSelf(
                std::vector<uint8_t>& buffer,
                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
        {
            const uint64_t start_buffer_size = buffer.size();
            const auto graph_state_serializer = std::bind(GraphNode<NodeValueType, Allocator>::Serialize, std::placeholders::_1, std::placeholders::_2, value_serializer);
            arc_utilities::SerializeVector<GraphNode<NodeValueType, Allocator>>(nodes_, buffer, graph_state_serializer);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        uint64_t DeserializeSelf(
                const std::vector<uint8_t>& buffer,
                const uint64_t current,
                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            const auto graph_state_deserializer = std::bind(GraphNode<NodeValueType, Allocator>::Deserialize, std::placeholders::_1, std::placeholders::_2, value_deserializer);
            const auto deserialized_nodes = arc_utilities::DeserializeVector<GraphNode<NodeValueType, Allocator>>(buffer, current, graph_state_deserializer);
            nodes_ = deserialized_nodes.first;
            return deserialized_nodes.second;
        }

        std::string Print() const
        {
            std::ostringstream strm;
            strm << "Graph - Nodes : ";
            if (nodes_.size() > 0)
            {
                strm << nodes_[0].Print();
                for (size_t idx = 1; idx < nodes_.size(); idx++)
                {
                    strm << "\n" << nodes_[idx].Print();
                }
            }
            return strm.str();
        }

        void ShrinkToFit()
        {
            nodes_.shrink_to_fit();
        }

        bool CheckGraphLinkage() const
        {
            return CheckGraphLinkage(GetNodesImmutable());
        }

        static bool CheckGraphLinkage(const Graph<NodeValueType, Allocator>& graph)
        {
            return CheckGraphLinkage(graph.GetNodesImmutable());
        }

        static bool CheckGraphLinkage(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
        {
            // Go through every node and make sure the edges are valid
            for (size_t idx = 0; idx < nodes.size(); idx++)
            {
                const GraphNode<NodeValueType, Allocator>& current_node = nodes[idx];
                // Check the in edges first
                const std::vector<GraphEdge>& in_edges = current_node.GetInEdgesImmutable();
                for (size_t in_edge_idx = 0; in_edge_idx < in_edges.size(); in_edge_idx++)
                {
                    const GraphEdge& current_edge = in_edges[in_edge_idx];
                    // Check from index to make sure it's in bounds
                    const int64_t from_index = current_edge.GetFromIndex();
                    if (from_index < 0 || from_index >= (int64_t)nodes.size())
                    {
                        return false;
                    }
                    // Check to index to make sure it matches our own index
                    const int64_t to_index = current_edge.GetToIndex();
                    if (to_index != (int64_t)idx)
                    {
                        return false;
                    }
                    // Check edge validity (edges to ourself are not allowed)
                    if (from_index == to_index)
                    {
                        return false;
                    }
                    // Check to make sure that the from index node is linked to us
                    const GraphNode<NodeValueType, Allocator>& from_node = nodes[(size_t)from_index];
                    const std::vector<GraphEdge>& from_node_out_edges = from_node.GetOutEdgesImmutable();
                    bool from_node_connection_valid = false;
                    // Make sure at least one out edge of the from index node corresponds to the current node
                    for (size_t from_node_out_edge_idx = 0; from_node_out_edge_idx < from_node_out_edges.size(); from_node_out_edge_idx++)
                    {
                        const GraphEdge& current_from_node_out_edge = from_node_out_edges[from_node_out_edge_idx];
                        if (current_from_node_out_edge.GetToIndex() == (int64_t)idx)
                        {
                            from_node_connection_valid = true;
                        }
                    }
                    if (from_node_connection_valid == false)
                    {
                        return false;
                    }
                }
                // Check the out edges second
                const std::vector<GraphEdge>& out_edges = current_node.GetOutEdgesImmutable();
                for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                {
                    const GraphEdge& current_edge = out_edges[out_edge_idx];
                    // Check from index to make sure it matches our own index
                    const int64_t from_index = current_edge.GetFromIndex();
                    if (from_index != (int64_t)idx)
                    {
                        return false;
                    }
                    // Check to index to make sure it's in bounds
                    const int64_t to_index = current_edge.GetToIndex();
                    if (to_index < 0 || to_index >= (int64_t)nodes.size())
                    {
                        return false;
                    }
                    // Check edge validity (edges to ourself are not allowed)
                    if (from_index == to_index)
                    {
                        return false;
                    }
                    // Check to make sure that the to index node is linked to us
                    const GraphNode<NodeValueType, Allocator>& to_node = nodes[(size_t)to_index];
                    const std::vector<GraphEdge>& to_node_in_edges = to_node.GetInEdgesImmutable();
                    bool to_node_connection_valid = false;
                    // Make sure at least one in edge of the to index node corresponds to the current node
                    for (size_t to_node_in_edge_idx = 0; to_node_in_edge_idx < to_node_in_edges.size(); to_node_in_edge_idx++)
                    {
                        const GraphEdge& current_to_node_in_edge = to_node_in_edges[to_node_in_edge_idx];
                        if (current_to_node_in_edge.GetFromIndex() == (int64_t)idx)
                        {
                            to_node_connection_valid = true;
                        }
                    }
                    if (to_node_connection_valid == false)
                    {
                        return false;
                    }
                }
            }
            return true;
        }

        const std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesImmutable() const
        {
            return nodes_;
        }

        std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesMutable()
        {
            return nodes_;
        }

        const GraphNode<NodeValueType, Allocator>& GetNodeImmutable(const int64_t index) const
        {
            assert(index >= 0);
            assert(index < (int64_t)nodes_.size());
            return nodes_[(size_t)index];
        }

        GraphNode<NodeValueType, Allocator>& GetNodeMutable(const int64_t index)
        {
            assert(index >= 0);
            assert(index < (int64_t)nodes_.size());
            return nodes_[(size_t)index];
        }

        int64_t AddNode(const GraphNode<NodeValueType, Allocator>& new_node)
        {
            nodes_.push_back(new_node);
            return (int64_t)(nodes_.size() - 1);
        }

        int64_t AddNode(const NodeValueType& new_value)
        {
            nodes_.push_back(GraphNode<NodeValueType, Allocator>(new_value));
            return (int64_t)(nodes_.size() - 1);
        }

        void AddEdgeBetweenNodes(const int64_t from_index, const int64_t to_index, const double edge_weight)
        {
            assert(from_index >= 0);
            assert(from_index < (int64_t)nodes_.size());
            assert(to_index >= 0);
            assert(to_index < (int64_t)nodes_.size());
            assert(from_index != to_index);
            const GraphEdge new_edge(from_index, to_index, edge_weight);
            GetNodeMutable(from_index).AddOutEdge(new_edge);
            GetNodeMutable(to_index).AddInEdge(new_edge);
        }

        void AddEdgesBetweenNodes(const int64_t first_index, const int64_t second_index, const double edge_weight)
        {
            assert(first_index >= 0);
            assert(first_index < (int64_t)nodes_.size());
            assert(second_index >= 0);
            assert(second_index < (int64_t)nodes_.size());
            assert(first_index != second_index);
            const GraphEdge first_edge(first_index, second_index, edge_weight);
            GetNodeMutable(first_index).AddOutEdge(first_edge);
            GetNodeMutable(second_index).AddInEdge(first_edge);
            const GraphEdge second_edge(second_index, first_index, edge_weight);
            GetNodeMutable(second_index).AddOutEdge(second_edge);
            GetNodeMutable(first_index).AddInEdge(second_edge);
        }

        /**
         * @brief GetConnectedComponentsUndirected
         * @return A vector of the component ids for each node, and the total number of components
         */
        std::pair<std::vector<uint32_t>, uint32_t> GetConnectedComponentsUndirected() const
        {
            size_t total_num_marked = 0;
            auto connected_components = std::make_pair(std::vector<uint32_t>(nodes_.size(), 0), 0u);

            for (size_t node_idx = 0; node_idx < nodes_.size() && total_num_marked < nodes_.size(); ++node_idx)
            {
                // If we have not yet marked this node, then mark it and anything it can reach
                if (connected_components.first[node_idx] == 0)
                {
                    connected_components.second++;
                    size_t num_marked = MarkConnectedComponentUndirected(node_idx, connected_components.first, connected_components.second);
                    total_num_marked += num_marked;
                }
            }
            return connected_components;
        }
    };

    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
    class SimpleDijkstrasAlgorithm
    {
    protected:

        class CompareIndexFn
        {
            public:

                constexpr bool operator()(const std::pair<int64_t, double>& lhs, const std::pair<int64_t, double>& rhs) const
                {
                    return lhs.second > rhs.second;
                }
        };

        SimpleDijkstrasAlgorithm() {}
    public:

        typedef std::pair<Graph<NodeValueType, Allocator>, std::pair<std::vector<int64_t>, std::vector<double>>> DijkstrasResult;

        static DijkstrasResult PerformDijkstrasAlgorithm(
                const Graph<NodeValueType, Allocator>& graph,
                const int64_t start_index)
        {
            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Start index out of range");
            }
            Graph<NodeValueType, Allocator> working_copy = graph;
            // Setup
            std::vector<int64_t> previous_index_map(working_copy.GetNodesImmutable().size(), -1);
            std::vector<double> distances(working_copy.GetNodesImmutable().size(), std::numeric_limits<double>::infinity());
            std::priority_queue<std::pair<int64_t, double>, std::vector<std::pair<int64_t, double>>, CompareIndexFn> queue;
            std::unordered_map<int64_t, uint32_t> explored(graph.GetNodesImmutable().size());
            for (size_t idx = 0; idx < working_copy.GetNodesImmutable().size(); idx++)
            {
                working_copy.GetNodeMutable((int64_t)idx).SetDistance(std::numeric_limits<double>::infinity());
                queue.push(std::make_pair((int64_t)idx, std::numeric_limits<double>::infinity()));
            }
            working_copy.GetNodeMutable(start_index).SetDistance(0.0);
            previous_index_map[(size_t)start_index] = start_index;
            distances[(size_t)start_index] = 0.0;
            queue.push(std::make_pair(start_index, 0.0));
            while (queue.size() > 0)
            {
                const std::pair<int64_t, double> top_node = queue.top();
                const int64_t& top_node_index = top_node.first;
                const double& top_node_distance = top_node.second;
                queue.pop();
                if (explored[top_node.first] > 0)
                {
                    // We've already been here
                    continue;
                }
                else
                {
                    // Note that we've been here
                    explored[top_node.first] = 1;
                    // Get our neighbors
                    const std::vector<GraphEdge>& neighbor_edges = working_copy.GetNodeImmutable(top_node_index).GetInEdgesImmutable();
                    // Go through our neighbors
                    for (size_t neighbor_idx = 0; neighbor_idx < neighbor_edges.size(); neighbor_idx++)
                    {
                        const int64_t neighbor_index = neighbor_edges[neighbor_idx].GetFromIndex();
                        const double neighbor_edge_weight = neighbor_edges[neighbor_idx].GetWeight();
                        const double new_neighbor_distance = top_node_distance + neighbor_edge_weight;
                        // Check against the neighbor
                        const double stored_neighbor_distance = working_copy.GetNodeImmutable(neighbor_index).GetDistance();
                        if (new_neighbor_distance < stored_neighbor_distance)
                        {
                            // We've found a better way to get to this node
                            // Check if it's already been explored
                            if (explored[neighbor_index] > 0)
                            {
                                // If it's already been explored, we just update it in place
                                working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                            }
                            else
                            {
                                // If it hasn't been explored, we need to update it and add it to the queue
                                working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                                queue.push(std::make_pair(neighbor_index, new_neighbor_distance));
                            }
                            // Update that we're the best previous node
                            previous_index_map[(size_t)neighbor_index] = top_node_index;
                            distances[(size_t)neighbor_index] = new_neighbor_distance;
                        }
                        else
                        {
                            // Do nothing
                            continue;
                        }
                    }
                }
            }
            return std::make_pair(working_copy, std::make_pair(previous_index_map, distances));
        }

        // These functions have not been tested.  Use with care.
        static uint64_t SerializeDijstrasResult(
                const DijkstrasResult& result,
                std::vector<uint8_t>& buffer,
                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
        {
            const uint64_t start_buffer_size = buffer.size();
            // Serialize the graph
            result.first.SerializeSelf(buffer, value_serializer);
            // Serialize the previous index
            const auto index_serializer = std::bind(arc_utilities::SerializeFixedSizePOD<int64_t>, std::placeholders::_1, std::placeholders::_2);
            SerializeVector(result.second.first, index_serializer);
            // Serialze the distances
            const auto distance_serializer = std::bind(arc_utilities::SerializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2);
            SerializeVector(result.second.second, distance_serializer);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        // These functions have not been tested.  Use with care.
        static std::pair<DijkstrasResult, uint64_t> DijstrasResult(
                const std::vector<uint8_t>& buffer,
                const uint64_t current,
                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            assert(current < buffer.size());
            uint64_t current_position = current;
            // Deserialize the graph itself
            std::pair<DijkstrasResult, uint64_t> deserialized;
            const auto graph_deserialized = Graph<NodeValueType, Allocator>::Deserialize(buffer, current_position, value_deserializer);
            deserialized.first.first = graph_deserialized.first;
            current_position += graph_deserialized.second;
            // Deserialize the previous index
            const auto index_deserializer = std::bind(arc_utilities::DeserializeFixedSizePOD<int64_t>, std::placeholders::_1, std::placeholders::_2);
            const auto prev_index_deserialized = arc_utilities::DeserializeVector<int64_t>(buffer, current_position, index_deserializer);
            deserialized.first.second.first = prev_index_deserialized.first;
            current_position += prev_index_deserialized.second;
            // Deserialize the distances
            const auto distance_deserializer = std::bind(arc_utilities::DeserializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2);
            const auto distance_deserialized = arc_utilities::DeserializeVector<double>(buffer, current_position, distance_deserializer);
            deserialized.first.second.second = distance_deserialized.first;
            current_position += distance_deserialized.second;
            // Figure out how many bytes were read
            deserialized.second = current_position - current;
            return deserialized;
        }
    };

    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
    class SimpleGraphAstar
    {
    protected:

        SimpleGraphAstar() {}

    public:

        static arc_helpers::AstarResult PerformLazyAstar(
                const Graph<NodeValueType, Allocator>& graph,
                const int64_t start_index,
                const int64_t goal_index,
                const std::function<bool(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& edge_validity_check_fn,
                const std::function<double(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& distance_fn,
                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
                const bool limit_pqueue_duplicates)
        {
            // Enforced sanity checks
            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Start index out of range");
            }
            if ((goal_index < 0) && (goal_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Goal index out of range");
            }
            if (start_index == goal_index)
            {
                throw std::invalid_argument("Start and goal indices must be different");
            }
            // Make helper function
            const auto heuristic_function = [&] (const int64_t node_index)
            {
                return heuristic_fn(graph.GetNodeImmutable(node_index).GetValueImmutable(), graph.GetNodeImmutable(goal_index).GetValueImmutable());
            };
            // Setup
            std::priority_queue<arc_helpers::AstarPQueueElement, std::vector<arc_helpers::AstarPQueueElement>, arc_helpers::CompareAstarPQueueElementFn> queue;
            // Optional map to reduce the number of duplicate items added to the pqueue
            // Key is the node index in the provided graph
            // Value is cost-to-come
            std::unordered_map<int64_t, double> queue_members_map;
            // Key is the node index in the provided graph
            // Value is a pair<backpointer, cost-to-come>
            // backpointer is the parent index in the provided graph
            std::unordered_map<int64_t, std::pair<int64_t, double>> explored;
            // Initialize
            queue.push(arc_helpers::AstarPQueueElement(start_index, -1, 0.0, heuristic_function(start_index)));
            if (limit_pqueue_duplicates)
            {
                queue_members_map[start_index] = 0.0;
            }
            // Search
            while (queue.size() > 0)
            {
                // Get the top of the priority queue
                const arc_helpers::AstarPQueueElement top_node = queue.top();
                queue.pop();
                // Remove from queue map if necessary
                if (limit_pqueue_duplicates)
                {
                    queue_members_map.erase(top_node.NodeID());
                }
                // Check if the node has already been discovered
                const auto explored_itr = explored.find(top_node.NodeID());
                // We have not been here before, or it is cheaper now
                const bool in_explored = (explored_itr != explored.end());
                const bool explored_is_better = (in_explored) ? (top_node.CostToCome() >= explored_itr->second.second) : false;
                if (!explored_is_better)
                {
                    // Add to the explored list
                    explored[top_node.NodeID()] = std::make_pair(top_node.Backpointer(), top_node.CostToCome());
                    // Check if we have reached the goal
                    if (top_node.NodeID() == goal_index)
                    {
                        break;
                    }
                    // Explore and add the children
                    const std::vector<GraphEdge>& out_edges = graph.GetNodeImmutable(top_node.NodeID()).GetOutEdgesImmutable();
                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                    {
                        // Get the next potential child node
                        const GraphEdge& current_out_edge = out_edges[out_edge_idx];
                        const int64_t child_node_index = current_out_edge.GetToIndex();
                        // Check if the top node->child edge is valid
                        if (edge_validity_check_fn(graph, current_out_edge))
                        {
                            // Compute the cost-to-come for the new child
                            const double parent_cost_to_come = top_node.CostToCome();
                            const double parent_to_child_cost = distance_fn(graph, current_out_edge);
                            const double child_cost_to_come = parent_cost_to_come + parent_to_child_cost;
                            // Check if the child state has already been explored
                            const auto explored_itr = explored.find(child_node_index);
                            // It is not in the explored list, or is there with a higher cost-to-come
                            const bool in_explored = (explored_itr != explored.end());
                            const bool explored_is_better = (in_explored) ? (child_cost_to_come >= explored_itr->second.second) : false;
                            // Check if the child state is already in the queue
                            bool queue_is_better = false;
                            if (limit_pqueue_duplicates)
                            {
                                const auto queue_members_map_itr = queue_members_map.find(child_node_index);
                                const bool in_queue = (queue_members_map_itr != queue_members_map.end());
                                queue_is_better = (in_queue) ? (child_cost_to_come >= queue_members_map_itr->second) : false;
                            }
                            // Only add the new state if we need to
                            if (!explored_is_better && !queue_is_better)
                            {
                                // Compute the heuristic for the child
                                const double child_heuristic = heuristic_function(child_node_index);
                                // Compute the child value
                                const double child_value = child_cost_to_come + child_heuristic;
                                queue.push(arc_helpers::AstarPQueueElement(child_node_index, top_node.NodeID(), child_cost_to_come, child_value));
                            }
                        }
                    }
                }
            }
            return arc_helpers::ExtractAstarResult(explored, start_index, goal_index);
        }

        static arc_helpers::AstarResult PerformLazyAstar(
                const Graph<NodeValueType, Allocator>& graph,
                const int64_t start_index,
                const int64_t goal_index,
                const std::function<bool(const NodeValueType&, const NodeValueType&)>& edge_validity_check_fn,
                const std::function<double(const NodeValueType&, const NodeValueType&)>& distance_fn,
                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
                const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
            {
                return edge_validity_check_fn(graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable());
            };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
            {
                return distance_fn(graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable());
            };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }

        static arc_helpers::AstarResult PerformAstar(
                const Graph<NodeValueType, Allocator>& graph,
                const int64_t start_index,
                const int64_t goal_index,
                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
                const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
            {
                UNUSED(graph);
                if (edge.GetWeight() < std::numeric_limits<double>::infinity())
                {
                    return true;
                }
                else
                {
                    return false;
                }
            };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
            {
                UNUSED(graph);
                return edge.GetWeight();
            };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }
    };

    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
    class GraphRandomWalk
    {
    protected:
        GraphRandomWalk() {}

    public:

        template <typename Generator>
        static std::vector<int64_t> PerformRandomWalk(
                const Graph<NodeValueType, Allocator>& graph,
                const int64_t start_index,
                const int64_t goal_index,
                Generator& generator)
        {
            std::uniform_int_distribution<int64_t> uniform_int_distribution;

            std::vector<int64_t> path(1, start_index);

            while (path.back() != goal_index)
            {
                // Collect data from the current node
                const int64_t curr_index = path.back();
                const auto& out_edges = graph.GetNodeImmutable(curr_index).GetOutEdgesImmutable();
                const auto num_edges = out_edges.size();

                // Determine which node to step to next
                std::uniform_int_distribution<int64_t>::param_type params(0, (int64_t)(num_edges - 1));
                uniform_int_distribution.param(params);
                const int64_t next_step = uniform_int_distribution(generator);
                const auto next_index = out_edges.at(next_step).GetToIndex();

                // If the next index is somewhere we've been already, then "trim" the loop off
                const auto it = std::find(path.begin(), path.end(), next_index);
                path.erase(it, path.end());

                // (Re)add the new index to the path
                path.push_back(next_index);
            }

            return path;
        }
    };
}

#endif // DIJKSTRAS_HPP


#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>

#include "common.hpp"
#include "emp_dist.hpp"

EmpDist::EmpDist(EmpDist& other)
    : pdfvals(other.pdfvals)
    , cdfvals(other.cdfvals)
    , med(other.med)
{
}

EmpDist::EmpDist(const unsigned int* vals, const unsigned int* lens, size_t n)
{
    double valsum = 0;
    for (size_t i = 0; i < n; ++i) {
        valsum += lens[i];
    }

    double cumpr = 0.0;
    unsigned int lastval = 0, maxval = 1;
    for (; lastval < n; ++lastval) {
        cumpr += lens[lastval] / valsum;
        maxval = vals[lastval];
        if (cumpr > 1.0 - 1e-6) {
            break;
        }
    }

    pdfvals.resize(maxval);
    valsum = 0.0;
    for (unsigned int i = 0; i < lastval; ++i) {
        valsum += lens[i];
    }

    for (unsigned int val = 0, i = 0; val < maxval; ) {
        if (val == vals[i]) {
            pdfvals[val] = lens[i] / valsum;
            ++val;
            ++i;
        }
        else if (val < vals[i]) {
            pdfvals[val] = 0.0;
            ++val;
        }
    }

    cdfvals.resize(maxval);
    cdfvals[0] = pdfvals[0];
    for (unsigned int val = 1; val < maxval; ++val) {
        cdfvals[val] = cdfvals[val - 1] + pdfvals[val];
    }

    // compute median
    size_t i = 0, j = n - 1;
    unsigned int u = lens[0], v = lens[n - 1];
    while (i < j) {
        if (u <= v) {
            v -= u;
            u = lens[++i];
        }
        else {
            u -= v;
            v = lens[--j];
        }
    }
    med = vals[i];
}

float EmpDist::median() const
{
    if (pdfvals.size() == 0) return NAN;
    else return med;
}

float EmpDist::pdf(unsigned int x) const
{
    return x < pdfvals.size() ? pdfvals[x] : 0.0;
}

float EmpDist::cdf(unsigned int x) const
{
    return x < cdfvals.size() ? cdfvals[x] : 1.0;
}


/***************************************************************************
 *   Copyright (C) 2007 by Marco Correia                                   *
 *   mvc@di.fct.unl.pt                                                     *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 ***************************************************************************/

#include <casper/kernel.h>
#include <casper/cp/set.h>
#include <casper/cp/int.h>
#include <casper/util/options.h>

#include <boost/math/special_functions/factorials.hpp>
#include <iostream>

using namespace Casper;
using namespace Casper::CP;
//using namespace std;

#if 0
/**
 * Computes the set of all n-ary sums of the elements in a set domain variable.
 * \note needs testing
 */
template<class Arg1,class Arg2>
struct NSumsEqual : IFilter
{
	typedef typename DomView<Set<int>,Arg1>::Dom	DomX;
	typedef typename DomView<Set<int>,Arg2>::Dom	DomY;

	NSumsEqual(CPSolver& solver,const Arg1& xx, const Arg2& yy, const uint& n) :
				  IFilter(solver),x(solver,xx),y(solver,yy),n(n) {}

	bool execute();
	void attach(INotifiable* f)
	{	x->attachOnDomain(f); y->attachOnDomain(f);	}
	void detach(INotifiable* f)
	{	x->detachOnDomain(f); y->detachOnDomain(f);	}

	Cost cost() const
	{	return quadraticLo;	}

	template<class Iteration>
	List<int> getSums(Iteration it, uint tsize);
	template<class Iteration>
	List<int> getSumsSorted(Iteration it, uint tsize);

	DomView<Set<int>,Arg1>			x;
	DomView<Set<int>,Arg2>			y;
	const uint						n;
};

template<class Arg1,class Arg2>
template<class Iteration>
List<int> NSumsEqual<Arg1,Arg2>::getSums(Iteration it, uint tsize)
{
	List<int> ret;
	if (tsize == 1)
	{
		for ( ; it.valid(); it.iterate())
			ret.pushBack(it.value());
		return ret;
	}
	uint c = 0;
	List<int> sums_1(getSums(it,tsize-1));
	for (List<int>::Iterator vit = sums_1.begin(); vit != sums_1.end(); ++vit)
	{
		Iteration iit(it);
		for (uint i = 0; iit.valid() and i < tsize-1; ++i)
			iit.iterate();
		for (uint i = 0; iit.valid() and i < c; ++i)
			iit.iterate();
		for ( ; iit.valid(); iit.iterate())
			ret.pushBack(iit.value()+*vit);
		++c;
	}
	return ret;
}

template<class Arg1,class Arg2>
template<class Iteration>
List<int> NSumsEqual<Arg1,Arg2>::getSumsSorted(Iteration it, uint tsize)
{
	List<int> sumsUnsorted(getSums(it,tsize));
	std::set<int> sumsV(sumsUnsorted.begin(),sumsUnsorted.end());
	List<int> ret(sumsV.begin(),sumsV.end());
	return ret;
}

template<class Arg1,class Arg2>
bool NSumsEqual<Arg1,Arg2>::execute()
{
//	std::cout << "begin: " << *x << " " << *y << std::endl;
	if (x->inSize() >= n)
		if (!Detail::setSafeInsertRange(*y,makeIt(getSumsSorted(makeInIt(*x),n))))
			return false;

	if (x->inSize()+x->possSize() >= n)
		if (!Detail::setSafeEraseRange(*y,makeDiffIt(makePossIt(*y),
										  makeIt(getSumsSorted(makeLUBIt(*x),n)))))
			return false;
//	std::cout << "end: " << *x << " " << *y << std::endl;
	// not sure if processing "in y" events makes sense. ignoring for now.

	// if an element r is removed from y then we remove from x.poss all e such that
	// e+s=r where s is a possible (n-1) sum already in x.in
/*	std::set<int> inSums_1(getSums(makeInIt(*x),n-1));
	for(typename DomX::PIterator pit = x->beginPoss(); pit != x->endPoss(); )
	{
		for (std::set<int>::iterator it = inSums_1.begin(); it != inSums_1.end(); ++it)
			for (yLUBDIt = y->lubDeltas().beginFrom(yLUBDeltasIt);
					yLUBDIt != y->lubDeltas().end(); ++yLUBDIt)
				for (typename DomX::DeltasIterator dit = yLUBDIt->begin();
						dit != yLUBDIt->end(); ++dit)
					if (*it + *pit == *dit)
					{
						if (x->safeErase(pit++))
							return false;
						goto next;
					}
		++pit;
		next:;
	}
	yLUBDeltasIt = y->lubDeltas().end();*/
	return true;
}

template<class Arg1,class Arg2>
Filter nSumsEqual(const Arg1& a1, const Arg2& a2, const uint& n)
{ return new (getState(a1,a2).getHeap()) NSumsEqual<Arg1,Arg2>(getState(a1,a2),a1,a2,n); }
#endif

template<class Arg1,class Arg2>
struct SortSubsetsWithSameSum : IFilter
{
	typedef typename DomView<Set<int>,Arg1>::Dom	DomX;
	typedef typename DomView<Set<int>,Arg2>::Dom	DomY;

	SortSubsetsWithSameSum(Solver& solver,const Arg1& xx, const Arg2& yy) :
				  IFilter(solver),x(solver,xx),y(solver,yy) {}

	bool execute();
	void attach(INotifiable* f)
	{	x->attachOnGLB(f); y->attachOnGLB(f);	}
	void detach(INotifiable* f)
	{	x->detachOnGLB(f); y->detachOnGLB(f);	}

	Cost cost() const
	{	return quadraticLo;	}

	struct Cmp {
		bool operator()(const Util::StdPair<int>& s1,const Util::StdPair<int>& s2) const
		{
			if (s1.first < s2.first)
				return true;
			if (s1.first > s2.first)
				return false;
			return s1.second < s2.second;
		}
	};

	typedef Util::StdList<Util::StdPair<int> > Sums;
	typedef std::set<Util::StdPair<int>,Cmp> AllSums;

	template<class Iteration>
	Sums getSums(Iteration it, const Sums&, uint tsize);
	template<class Iteration>
	AllSums getSums(Iteration it, uint tsize);

	DomView<Set<int>,Arg1>			x;
	DomView<Set<int>,Arg2>			y;
};

template<class Arg1,class Arg2>
template<class Iteration>
typename SortSubsetsWithSameSum<Arg1,Arg2>::Sums
SortSubsetsWithSameSum<Arg1,Arg2>::getSums(Iteration it,const Sums& sums_1,
												uint tsize)
{
	assert(tsize>1);
	Sums ret;
	uint c = 0;
	for (Sums::Iterator vit = sums_1.begin(); vit != sums_1.end(); ++vit)
	{
		Iteration iit(it);
		for (uint i = 0; iit.valid() and i < tsize-1; ++i)
			iit.iterate();
		for (uint i = 0; iit.valid() and i < c; ++i)
			iit.iterate();
		for ( ; iit.valid(); iit.iterate())
			ret.pushBack(Util::StdPair<int>(iit.value()+vit->first,iit.value()));
		++c;
	}
	return ret;
}

template<class Arg1,class Arg2>
template<class Iteration>
typename SortSubsetsWithSameSum<Arg1,Arg2>::AllSums
SortSubsetsWithSameSum<Arg1,Arg2>::getSums(Iteration it,uint tsize)
{
	AllSums all;

	Sums* xold = new Sums();
	for (Iteration iit(it) ; iit.valid(); iit.iterate())
	{
		xold->pushBack(Util::StdPair<int>(iit.value(),iit.value()));
		all.insert(Util::StdPair<int>(iit.value(),iit.value()));
	}

	for (uint i = 2; i <= tsize; ++i)
	{
		Sums* xnew = new Sums(getSums(it,*xold,i));
		all.insert(xnew->begin(),xnew->end());
		delete xold;
		xold = xnew;
	}
	delete xold;
	return all;
}

template<class Arg1,class Arg2>
bool SortSubsetsWithSameSum<Arg1,Arg2>::execute()
{
	AllSums sumsx(getSums(makeInIt(*x),x->inSize()));
	AllSums sumsy(getSums(makeInIt(*y),y->inSize()));
	typename AllSums::iterator itx = sumsx.begin();
	typename AllSums::iterator ity = sumsy.begin();

/*	std::cout << "xin: " << x->in() << std::endl;
	std::cout << "sumsx: ";
	print(sumsx.begin(),sumsx.end());
	std::cout << std::endl;
*/
	while (itx != sumsx.end() and ity != sumsy.end())
	{
		if (itx->first < ity->first)
			++itx;
		if (ity->first < itx->first)
			++ity;
		if (itx->first == ity->first)
		{
			if (itx->second > ity->second)
			{
		//		std::cout << "failing\n";
				return false;
			}
			typename AllSums::iterator next(itx);
			++next;
			if (next != sumsx.end() and next->first == ity->first)
				++itx;
			else
				++ity;
		}
	}
	return true;
}

template<class Arg1,class Arg2>
Filter sortSubsetsWithSameSum(const Arg1& a1, const Arg2& a2)
{ return new (getState(a1,a2).getHeap()) SortSubsetsWithSameSum<Arg1,Arg2>(getState(a1,a2),a1,a2); }

struct GreedyLabeling : IGoal
{
	GreedyLabeling(Store& store,const VarArray<IntSet>& vars) : IGoal(),store(store),
			vars(vars) {}
	Goal execute()
	{
		// select maximum unassigned number
		int maxn = limits<int>::min();
		for (uint i = 0; i < vars.size(); ++i)
			for (SetFD<int>::PIterator it = vars[i].domain().beginPoss();
					it != vars[i].domain().endPoss(); ++it)
				if (*it > maxn)
					maxn = *it;

		// find the set with the smallest sum
		int mins = limits<int>::max();
		int minidx = -1;
		for (uint i = 0; i < vars.size(); ++i)
		{
			uint sumIn = 0;
			if (vars[i].domain().findInPoss(maxn)!=vars[i].domain().endPoss())
			{
				for (SetFD<int>::IIterator it = vars[i].domain().beginIn();
						it != vars[i].domain().endIn(); ++it)
					sumIn += *it;
				if (sumIn < mins)
				{
					mins = sumIn;
					minidx = i;
				}
			}
		}
		if (minidx<0)
			return succeed();
		return Goal(store,
				(post(store,member(maxn,vars[minidx])) or post(store,notMember(maxn,vars[minidx]))) and
				Goal(this));
	}
	Store& store;
	VarArray<IntSet> vars;
};

Goal greedyLabeling(Store& store,VarArray<IntSet> vars)
{	return new (store) GreedyLabeling(store,vars);	}

void partition(uint nvalues, uint nsets, int optimum = -2)
{
	Solver solver;
	IntSetVarArray vars(solver,nsets,range(1,nvalues));

//	for (uint i = 0; i < vars.count(); ++i)
//		cout << vars[i].pDomain() << " " << "A" << endl;

#if 1
	for (uint i = 0; i < vars.count(); i++)
		for (uint j = i+1; j < vars.count(); j++)
			solver.post( disjoint(vars(i),vars(j)) );
	/*solver.post(unionEqual(vars,Var<IntSet>(solver,
						List<int>(range(1,nvalues).begin(),range(1,nvalues).end()),
						List<int>())));*/

	VarArray<IntSet> avars(solver,nsets,range(1,nvalues));
	avars[0] = vars[0];
	for (uint i = 0; i < nsets-1; i++)
		solver.post( unionEqual(avars(i),vars(i+1),avars(i+1)) );
	solver.post(avars(nsets-1)==Var<IntSet>(solver,
						Util::StdList<int>(range(1,nvalues).begin(),range(1,nvalues).end()),
						Util::StdList<int>()) );

#else
	solver.post(partition(vars));
#endif

	const uint total = nvalues/2.0*(2+(nvalues-1));
	IntVar maxSums(solver,std::ceil(((float)total)/nsets),total);

	if (optimum==-2)
	{
		const float magicNumberF = ((float)total)/nsets;
		const uint magicNumber = (int)magicNumberF;
		if (magicNumber!=magicNumberF)	{
			std::cout << "trivially impossible!\n";
			return;
		}

		for (uint p = 0; p < nsets; ++p)
			solver.post(sumEqual(vars[p],magicNumber));
	}
	else
	if (optimum==-1)
	{
		// at least one element per set so that we can
		// enforce symmetry breaking constraints below

		IntVarArray sums(solver,nsets,1,total);
		for (uint p = 0; p < nsets; ++p)
			solver.post(sumEqual(vars[p],sums[p]));
		solver.post(max(sums)==maxSums);
		solver.setExplorer(bbMinimize(solver,maxSums));
	}
	else
		for (uint p = 0; p < nsets; ++p)
		{
			IntVar s(solver,1,total);
			solver.post(sumEqual(vars[p],s));
			solver.post(s<=optimum);
		}

	// symmetry breaking: sets are interchangeable
#if 1
	for (uint p = 1; p < nsets; ++p)
		solver.post(min(vars[p])>min(vars[p-1]));
#else
	for (uint p1 = 0; p1 < nsets; ++p1)
		for (uint p2 = p1+1; p2 < nsets; ++p2)
			solver.post( sortSubsetsWithSameSum(vars[p1],vars[p2]));
#endif

//	bool ret = solver.solve( label( vars, selectVarFFRR(vars) ) );
	bool ret = solver.solve( greedyLabeling(solver,vars) );

	if (optimum==-2 or optimum>=0)
		std::cout << ret << std::endl << vars << std::endl;
	else
		while (ret)
		{
			std::cout << vars << " : " << maxSums << " optimimum="
						<< ((float)total)/nsets << std::endl;
			ret = solver.next();
		}
	std::cout << solver.getStats() << std::endl
				 << solver.getCPUTimer() << std::endl;
}

int main(int argc, char** argv)
{
	if (argc != 3 and argc != 4)
	{
		std::cout << "usage: " << argv[0] << " nvalues nsets [optimum]\n";
		std::cout << "set optimum=-2 for perfect partition, optimum=-1 for minimization\n";
		return 1;
	}
	::partition(atoi(argv[1]),atoi(argv[2]),argc==3?-1:atoi(argv[3]));
}