/**
 * @file tree_traits_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.
 */
#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 "old_boost_test_definitions.hpp"

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

BOOST_AUTO_TEST_SUITE(RectangleTreeTest);

// Be careful!  When writing new tests, always get the boolean value and store
// it in a temporary, because the Boost unit test macros do weird things and
// will cause bizarre problems.

// 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.Children()[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.Points()[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.Points()[i])));
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
    {
      for (size_t j = 0; j < tree.Bound().Dim(); j++)
        BOOST_REQUIRE(tree.Bound()[j].Contains(tree.Children()[i]->Bound()[j]));

      CheckContainment(*(tree.Children()[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.LocalDataset().col(j)[i] < min)
          min = tree.LocalDataset().col(j)[i];
        if (tree.LocalDataset().col(j)[i] > max)
          max = tree.LocalDataset().col(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 ensure that the dataset for the tree, and the datasets stored
 * in each leaf node are in sync.
 * @param tree The tree to check.
 */
template<typename TreeType>
void CheckSync(const TreeType& tree)
{
  if (tree.IsLeaf())
  {
    for (size_t i = 0; i < tree.Count(); i++)
    {
      for (size_t j = 0; j < tree.LocalDataset().n_rows; j++)
      {
        BOOST_REQUIRE_EQUAL(tree.LocalDataset().col(i)[j],
                            tree.Dataset().col(tree.Points()[i])[j]);
      }
    }
  }
  else
  {
    for (size_t i = 0; i < tree.NumChildren(); i++)
      CheckSync(*tree.Children()[i]);
  }
}

// Test to ensure that the dataset used by the whole tree (and the traversers)
// is in sync with the datasets stored in each leaf node.
BOOST_AUTO_TEST_CASE(TreeLocalDatasetInSync)
{
  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);
  CheckSync(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.Children()[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.Children()[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.Children()[i]);
      if (n < m)
        m = n;
    }
    min += m;
  }

  return min;
}

// 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);
  CheckSync(tree);
  CheckExactContainment(tree);

  // Single-tree search.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      RTree> allknn1(&tree, true);

  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  allknn1.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.
  AllkNN allknn2(newDataset, true, true);

  allknn2.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.
  dataset.reshape(8, 1000 + numIter);
  arma::mat tmpData;
  tmpData.randu(8, numIter);
  for (int i = 0; i < numIter; 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);
  CheckSync(tree);
  CheckExactContainment(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> allknn1(&tree, true);

  allknn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  AllkNN allknn2(dataset, true, true);

  allknn2.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);

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

  BOOST_REQUIRE_EQUAL(rTree.NumDescendants(), 1000);

  CheckSync(rTree);
  CheckContainment(rTree);
  CheckExactContainment(rTree);
  CheckHierarchy(rTree);

  allknn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  AllkNN allknn2(dataset, true, true);

  allknn2.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.
/** This is known to not work: see #368.
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 RectangleTree<
      XTreeSplit<RStarTreeDescentHeuristic,
                 NeighborSearchStat<NearestNeighborSort>,
                 arma::mat>,
      RStarTreeDescentHeuristic,
      NeighborSearchStat<NearestNeighborSort>,
      arma::mat> TreeType;
  TreeType xTree(dataset, 20, 6, 5, 2, 0);

  // Nearest neighbor search with the X tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, TreeType>
      allknn1(&xTree, dataset, true);

  BOOST_REQUIRE_EQUAL(xTree.NumDescendants(), numP);

  CheckSync(xTree);
  //CheckContainment(xTree);
  CheckExactContainment(xTree);
  CheckHierarchy(xTree);

  allknn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  AllkNN allknn2(dataset, true, true);

  allknn2.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.Children()[firstChild]->NumChildren() == 2)
  {
    firstChild = 1;
    secondChild = 0;
  }

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

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

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

  int firstPrime = 0, secondPrime = 1;
  if (rTree.Children()[secondChild]->Children()[firstPrime]->Count() == 3)
  {
    firstPrime = 1;
    secondPrime = 0;
  }

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

  BOOST_REQUIRE_EQUAL(
      rTree.Children()[secondChild]->Children()[secondPrime]->Count(), 3);
  BOOST_REQUIRE_CLOSE(
      rTree.Children()[secondChild]->Children()[secondPrime]->Bound()[0].Lo(),
      0.9, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Children()[secondChild]->Children()[secondPrime]->Bound()[0].Hi(),
      1.0, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Children()[secondChild]->Children()[secondPrime]->Bound()[1].Lo(),
      0.1, 1e-15);
  BOOST_REQUIRE_CLOSE(
      rTree.Children()[secondChild]->Children()[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.Children()[firstChild]->NumChildren() == 2)
  {
    firstChild = 1;
    secondChild = 0;
  }

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

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

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

  int firstPrime = 0, secondPrime = 1;
  if (rTree.Children()[secondChild]->Children()[firstPrime]->Count() == 3)
  {
    firstPrime = 1;
    secondPrime = 0;
  }

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

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

BOOST_AUTO_TEST_SUITE_END();


#include <boost/python/module.hpp>
#include <boost/python/def.hpp>

#include <boost_adaptbx/tuple_conversion.h>
#include <boost/tuple/tuple.hpp>
#include <boost/thread.hpp>
#include <boost/ref.hpp>

namespace boost_adaptbx { namespace python { namespace {

const int N=100000;

struct pi_computation {
  double pi;
  void operator()() {
      pi = 0;
      double a=1, b=3;
      for(int i=1; i<2*N; ++i) {
        pi += 1/a - 1/b;
        a += 4;
        b += 4;
      }
      pi *= 4;
  }
};

struct e_computation {
  double e;
  void operator()() {
      e = 1;
      double a = 1;
      for(int i=1; i<N; ++i) {
        a /= i;
        e += a;
      }
  }
};

boost::tuple<double, double> test_boost_thread() {
  double e, pi;
  pi_computation compute_pi;
  e_computation compute_e;
  tuple_conversion::to_python<boost::tuple<double, double> >();
  boost::thread pi_worker(boost::ref(compute_pi));
  boost::thread e_worker(boost::ref(compute_e));
  pi_worker.join(); e_worker.join();
  return boost::make_tuple(compute_pi.pi, compute_e.e);
}

void wrap_all() {
  using namespace boost::python;
  def("test_boost_thread", test_boost_thread);
}

}}}

BOOST_PYTHON_MODULE(boost_adaptbx_boost_thread_test_ext)
{
  boost_adaptbx::python::wrap_all();
}


//
// Created by localuser on 22/07/19.
//

#include <iostream>
#include <thread>
#include <pcl/common/centroid.h>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <algorithm>
#include <Eigen/Dense>
#include <pcl/common/pca.h>
#include <typeinfo>

#include "./include/Normal2dEstimation.h"

int main(){

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr  norm_cloud(new pcl::PointCloud<pcl::Normal>);
    pcl::io::loadPCDFile("../sample.pcd", *cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    Normal2dEstimation norm_estim;
    norm_estim.setInputCloud(cloud);
    norm_estim.setSearchMethod (tree);

     norm_estim.setRadiusSearch (30);

    norm_estim.compute(norm_cloud);

    pcl::visualization::PCLVisualizer viewer;
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
    std::cout << norm_cloud->points.size()<<"  "<<cloud->points.size()<<std::endl;
    viewer.addPointCloud<pcl::PointXYZ>(cloud,"cloud");
    viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, norm_cloud,1.,10.0, "cloud_norm");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_norm");
    while (!viewer.wasStopped ())
    {
        viewer.spinOnce (1);
    }
    return 0;
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file countl_zero.hpp
 *
 * This header defines \c countl_zero algorithm, which counts the number of consecutive
 * most significant zero bits in an integer.
 */

#ifndef BOOST_BIT_OPS_COUNTING_COUNTL_ZERO_HPP_INCLUDED_
#define BOOST_BIT_OPS_COUNTING_COUNTL_ZERO_HPP_INCLUDED_

#include <limits>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/int_sizes.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/integral_constant.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>

#if defined(__GNUC__)
#if defined(BOOST_BIT_OPS_DETAIL_HAS_LZCNT)
#include <x86intrin.h>
#endif
#elif defined(_MSC_VER)
#include <intrin.h>
#endif

namespace boost {
namespace bit_ops {

namespace detail {

#if defined(BOOST_BIT_OPS_DETAIL_HAS_BUILTIN_CLZ)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= BOOST_BIT_OPS_DETAIL_SIZEOF_INT, unsigned int >::type countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_clz(value) - (std::numeric_limits< unsigned int >::digits - std::numeric_limits< T >::digits));
}

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LONG, unsigned int >::type countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_clzl(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG, unsigned int >::type countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_clzll(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

#elif defined(BOOST_BIT_OPS_DETAIL_HAS_BIT_SCAN_REVERSE)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= BOOST_BIT_OPS_DETAIL_SIZEOF_LONG, unsigned int >::type countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    unsigned long pos;
    _BitScanReverse(&pos, value);
    return static_cast< unsigned int >((std::numeric_limits< T >::digits - 1u) - pos);
}

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG, unsigned int >::type countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
#if defined(_M_AMD64) || defined(_M_ARM64)
    unsigned long pos;
    _BitScanReverse64(&pos, value);
    return static_cast< unsigned int >((std::numeric_limits< T >::digits - 1u) - pos);
#else
    unsigned long upper_pos = 31u;
    unsigned long pos;
    if (!_BitScanReverse(&pos, static_cast< unsigned int >(value >> 32u)))
    {
        _BitScanReverse(&pos, static_cast< unsigned int >(value));
        upper_pos = std::numeric_limits< T >::digits - 1u;
    }
    return static_cast< unsigned int >(upper_pos - pos);
#endif
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

#else

template< typename T, unsigned int N >
inline unsigned int countl_zero_nz(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    unsigned int count = 0u;
    unsigned int digits = std::numeric_limits< T >::digits / 2u;
    T mask = ~((static_cast< T >(1u) << digits) - 1u);
    for (; digits > 0u; digits >>= 1u, mask <<= digits)
    {
        if ((value & mask) == 0u)
        {
            count += digits;
            value <<= digits;
        }
    }

    return count;
}

#endif

#if defined(BOOST_BIT_OPS_DETAIL_HAS_LZCNT)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= 2u, unsigned int >::type countl_zero(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__lzcnt16(value) - (16u - std::numeric_limits< T >::digits));
}

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 4u, unsigned int >::type countl_zero(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__lzcnt(value));
}

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 8u, unsigned int >::type countl_zero(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
#if defined(__x86_64__) || defined(_M_AMD64)
    return static_cast< unsigned int >(__lzcnt64(value));
#else
    unsigned int count = static_cast< unsigned int >(__lzcnt(static_cast< unsigned int >(value >> 32u)));
    if (count >= 32u)
        count += static_cast< unsigned int >(__lzcnt(static_cast< unsigned int >(value)));
    return count;
#endif
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_LZCNT)

} // namespace detail

/*!
 * \brief Returns the number of consecutive most significant zero bits in \a value
 *
 * \pre \a value must not be zero
 */
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type countl_zero_nz(T value) BOOST_NOEXCEPT
{
    return bit_ops::detail::countl_zero_nz(value, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
}

//! Returns the number of consecutive most significant zero bits in \a value
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type countl_zero(T value) BOOST_NOEXCEPT
{
#if defined(BOOST_BIT_OPS_DETAIL_HAS_LZCNT)
    return bit_ops::detail::countl_zero(value, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
#else
    return value == 0u ? static_cast< unsigned int >(std::numeric_limits< T >::digits) : bit_ops::countl_zero_nz(value);
#endif
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_COUNTING_COUNTL_ZERO_HPP_INCLUDED_


#include "fuzzy_clustering.hpp"
#include <boost/foreach.hpp>
#include <iostream>
#include <time.h>
namespace SoftC {

  void Fuzzy::initRandom () {
    srand ((unsigned int) time (NULL));
    float normalization_factor;
    for (int j = 0 ; j < number_points_; j++) {
      normalization_factor = 0.0;
      for (int i = 0; i < number_clusters_; i++)
        normalization_factor +=
          membership_.at<float> (j, i) = (rand () / (RAND_MAX + 0.0));
      // Normalization
      for (int i = 0; i < number_clusters_; i++)
        membership_.at<float> (j, i) /= normalization_factor;
    }
    computeCentroids();
  }

  void Fuzzy::initKmeansPP () {
    srand ((unsigned int) time (NULL));
    std::vector<int> center_indexes (0);
    std::vector<bool> already_selected_indexes (number_points_, false);

    // Select first cluster centroids by random.
    int first_index = rand () % number_points_;
    center_indexes.push_back (first_index);
    already_selected_indexes[first_index] = true;

    while (center_indexes.size () < number_clusters_) {
      std::vector<float> nearest_distances (number_points_, 0.0);
      for (int p = 0; p < number_points_; ++p) {
        if (already_selected_indexes[p])
          continue;

        cv::Mat point = rows_.row (p);
        std::vector<float> distances_from_centers (0);

        // Calculate distances from all centroids.
        for (int c = 0; c < center_indexes.size (); ++c) {
          int center_index = center_indexes[c];
          cv::Mat center = rows_.row (center_index);
          float dist = calc_dist (point, center, kSoftCDistL2);
          distances_from_centers.push_back (dist);
        }

        // Find nearest centroid.
        int nearest_center_index = center_indexes[0];
        float min = distances_from_centers[0];
        for (int c = 1; c < distances_from_centers.size (); ++c) {
          float dist = distances_from_centers[c];
          if (dist < min) {
            min = dist;
            nearest_center_index = center_indexes[c];
          }
        }
        nearest_distances[p] = min;
      }

      assert (nearest_distances.size () == number_points_);

      float max = nearest_distances[0];
      float max_index = 0;
      for (int p = 1; p < nearest_distances.size (); ++p) {
        float dist = nearest_distances[p];
        if (dist > max) {
          max = dist;
          max_index = p;
        }
      }
      center_indexes.push_back (max_index);
      already_selected_indexes[max_index] = true;
    }

    for (int j = 0; j < center_indexes.size (); ++j) {
      for (int d = 0; d < dimension_; ++d) {
        // FIXME: Avoid zero devide.
        centroids_.at<float> (j, d) = rows_.at<float> (center_indexes[j], d) + 0.001;
      }
    }

    updateMembership ();
    computeCentroids2();
  }

  void Fuzzy::initEverything () {
    switch (init_type_) {
      case kSoftCInitRandom:
        initRandom ();
        break;
      case kSoftCInitKmeansPP:
        initKmeansPP ();
        break;
      default:
        break;
    }
  }

  void Fuzzy::computeCentroids(){
    // Update centroids
    for (int j = 0; j < number_clusters_; j++)
      for (int i = 0 ; i < number_points_; i++)
        for (int f = 0; f < dimension_; f++)
          centroids_.at<float> (j, f) += membership_.at<float> (i, j) * rows_.at<float> (i, f);
    //    *p_centroids_ = prod (*p_membership_,        rows_);
    //   n_clusters   =      n_clusters          rows.size1()
    // X [rows.size2()=      X [rows.size1()=    X [rows.size2=
    //    =size_of_a_point_]    =number_points_]    size_of_a_point]
    std::vector<float> sum_uk (number_clusters_, 0);
    for (int j = 0; j < number_clusters_; j++)
      for (int i = 0 ; i < number_points_; i++)
        sum_uk[j] += membership_.at<float> (j, i);
    // Normalization
    for (int j = 0; j < number_clusters_; j++)
      for (int f = 0 ; f < dimension_; f++)
        centroids_.at<float> (j, f) /= sum_uk[j];
  }

  void Fuzzy::computeCentroids2 (){
    cv::Mat u_ji_m  = cv::Mat::zeros (number_points_, number_clusters_, CV_32FC1);

    //　Initialization
    for (int j = 0; j < number_clusters_; j++)
      for (int f = 0; f < dimension_; f++)
        centroids_.at<float> (j, f) = 0.0;
    // weight ** fuzziness
    for (int j = 0; j < number_clusters_; j++)
      for (int i = 0 ; i < number_points_; i++)
        u_ji_m.at<float> (i, j) = pow ( membership_.at<float> (i, j), fuzziness_);
    // Update centroids
    for (int j = 0; j < number_clusters_; j++)
      for (int i = 0 ; i < number_points_; i++)
        for (int f = 0; f < dimension_; f++)
          centroids_.at<float> (j, f) += u_ji_m.at<float> (i, j) * rows_.at<float> (i, f);

    // Normalization
    float normalization;
    for (int j = 0; j < number_clusters_; j++){
      normalization = 0.0;
      for (int i = 0 ; i < number_points_; i++)
        normalization += u_ji_m.at<float> (i, j);
      for (int f = 0; f < dimension_; f++)
        centroids_.at<float> (j, f) /= normalization;
    }
  }

  float Fuzzy::calc_dist (
      const cv::Mat &point,
      const cv::Mat &center,
      const SoftCDistType dist_type
      )
  {
    float f_dist = 0.f;
    int dimension = point.cols;

    switch (dist_type) {
      case kSoftCDistL1:
        {
          // L1, Manhattan
          for (int d = 0; d < dimension; d++) {
            f_dist += fabs (point.at<float> (0,d) - center.at<float> (0,d));
          }
        }
        break;
      case kSoftCDistL2:
        {
          // L2, Euclid
          for (int d = 0; d < dimension; d++) {
            float t = point.at<float> (0,d) - center.at<float> (0,d);
            f_dist += t * t;
          }
        }
        break;
      case kSoftCDistHistInter:   // TODO
        {
          // HIstogram intersection
          float sum_p = 0.f;
          for (int d = 0; d < dimension; d++) {
            float p = point.at<float> (0,d);
            float c = center.at<float> (0,d);
            float min = p < c ? p : c;
            f_dist += min;
//            f_dist += ((p + c - fabs (p - c)) / 2);
            sum_p += p;
          }
          f_dist /= sum_p;
        }
        break;
      default:
        std::cout << "Error while calculating distance for clustering" << std::endl;
        break;
    }
    return f_dist;
  }

  bool Fuzzy::updateMembership () {
    cv::Mat matrix_norm_one_xi_minus_cj = cv::Mat::zeros (number_clusters_, number_points_, CV_32FC1);
    // Initialization
    for (unsigned int i = 0 ; i < number_points_; i++)
      for (unsigned int j = 0; j < number_clusters_; j++)
        matrix_norm_one_xi_minus_cj.at<float> (j, i) = 0.0;

    for (unsigned int i = 0 ; i < number_points_; i++) {
      // Calculate distances from each cluter.
      cv::Mat point = rows_.row (i);
      for (unsigned int j = 0; j < number_clusters_; j++) {
        cv::Mat center = centroids_.row (j);
        matrix_norm_one_xi_minus_cj.at<float> (j, i)
          = calc_dist (point, center, dist_type_);
      }
    }

    float coeff;
    for (unsigned int i = 0 ; i < number_points_; i++) {
      for (unsigned int j = 0; j < number_clusters_; j++) {
        coeff = 0.0;
        for (unsigned int k = 0; k < number_clusters_; k++) {

//        if (matrix_norm_one_xi_minus_cj.at<float> (j, i) == 0) {
//          coeff += pow (0, 2.0 / (fuzziness_ - 1.0));
//        } else if (matrix_norm_one_xi_minus_cj.at<float> (k, i) == 0) {
//          coeff += pow (1000000.0, 2.0 / (fuzziness_ - 1.0));
//        } else {
          coeff +=
            pow ( (matrix_norm_one_xi_minus_cj.at<float> (j, i) /
                  matrix_norm_one_xi_minus_cj.at<float> (k, i)) ,
                2.0 / (fuzziness_ - 1.0) );
        }

//        if (coeff == 0) {
//          new_membership_.at<float> (i, j) = 1.0;
//        } else {
          new_membership_.at<float> (i, j) = 1.0 / coeff;
//        }
      }
    }

    if (!can_stop() ){
      membership_ = new_membership_.clone ();
      return false;
    }
    return true;
  }
};


/*  ==========================================================================================
    Author: Leonardo Citraro
    Company:
    Filename: KDtree.hpp
    Last modifed:   03.01.2017 by Leonardo Citraro
    Description:    KDtree

    ==========================================================================================
    Copyright (c) 2016 Leonardo Citraro <ldo.citraro@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy of this
    software and associated documentation files (the "Software"), to deal in the Software
    without restriction, including without limitation the rights to use, copy, modify,
    merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
    permit persons to whom the Software is furnished to do so, subject to the following
    conditions:

    The above copyright notice and this permission notice shall be included in all copies
    or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
    INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
    PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
    FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
    OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.
    ==========================================================================================
*/
#ifndef __KDTREE_HPP__
#define __KDTREE_HPP__

//~ #define DEBUG 1

#include "metrics.hpp"
#include <iostream>
#include <numeric>
#include <algorithm>
#include <memory>
#include <vector>
#include <utility>
#include <Eigen/Dense>

template<typename T>
using Matrix = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>;
template<typename T>
using MatrixView = Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>>;
template<typename T>
using VectorRow = Eigen::Matrix<T,Eigen::Dynamic,1>;
template<typename T>
using VectorRowView = Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>>;
template<typename T>
using VectorCol = Eigen::Matrix<T,1,Eigen::Dynamic>;
template<typename T>
using VectorColView = Eigen::Map<Eigen::Matrix<T,1,Eigen::Dynamic>>;
using Indexes = std::vector<int>;

template<typename Vector>
Indexes sort_indexes(const Vector& v) {
    Indexes idxs(v.size());
    std::iota(std::begin(idxs), std::end(idxs), 0);
    std::sort(std::begin(idxs), std::end(idxs), [&v](int i1, int i2) {return v[i1] < v[i2];});
    return idxs;
}

template<typename Vector>
Vector extract(const Vector& src, const int start, const int end) {
    Vector dest(end-start);
    for (int i = 0; i < dest.size(); ++i)
        dest[i] = src[i+start];
    return dest;
}

template<typename Vector1, typename Vector2>
Vector1 extract(const Vector1& src, const Vector2& idxs) {
    Vector1 dest(idxs.size());
    for (int i = 0; i < dest.size(); ++i)
        dest[i] = src[idxs[i]];
    return dest;
}

template<typename T, size_t ROWS, size_t COLS>
class KDnode {
private:
    const std::array<std::array<T,COLS>,ROWS>* _original_data;  ///< a pointer to the original data
    Indexes                 _indexes;                           ///< the indexes of the rows that belong to the current node
    KDnode<T,ROWS,COLS>*    _parent;                            ///< pointer to the parent node
    const int               _n_samples_split;                   ///< the minimum number of samples required to split a node
    std::vector<T>          _data_sliced_vector;                ///< vector used to construct the Eigen::Map (view)
    MatrixView<T>           _data_sliced = MatrixView<T>(nullptr,0,0); ///< a view (no copy) of a portion of the original data
    T                       _split_point;                       ///< the splitting threshold on the axe given by _split_axe
    std::vector<T>          _split_points;                      ///< collection of the previous _split_point
    const int               _split_axe;                         ///< index of the column used to decide where to split
    std::vector<int>        _split_axes;                        ///< collection of the previous _split_axes
    std::unique_ptr<KDnode<T,ROWS,COLS>> _left, _right;         ///< pointer to the next nodes
    const bool              _root_node;                         ///< true if node == root node
    bool                    _terminal_node;                     ///< true if node == terminal node
    const int               _depth;                             ///< node tree depth, the root node has a depth of 0

public:
    KDnode(const std::array<std::array<T,COLS>,ROWS>* data, const Indexes& indexes, KDnode<T,ROWS,COLS>* parent, const int split_axe,
    const bool root_node, const int n_samples_split, const int depth, const std::vector<T>& split_points = {}, const std::vector<int>& split_axes = {})
        : _original_data(data), _indexes(indexes), _parent(parent), _split_axe(split_axe), _n_samples_split(n_samples_split),
        _root_node(root_node), _terminal_node(false), _split_points(split_points), _split_axes(split_axes), _depth(depth){

        _split_axes.push_back(_split_axe);

        build_view();

        // split node until we have less than the desired number of samples per split
        if(indexes.size() > _n_samples_split) {
            split_node();
        } else {
            _terminal_node = true;
            _split_points = {};
        }
    }
    ~KDnode() {
        _parent = nullptr;
    }

    auto get_indexes() const                    { return _indexes; }
    auto get_split_axe() const                  { return _split_axe; }
    auto get_parent_split_axe() const           { return _parent? _parent->get_split_axe() : -1; }
    auto get_data_sliced() const                { return _data_sliced; }
    auto get_data_sliced_vector() const         { return _data_sliced_vector; }
    auto get_split_point() const                { return _split_point; }
    auto get_parent_split_point() const         { return _parent? _parent->get_split_point() : T();}
    auto get_branch_split_points() const        { return _split_points; }
    auto get_parent_branch_split_points() const { return _parent? _parent->get_branch_split_points() : std::vector<T>{};}
    auto get_branch_split_axes() const          { return _split_axes; }
    auto get_depth() const                      { return _depth; }
    auto go_left() const                        { return _left.get(); }
    auto go_right() const                       { return _right.get(); }
    auto go_back() const                        { return _parent; }
    auto is_root() const                        { return _root_node; }
    auto is_end() const                         { return _terminal_node; }

private:
    /// build the view of the original data
    void build_view() {
        int rows = _indexes.size();

        // concatenate all the necessary rows without making copies
        _data_sliced_vector.reserve(rows*COLS);
        for(int i=0; i<rows; ++i)
            std::move((*_original_data)[_indexes[i]].begin(), (*_original_data)[_indexes[i]].end(), std::back_inserter(_data_sliced_vector));

        // build the view using Eigen::Map:
        // Eigen::Map require to be initialised at compile-time, therefore, we are obliged to init the Map using
        // a nullptr. During run-time, we make use of the C++ "placement new" syntax to chnage the Map as explained here:
        // https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html#TutorialMapPlacementNew
        new (&_data_sliced) MatrixView<T>(&_data_sliced_vector[0], rows, COLS);
    }

    /// create a new left and new right nodes
    void split_node() {
        // sort the column used to decide where to split
        VectorRow<T> feature_to_split = _data_sliced.col(_split_axe);
        auto sorted_indices = sort_indexes(feature_to_split);
        _indexes = extract(_indexes, sorted_indices);

        // get the new indexes of the left and right nodes
        int indexes_size = _indexes.size();
        int median_pos = indexes_size/2;
        int new_split_axe = (_split_axe+1)%(*_original_data)[0].size();
        Indexes indices_left = extract(_indexes, 0, median_pos);
        Indexes indices_right = extract(_indexes, median_pos, indexes_size);

        // calculate the split point
        if(indexes_size%2 == 0) {
            T p1 = _data_sliced(sorted_indices[median_pos-1],_split_axe);
            T p2 = _data_sliced(sorted_indices[median_pos],_split_axe);
            _split_point = (p1 + p2)/2.0;
        } else {
            _split_point = _data_sliced(sorted_indices[median_pos],_split_axe);
        }
        //~ if(indexes_size%2 == 0) {
            //~ _split_point = _data_sliced(sorted_indices[median_pos-1],_split_axe);
        //~ } else {
            //~ _split_point = _data_sliced(sorted_indices[median_pos],_split_axe);
        //~ }
        _split_points.push_back(_split_point);

        // make a new left and a new right nodes
        _left = std::make_unique<KDnode<T,ROWS,COLS>>(_original_data, indices_left, this, new_split_axe,
                                                        false, _n_samples_split, _depth+1, _split_points, _split_axes);
        _right = std::make_unique<KDnode<T,ROWS,COLS>>(_original_data, indices_right, this, new_split_axe,
                                                        false, _n_samples_split, _depth+1, _split_points, _split_axes);
    }
};

template<typename T, size_t ROWS, size_t COLS>
class KDtree {
private:
    const std::array<std::array<T,COLS>,ROWS>* _original_data;  ///< a pointer to the original data
    std::unique_ptr<KDnode<T,ROWS,COLS>> node0;                 ///< pointer to the root node
    const int _n_samples_split;                                 ///< the minimum number of samples required to split a node

public:
    KDtree(const std::array<std::array<T,COLS>,ROWS>* original_data, const int n_samples_split = 1)
        : _original_data(original_data), _n_samples_split(n_samples_split) {
        Indexes indexes0(_original_data->size());
        std::iota(std::begin(indexes0), std::end(indexes0), 0);
        node0 = std::make_unique<KDnode<T,ROWS,COLS>>(_original_data, indexes0, nullptr, 0, true, _n_samples_split, 0);
    }
    ~KDtree() {}

    auto get_original_data() const     { return _original_data; }
    auto get_n_samples_split() const   { return _n_samples_split; }
    auto get_node0() const             { return node0.get(); }

    auto go_last_node(const std::array<T,COLS>& sample) const {
        auto node = get_node0();
        while(!node->is_end()) {
            auto split_axe = node->get_split_axe();
            if(sample[split_axe] > node->get_split_point())
                node = node->go_right();
            else
                node = node->go_left();
        }
        return node;
    }

    template<template<typename> class Dist>
    std::vector<int> find_k_nearest(const int k, std::array<T,COLS>& sample, const int approx = 0) const {

        Dist<T> distance_metric;

        // go to the last node that contains the sample
        auto node = go_last_node(sample);

        // go backward until we reach a node with enough samples to start the searching algorithm
        auto node_indexes = node->get_indexes();
        while(node_indexes.size() < k) {
            node = node->go_back();
            if(!node) {
                std::cerr << "Error: The number of nearest neighbours k is greater than the dataset size!\n";
                exit(-1);
            }
            node_indexes = node->get_indexes();
        }
        auto branch_split_points = node->get_parent_branch_split_points();
        auto branch_split_axes = node->get_branch_split_axes();
#if DEBUG
        std::cout << "depth=" << node->get_depth() << "\n";
#endif
        // compute the distance between the sample and all the borders up to the root node
        std::vector<T> border_distances(branch_split_points.size());
        for(int i=0; i<border_distances.size(); ++i) {
            border_distances[i] = distance_metric(sample[branch_split_axes[i]], branch_split_points[i]);
        }
#if DEBUG
        std::cout << "border_distances.size()=" << border_distances.size() << "\n";
#endif
        // find the k nearest neighbours
        std::pair<Indexes,std::vector<T>>  k_nearest = retrieve_k_nearest(k, sample, node_indexes, distance_metric);

        // compare the farthest of the k nearest point with the border distances. If one of the borders
        // is closer to the test point than the farthest of the k nearest point it means that we have to go backward
        // in the tree. The node in which the border was created defines the number of backward steps we have to do
        // to reach the appropriate node.
        int go_back_n_times = 0;
        T farthest_samples_distance = k_nearest.second[k-1];
        for(int i=0; i<border_distances.size(); ++i) {
#if DEBUG
            std::cout << "farthest_samples_distance=" << farthest_samples_distance << " border_distances=" << border_distances[i] << "\n";
#endif
            if(farthest_samples_distance>border_distances[i] ) {
                go_back_n_times = border_distances.size()-i;
                break;
            }
        }
#if DEBUG
        std::cout << "go_back_n_times=" << go_back_n_times << "\n";
#endif
        // reduce the searching space to reduce execution time.
        // WARNING: If approx > 0 the KDtree might not find the exact nearest sample
        go_back_n_times -= approx;

        // skip this part if we stay in the same node
        if(go_back_n_times != 0) {
            // go back in the tree and re-find the k nearest neighbours
            for(int i=0; i<go_back_n_times; ++i)
                node = node->go_back();

            node_indexes = node->get_indexes();
#if DEBUG
            std::cout << "depth=" << node->get_depth() << "\n";
            std::cout << "node_indexes.size()=" << node_indexes.size() << "\n";
#endif
            k_nearest = retrieve_k_nearest(k, sample, node_indexes, distance_metric);

        }
        return k_nearest.first;
    }

public:

    template<template<typename> class Dist>
    inline std::pair<Indexes,std::vector<T>> retrieve_k_nearest(const int k, const std::array<T,COLS>& sample,
                                                            const Indexes& indexes, Dist<T>& distance_metric) const {

        //~ // special simpler case when indexes.size() == 1
        if(indexes.size() == 1) {
            std::vector<T> k_nearest_distances = {distance_metric(sample, (*_original_data)[indexes[0]])};
            Indexes k_nearest_absolute_idx = {indexes[0]};
            return std::make_pair<Indexes,std::vector<T>>(std::move(k_nearest_absolute_idx), std::move(k_nearest_distances));
        } else {
            std::vector<T> distances(indexes.size());
            std::vector<T> k_nearest_distances(k);
            Indexes k_nearest_relative_idx(k);
            Indexes k_nearest_absolute_idx(k);

            T d0 = distance_metric(sample, (*_original_data)[indexes[0]]);
            distances[0] = d0;
            k_nearest_distances[0] = d0;
            k_nearest_absolute_idx[0] = indexes[0];
            k_nearest_relative_idx[0] = 0;

            for(int i=1; i<indexes.size(); ++i) {
                T d = distance_metric(sample, (*_original_data)[indexes[i]]);
                distances[i] = d;
                if(d < d0) {
                    k_nearest_distances[0] = d;
                    k_nearest_absolute_idx[0] = indexes[i];
                    k_nearest_relative_idx[0] = i;
                    d0 = d;
#if DEBUG
                    std::cout << "d=" << d << "\n";
#endif
                }
            }
#if DEBUG
            std::cout << "k_nearest_distances[0]=" << k_nearest_distances[0] << "\n";
#endif
            if(k>1) {
                distances[k_nearest_relative_idx[0]] = -1;

                for(int kk=1; kk<k; ++kk) {
                    int not_used_yet;
                    for(int i=0; i<indexes.size(); ++i) {
                        if(distances[i] != -1) {
                            not_used_yet = i;
                            break;
                        }
                    }
                    k_nearest_distances[kk] = distances[not_used_yet];
                    k_nearest_absolute_idx[kk] = indexes[not_used_yet];
                    k_nearest_relative_idx[kk] = not_used_yet;

                    for(int i=1; i<indexes.size(); ++i) {
                        if(distances[i] < k_nearest_distances[kk] && distances[i] != -1) {
                            k_nearest_distances[kk] = distances[i];
                            k_nearest_absolute_idx[kk] = indexes[i];
                            k_nearest_relative_idx[kk] = i;
                        }
                    }
                    distances[k_nearest_relative_idx[kk]] = -1;
                }
            }
            return std::make_pair<Indexes,std::vector<T>>(std::move(k_nearest_absolute_idx), std::move(k_nearest_distances));
        }
    }
};
#endif


#ifndef _CLASS_DATA
#define _CLASS_DATA

#include <armadillo>

using namespace arma;

class Data
/*
Declare the class of Data, which contains all the information about the data
*/
{
    public:
        //nLocations: the number of locations in the raw data, including observations and missing values
        //numObservationsInRawData: the number of observations in the raw data, may including duplicates
        unsigned long nLocations, numObservationsInRawData, numPredictionsInRawData;

        //observationLon: longitude of observations
        //observationLat: latitude of observations
        //observationResiduals: residuals after a linear regression to longitude, latitude, and a constant
        double *observationLon, *observationLat, *observationResiduals;

        //predictionLon: longitude for predictions
        //predictionLat: latitude of predictions
        double *predictionLon, *predictionLat;

        //coefficients: the coefficients in the regression model
        vec coefficients;

        //domainBoundaries: boundry of the spatial domain, an array of (minLon,maxLon,minLat,maxLat)
        double domainBoundaries[4];

        //Loads data from file with name DATA_FILE_NAME. The data file is assumed to begin with the number of locations (type: unsigned 64-bit integer), followed by an array of longitudes, latitudes, and observations (type: 64-bit real number)
        void load_data();

        //Print a brief summary of the data
        void print_data_summary();

        //Destructor
        ~Data();

    private:

        //Eliminate duplicate observations and predictions
        void eliminate_duplicate(double *& observationValues);
};
#endif


#include <fstream>
#include <iostream>
#include <string>

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

#include "range.hpp"
#include "options.hpp"

template<class Graph>
void show(Graph const & g) {
  for(auto v : range(vertices(g))) {
    std::cout << v << ": ";
    for(auto w : range(adjacent_vertices(v, g)))
      std::cout << w << " ";
    std::cout << std::endl;
  }
}

int main(int argc, char* argv[]) {
  options opt(argc, argv);
  auto f = opt.get<std::string>("-f", "");
  std::cout << f << std::endl;

  std::ifstream inFile;
  inFile.open(f, std::ifstream::in);

  typedef boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS> Graph;
  Graph g;

  boost::dynamic_properties dp;
  read_graphml(inFile, g, dp);

  std::cout << num_vertices(g) << " " << num_edges(g) << std::endl;

  show(g);

  return 0;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file bes_io_test.cpp
/// \brief Some io tests for boolean equation systems.

#include <sstream>
#include <string>
#include <boost/test/minimal.hpp>
#include "mcrl2/bes/boolean_equation_system.h"
#include "mcrl2/bes/parse.h"
#include "mcrl2/bes/io.h"
#include "mcrl2/bes/print.h"
#include "mcrl2/pbes/pbes.h"

using namespace mcrl2;
using namespace mcrl2::bes;

std::string bes1 =
  "pbes              \n"
  "                  \n"
  "nu X1 = X2 && X1; \n"
  "mu X2 = X1 || X2; \n"
  "                  \n"
  "init X1;          \n"
  ;

void test_parse_bes()
{
  boolean_equation_system b;
  std::stringstream from(bes1);
  from >> b;
  std::cout << "b = \n" << bes::pp(b) << std::endl;

  // check if the pretty printed BES can be parsed again
  std::string bes2 = bes::pp(b);
  std::stringstream from2(bes1);
  from2 >> b;

}

void test_bes()
{
  boolean_equation_system b;
  std::stringstream bes_stream(bes1);
  bes_stream >> b;

  std::stringstream out;
  bes::save_bes_cwi(b, out);
}

void test_pbes()
{
  pbes_system::pbes b;
  std::stringstream bes_stream(bes1);
  bes_stream >> b;

  std::stringstream out;
  bes::save_bes_cwi(b, out);
}

void test_pgsolver()
{
  boolean_equation_system b;
  std::stringstream bes_stream(bes1);
  bes_stream >> b;

  std::stringstream out;
  bes::save_bes_pgsolver(b, out);

  std::clog << out.str() << std::endl;
}

int test_main(int argc, char* argv[])
{
  test_parse_bes();
  test_bes();
  test_pbes();
  test_pgsolver();

  return 0;
}


#ifndef OBJECT_3D_HPP_
#define OBJECT_3D_HPP_

#define GLM_ENABLE_EXPERIMENTAL

#include <vector>
#include <array>
#include <memory>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <polyscope/polyscope.h>
#include <polyscope/point_cloud.h>
#include <polyscope/surface_mesh.h>
#include <polyscope/surface_mesh_io.h>

#include "mesh_io.hpp"
#include "data3d.hpp"

class Object3D {

public:
    Object3D(std::string path);
    Object3D(std::string path, std::string name);
    ~Object3D();

    void draw();
    void draw_point_cloud();
    void draw_mesh();

    void set_points(std::vector<std::array<double, 3>> &points);
    void set_vertices_random_colors(std::vector<std::array<double, 3>> &rand_colors);
    std::vector<std::array<double, 3>> get_vertices_random_colors();

    void set_faces_random_colors(std::vector<std::array<double, 3>> &rand_colors);
    std::vector<std::array<double, 3>> get_faces_random_colors();

    bool is_mesh();

    static bool compute_conformal;

protected:
    void load_file();

    void generate_random_colors();

private:
    std::string path;
    std::string name;

    Data3DPtr data;

    std::vector<std::array<double, 3>> v_rand_colors;
    std::vector<std::array<double, 3>> f_rand_colors;

    bool has_faces;
    bool has_texture;

};

#endif // OBJECT_3D_HPP_


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename ArgMatrix>
void inline f(const ArgMatrix&) {}

template <typename ResMatrix, typename ArgMatrix>
void test(const ResMatrix& , const ArgMatrix& B)
{
    ResMatrix C(trans(B));

    std::cout << "trans(B) is \n" << C;
    std::cout << "trans(B) is \n" << trans(B);
    std::cout << "trace(B) is " << trace(B) << "\n";

    // ResMatrix D(A * trans(B));

#if 0 // Can't convert so far
    ResMatrix D= B;
    std::cout << "D(B) is \n" << D;

    f<ResMatrix>(B);
#endif
}

int main(int , char**)
{
    using namespace mtl;
    typedef mat::parameters<tag::row_major, mtl::index::c_index, mtl::fixed::dimensions<2, 2>, true> fmat_para;

    float ma[2][2]= {{2., 3.}, {4., 5.}};

    dense2D<float>                   A_dyn(ma);
    dense2D<float, fmat_para>        A_stat(ma);

    test(A_dyn, A_dyn);
    test(A_dyn, A_stat);
    test(A_stat, A_stat);
    test(A_stat, A_dyn);

    typedef mtl::vec::fixed::dimension< 2 > fsize;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::col_major, fsize, true> >     rf;
    rf = 1.0;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::col_major, fsize, true> >     rf2;
    const static double swap = 1.0;
    swap*(rf + A_stat*rf);
    return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121017    E. Dekens         Code created.
 *
 *    References
 *
 *    Notes
 *
 */

#include <cmath>
#include <limits>
#include <stdexcept>

#include <boost/exception/all.hpp>
#include <boost/math/constants/constants.hpp>

#include "Eigen/Core"

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h"

namespace tudat
{
namespace gravitation
{

//! Compute gravitational acceleration due to multiple spherical harmonics terms, defined using
//! geodesy-normalization.
Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const Eigen::MatrixXd& cosineHarmonicCoefficients,
        const Eigen::MatrixXd& sineHarmonicCoefficients,
        boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    // Set highest degree and order.
    const int highestDegree = cosineHarmonicCoefficients.rows( );
    const int highestOrder = cosineHarmonicCoefficients.cols( );

    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = Eigen::Vector3d::Zero( );

    // Convert Cartesian coordinates to cylindrical.
    const Eigen::Vector3d cylindricalCoordinates = coordinate_conversions::
            convertCartesianToCylindrical( positionOfBodySubjectToAcceleration );

    // Compute radius coordinate.
    sphericalpositionOfBodySubjectToAcceleration( 0 )
            = std::sqrt( cylindricalCoordinates( 0 ) * cylindricalCoordinates( 0 )
                       + cylindricalCoordinates( 2 ) * cylindricalCoordinates( 2 ) );

    // If radius coordinate is smaller than planetary radius...
    if ( sphericalpositionOfBodySubjectToAcceleration( 0 ) < equatorialRadius )
    {
        // ...throw runtime error.
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            "Distance to origin is smaller than the size of the main body." ) ) );
    }

    // If radius coordinate is zero...
    if ( std::fabs( cylindricalCoordinates( 0 ) ) < std::numeric_limits< double >::epsilon( ) )
    {
        // ...set latitude coordinate to 90 degrees.
        sphericalpositionOfBodySubjectToAcceleration( 1 )
                = mathematical_constants::PI / 2.0;
    }

    // Else...
    else
    {
        // ...compute latitude coordinate.
        sphericalpositionOfBodySubjectToAcceleration( 1 )
                = std::atan( cylindricalCoordinates( 2 ) / cylindricalCoordinates( 0 ) );
    }

    // Compute longitude coordinate.
    sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );
    basic_mathematics::LegendreCache& legendreCacheReference = *( sphericalHarmonicsCache->getLegendreCache( ) );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Initialize gradient vector.
    Eigen::Vector3d sphericalGradient = Eigen::Vector3d::Zero( );

    // Loop through all degrees.
    for ( int degree = 0; degree < highestDegree; degree++ )
    {
        // Loop through all orders.
        for ( int order = 0; ( order <= degree ) && ( order < highestOrder ); order++ )
        {
            // Compute geodesy-normalized Legendre polynomials.
            const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial(
                        degree, order, legendreCacheReference );
            const double incrementedLegendrePolynomial =
                    basic_mathematics::computeGeodesyLegendrePolynomial(
                        degree, order + 1, legendreCacheReference );

            // Compute geodesy-normalized Legendre polynomial derivative.
            const double legendrePolynomialDerivative =
                    basic_mathematics::computeGeodesyLegendrePolynomialDerivative(
                        degree, order,
                        sineOfAngle,
                        legendrePolynomial, incrementedLegendrePolynomial );

            // Compute the potential gradient of a single spherical harmonic term.
            sphericalGradient += basic_mathematics::computePotentialGradient(
                        sphericalpositionOfBodySubjectToAcceleration,
                        preMultiplier,
                        degree,
                        order,
                        cosineHarmonicCoefficients( degree, order ),
                        sineHarmonicCoefficients( degree, order ),
                        legendrePolynomial,
                        legendrePolynomialDerivative, sphericalHarmonicsCache );
        }
    }

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector) and
    // return the resulting acceleration vector.
    return coordinate_conversions::convertSphericalToCartesianGradient(
                sphericalGradient, positionOfBodySubjectToAcceleration );
}

//! Compute gravitational acceleration due to single spherical harmonics term.
Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration;

    // Convert Cartesian coordinates to cylindrical.
    const Eigen::Vector3d cylindricalCoordinates = coordinate_conversions::
            convertCartesianToCylindrical( positionOfBodySubjectToAcceleration );

    // Compute radius coordinate.
    sphericalpositionOfBodySubjectToAcceleration( 0 )
            = std::sqrt(cylindricalCoordinates( 0 ) * cylindricalCoordinates( 0 )
                       + cylindricalCoordinates( 2 ) * cylindricalCoordinates( 2 ) );

    // If radius coordinate is smaller than planetary radius...
    if (sphericalpositionOfBodySubjectToAcceleration( 0 ) < equatorialRadius)
    {
        // ...trow runtime error.
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            "Distance to origin is smaller than the size of the main body." ) ) );
    }

    // If radius coordinate is zero...
    if ( std::fabs( cylindricalCoordinates( 0 ) ) < std::numeric_limits< double >::epsilon( ) )
    {
        // ...set latitude coordinate to 90 degrees.
        sphericalpositionOfBodySubjectToAcceleration( 1 )
                = mathematical_constants::PI / 2.0;
    }

    // Else...
    else
    {
        // ...compute latitude coordinate.
        sphericalpositionOfBodySubjectToAcceleration( 1 )
                = std::atan( cylindricalCoordinates( 2 ) / cylindricalCoordinates( 0 ) );
    }

    // Compute longitude coordinate.
    sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );
    basic_mathematics::LegendreCache& legendreCacheReference = *( sphericalHarmonicsCache->getLegendreCache( ) );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Compute geodesy-normalized Legendre polynomials.
    const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial(
                degree, order, legendreCacheReference );
    const double incrementedLegendrePolynomial =
            basic_mathematics::computeGeodesyLegendrePolynomial(
                degree, order + 1,  legendreCacheReference );

    // Compute geodesy-normalized Legendre polynomial derivative.x
    const double legendrePolynomialDerivative =
            basic_mathematics::computeGeodesyLegendrePolynomialDerivative(
                degree, order,
                sineOfAngle, legendrePolynomial,
                incrementedLegendrePolynomial );

    // Compute the potential gradient resulting from the spherical harmonic term.
    const Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient(
                sphericalpositionOfBodySubjectToAcceleration,
                preMultiplier,
                degree,
                order,
                cosineHarmonicCoefficient,
                sineHarmonicCoefficient,
                legendrePolynomial,
                legendrePolynomialDerivative, sphericalHarmonicsCache );

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector),
    // and return resulting acceleration vector.
    return coordinate_conversions::convertSphericalToCartesianGradient(
                sphericalGradient, positionOfBodySubjectToAcceleration );
}

} // namespace gravitation
} // namespace tudat


/*! file one-solver-anneal.cpp
 *
 * @copyright Licensed under MIT license by hyperQ – Ewa Hendzel
 *
 */

#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>

#include "CL/sycl.hpp"
#include "helpers/devices.hpp"
#include "helpers/qubo_helpers.hpp"
#include "model/qubo.hpp"
#include "model/solution.hpp"
#include "simulated_annealing/annealing.hpp"

namespace po = boost::program_options;
namespace sycl = cl::sycl;

using queue_ptr = std::unique_ptr<sycl::queue>;

void construct_linear_beta_schedule(std::vector<double> &schedule,
                                    double beta_min, double beta_max,
                                    uint num_iter) {
  for (auto i = 0; i < num_iter; i++) {
    schedule[i] = beta_min + beta_max * i / static_cast<double>(num_iter - 1);
  }
}

void construct_geometric_beta_schedule(std::vector<double> &schedule,
                                       double beta_min, double beta_max,
                                       uint num_iter) {
  schedule[0] = beta_min;
  auto alpha = pow(beta_max / beta_min, 1.0 / (num_iter - 1));
  for (auto i = 1; i < num_iter; i++) {
    schedule[i] = (schedule[i - 1]) * alpha;
  }
}

int main(int argc, char *argv[]) {

  std::string input_file;
  std::string output_file;
  std::string schedule_type;
  std::string device_type;

  uint num_iter;
  uint num_tries;

  double beta_min, beta_max;

  try {

    po::options_description options("Allowed options");
    options.add_options()("help", "produce help message")(
        "input", po::value<std::string>(&input_file), "input file")(
        "output", po::value<std::string>(&output_file), "output file")(
        "num-iter", po::value<uint>(&num_iter)->default_value(100),
        "number of iterations of the algorithm")(
        "num-tries", po::value<uint>(&num_tries)->default_value(100),
        "number of trajectories to try")(
        "schedule-type",
        po::value<std::string>(&schedule_type)->default_value("geometric"),
        "type of beta schedule tu use, either linear or geometric")(
        "beta-min", po::value<double>(&beta_min)->default_value(0.1),
        "minimum value of beta in the annealing schedule (default 0.1)")(
        "beta-max", po::value<double>(&beta_max)->default_value(1.0),
        "maximum value of beta in the annealing schedule (default 1.0)")(
        "device-type",
        po::value<std::string>(&device_type)->default_value("host"),
        "device type to use (cpu, gpu or host)");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << options << std::endl;
      return 0;
    }

    if (!vm.count("input")) {
      std::cerr << "No input file provided." << std::endl;
      return -1;
    }

    if (!vm.count("output")) {
      std::cerr << "No output file provided." << std::endl;
      return -1;
    }

    if (device_type != "cpu" && device_type != "gpu" && device_type != "host") {
      std::cerr << "Unknown device type: " << device_type << std::endl;
      return -1;
    }

    if (schedule_type != "linear" && schedule_type != "geometric") {
      std::cerr << "Unknown beta schedule: " << schedule_type << std::endl;
      return -1;
    }

    if (beta_max < 0 || beta_min < 0) {
      std::cerr
          << "Invalid schedule, both ends of beta range need to be positive"
          << std::endl;
      return -1;
    }

    if (beta_min >= beta_max) {
      std::cerr
          << "Invalid schedule, initial beta is not lesser than final beta"
          << std::endl;
      return -1;
    }

    std::cout << "Reading input from: " << input_file << std::endl;

    std::cout << "Output will be saved to: " << output_file << std::endl;

    std::cout << "Schedule type: " << schedule_type << std::endl;

    std::cout << "Beta range: [" << beta_min << ", " << beta_max << "]"
              << std::endl;

    std::cout << "Number of iterations: " << num_iter << std::endl;
    std::cout << "Number of tries: " << num_tries << std::endl;

    std::ifstream qubo_file(input_file);

    if (!qubo_file) {
      std::cerr << "can not open input file: " << input_file << std::endl;
      return -1;
    }

    qubo_file.unsetf(std::ios::skipws);
    auto instance = qubo::QUBOModel<int, double>::load(qubo_file);

    queue_ptr q_ptr;

    try {
      q_ptr = queue_ptr(
          new sycl::queue(*devices::construct_device_selector(device_type)));
    } catch (std::runtime_error) {
      std::cerr << "No devices of given type could be initialized."
                << std::endl;
    }

    std::cout << "Using device: "
              << q_ptr->get_device().get_info<sycl::info::device::name>()
              << std::endl;

    std::vector<double> beta_schedule(num_iter);

    if (schedule_type == "linear") {
      construct_linear_beta_schedule(beta_schedule, beta_min, beta_max,
                                     num_iter);
    } else {
      construct_geometric_beta_schedule(beta_schedule, beta_min, beta_max,
                                        num_iter);
    }

    auto solution =
        sa::anneal(instance, *q_ptr, beta_schedule, num_iter, num_tries);

    std::ofstream results_file(output_file);

    solution.save(results_file);

    results_file.close();
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  } catch (...) {
    std::cerr << "Exception of unknown type!\n";
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// fusion::detail::joint_view::flatten_bind_range.hpp                 	 	 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_FUSION_JOINT_VIEW_FLATTEN_BIND_RANGE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_FUSION_JOINT_VIEW_FLATTEN_BIND_RANGE_HPP_ER_2010
#include <vector>
#include <boost/type_traits.hpp>
#include <boost/range.hpp>

#include <boost/iterator/zip_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/statistics/detail/fusion/joint_view/bind_iterator.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace fusion{
namespace joint_view{
namespace result_of{

    // Maps {x[i]:i=1,...,n} and {{y[i][j]:j=1,...,n[j]} : i=1,...,n}
    // to {joint_view(x[i],y[i][j]) : i=1,...,n; j=1,...,n[i]}
    template<typename ItX,typename ItArY>
    struct flatten_bind_range{

        template<typename It>
        struct it_value : boost::remove_cv<
        	typename boost::remove_reference<
        		typename boost::iterator_reference<It>::type
        	>::type
        >{};

        template<typename X,typename Ry>
        struct fun
        {
            typedef
            	detail::fusion::joint_view::result_of::bind_range<X,Ry> meta_;
            typedef typename meta_::type result_type;

            fun(){}

            template<typename T>
            result_type operator()(const T& tuple)const{
                return meta_::call(
                    boost::get<0>(tuple),
                    boost::get<1>(tuple)
                );
            }

        };

        typedef fun<
            typename it_value<ItX>::type,
            typename it_value<ItArY>::type
        > fun_;

        typedef boost::tuple<ItX,ItArY> 						it_tuple_;
        typedef boost::zip_iterator<it_tuple_> 					zip_it_;
        typedef boost::transform_iterator<fun_,zip_it_>			t_it_;
        typedef boost::flatten_iterator<t_it_>					flat_it_;
        typedef boost::iterator_range<flat_it_> 				type;

        static type call(ItX b_x,ItX e_x,ItArY b_y,ItArY e_y){
            zip_it_ 	zip_it_b( it_tuple_(b_x,b_y) );
            zip_it_ 	zip_it_e( it_tuple_(e_x,e_y) );
            t_it_ 		t_it_b(zip_it_b,fun_());
            t_it_ 		t_it_e(zip_it_e,fun_());
            flat_it_ 	flat_it_b(t_it_b,t_it_e);
            flat_it_ 	flat_it_e(t_it_e,t_it_e);
            return type( flat_it_b, flat_it_e );
        };
	};
}

    template<typename ItX,typename ItArY>
    typename detail::fusion::joint_view
    	::result_of::flatten_bind_range<ItX,ItArY>::type
    flatten_bind_range(ItX b1,ItX e1,ItArY b2,ItArY e2){
        typedef detail::fusion::joint_view::result_of
        	::flatten_bind_range<ItX,ItArY> meta_;
        return meta_::call(b1,e1,b2,e2);
    }

}// joint_view
}// fusion
}// detail
}// statistics
}// boost

#endif