/**
 * @file allkrann_search_test.cpp
 *
 * Unit tests for the 'RASearch' class and consequently the
 * 'RASearchRules' class
 *
 * 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 <time.h>
#include <mlpack/core.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/core/tree/cover_tree.hpp>

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

#include <mlpack/methods/rann/ra_search.hpp>
#include <mlpack/methods/rann/ra_model.hpp>

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

BOOST_AUTO_TEST_SUITE(KRANNTest);

// Test the correctness and guarantees of KRANN when in naive mode.
BOOST_AUTO_TEST_CASE(NaiveGuaranteeTest)
{
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  RASearch<> rsRann(refData, true, false, 1.0);

  arma::mat qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    rsRann.Search(queryData, 1, neighbors, distances);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-RS: RANN guarantee fails on " << numQueriesFail
      << " queries." << endl;

  // assert that at most 5% of the queries fall out of this threshold
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test single-tree rank-approximate search (harder to test because of
// the randomness involved).
BOOST_AUTO_TEST_CASE(SingleTreeSearch)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  RASearch<> tssRann(refData, false, true, 1.0, 0.95, false, false);

  // The relative ranks for the given query reference pair
  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tssRann.Search(queryData, 1, neighbors, distances);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSS: RANN guarantee fails on " << numQueriesFail
      << " queries." << endl;

  // Assert that at most 5% of the queries fall out of this threshold.
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test dual-tree rank-approximate search (harder to test because of the
// randomness involved).
BOOST_AUTO_TEST_CASE(DualTreeSearch)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  RASearch<> tsdRann(refData, false, false, 1.0, 0.95, false, false, 5);

  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  // Build query tree by hand.
  typedef KDTree<EuclideanDistance, RAQueryStat<NearestNeighborSort>,
      arma::mat> TreeType;
  std::vector<size_t> oldFromNewQueries;
  TreeType queryTree(queryData, oldFromNewQueries);

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tsdRann.Search(&queryTree, 1, neighbors, distances);

    for (size_t i = 0; i < queryData.n_cols; i++)
    {
      const size_t oldIndex = oldFromNewQueries[i];
      if (qrRanks(oldIndex, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;
    }

    neighbors.reset();
    distances.reset();

    tsdRann.ResetQueryTree(&queryTree);
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSD: RANN guarantee fails on " << numQueriesFail
      << " queries." << endl;

  // assert that at most 5% of the queries fall out of this threshold
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test rank-approximate search with just a single dataset.  These tests just
// ensure that the method runs okay.
BOOST_AUTO_TEST_CASE(SingleDatasetNaiveSearch)
{
  arma::mat dataset(5, 2500);
  dataset.randn();

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  RASearch<> naive(dataset, true);

  naive.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 2500);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 2500);
}

// Test rank-approximate search with just a single dataset in single-tree mode.
// These tests just ensure that the method runs okay.
BOOST_AUTO_TEST_CASE(SingleDatasetSingleSearch)
{
  arma::mat dataset(5, 2500);
  dataset.randn();

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  RASearch<> single(dataset, false, true);

  single.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 2500);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 2500);
}

// Test rank-approximate search with just a single dataset in dual-tree mode.
// These tests just ensure that the method runs okay.
BOOST_AUTO_TEST_CASE(SingleDatasetSearch)
{
  arma::mat dataset(5, 2500);
  dataset.randn();

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  RASearch<> allkrann(dataset);
  allkrann.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 2500);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 2500);
}

// Test single-tree rank-approximate search with cover trees.
BOOST_AUTO_TEST_CASE(SingleCoverTreeTest)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  typedef RASearch<NearestNeighborSort, EuclideanDistance, arma::mat,
      StandardCoverTree> RACoverTreeSearch;

  RACoverTreeSearch tssRann(refData, false, true, 1.0, 0.95, false, false, 5);

  // The relative ranks for the given query reference pair.
  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tssRann.Search(queryData, 1, neighbors, distances);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSS (cover tree): RANN guarantee fails on "
      << numQueriesFail << " queries." << endl;

  // Assert that at most 5% of the queries fall out of this threshold.
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test dual-tree rank-approximate search with cover trees.
BOOST_AUTO_TEST_CASE(DualCoverTreeTest)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  typedef StandardCoverTree<EuclideanDistance, RAQueryStat<NearestNeighborSort>,
      arma::mat> TreeType;
  typedef RASearch<NearestNeighborSort, EuclideanDistance, arma::mat,
      StandardCoverTree> RACoverTreeSearch;

  TreeType refTree(refData);
  TreeType queryTree(queryData);

  RACoverTreeSearch tsdRann(&refTree, false, 1.0, 0.95, false, false, 5);

  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tsdRann.Search(&queryTree, 1, neighbors, distances);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();

    tsdRann.ResetQueryTree(&queryTree);
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSD (cover tree): RANN guarantee fails on "
      << numQueriesFail << " queries." << endl;

  // assert that at most 5% of the queries fall out of this threshold
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test single-tree rank-approximate search with ball trees.
// This is known to not work right now.
/*
BOOST_AUTO_TEST_CASE(SingleBallTreeTest)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  typedef BinarySpaceTree<BallBound<>, RAQueryStat<NearestNeighborSort> >
      TreeType;
  typedef RASearch<NearestNeighborSort, metric::EuclideanDistance, TreeType>
      RABallTreeSearch;

  RABallTreeSearch tssRann(refData, queryData, false, true);

  // The relative ranks for the given query reference pair.
  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 30;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tssRann.Search(1, neighbors, distances, 1.0, 0.95, false, false, 5);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSS (ball tree): RANN guarantee fails on "
      << numQueriesFail << " queries." << endl;

  // Assert that at most 5% of the queries fall out of this threshold.
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}

// Test dual-tree rank-approximate search with Ball trees.
BOOST_AUTO_TEST_CASE(DualBallTreeTest)
{
  arma::mat refData;
  arma::mat queryData;

  data::Load("rann_test_r_3_900.csv", refData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Search for 1 rank-approximate nearest-neighbors in the top 30% of the point
  // (rank error of 3).
  arma::Mat<size_t> neighbors;
  arma::mat distances;

  typedef BinarySpaceTree<BallBound<>, RAQueryStat<NearestNeighborSort> >
    TreeType;
  typedef RASearch<NearestNeighborSort, metric::EuclideanDistance, TreeType>
      RABallTreeSearch;

  TreeType refTree(refData);
  TreeType queryTree(queryData);

  RABallTreeSearch tsdRann(&refTree, &queryTree, refData, queryData, false);

  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  size_t numRounds = 1000;
  arma::Col<size_t> numSuccessRounds(queryData.n_cols);
  numSuccessRounds.fill(0);

  // 1% of 900 is 9, so the rank is expected to be less than 10.
  size_t expectedRankErrorUB = 10;

  for (size_t rounds = 0; rounds < numRounds; rounds++)
  {
    tsdRann.Search(1, neighbors, distances, 1.0, 0.95, false, false, 5);

    for (size_t i = 0; i < queryData.n_cols; i++)
      if (qrRanks(i, neighbors(0, i)) < expectedRankErrorUB)
        numSuccessRounds[i]++;

    neighbors.reset();
    distances.reset();

    tsdRann.ResetQueryTree();
  }

  // Find the 95%-tile threshold so that 95% of the queries should pass this
  // threshold.
  size_t threshold = floor(numRounds *
      (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
  size_t numQueriesFail = 0;
  for (size_t i = 0; i < queryData.n_cols; i++)
    if (numSuccessRounds[i] < threshold)
      numQueriesFail++;

  Log::Warn << "RANN-TSD (Ball tree): RANN guarantee fails on "
      << numQueriesFail << " queries." << endl;

  // assert that at most 5% of the queries fall out of this threshold
  // 5% of 100 queries is 5.
  size_t maxNumQueriesFail = 6;

  BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
}
*/

/**
 * Make sure that the neighborPtr matrix isn't accidentally deleted.
 * See issue #478.
 */
BOOST_AUTO_TEST_CASE(NeighborPtrDeleteTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);

  // Build the tree ourselves.
  std::vector<size_t> oldFromNewReferences;
  RASearch<>::Tree tree(dataset);
  RASearch<> allkrann(&tree);

  // Now make a query set.
  arma::mat queryset = arma::randu<arma::mat>(5, 50);
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  allkrann.Search(queryset, 3, neighbors, distances);

  // These will (hopefully) fail is either the neighbors or the distances matrix
  // has been accidentally deleted.
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 50);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 50);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

/**
 * Test that the rvalue reference move constructor works.
 */
BOOST_AUTO_TEST_CASE(MoveConstructorTest)
{
  arma::mat dataset = arma::randu<arma::mat>(3, 200);
  arma::mat copy(dataset);

  KRANN moveknn(std::move(copy));
  KRANN knn(dataset);

  BOOST_REQUIRE_EQUAL(copy.n_elem, 0);
  BOOST_REQUIRE_EQUAL(moveknn.ReferenceSet().n_rows, 3);
  BOOST_REQUIRE_EQUAL(moveknn.ReferenceSet().n_cols, 200);

  arma::mat moveDistances, distances;
  arma::Mat<size_t> moveNeighbors, neighbors;

  moveknn.Search(1, moveNeighbors, moveDistances);
  knn.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(moveNeighbors.n_rows, neighbors.n_rows);
  BOOST_REQUIRE_EQUAL(moveNeighbors.n_rows, neighbors.n_rows);
  BOOST_REQUIRE_EQUAL(moveNeighbors.n_cols, neighbors.n_cols);
  BOOST_REQUIRE_EQUAL(moveDistances.n_rows, distances.n_rows);
  BOOST_REQUIRE_EQUAL(moveDistances.n_cols, distances.n_cols);
}

/**
 * Test that the dataset can be retrained with the move Train() function.
 */
BOOST_AUTO_TEST_CASE(MoveTrainTest)
{
  arma::mat dataset = arma::randu<arma::mat>(3, 200);

  // Do it in tree mode, and in naive mode.
  KRANN knn;
  knn.Train(std::move(dataset));

  arma::mat distances;
  arma::Mat<size_t> neighbors;
  knn.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(dataset.n_elem, 0);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 200);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 200);

  dataset = arma::randu<arma::mat>(3, 300);
  knn.Naive() = true;
  knn.Train(std::move(dataset));
  knn.Search(1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(dataset.n_elem, 0);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 300);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 300);
}

/**
 * Make sure the RAModel class works.
 */
BOOST_AUTO_TEST_CASE(RAModelTest)
{
  // Ensure that we can build an RAModel<NearestNeighborSearch> and get correct
  // results.
  typedef RAModel<NearestNeighborSort> KNNModel;

  arma::mat queryData, referenceData;
  data::Load("rann_test_r_3_900.csv", referenceData, true);
  data::Load("rann_test_q_3_100.csv", queryData, true);

  // Build all the possible models.
  KNNModel models[20];
  models[0] = KNNModel(KNNModel::TreeTypes::KD_TREE, false);
  models[1] = KNNModel(KNNModel::TreeTypes::KD_TREE, true);
  models[2] = KNNModel(KNNModel::TreeTypes::COVER_TREE, false);
  models[3] = KNNModel(KNNModel::TreeTypes::COVER_TREE, true);
  models[4] = KNNModel(KNNModel::TreeTypes::R_TREE, false);
  models[5] = KNNModel(KNNModel::TreeTypes::R_TREE, true);
  models[6] = KNNModel(KNNModel::TreeTypes::R_STAR_TREE, false);
  models[7] = KNNModel(KNNModel::TreeTypes::R_STAR_TREE, true);
  models[8] = KNNModel(KNNModel::TreeTypes::X_TREE, false);
  models[9] = KNNModel(KNNModel::TreeTypes::X_TREE, true);
  models[10] = KNNModel(KNNModel::TreeTypes::HILBERT_R_TREE, false);
  models[11] = KNNModel(KNNModel::TreeTypes::HILBERT_R_TREE, true);
  models[12] = KNNModel(KNNModel::TreeTypes::R_PLUS_TREE, false);
  models[13] = KNNModel(KNNModel::TreeTypes::R_PLUS_TREE, true);
  models[14] = KNNModel(KNNModel::TreeTypes::R_PLUS_PLUS_TREE, false);
  models[15] = KNNModel(KNNModel::TreeTypes::R_PLUS_PLUS_TREE, true);
  models[16] = KNNModel(KNNModel::TreeTypes::UB_TREE, false);
  models[17] = KNNModel(KNNModel::TreeTypes::UB_TREE, true);
  models[18] = KNNModel(KNNModel::TreeTypes::OCTREE, false);
  models[19] = KNNModel(KNNModel::TreeTypes::OCTREE, true);

  arma::Mat<size_t> qrRanks;
  data::Load("rann_test_qr_ranks.csv", qrRanks, true, false); // No transpose.

  for (size_t j = 0; j < 3; ++j)
  {
    for (size_t i = 0; i < 20; ++i)
    {
      // We only have std::move() constructors so make a copy of our data.
      arma::mat referenceCopy(referenceData);
      if (j == 0)
        models[i].BuildModel(std::move(referenceCopy), 20, false, false);
      if (j == 1)
        models[i].BuildModel(std::move(referenceCopy), 20, false, true);
      if (j == 2)
        models[i].BuildModel(std::move(referenceCopy), 20, true, false);

      // Set the search parameters.
      models[i].Tau() = 1.0;
      models[i].Alpha() = 0.95;
      models[i].SampleAtLeaves() = false;
      models[i].FirstLeafExact() = false;
      models[i].SingleSampleLimit() = 5;

      arma::Mat<size_t> neighbors;
      arma::mat distances;

      arma::Col<size_t> numSuccessRounds(queryData.n_cols);
      numSuccessRounds.fill(0);

      // 1% of 900 is 9, so the rank is expected to be less than 10.
      size_t expectedRankErrorUB = 10;

      size_t numRounds = 100;
      for (size_t round = 0; round < numRounds; round++)
      {
        arma::mat queryCopy(queryData);
        models[i].Search(std::move(queryCopy), 1, neighbors, distances);
        for (size_t k = 0; k < queryData.n_cols; k++)
          if (qrRanks(k, neighbors(0, k)) < expectedRankErrorUB)
            numSuccessRounds[k]++;

        neighbors.reset();
        distances.reset();
      }

      // Find the 95%-tile threshold so that 95% of the queries should pass this
      // threshold.
      size_t threshold = floor(numRounds *
          (0.95 - (1.96 * sqrt(0.95 * 0.05 / numRounds))));
      size_t numQueriesFail = 0;
      for (size_t k = 0; k < queryData.n_cols; k++)
        if (numSuccessRounds[k] < threshold)
          numQueriesFail++;

      // assert that at most 5% of the queries fall out of this threshold
      // 5% of 100 queries is 5.
      size_t maxNumQueriesFail = 25; // See #734 for why this is so high.

      BOOST_REQUIRE_LT(numQueriesFail, maxNumQueriesFail);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_nlez.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_nlez real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_nlez;
  using r_t = decltype(is_nlez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_nlez(bs::Inf<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Minf<T>()), r_t(false));
  STF_EQUAL(is_nlez(bs::Nan<T>()), r_t(true));
#endif
  STF_EQUAL(is_nlez(-bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_nlez(bs::Half<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_nlez(bs::One<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Quarter<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_nlez signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_nlez;
  using r_t = decltype(is_nlez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_nlez(bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_nlez(bs::One<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_nlez unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_nlez;
  using r_t = decltype(is_nlez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_nlez(bs::One<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_nlez(bs::Zero<T>()), r_t(false));
}

STF_CASE ( "is_nlez bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_nlez;
  using r_t = decltype(is_nlez(bool()));

  // return type conformity test
   STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_nlez(true), true);
  STF_EQUAL(is_nlez(false), false);
}


#include "widgetpreview.h"
#include <QPainter>
#include <QPaintEvent>
#include <Eigen/Core>
#include <tinycolormap.hpp>
#include "core.h"
#include "mainwindow.h"

using Eigen::VectorXd;
using Eigen::Vector3d;

namespace
{
Core& core = Core::getInstance();
}

WidgetPreview::WidgetPreview(QWidget *parent) :
    QWidget(parent)
{
}

void WidgetPreview::paintEvent(QPaintEvent *event)
{
    QPainter painter(this);
    const QRect& rect = event->rect();

    const VectorXd x = core.computeParametersFromSlider(core.mainWindow->obtainSliderPosition());
    const double   y = core.evaluateObjectiveFunction(x);
    const auto     c = tinycolormap::GetJetColor(y);

    const QBrush backgroundBrush = QBrush(QColor(c[0] * 255.0, c[1] * 255.0, c[2] * 255.0));
    painter.setRenderHint(QPainter::Antialiasing);
    painter.fillRect(rect, backgroundBrush);
}


/**
BSD 3-Clause License

This file is part of the code accompanying the paper
From Planes to Corners: Multi-Purpose Primitive Detection in Unorganized 3D Point Clouds
by C. Sommer, Y. Sun, L. Guibas, D. Cremers and T. Birdal,
accepted for Publication in IEEE Robotics and Automation Letters (RA-L) 2020.

Copyright (c) 2019, Christiane Sommer.
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 copyright holder 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.
*/

// includes
#include <iostream>
#include <fstream>
#include "definitions.h"
// libraries
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <sophus/so3.hpp>
#include <CLI/CLI.hpp>
// classes
#include "Timer.h"
#include "Plane.h"
#include "graph/PlaneGraph.h"
#include "graph/ParallelPlaneGraph.h"
#include "PPF/PairDetector.h"
#include "corners/Corner.h"
// function includes
#include "io/load_ply_cloud.h"
#include "visualize/pcshow.h"

/*
 * main
 */
int main(int argc, char** argv) {

    Timer T;

    // parse setttings from command line
    std::string ply_file;
    int min_votes = 5;
    double d_min = .1, d_max = 1.;
    int sampling = 50;

    CLI::App app{"Detect and refine orthogonal plane configurations"};
    app.add_option("--img", ply_file, "ply file path, from data/")->required();
    app.add_option("--min-votes", min_votes, "minimal number of votes to accept configuration");
    app.add_option("--dmin", d_min, "minimal distance between two points");
    app.add_option("--dmax", d_max, "maximal distance between two points");
    app.add_option("--sampling", sampling, "sampling of points");

    try {
        app.parse(argc, argv);
    } catch (const CLI::ParseError& e) {
        return app.exit(e);
    }

    // load data
    std::string filepath = "../data/";
    std::vector<Eigen::Vector3f> points;
    std::vector<Eigen::Vector3f> normals;

    T.tic();
    read_ply_file(filepath+ply_file, points, normals);
    T.toc("read data");
    std::cout << points.size() << " points loaded." << std::endl;
    std::cout << normals.size() << " normals loaded." << std::endl;

    pcshow(points);

    // pairing
    ppf::PairDetector pairDet(d_max, d_min, min_votes);
    PlaneGraph planeMap(pairDet.para_thresh(), pairDet.distance_bin());
    T.tic();
    pairDet.detect_ortho_pairs(points, normals, planeMap);
    T.toc("time pairing PPF ");
    planeMap.print_info();

    std::cout << std::endl << "Thresholds:\tAngle:\t" << pairDet.para_thresh() << "\tDistance:\t" << pairDet.distance_bin() << std::endl << std::endl;

    // clustering & filtering
    T.tic();
    planeMap.cluster_graph_vertices();
    T.toc("clustering planes");
    planeMap.print_info();
    planeMap.print_parameters();
    planeMap.print_edges();

    // reduce graph to ParallelPlaneGraph
    T.tic();
    ParallelPlaneGraph redMap = planeMap.reduce_graph();
    T.toc("graph reduction");
    redMap.print_info();
    redMap.print_edges();
    T.tic();
    redMap.triangle_reduce();
    T.toc("triangle reduction");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    // coarse refinement
    double lambda = .01 * points.size();
    T.tic();
    redMap.refine(points, normals, lambda, pairDet.distance_bin(), sampling);
    redMap.cluster_graph_vertices();
    T.toc("coarse refine and clustering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    // find triangles
    std::vector<Graph<ParallelPlaneSet>::Triangle> triangles;
    T.tic();
    redMap.find_triangles(triangles);
    T.toc("find triangles in reduced graph");
    redMap.print_triangles(triangles);
    // if no triangles found: stop here
    if (triangles.size() < 1) {
        std::cout << "No corners found, return." << std::endl;
        return 0;
    }

    // neighborhood search: Kd-Tree
    const PointCloudAdaptor pc2kd(points);
    KDTree index(3, pc2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10)); // arguments: dimensionality, point cloud adaptor, max leafs
    index.buildIndex();
    std::vector<Corner> corners;
    std::vector<size_t> indices;

    // find good corners
    T.tic();
    for (const auto& t : triangles) {
        Vec3 n1 = redMap.vertices()[t.first].n();
        Vec3 n2 = redMap.vertices()[t.second].n();
        Vec3 n3 = redMap.vertices()[t.third].n();
        for (const auto d1 : redMap.vertices()[t.first].ds()) for (const auto d2 : redMap.vertices()[t.second].ds()) for (const auto d3 : redMap.vertices()[t.third].ds()) {
            indices = Corner::eval_candidate(points, normals, n1, d1, n2, d2, n3, d3, index);
            std::cout << (-d1*n1 - d2*n2 - d3*n3).transpose() << std::endl;
            if (indices.size() > 0) {
                Corner c(n1, d1, n2, d2, n3, d3);
                c.set_indices(indices);
                if (c.refine(points)) { // only accept if refinement converges
                    corners.push_back(c);
                }
            }
        }
    }
    T.toc("evaluated corner candidates");

    for (const auto& c : corners) {
        std::cout << c << std::endl << std::endl;
    }

    pcshow_corners(points, corners);

}


#ifndef CONCEPTS_DIMENSIONS_TO_HPP_INCLUDED
#define CONCEPTS_DIMENSIONS_TO_HPP_INCLUDED

#include <limits>
#include <algorithm>

#include <boost/hana/core/to.hpp>
#include <boost/hana/value.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/assert.hpp>

#include <gem/fwd/dimensions.hpp>

namespace gem::concepts {

template <Dimension D1, Dimension D2>
concept bool ConvertibleDimensionPair =
    Dimension<D1> && Dimension<D2> &&
    boost::hana::value((D1::min <= D2::max) && (D1::max >= D2::min));

template <detail::Integral T1, Dimension D2>
concept bool ConvertibleIntegralDimensionPair =
    gem::concepts::detail::Integral<T1> && Dimension<D2> &&
    std::numeric_limits<T1>::min() <= boost::hana::value(D2::max) &&
    std::numeric_limits<T1>::max() >= boost::hana::value(D2::min);

template <Dimension D1, detail::Integral T2>
concept bool ConvertibleDimensionIntegralPair =
    Dimension<D1> && gem::concepts::detail::Integral<T2> &&
    boost::hana::value(D1::min) <= std::numeric_limits<T2>::max() &&
    boost::hana::value(D1::max) >= std::numeric_limits<T2>::min();

template <detail::Integral T1, CompileTimeDimension D2>
concept bool ConvertibleIntegralCompileTimeDimensionPair =
    gem::concepts::detail::Integral<T1> && CompileTimeDimension<D2> &&
    std::numeric_limits<T1>::min() <= boost::hana::value(D2::max) &&
    std::numeric_limits<T1>::max() >= boost::hana::value(D2::min);

template <detail::Integral T1, CompileTimeDimension D2>
concept bool ConvertibleIntegralRuntimeDimensionPair =
    gem::concepts::detail::Integral<T1> && RuntimeDimension<D2> &&
    std::numeric_limits<T1>::min() <= boost::hana::value(D2::max) &&
    std::numeric_limits<T1>::max() >= boost::hana::value(D2::min);

}  // namespace gem::concepts

namespace boost::hana {

//////////////////////////////////////////////////////////////////////////
// Dimension <-> Dimension
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleDimensionPair {D1, D2}
struct to_impl<D1, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, typename D1::dim_t>::value &&
              value((D2::max <= D1::max) && (D2::min >= D1::min))>
{
    static constexpr inline auto
    apply(const D2& d) noexcept -> D1
    {
        return {d.value()};
    }
};

//////////////////////////////////////////////////////////////////////////
// Dimension -> Itegral
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleIntegralDimensionPair {T1, D2}
struct to_impl<T1, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, T1>::value &&
              (std::numeric_limits<T1>::max() >= value(D2::max)) &&
              (std::numeric_limits<T1>::min() <= value(D2::min))>
{
    static constexpr inline auto
    apply(const D2& d) noexcept -> T1
    {
        return {d.value()};
    }
};

//////////////////////////////////////////////////////////////////////////
// Itegral -> Dimension
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleDimensionIntegralPair {D1, T2}
struct to_impl<D1, T2> : // To <- from.
    embedding<is_embedded<T2, typename D1::dim_t>::value &&
              (value(D1::max) >= std::numeric_limits<T2>::max()) &&
              (value(D1::min) <= std::numeric_limits<T2>::min())>
{
    static constexpr inline auto
    apply(const T2& d) noexcept -> D1
    {
        return {d};
    }
};

//////////////////////////////////////////////////////////////////////////
// Dimension -> Ingegral constant
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleIntegralCompileTimeDimensionPair {T1, D2}
struct to_impl<integral_constant_tag<T1>, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, T1>::value>
{
    static constexpr inline auto
    apply(const D2&) noexcept -> integral_constant<T1, value(D2::hint)>
    {
        return {};
    }
};

//////////////////////////////////////////////////////////////////////////
// Ingegral constant -> Dimension
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleDimensionIntegralPair {D1, T2}
struct to_impl<D1, integral_constant_tag<T2>>: // To <- from.
    embedding<is_embedded<T2, typename D1::dim_t>::value &&
              (value(D1::max) >= std::numeric_limits<T2>::max()) &&
              (value(D1::min) <= std::numeric_limits<T2>::min())>
{
    template <T2 cv>
    static constexpr inline auto
    apply(const boost::hana::integral_constant<T2, cv>& d) noexcept -> D1
    {
        return {d};
    }
};

//////////////////////////////////////////////////////////////////////////
// Dimension <-> Dimension tag
//////////////////////////////////////////////////////////////////////////

gem::concepts::ConvertibleIntegralCompileTimeDimensionPair {T1, D2}
struct to_impl<gem::CompileTimeDimensionTag<T1>, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, T1>::value>
{
    static constexpr inline auto
    apply(const D2&) noexcept ->
        gem::Dimension<T1, value(D2::hint), value(D2::max), value(D2::min)>
    {
        return {};
    }
};

gem::concepts::ConvertibleIntegralRuntimeDimensionPair {T1, D2}
struct to_impl<gem::RuntimeDimensionTag<T1>, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, T1>::value>
{
    static constexpr inline auto
    apply(const D2& d2) noexcept ->
        gem::Dimension<T1, value(D2::hint), value(D2::max), value(D2::min)>
    {
        return {d2.value()};
    }
};

gem::concepts::ConvertibleIntegralCompileTimeDimensionPair {T1, D2}
struct to_impl<gem::RuntimeDimensionTag<T1>, D2> : // To <- from.
    embedding<is_embedded<typename D2::dim_t, T1>::value>
{
    static constexpr inline auto
    apply(const D2& d2) noexcept ->
        gem::Dimension<T1, value(D2::hint), value(D2::max), value(D2::min)>
    {
        return {d2.value()};
    }
};

}  // namespace boost::hana

#endif  // !CONCEPTS_DIMENSIONS_TO_HPP_INCLUDED


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// 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 knapsack_unbounded.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */
#ifndef PAAL_KNAPSACK_UNBOUNDED_HPP
#define PAAL_KNAPSACK_UNBOUNDED_HPP

#include "paal/dynamic/knapsack/fill_knapsack_dynamic_table.hpp"
#include "paal/dynamic/knapsack/get_bound.hpp"
#include "paal/dynamic/knapsack/knapsack_common.hpp"
#include "paal/greedy/knapsack_unbounded_two_app.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/knapsack_utils.hpp"
#include "paal/utils/less_pointees.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/adaptor/reversed.hpp>
#include <boost/optional.hpp>

#include <vector>

namespace paal {

namespace detail {
/**
 * @brief For knapsack dynamic algorithm for given element the table has to be
 * traversed from the lowest to highest element
 */
struct knapsack_get_position_range {
    template <typename T>
    auto operator()(T begin, T end)->decltype(irange(begin, end)) {
        return irange(begin, end);
    }
};

template <typename KnapsackData,
          typename GetBestElement, typename ValuesComparator,
          typename ReturnType = typename KnapsackData::return_type>
ReturnType knapsack_unbounded_dynamic(
    KnapsackData knap_data,
    GetBestElement getBest, ValuesComparator compareValues) {
    using Value = typename KnapsackData::value;
    using Size  = typename KnapsackData::size;
    using ObjectsIter = typename KnapsackData::object_iter;
    using ObjIterWithValueOrNull =
        boost::optional<std::pair<ObjectsIter, Value>>;
    std::vector<ObjIterWithValueOrNull> objectOnSize(knap_data.get_capacity() + 1);

    auto compare = [ = ](const ObjIterWithValueOrNull & left,
                         const ObjIterWithValueOrNull & right) {
        return compareValues(left->second, right->second);
    };

    auto objectOnSizeBegin = objectOnSize.begin();
    auto objectOnSizeEnd = objectOnSize.end();
    fill_knapsack_dynamic_table(objectOnSizeBegin, objectOnSizeEnd, knap_data.get_objects(), knap_data.get_size(),
                                [&](ObjIterWithValueOrNull val, ObjectsIter obj)
                                    ->ObjIterWithValueOrNull{
        return std::make_pair(obj, val->second + knap_data.get_value(*obj));
    },
                                compare, [](ObjIterWithValueOrNull & val) {
        val = std::make_pair(ObjectsIter{}, Value{});
    },
                                detail::knapsack_get_position_range());

    // getting position of the max value in the objectOnSize array
    auto maxPos = getBest(objectOnSizeBegin, objectOnSizeEnd, compare);

    // setting solution
    auto remainingSpaceInKnapsack = maxPos;
    while (remainingSpaceInKnapsack != objectOnSizeBegin) {
        assert(*remainingSpaceInKnapsack);
        auto && obj = *((*remainingSpaceInKnapsack)->first);
        knap_data.out(obj);
        remainingSpaceInKnapsack -= knap_data.get_size(obj);
    }

    // returning result
    if (maxPos != objectOnSizeEnd) {
        assert(*maxPos);
        return ReturnType((*maxPos)->second, maxPos - objectOnSizeBegin);
    } else {
        return ReturnType(Value{}, Size{});
    }
}

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam OutputIterator
 * @param objects given objects
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename KnapsackData,
          typename ReturnType = typename KnapsackData::return_type,
          typename Size       = typename KnapsackData::size>
ReturnType knapsack(KnapsackData knap_data,
         unbounded_tag, integral_value_tag, retrieve_solution_tag) {
    using ValueType = typename KnapsackData::value;
    using ObjectsIter = typename KnapsackData::object_iter;
    using TableElementType = boost::optional<std::pair<ObjectsIter, ValueType>>;

    auto && objects = knap_data.get_objects();

    if (boost::empty(objects)) {
        return ReturnType{};
    }
    auto maxSize = get_value_bound(knap_data, unbounded_tag{}, upper_tag{});
    auto ret = knapsack_unbounded_dynamic(
            detail::make_knapsack_data(
        knap_data.get_objects(), maxSize, knap_data.get_value(), knap_data.get_size(), knap_data.get_output_iter()),
        get_max_element_on_value_indexed_collection<TableElementType, Size>(
            TableElementType(std::make_pair(ObjectsIter{}, knap_data.get_capacity() + 1))),
        utils::greater{});
    return ReturnType(ret.second, ret.first);
}

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam OutputIterator
 * @param oBegin given objects
 * @param oEnd
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename KnapsackData>
typename KnapsackData::return_type
knapsack(KnapsackData knap_data,
         unbounded_tag, integral_size_tag, retrieve_solution_tag) {
    using Value = typename KnapsackData::value;
    return knapsack_unbounded_dynamic(std::move(knap_data),
        detail::get_max_element_on_capacity_indexed_collection<Value>(),
        utils::less{});
}

} // detail

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam Objects
 * @tparam OutputIterator
 * @tparam ObjectSizeFunctor
 * @tparam ObjectValueFunctor
 * @param oBegin given objects
 * @param oEnd
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename Objects, typename OutputIterator,
          typename ObjectSizeFunctor,
          typename ObjectValueFunctor = utils::return_one_functor>
typename detail::knapsack_base<Objects, ObjectSizeFunctor,
                               ObjectValueFunctor>::return_type
knapsack_unbounded(Objects && objects,
         detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
             capacity, // capacity is of size type
         OutputIterator out, ObjectSizeFunctor size,
         ObjectValueFunctor value = ObjectValueFunctor()) {
    return detail::knapsack_check_integrality(detail::make_knapsack_data(std::forward<Objects>(objects), capacity, size,
                                              value, out), detail::unbounded_tag{},
                                              detail::retrieve_solution_tag());
}

} // paal

#endif // PAAL_KNAPSACK_UNBOUNDED_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/*!\file NE....cpp
  \brief \ref EMNE_MULTIBODY - C++ input file, Time-Stepping version - O.B.

  A multibody example.
  Direct description of the model.
  Simulation with a Time-Stepping scheme.
*/

#include "SiconosKernel.hpp"
#include "KneeJointR.hpp"
#include "PrismaticJointR.hpp"
#include <boost/math/quaternion.hpp>
using namespace std;

/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}
void tipTrajectories(SP::SiconosVector  q, double * traj, double length)
{
  double positionInInertialFrame[3];
  double positionInSpatialFrame[3];
  // Output the position of the tip of beam1
  positionInInertialFrame[0]=length/2;
  positionInInertialFrame[1]=0.0;
  positionInInertialFrame[2]=0.0;

  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[0] = positionInSpatialFrame[0];
  traj[1] = positionInSpatialFrame[1];
  traj[2] = positionInSpatialFrame[2];

  // std::cout <<  "positionInSpatialFrame[0]" <<  positionInSpatialFrame[0]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[1]" <<  positionInSpatialFrame[1]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[2]" <<  positionInSpatialFrame[2]<<std::endl;

  positionInInertialFrame[0]=-length/2;
  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[3]= positionInSpatialFrame[0];
  traj[4] = positionInSpatialFrame[1];
  traj[5] = positionInSpatialFrame[2];
}

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

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;
    unsigned int qDim = 7;
    unsigned int nDim = 6;
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.01;                // time step
    int N = 1000;
    double L1 = 1.0;
    double L2 = 1.0;
    double L3 = 1.0;
    double theta = 1.0;              // theta for MoreauJeanOSI integrator
    double g = 9.81; // Gravity
    double m = 1.;

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    FILE * pFile;
    pFile = fopen("data.h", "w");
    if (pFile == NULL)
    {
      printf("fopen exampleopen filed!\n");
      fclose(pFile);
    }

    cout << "====> Model loading ..." << endl << endl;
    // -- Initial positions and velocities --
    SP::SiconosVector q03(new SiconosVector(qDim));
    SP::SiconosVector v03(new SiconosVector(nDim));
    SP::SimpleMatrix I3(new SimpleMatrix(3, 3));
    v03->zero();
    I3->eye();
    I3->setValue(0, 0, 0.1);
    q03->zero();
    (*q03)(2) = -L1 * sqrt(2.0) - L1 / 2;

    double angle = M_PI / 2;
    SiconosVector V1(3);
    V1.zero();
    V1.setValue(0, 0);
    V1.setValue(1, 1);
    V1.setValue(2, 0);
    q03->setValue(3, cos(angle / 2));
    q03->setValue(4, V1.getValue(0)*sin(angle / 2));
    q03->setValue(5, V1.getValue(1)*sin(angle / 2));
    q03->setValue(6, V1.getValue(2)*sin(angle / 2));

    SP::NewtonEulerDS bouncingbeam(new NewtonEulerDS(q03, v03, m, I3));
    // -- Set external forces (weight) --
    SP::SiconosVector weight3(new SiconosVector(nDof));
    (*weight3)(2) = -m * g;
    bouncingbeam->setFExtPtr(weight3);

    // --------------------
    // --- Interactions ---
    // --------------------

    // Interaction with the floor
    double e = 0.9;
    SP::SimpleMatrix H(new SimpleMatrix(1, qDim));
    SP::SiconosVector eR(new SiconosVector(1));
    eR->setValue(0, 2.3);
    H->zero();
    (*H)(0, 2) = 1.0;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
    SP::NewtonEulerR relation0(new NewtonEulerR());
    relation0->setJachq(H);
    relation0->setE(eR);
    cout << "main jacQH" << endl;
    relation0->jachq()->display();

    // Interactions
    // Building the prismatic joint for bouncingbeam
    // input  - the first concerned DS : bouncingbeam
    //        - an axis in the spatial frame (absolute frame)
    // SP::SimpleMatrix H4(new SimpleMatrix(PrismaticJointR::numberOfConstraints(), qDim));
    // H4->zero();

    SP::SiconosVector axe1(new SiconosVector(3));
    axe1->zero();
    axe1->setValue(2, 1);
    SP::PrismaticJointR relation4(new PrismaticJointR(axe1, false, bouncingbeam));
    SP::NonSmoothLaw nslaw4(new EqualityConditionNSL(relation4->numberOfConstraints()));

    SP::Interaction inter4(new Interaction(nslaw4, relation4));
    SP::Interaction interFloor(new Interaction(nslaw0, relation0));

    // -------------
    // --- Model ---
    // -------------
    SP::NonSmoothDynamicalSystem myModel(new NonSmoothDynamicalSystem(t0, T));
    // add the dynamical system in the non smooth dynamical system
    myModel->insertDynamicalSystem(bouncingbeam);
    // link the interaction and the dynamical system

    myModel->link(inter4, bouncingbeam);
    myModel->link(interFloor, bouncingbeam);
    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --

    SP::MoreauJeanOSI OSI3(new MoreauJeanOSI(theta));

    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new MLCP());

    // -- (4) Simulation setup with (1) (2) (3)
    SP::TimeStepping s(new TimeStepping(myModel, t, OSI3, osnspb));
    s->setNewtonTolerance(5e-4);
    s->setNewtonMaxIteration(50);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 15 + 7;
    SimpleMatrix dataPlot(N, outputSize);
    SimpleMatrix bouncingbeamPlot(2,3*N);

    SP::SiconosVector q3 = bouncingbeam->q();
    SP::SiconosVector y= interFloor->y(0);
    SP::SiconosVector ydot= interFloor->y(1);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 0;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    SP::SiconosVector yAux(new SiconosVector(3));
    yAux->setValue(0, 1);
    SP::SimpleMatrix Jaux(new SimpleMatrix(3, 3));
    Index dimIndex(2);
    Index startIndex(4);
    fprintf(pFile, "double T[%d*%d]={", N + 1, outputSize);
    double beamTipTrajectories[6];

    for (k = 0; k < N; k++)
    {
      // solve ...
      s->advanceToEvent();

      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();

      dataPlot(k, 1) = (*q3)(0);
      dataPlot(k, 2) = (*q3)(1);
      dataPlot(k, 3) = (*q3)(2);
      dataPlot(k, 4) = (*q3)(3);
      dataPlot(k, 5) = (*q3)(4);
      dataPlot(k, 6) = (*q3)(5);
      dataPlot(k, 7) = (*q3)(6);

      dataPlot(k, 8) = y->norm2();
      dataPlot(k, 9) = ydot->norm2();

      tipTrajectories(q3,beamTipTrajectories,L3);
      bouncingbeamPlot(0,3*k) = beamTipTrajectories[0];
      bouncingbeamPlot(0,3*k+1) = beamTipTrajectories[1];
      bouncingbeamPlot(0,3*k+2) = beamTipTrajectories[2];
      bouncingbeamPlot(1,3*k) = beamTipTrajectories[3];
      bouncingbeamPlot(1,3*k+1) = beamTipTrajectories[4];
      bouncingbeamPlot(1,3*k+2) = beamTipTrajectories[5];

      //printf("reaction1:%lf \n", interFloor->lambda(1)->getValue(0));

      for (unsigned int jj = 0; jj < outputSize; jj++)
      {
        if ((k || jj))
          fprintf(pFile, ",");
        fprintf(pFile, "%f", dataPlot(k, jj));
      }
      fprintf(pFile, "\n");
      s->nextStep();
      ++show_progress;
    }
    fprintf(pFile, "};");
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    ioMatrix::write("NE_BouncingBeam.dat", "ascii", dataPlot, "noDim");
    ioMatrix::write("NE_BouncingBeam_beam.dat", "ascii", bouncingbeamPlot, "noDim");

    double error=0.0, eps=1e-12;
    if ((error=ioMatrix::compareRefFile(dataPlot, "NE_BouncingBeam.ref", eps)) >= 0.0
        && error > eps)
      return 1;

    fclose(pFile);
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in NE_...cpp" << endl;
  }

}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "pidcontroller.h"
#include<rosgraph_msgs/Clock.h>
#include <std_msgs/Empty.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"
#include <qthread.h>
#include<sensor_msgs/Imu.h>
#include<gazebo_msgs/ApplyBodyWrenchRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include<gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"
#include "surenastateestimation.h"
#include<sensor_msgs/JointState.h>
#include<std_msgs/Float64.h>
#include<termios.h>

using namespace  std;
using namespace  Eigen;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

double q0, q1, q2, q3; // x~q1 y~q2 z~q3 w~q0
double Roll, Pitch, Yaw;
bool QcInitialBool;

void RecieveIMUOrientation(const sensor_msgs::Imu & msg)
{
    ROS_INFO("%f",msg.orientation.w);
    q0 = msg.orientation.w;
    q1 = msg.orientation.x;
    q2 = msg.orientation.y;
    q3 = msg.orientation.z;

    Roll = msg.orientation.x;
    Pitch = msg.orientation.y;
    Yaw = msg.orientation.z;
}

void QcInitial(const sensor_msgs::JointState & msg){
    if (QcInitialBool){
        ROS_INFO("Initialized!");
        QcInitialBool=false;
    }

}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

int main(int argc, char **argv)
{
    QcInitialBool=true;
    SurenaStateEstimation StateEstimation;
    ros::init(argc, argv, "myNode");
    ros::NodeHandle nh;
    ros::Publisher SendJointData  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
    ros::Publisher SendDeltaPosition = nh.advertise<std_msgs::Float32MultiArray>("/surena/PelvisTilt",1000);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, QcInitial);
    ros::Subscriber  IMU_xsens = nh.subscribe("/surena/imu_state", 1000, RecieveIMUOrientation);

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::Float32MultiArray msgTilt;
    ROS_INFO("Press Any Key to Start!");
    getch();
    ROS_INFO("started!");
    while (ros::ok())
    {
//        if (QcInitialBool) {
//            ROS_INFO("qc is initializing!");
//            ros::spinOnce();
//            continue;
//        }
        //        q0 = 0.25;
        //        q1 = 0.25;
        //        q2 = 0.25;
        //        q3 = 0.25;
        MatrixXd RotationMatrix = StateEstimation.MatrixRotationPelvisRPY(Roll,Pitch,0); //yaw is zero intentionally because the real value contains drift.
        MatrixXd LocalPosition(3,1);
//        LocalPosition << 113-30, 115, 0;
        LocalPosition << 0, 0, 10;
        MatrixXd DeltaPosition;
        DeltaPosition = StateEstimation.PelvisLocalChange(RotationMatrix,LocalPosition);
        msg.data.clear();
        msgTilt.data.clear();
        for(int  i = 0;i < 28;i++)
        {
            msg.data.push_back(0);
        }
        for(int j=1;j<4;j++)
        {
            msgTilt.data.push_back(DeltaPosition(j-1,0));
        }
        SendJointData.publish(msg);
        SendDeltaPosition.publish(msgTilt);
        //ROS_INFO("%f%f%f",msgTilt(0,0),msgTilt(1,0),msgTilt(2,0));
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


#include <dbconnector/dbconnector.hpp>
#include <boost/math/distributions.hpp>
#include <modules/prob/student.hpp>
#include <modules/prob/boost.hpp>
#include <limits>
#include <string>

#include "clustered_errors.hpp"
#include "clustered_errors_state.hpp"

using namespace madlib::dbal::eigen_integration;

using std::string;

namespace madlib {
namespace modules {
namespace regress {

// ------------------------------------------------------------------------

typedef ClusteredState<RootContainer> IClusteredState;
typedef ClusteredState<MutableRootContainer> MutableClusteredState;

// ------------------------------------------------------------------------

// function used by linear and logistic transitions
AnyType __clustered_common_transition (AnyType& args, string regressionType,
                                       void (*func)(
                                           MutableClusteredState&,
                                           const MappedColumnVector&,
                                           const double& y))
{
    // Early return because of an exception has been "thrown"
    // (actually "warning") in the previous invocations
    if (args[0].isNull())
        return Null();
    MutableClusteredState state = args[0].getAs<MutableByteString>();

   if (args[1].isNull() || args[2].isNull()) {
        return args[0];
    }

    // Get x as a vector of double
    MappedColumnVector x;
    try{
        // an exception is raised in the backend if args[2] contains nulls
        MappedColumnVector xx = args[2].getAs<MappedColumnVector>();
        // x is a const reference, we can only rebind to change its pointer
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    double y;
    if (regressionType == "log") //Logistic regression
        y = args[1].getAs<bool>() ? 1. : -1;
    else if(regressionType == "lin") // Linear regression
        y = args[1].getAs<double>();
    else if(regressionType == "mlog")//Multi-logistic regression
    	y = args[1].getAs<int>();

    // const MappedColumnVector& x = args[2].getAs<MappedColumnVector>();

    if (!std::isfinite(y)) {
        //throw std::domain_error("Dependent variables are not finite.");
        warning("Dependent variables are not finite.");
        return Null();
    } else if (x.size() > std::numeric_limits<uint16_t>::max()) {
        //throw std::domain_error("Number of independent variables cannot be "
        //                        "larger than 65535.");
        warning("Number of independent variables cannot be larger than 65535.");
        return Null();
    }

    if (state.numRows == 0) {
        if(regressionType == "mlog")
        {
            if (args[4].isNull() || args[5].isNull()) {
                return args[0];
            }
        	state.numCategories = static_cast<uint16_t>(args[4].getAs<int>());
        	state.refCategory = static_cast<uint16_t>(args[5].getAs<int>());
        } else {
        	state.numCategories = 2;
        	state.refCategory = 0;
        }

        state.widthOfX = static_cast<uint16_t>(x.size() * (state.numCategories-1));
        state.resize();

        if(regressionType == "mlog") {
	            MappedMatrix coefMat = args[3].getAs<MappedMatrix>();
                Matrix mat = coefMat;
                mat.transposeInPlace();
                mat.resize(coefMat.size(), 1);
                state.coef = mat;
        } else {
            const MappedColumnVector& coef = args[3].getAs<MappedColumnVector>();
            state.coef = coef;
        }
        state.meat_half.setZero();
    }

    // dimension check
    if (state.widthOfX != static_cast<uint16_t>(x.size() * (state.numCategories-1))) {
        //throw std::runtime_error("Inconsistent numbers of independent "
        //                         "variables.");
        warning("Inconsistent numbers of independent variables.");
        return Null();
    }
    state.numRows++;
    (*func)(state, x, y);
    return state.storage();
}

// ------------------------------------------------------------------------

// function used by linear and logistic merges
AnyType __clustered_common_merge (AnyType& args)
{
    // In case the aggregator should be terminated because
    // an exception has been "thrown" in the transition function
    if(args[0].isNull() || args[1].isNull())
        return Null();
    MutableClusteredState state1 = args[0].getAs<MutableByteString>();
    IClusteredState state2 = args[1].getAs<ByteString>();

    if (state1.numRows == 0)
        return state2.storage();
    else if (state2.numRows == 0)
        return state1.storage();

    state1.numRows += state2.numRows;
    state1.bread += state2.bread;
    state1.meat_half += state2.meat_half;

    return state1.storage();
}

// ------------------------------------------------------------------------

// function used by linear and logistic finals
AnyType __clustered_common_final (AnyType& args)
{
    // In case the aggregator should be terminated because
    // an exception has been "thrown" in the transition function
    if (args[0].isNull())
        return Null();
    IClusteredState state = args[0].getAs<ByteString>();
    if (state.numRows == 0) return Null();

    Allocator& allocator = defaultAllocator();
    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        state.bread, EigenvaluesOnly, ComputePseudoInverse);

    int k = static_cast<int>(state.widthOfX);

    Matrix meat(k, k);
    meat = trans(state.meat_half) * state.meat_half;

    MutableNativeColumnVector meatvec;
    MutableNativeColumnVector breadvec;
    meatvec.rebind(allocator.allocateArray<double>(k*k));
    breadvec.rebind(allocator.allocateArray<double>(k*k));
    int count = 0;
    for (int i = 0; i < k; i++)
        for (int j = 0; j < k; j++) {
            meatvec(count) = meat(i,j);
            breadvec(count) = state.bread(i,j);
            count++;
        }

    AnyType tuple;

    tuple << meatvec << breadvec;

    return tuple;
}

// ------------------------------------------------------------------------

// Compute the stats from coef and errs
AnyType clustered_compute_stats (AnyType& args,
                                 void (*func)(
                                     MutableNativeColumnVector&,
                                     MutableNativeColumnVector&,
                                     int, int), bool ismlogr)
{
    //const MappedColumnVector& coef = args[0].getAs<MappedColumnVector>();
    ColumnVector coef;
    if (ismlogr){
	    MappedMatrix coefMat = args[0].getAs<MappedMatrix>();
        Matrix mat = coefMat;
        mat.transposeInPlace();
        mat.resize(coefMat.size(), 1);
        coef = mat;
    }else{
        coef = args[0].getAs<MappedColumnVector>();
    }
    const MappedColumnVector& meatvec = args[1].getAs<MappedColumnVector>();
    const MappedColumnVector& breadvec = args[2].getAs<MappedColumnVector>();
    int mcluster = args[3].getAs<int>();
    int numRows = args[4].getAs<int>();
    int k = static_cast<int>(coef.size());
    Matrix bread(k,k);
    Matrix meat(k,k);
    int count = 0;

    for (int i = 0; i < k; i++)
        for (int j = 0; j < k; j++)
        {
            meat(i,j) = meatvec(count);
            bread(i,j) = breadvec(count);
            count++;
        }

    if (mcluster == 1) {
        //throw std::domain_error ("Clustered variance error: Number of clusters cannot be smaller than 2!");
        warning("Clustered variance error: Number of clusters cannot be smaller than 2!");
        return Null();
    }
    double dfc = (mcluster / (mcluster - 1.)) * ((numRows - 1.) / (numRows - k));

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        bread, EigenvaluesOnly, ComputePseudoInverse);
    Matrix inverse_of_bread = decomposition.pseudoInverse();
    Matrix cov(k, k);
    cov = inverse_of_bread * meat * inverse_of_bread;

    MutableNativeColumnVector errs;
    MutableNativeColumnVector stats;
    MutableNativeColumnVector pValues;
    Allocator& allocator = defaultAllocator();

    errs.rebind(allocator.allocateArray<double>(k));
    stats.rebind(allocator.allocateArray<double>(k));
    pValues.rebind(allocator.allocateArray<double>(k));
    for (int i = 0; i < k; i++)
    {
        if (inverse_of_bread(i,i) < 0)
            errs(i) = 0;
        else
            errs(i) = std::sqrt(cov(i,i) * dfc);

        if (coef(i) == 0 && errs(i) == 0)
            stats(i) = 0;
        else
            stats(i) = coef(i) / errs(i);
    }

    if (numRows > k)
        (*func)(pValues, stats, numRows, k);

    AnyType tuple;

	tuple << coef << errs << stats
		  << (numRows > k
			  ? pValues
			  : Null());
    return tuple;
}

// ------------------------------------------------------------------------

// compute t-stats
void __compute_t_stats (MutableNativeColumnVector& pValues,
                        MutableNativeColumnVector& stats,
                        int numRows, int k)
{
    for (int i = 0; i < k; i++)
        pValues(i) = 2. * prob::cdf(
            boost::math::complement(
                prob::students_t(static_cast<double>(numRows - k)),
                std::fabs(stats(i))));
}
// ------------------------------------------------------------------------

// compute t-stats
void __compute_z_stats (MutableNativeColumnVector& pValues,
                        MutableNativeColumnVector& stats,
                        int numRows, int k)
{
    (void)numRows;
    for (int i = 0; i < k; i++)
        pValues(i) = 2. * prob::cdf(
            boost::math::complement(prob::normal(), std::fabs(stats(i))));
}

// ------------------------------------------------------------------------
// linear clustered
// ------------------------------------------------------------------------

void __linear_trans_compute (MutableClusteredState& state,
                             const MappedColumnVector& x, const double& y)
{
    // On Redhat, I have to use the next a few lines instead of
    // state.meat_half += (y - trans(state.coef) * x) * x;
    double sm = 0;
    for (int i = 0; i < state.widthOfX; i++) sm += state.coef(i) * x(i);
    sm = y - sm;
    for (int i = 0; i < state.widthOfX; i++)
        state.meat_half(0,i) += sm * x(i);

    state.bread += x * trans(x);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_lin_transition::run (AnyType& args)
{
    return __clustered_common_transition(args, "lin", __linear_trans_compute);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_lin_merge::run (AnyType& args)
{
    return __clustered_common_merge(args);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_lin_final::run (AnyType& args)
{
    return __clustered_common_final(args);
}

// ------------------------------------------------------------------------

AnyType clustered_lin_compute_stats::run (AnyType& args)
{
    return clustered_compute_stats(args, __compute_t_stats, false);
}

// ------------------------------------------------------------------------
// Logistic clustered standard errors
// ------------------------------------------------------------------------

inline double sigma(double x) {
	return 1. / (1. + std::exp(-x));
}

void __logistic_trans_compute (MutableClusteredState& state,
                               const MappedColumnVector& x, const double& y)
{
    double sm = 0;
    for (int i = 0; i < state.widthOfX; i++) sm += state.coef(i) * x(i);

    double sgn = y > 0 ? -1 : 1;
    double t1 = sigma(sgn * sm);
    double t2 = sigma(-sgn * sm);

    for (int i = 0; i < state.widthOfX; i++)
        state.meat_half(0,i) += t1 * sgn * x(i);

    state.bread += (t1 * t2) * (x * trans(x));
}

// ------------------------------------------------------------------------

AnyType __clustered_err_log_transition::run (AnyType& args)
{
    return __clustered_common_transition(args, "log", __logistic_trans_compute);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_log_merge::run (AnyType& args)
{
    return __clustered_common_merge(args);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_log_final::run (AnyType& args)
{
    return __clustered_common_final(args);
}

// ------------------------------------------------------------------------

AnyType clustered_log_compute_stats::run (AnyType& args)
{
    return clustered_compute_stats(args, __compute_z_stats, false);
}

// ------------------------------------------------------------------------
// Multi-Logistic clustered standard errors
// ------------------------------------------------------------------------

void __mlogistic_trans_compute (MutableClusteredState& state,
                               const MappedColumnVector& x, const double& y)
{
	int numCategories = state.numCategories -1;
	ColumnVector yVec(numCategories);
    yVec.fill(0);

    //Pivot around the reference category
    if (y > state.refCategory) {
        yVec((int)y - 1) = 1;
    } else if (y < state.refCategory) {
        yVec((int)y) = 1;
    }

//    if ((int)y != 0) {
 //       yVec(((int)y) - 1) = 1;
 //   }

	/*
    Compute the parameter vector (the 'pi' vector in the documentation)
    for the data point being processed.
    Casting the coefficients into a matrix makes the calculation simple.
    */
    Matrix coef = state.coef;
    coef.resize(numCategories, state.widthOfX/numCategories);

    //Store the intermediate calculations because we'll reuse them in the LLH
    ColumnVector t1 = x; //t1 is vector of size state.widthOfX
    t1 = coef*x;
    /*
        Note: The above 2 lines could have been written as:
        ColumnVector t1 = -coef*x;

        but this creates warnings. These warnings are somehow related to the factor
        that x is an immutable type.
    */

    ColumnVector t2 = t1.array().exp();
    double t3 = 1 + t2.sum();
    ColumnVector pi = t2/t3;
    //The gradient matrix has numCatergories rows and widthOfX columns
    Matrix grad = -yVec * x.transpose() + pi * x.transpose();
    //We cast the gradient into a vector to make the math easier.
    grad.resize(state.widthOfX, 1);
    for (int i = 0; i < state.widthOfX; i++)
    {
		state.meat_half(0,i) += grad(i);
	}

	// Compute the 'a' matrix.
    Matrix a(numCategories,numCategories);
    Matrix piDiag = pi.asDiagonal();
    a = pi * pi.transpose() - piDiag;

    //Start the Hessian calculations
    //Matrix X_transp_AX(numCategories * state.widthOfX, numCategories * state.widthOfX);
 	Matrix X_transp_AX( (int)state.widthOfX, (int)state.widthOfX);
    /*
        Again: The following 3 lines could have been written as
        Matrix XXTrans = x * x.transpose();
        but it creates warnings related to the type of x. Here is an easy fix
    */
    Matrix cv_x = x;
    Matrix XXTrans = trans(cv_x);
    XXTrans = cv_x * XXTrans;

    //Eigen doesn't supported outer-products for matrices, so we have to do our own.
    //This operation is also known as a tensor-product.
    for (int i1 = 0; i1 < (state.widthOfX/numCategories); i1++){
         for (int i2 = 0; i2 < (state.widthOfX/numCategories); i2++){
            int rowOffset = numCategories * i1;
            int colOffset = numCategories * i2;

            X_transp_AX.block(rowOffset, colOffset, numCategories,  numCategories) = XXTrans(i1, i2) * a;
        }
    }

    state.bread += -1*X_transp_AX;
}

// ------------------------------------------------------------------------

AnyType __clustered_err_mlog_transition::run (AnyType& args)
{
    return __clustered_common_transition(args, "mlog", __mlogistic_trans_compute);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_mlog_merge::run (AnyType& args)
{
    return __clustered_common_merge(args);
}

// ------------------------------------------------------------------------

AnyType __clustered_err_mlog_final::run (AnyType& args)
{
    return __clustered_common_final(args);
}

// ------------------------------------------------------------------------

AnyType clustered_mlog_compute_stats::run (AnyType& args)
{
    return clustered_compute_stats(args,  __compute_z_stats, true);
}

}
}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Fabian Köhler <fabian2804@googlemail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#define GL_GLEXT_PROTOTYPES
#include <GL/gl.h>
#include <GL/glext.h>

#include <QtGlobal>
#if QT_VERSION >= 0x050000
#include <QtWidgets>
#else
#include <QtGui>
#endif
#include <QtOpenGL>
#include <QTimer>

#include <boost/program_options.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#define BOOST_COMPUTE_DEBUG_KERNEL_COMPILATION
#include <boost/compute/system.hpp>
#include <boost/compute/interop/opengl.hpp>
#include <boost/compute/source.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/algorithm.hpp>

namespace compute = boost::compute;
namespace po = boost::program_options;

using compute::uint_;
using compute::float4_;

const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
    __kernel void updateVelocity(__global const float4* position, __global float4* velocity, float dt, uint N)
    {
        uint gid = get_global_id(0);

        float4 r = { 0.0f, 0.0f, 0.0f, 0.0f };
        float f = 0.0f;
        for(uint i = 0; i != gid; i++) {
            if(i != gid) {
                r = position[i]-position[gid];
                f = length(r)+0.001f;
                f *= f*f;
                f = dt/f;
                velocity[gid] += f*r;
            }
        }
    }
    __kernel void updatePosition(__global float4* position, __global const float4* velocity, float dt)
    {
        uint gid = get_global_id(0);

        position[gid].xyz += dt*velocity[gid].xyz;
    }
);

class NBodyWidget : public QGLWidget
{
    Q_OBJECT

public:
    NBodyWidget(std::size_t particles, float dt, QWidget* parent = 0);
    ~NBodyWidget();

    void initializeGL();
    void resizeGL(int width, int height);
    void paintGL();
    void updateParticles();
    void keyPressEvent(QKeyEvent* event);

private:
    QTimer* timer;

    compute::context m_context;
    compute::command_queue m_queue;
    compute::program m_program;
    compute::opengl_buffer m_position;
    compute::vector<float4_>* m_velocity;
    compute::kernel m_velocity_kernel;
    compute::kernel m_position_kernel;

    bool m_initial_draw;

    const uint_ m_particles;
    const float m_dt;
};

NBodyWidget::NBodyWidget(std::size_t particles, float dt, QWidget* parent)
    : QGLWidget(parent), m_initial_draw(true), m_particles(particles), m_dt(dt)
{
    // create a timer to redraw as fast as possible
    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(updateGL()));
    timer->start(1);
}

NBodyWidget::~NBodyWidget()
{
    delete m_velocity;

    // delete the opengl buffer
    GLuint vbo = m_position.get_opengl_object();
    glDeleteBuffers(1, &vbo);
}

void NBodyWidget::initializeGL()
{
    // create context, command queue and program
    m_context = compute::opengl_create_shared_context();
    m_queue = compute::command_queue(m_context, m_context.get_device());
    m_program = compute::program::create_with_source(source, m_context);
    m_program.build();

    // prepare random particle positions that will be transferred to the vbo
    float4_* temp = new float4_[m_particles];
    boost::random::uniform_real_distribution<float> dist(-0.5f, 0.5f);
    boost::random::mt19937_64 gen;
    for(size_t i = 0; i < m_particles; i++) {
        temp[i][0] = dist(gen);
        temp[i][1] = dist(gen);
        temp[i][2] = dist(gen);
        temp[i][3] = 1.0f;
    }

    // create an OpenGL vbo
    GLuint vbo = 0;
    glGenBuffers(1, &vbo);
    glBindBuffer(GL_ARRAY_BUFFER, vbo);
    glBufferData(GL_ARRAY_BUFFER, m_particles*sizeof(float4_), temp, GL_DYNAMIC_DRAW);

    // create a OpenCL buffer from the vbo
    m_position = compute::opengl_buffer(m_context, vbo);
    delete[] temp;

    // create buffer for velocities
    m_velocity = new compute::vector<float4_>(m_particles, m_context);
    compute::fill(m_velocity->begin(), m_velocity->end(), float4_(0.0f, 0.0f, 0.0f, 0.0f), m_queue);

    // create compute kernels
    m_velocity_kernel = m_program.create_kernel("updateVelocity");
    m_velocity_kernel.set_arg(0, m_position);
    m_velocity_kernel.set_arg(1, m_velocity->get_buffer());
    m_velocity_kernel.set_arg(2, m_dt);
    m_velocity_kernel.set_arg(3, m_particles);
    m_position_kernel = m_program.create_kernel("updatePosition");
    m_position_kernel.set_arg(0, m_position);
    m_position_kernel.set_arg(1, m_velocity->get_buffer());
    m_position_kernel.set_arg(2, m_dt);
}
void NBodyWidget::resizeGL(int width, int height)
{
    // update viewport
    glViewport(0, 0, width, height);
}
void NBodyWidget::paintGL()
{
    // clear buffer
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // check if this is the first draw
    if(m_initial_draw) {
        // do not update particles
        m_initial_draw = false;
    } else {
        // update particles
        updateParticles();
    }

    // draw
    glVertexPointer(4, GL_FLOAT, 0, 0);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_POINTS, 0, m_particles);
    glFinish();
}
void NBodyWidget::updateParticles()
{
    // enqueue kernels to update particles and make sure that the command queue is finished
    compute::opengl_enqueue_acquire_buffer(m_position, m_queue);
    m_queue.enqueue_1d_range_kernel(m_velocity_kernel, 0, m_particles, 0).wait();
    m_queue.enqueue_1d_range_kernel(m_position_kernel, 0, m_particles, 0).wait();
    m_queue.finish();
    compute::opengl_enqueue_release_buffer(m_position, m_queue);
}
void NBodyWidget::keyPressEvent(QKeyEvent* event)
{
    if(event->key() == Qt::Key_Escape) {
        this->close();
    }
}

int main(int argc, char** argv)
{
    // parse command line arguments
    po::options_description options("options");
    options.add_options()
        ("help", "show usage")
        ("particles", po::value<uint_>()->default_value(1000), "number of particles")
        ("dt", po::value<float>()->default_value(0.00001f), "width of each integration step");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);

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

    const uint_ particles = vm["particles"].as<uint_>();
    const float dt = vm["dt"].as<float>();

    QApplication app(argc, argv);
    NBodyWidget nbody(particles, dt);

    nbody.show();

    return app.exec();
}

#include "nbody.moc"