/**
 * @file kde_test.cpp
 * @author Roberto Hueso
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/kde/kde.hpp>
#include <mlpack/core/tree/binary_space_tree.hpp>
#include <mlpack/core/tree/octree.hpp>
#include <mlpack/core/tree/cover_tree.hpp>
#include <mlpack/core/tree/rectangle_tree.hpp>

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

using namespace mlpack;
using namespace mlpack::kde;
using namespace mlpack::metric;
using namespace mlpack::tree;
using namespace mlpack::kernel;

using namespace boost::serialization;

BOOST_AUTO_TEST_SUITE(KDETest);

// Brute force gaussian KDE.
template <typename KernelType>
void BruteForceKDE(const arma::mat& reference,
                   const arma::mat& query,
                   arma::vec& densities,
                   KernelType& kernel)
{
  metric::EuclideanDistance metric;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    for (size_t j = 0; j < reference.n_cols; ++j)
    {
      double distance = metric.Evaluate(query.col(i), reference.col(j));
      densities(i) += kernel.Evaluate(distance);
    }
  }
  densities /= reference.n_cols;
}

/**
 * Test if simple case is correct according to manually calculated results.
 */
BOOST_AUTO_TEST_CASE(KDESimpleTest)
{
  // Transposed reference and query sets because it's easier to read.
  arma::mat reference = { {-1.0, -1.0},
                          {-2.0, -1.0},
                          {-3.0, -2.0},
                          { 1.0,  1.0},
                          { 2.0,  1.0},
                          { 3.0,  2.0} };
  arma::mat query = { { 0.0,  0.5},
                      { 0.4, -3.0},
                      { 0.0,  0.0},
                      {-2.1,  1.0} };
  arma::inplace_trans(reference);
  arma::inplace_trans(query);
  arma::vec estimations;
  // Manually calculated results.
  arma::vec estimationsResult = {0.08323668699564207296148765,
                                 0.00167470061366603324010116,
                                 0.07658867126520703394465527,
                                 0.01028120384800740999553525};
  KDE<GaussianKernel,
      EuclideanDistance,
      arma::mat,
      KDTree>
      kde(0.0, 0.01, GaussianKernel(0.8));
  kde.Train(reference);
  kde.Evaluate(query, estimations);
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(estimations[i], estimationsResult[i], 0.01);
}

/**
 * Test Train(Tree...) and Evaluate(Tree...).
 */
BOOST_AUTO_TEST_CASE(KDETreeAsArguments)
{
  // Transposed reference and query sets because it's easier to read.
  arma::mat reference = { {-1.0, -1.0},
                          {-2.0, -1.0},
                          {-3.0, -2.0},
                          { 1.0,  1.0},
                          { 2.0,  1.0},
                          { 3.0,  2.0} };
  arma::mat query = { { 0.0,  0.5},
                      { 0.4, -3.0},
                      { 0.0,  0.0},
                      {-2.1,  1.0} };
  arma::inplace_trans(reference);
  arma::inplace_trans(query);
  arma::vec estimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec estimationsResult = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.8;

  // Get brute force results.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                estimationsResult,
                                kernel);

  // Get dual-tree results.
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries, oldFromNewReferences;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 2);
  Tree* referenceTree = new Tree(reference, oldFromNewReferences, 2);
  KDE<GaussianKernel,
      EuclideanDistance,
      arma::mat,
      KDTree>
      kde(0.0, 1e-6, GaussianKernel(kernelBandwidth));
  kde.Train(referenceTree, &oldFromNewReferences);
  kde.Evaluate(queryTree, std::move(oldFromNewQueries), estimations);
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(estimations[i], estimationsResult[i], 0.01);
  delete queryTree;
  delete referenceTree;
}

/**
 * Test dual-tree implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(GaussianKDEBruteForceTest)
{
  arma::mat reference = arma::randu(2, 200);
  arma::mat query = arma::randu(2, 60);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.12;
  const double relError = 0.05;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test single-tree implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(GaussianSingleKDEBruteForceTest)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.3;
  const double relError = 0.04;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::SINGLE_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test single-tree implementation results against brute force results using
 * a cover-tree and Epanechnikov kernel.
 */
BOOST_AUTO_TEST_CASE(EpanechnikovCoverSingleKDETest)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 1.1;
  const double relError = 0.08;

  // Brute force KDE.
  EpanechnikovKernel kernel(kernelBandwidth);
  BruteForceKDE<EpanechnikovKernel>(reference,
                                    query,
                                    bfEstimations,
                                    kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<EpanechnikovKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
      kde(relError, 0.0, kernel, KDEMode::SINGLE_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test single-tree implementation results against brute force results using
 * a cover-tree and Gaussian kernel.
 */
BOOST_AUTO_TEST_CASE(GaussianCoverSingleKDETest)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 1.1;
  const double relError = 0.08;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
      kde(relError, 0.0, kernel, KDEMode::SINGLE_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test single-tree implementation results against brute force results using
 * an octree and Epanechnikov kernel.
 */
BOOST_AUTO_TEST_CASE(EpanechnikovOctreeSingleKDETest)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 1.0;
  const double relError = 0.05;

  // Brute force KDE.
  EpanechnikovKernel kernel(kernelBandwidth);
  BruteForceKDE<EpanechnikovKernel>(reference,
                                    query,
                                    bfEstimations,
                                    kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<EpanechnikovKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::Octree>
      kde(relError, 0.0, kernel, KDEMode::SINGLE_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test BallTree dual-tree implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(BallTreeGaussianKDETest)
{
  arma::mat reference = arma::randu(2, 200);
  arma::mat query = arma::randu(2, 60);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.4;
  const double relError = 0.05;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // BallTree KDE.
  typedef BallTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries, oldFromNewReferences;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 2);
  Tree* referenceTree = new Tree(reference, oldFromNewReferences, 2);
  KDE<GaussianKernel,
      EuclideanDistance,
      arma::mat,
      BallTree>
      kde(relError, 0.0, GaussianKernel(kernelBandwidth));
  kde.Train(referenceTree, &oldFromNewReferences);
  kde.Evaluate(queryTree, std::move(oldFromNewQueries), treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);

  delete queryTree;
  delete referenceTree;
}

/**
 * Test Octree dual-tree implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(OctreeGaussianKDETest)
{
  arma::mat reference = arma::randu(2, 500);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.3;
  const double relError = 0.01;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::Octree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test RTree dual-tree implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(RTreeGaussianKDETest)
{
  arma::mat reference = arma::randu(2, 500);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.3;
  const double relError = 0.01;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::RTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test Standard Cover Tree dual-tree implementation results against brute
 * force results using Gaussian kernel.
 */
BOOST_AUTO_TEST_CASE(StandardCoverTreeGaussianKDETest)
{
  arma::mat reference = arma::randu(2, 500);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.3;
  const double relError = 0.01;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test Standard Cover Tree dual-tree implementation results against brute
 * force results using Epanechnikov kernel.
 */
BOOST_AUTO_TEST_CASE(StandardCoverTreeEpanechnikovKDETest)
{
  arma::mat reference = arma::randu(2, 500);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.3;
  const double relError = 0.01;

  // Brute force KDE.
  EpanechnikovKernel kernel(kernelBandwidth);
  BruteForceKDE<EpanechnikovKernel>(reference,
                                    query,
                                    bfEstimations,
                                    kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<EpanechnikovKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test duplicated value in reference matrix.
 */
BOOST_AUTO_TEST_CASE(DuplicatedReferenceSampleKDETest)
{
  arma::mat reference = arma::randu(2, 30);
  arma::mat query = arma::randu(2, 10);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.4;
  const double relError = 0.05;

  // Duplicate value.
  reference.col(2) = reference.col(3);

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Dual-tree KDE.
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries, oldFromNewReferences;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 2);
  Tree* referenceTree = new Tree(reference, oldFromNewReferences, 2);
  KDE<GaussianKernel,
      EuclideanDistance,
      arma::mat,
      KDTree>
      kde(relError, 0.0, GaussianKernel(kernelBandwidth));
  kde.Train(referenceTree, &oldFromNewReferences);
  kde.Evaluate(queryTree, oldFromNewQueries, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);

  delete queryTree;
  delete referenceTree;
}

/**
 * Test duplicated value in query matrix.
 */
BOOST_AUTO_TEST_CASE(DuplicatedQuerySampleKDETest)
{
  arma::mat reference = arma::randu(2, 30);
  arma::mat query = arma::randu(2, 10);
  arma::vec estimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.4;
  const double relError = 0.05;

  // Duplicate value.
  query.col(2) = query.col(3);

  // Dual-tree KDE.
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries, oldFromNewReferences;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 2);
  Tree* referenceTree = new Tree(reference, oldFromNewReferences, 2);
  KDE<GaussianKernel,
      EuclideanDistance,
      arma::mat,
      KDTree>
      kde(relError, 0.0, GaussianKernel(kernelBandwidth));
  kde.Train(referenceTree, &oldFromNewReferences);
  kde.Evaluate(queryTree, oldFromNewQueries, estimations);

  // Check whether results are equal.
  BOOST_REQUIRE_CLOSE(estimations[2], estimations[3], relError * 100);

  delete queryTree;
  delete referenceTree;
}

/**
 * Test dual-tree breadth-first implementation results against brute force
 * results.
 */
BOOST_AUTO_TEST_CASE(BreadthFirstKDETest)
{
  arma::mat reference = arma::randu(2, 200);
  arma::mat query = arma::randu(2, 60);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.8;
  const double relError = 0.01;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Breadth-First KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree,
      tree::KDTree<metric::EuclideanDistance,
                   kde::KDEStat,
                   arma::mat>::template BreadthFirstDualTreeTraverser>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test 1-dimensional implementation results against brute force results.
 */
BOOST_AUTO_TEST_CASE(OneDimensionalTest)
{
  arma::mat reference = arma::randu(1, 200);
  arma::mat query = arma::randu(1, 60);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.01;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(bfEstimations[i], treeEstimations[i], relError * 100);
}

/**
 * Test a case where an empty reference set is given to train the model.
 */
BOOST_AUTO_TEST_CASE(EmptyReferenceTest)
{
  arma::mat reference;
  arma::mat query = arma::randu(1, 10);
  arma::vec estimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.01;

  // KDE.
  metric::EuclideanDistance metric;
  GaussianKernel kernel(kernelBandwidth);
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);

  // When training using the dataset matrix.
  BOOST_REQUIRE_THROW(kde.Train(reference), std::invalid_argument);

  // When training using a tree.
  std::vector<size_t> oldFromNewReferences;
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  Tree* referenceTree = new Tree(reference, oldFromNewReferences, 2);
  BOOST_REQUIRE_THROW(
      kde.Train(referenceTree, &oldFromNewReferences), std::invalid_argument);

  delete referenceTree;
}

/**
 * Tests when reference set values and query set values dimensions don't match.
 */
BOOST_AUTO_TEST_CASE(EvaluationMatchDimensionsTest)
{
  arma::mat reference = arma::randu(3, 10);
  arma::mat query = arma::randu(1, 10);
  arma::vec estimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.01;

  // KDE.
  metric::EuclideanDistance metric;
  GaussianKernel kernel(kernelBandwidth);
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);

  // When evaluating using the query dataset matrix.
  BOOST_REQUIRE_THROW(kde.Evaluate(query, estimations),
                    std::invalid_argument);

  // When evaluating using a query tree.
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 3);
  BOOST_REQUIRE_THROW(kde.Evaluate(queryTree, oldFromNewQueries, estimations),
                    std::invalid_argument);
  delete queryTree;
}

/**
 * Tests when an empty query set is given to be evaluated.
 */
BOOST_AUTO_TEST_CASE(EmptyQuerySetTest)
{
  arma::mat reference = arma::randu(1, 10);
  arma::mat query;
  // Set estimations to the wrong size.
  arma::vec estimations(33, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.01;

  // KDE.
  metric::EuclideanDistance metric;
  GaussianKernel kernel(kernelBandwidth);
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);

  // The query set must be empty.
  BOOST_REQUIRE_EQUAL(query.n_cols, 0);
  // When evaluating using the query dataset matrix.
  BOOST_REQUIRE_NO_THROW(kde.Evaluate(query, estimations));

  // When evaluating using a query tree.
  typedef KDTree<EuclideanDistance, kde::KDEStat, arma::mat> Tree;
  std::vector<size_t> oldFromNewQueries;
  Tree* queryTree = new Tree(query, oldFromNewQueries, 3);
  BOOST_REQUIRE_NO_THROW(
      kde.Evaluate(queryTree, oldFromNewQueries, estimations));
  delete queryTree;

  // Estimations must be empty.
  BOOST_REQUIRE_EQUAL(estimations.size(), 0);
}

/**
 * Tests serialiation of KDE models.
 */
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  // Initial KDE model to be serialized.
  const double relError = 0.25;
  const double absError = 0.0;
  const bool monteCarlo = false;
  const double MCProb = 0.8;
  const size_t initialSampleSize = 35;
  const double entryCoef = 5;
  const double breakCoef = 0.6;
  arma::mat reference = arma::randu(4, 800);
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
    kde(relError,
        absError,
        GaussianKernel(0.25),
        KDEMode::DUAL_TREE_MODE,
        metric::EuclideanDistance(),
        monteCarlo,
        MCProb,
        initialSampleSize,
        entryCoef,
        breakCoef);
  kde.Train(reference);

  // Get estimations to compare.
  arma::mat query = arma::randu(4, 100);;
  arma::vec estimations = arma::vec(query.n_cols, arma::fill::zeros);
  kde.Evaluate(query, estimations);

  // Initialize serialized objects.
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree> kdeXml, kdeText, kdeBinary;
  SerializeObjectAll(kde, kdeXml, kdeText, kdeBinary);

  // Check everything is correct.
  BOOST_REQUIRE_CLOSE(kde.RelativeError(), relError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeXml.RelativeError(), relError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeText.RelativeError(), relError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeBinary.RelativeError(), relError, 1e-8);

  BOOST_REQUIRE_CLOSE(kde.AbsoluteError(), absError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeXml.AbsoluteError(), absError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeText.AbsoluteError(), absError, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeBinary.AbsoluteError(), absError, 1e-8);

  BOOST_REQUIRE_EQUAL(kde.IsTrained(), true);
  BOOST_REQUIRE_EQUAL(kdeXml.IsTrained(), true);
  BOOST_REQUIRE_EQUAL(kdeText.IsTrained(), true);
  BOOST_REQUIRE_EQUAL(kdeBinary.IsTrained(), true);

  const KDEMode mode = KDEMode::DUAL_TREE_MODE;
  BOOST_REQUIRE_EQUAL(kde.Mode(), mode);
  BOOST_REQUIRE_EQUAL(kdeXml.Mode(), mode);
  BOOST_REQUIRE_EQUAL(kdeText.Mode(), mode);
  BOOST_REQUIRE_EQUAL(kdeBinary.Mode(), mode);

  BOOST_REQUIRE_EQUAL(kde.MonteCarlo(), monteCarlo);
  BOOST_REQUIRE_EQUAL(kdeXml.MonteCarlo(), monteCarlo);
  BOOST_REQUIRE_EQUAL(kdeText.MonteCarlo(), monteCarlo);
  BOOST_REQUIRE_EQUAL(kdeBinary.MonteCarlo(), monteCarlo);

  BOOST_REQUIRE_CLOSE(kde.MCProb(), MCProb, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeXml.MCProb(), MCProb, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeText.MCProb(), MCProb, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeBinary.MCProb(), MCProb, 1e-8);

  BOOST_REQUIRE_EQUAL(kde.MCInitialSampleSize(), initialSampleSize);
  BOOST_REQUIRE_EQUAL(kdeXml.MCInitialSampleSize(), initialSampleSize);
  BOOST_REQUIRE_EQUAL(kdeText.MCInitialSampleSize(), initialSampleSize);
  BOOST_REQUIRE_EQUAL(kdeBinary.MCInitialSampleSize(), initialSampleSize);

  BOOST_REQUIRE_CLOSE(kde.MCEntryCoef(), entryCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeXml.MCEntryCoef(), entryCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeText.MCEntryCoef(), entryCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeBinary.MCEntryCoef(), entryCoef, 1e-8);

  BOOST_REQUIRE_CLOSE(kde.MCBreakCoef(), breakCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeXml.MCBreakCoef(), breakCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeText.MCBreakCoef(), breakCoef, 1e-8);
  BOOST_REQUIRE_CLOSE(kdeBinary.MCBreakCoef(), breakCoef, 1e-8);

  // Test if execution gives the same result.
  arma::vec xmlEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec textEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec binEstimations = arma::vec(query.n_cols, arma::fill::zeros);

  kdeXml.Evaluate(query, xmlEstimations);
  kdeText.Evaluate(query, textEstimations);
  kdeBinary.Evaluate(query, binEstimations);

  for (size_t i = 0; i < query.n_cols; ++i)
  {
    BOOST_REQUIRE_CLOSE(estimations[i], xmlEstimations[i], relError * 100);
    BOOST_REQUIRE_CLOSE(estimations[i], textEstimations[i], relError * 100);
    BOOST_REQUIRE_CLOSE(estimations[i], binEstimations[i], relError * 100);
  }
}

/**
 * Test if the copy constructor and copy operator works properly.
 */
BOOST_AUTO_TEST_CASE(CopyConstructor)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec estimations1, estimations2, estimations3;
  const double kernelBandwidth = 1.5;
  const double relError = 0.05;

  typedef KDE<GaussianKernel, metric::EuclideanDistance, arma::mat>
      KDEType;

  // KDE.
  KDEType kde(relError, 0, kernel::GaussianKernel(kernelBandwidth));
  kde.Train(std::move(reference));

  // Copy constructor KDE.
  KDEType constructor(kde);

  // Copy operator KDE.
  KDEType oper = kde;

  // Evaluations.
  kde.Evaluate(query, estimations1);
  constructor.Evaluate(query, estimations2);
  oper.Evaluate(query, estimations3);

  // Check results.
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    BOOST_REQUIRE_CLOSE(estimations1[i], estimations2[i], 1e-10);
    BOOST_REQUIRE_CLOSE(estimations2[i], estimations3[i], 1e-10);
  }
}

/**
 * Test if the move constructor works properly.
 */
BOOST_AUTO_TEST_CASE(MoveConstructor)
{
  arma::mat reference = arma::randu(2, 300);
  arma::mat query = arma::randu(2, 100);
  arma::vec estimations1, estimations2, estimations3;
  const double kernelBandwidth = 1.2;
  const double relError = 0.05;

  typedef KDE<EpanechnikovKernel, metric::EuclideanDistance, arma::mat>
      KDEType;

  // KDE.
  KDEType kde(relError, 0, kernel::EpanechnikovKernel(kernelBandwidth));
  kde.Train(std::move(reference));
  kde.Evaluate(query, estimations1);

  // Move constructor KDE.
  KDEType constructor(std::move(kde));
  constructor.Evaluate(query, estimations2);

  // Check results.
  BOOST_REQUIRE_THROW(kde.Evaluate(query, estimations3), std::runtime_error);
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(estimations1[i], estimations2[i], 1e-10);
}

/**
 * Test if an untrained KDE works properly.
 */
BOOST_AUTO_TEST_CASE(NotTrained)
{
  arma::mat query = arma::randu(1, 10);
  std::vector<size_t> oldFromNew;
  arma::vec estimations;

  KDE<> kde;
  KDE<>::Tree queryTree(query, oldFromNew);

  // Check results.
  BOOST_REQUIRE_THROW(kde.Evaluate(query, estimations), std::runtime_error);
  BOOST_REQUIRE_THROW(kde.Evaluate(&queryTree, oldFromNew, estimations),
                      std::runtime_error);
  BOOST_REQUIRE_THROW(kde.Evaluate(estimations), std::runtime_error);
}

/**
 * Test single KD-tree implementation results against brute force results using
 * Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianSingleKDTreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.35;
  const double relError = 0.05;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::SINGLE_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        2,
        0.7);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test single cover-tree implementation results against brute force results
 * using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianSingleCoverTreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.35;
  const double relError = 0.05;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::SINGLE_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        2,
        0.7);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test single octree implementation results against brute force results
 * using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianSingleOctreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 100);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.55;
  const double relError = 0.02;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::Octree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::SINGLE_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        3,
        0.8);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test dual kd-tree implementation results against brute force results
 * using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianDualKDTreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.4;
  const double relError = 0.05;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::DUAL_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        3,
        0.8);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test dual Cover-tree implementation results against brute force results
 * using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianDualCoverTreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.5;
  const double relError = 0.025;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::DUAL_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        3,
        0.8);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test dual octree implementation results against brute force results
 * using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianDualOctreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.03;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::Octree>
    kde(relError,
        0.0,
        kernel,
        KDEMode::DUAL_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        3,
        0.8);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

/**
 * Test dual kd-tree breadth first traversal implementation results against
 * brute force results using Monte Carlo estimations when possible.
 */
BOOST_AUTO_TEST_CASE(GaussianBreadthDualKDTreeMonteCarloKDE)
{
  arma::mat reference = arma::randu(2, 3000);
  arma::mat query = arma::randu(2, 200);
  arma::vec bfEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  arma::vec treeEstimations = arma::vec(query.n_cols, arma::fill::zeros);
  const double kernelBandwidth = 0.7;
  const double relError = 0.025;

  // Brute force KDE.
  GaussianKernel kernel(kernelBandwidth);
  BruteForceKDE<GaussianKernel>(reference,
                                query,
                                bfEstimations,
                                kernel);

  // Optimized KDE.
  metric::EuclideanDistance metric;
  KDE<GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::KDTree,
      tree::KDTree<metric::EuclideanDistance,
                   kde::KDEStat,
                   arma::mat>::template BreadthFirstDualTreeTraverser>
    kde(relError,
        0.0,
        kernel,
        KDEMode::DUAL_TREE_MODE,
        metric,
        true,
        0.95,
        100,
        3,
        0.8);
  kde.Train(reference);
  kde.Evaluate(query, treeEstimations);

  // The Monte Carlo estimation has a random component so it can fail. Therefore
  // we require a reasonable amount of results to be right.
  size_t correctResults = 0;
  for (size_t i = 0; i < query.n_cols; ++i)
  {
    const double resultRelativeError =
      std::abs((bfEstimations[i] - treeEstimations[i]) / bfEstimations[i]);
    if (resultRelativeError < relError)
      ++correctResults;
  }

  BOOST_REQUIRE_GT(correctResults, 70);
}

BOOST_AUTO_TEST_SUITE_END();


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)
/*
    boost::math::fft example 05

    several engines, different complex types.
*/

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>
#include <boost/multiprecision/mpc.hpp>
#ifdef BOOST_MATH_USE_FLOAT128
#include <boost/multiprecision/complex128.hpp>
#endif

#if defined(__GNUC__)
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#endif
#include <boost/math/fft/bsl_backend.hpp>
#include <boost/core/demangle.hpp>
#include <iostream>
#include <vector>
#include <complex>

template<class T>
void print(const std::vector< T >& V)
{
    for(auto i=0UL;i<V.size();++i)
        std::cout << "V[" << i << "] = " << std::setprecision(std::numeric_limits<typename T::value_type>::digits10 + 4)
            << V[i].real() << ", " << V[i].imag() << '\n';
}

template<class Complex>
void test_bsl() {
    std::cout << "BSL engine with " << boost::core::demangle(typeid(Complex).name()) << "\n";
    std::cout << "Real type is    " << boost::core::demangle(typeid(typename Complex::value_type).name()) << "\n";
    std::vector< Complex > A{1.0,2.0,3.0,4.0},B(A.size());
    // forward transform, out-of-place
    boost::math::fft::transform<boost::math::fft::bsl_dft<Complex>>::forward(A.cbegin(),A.cend(),B.begin());
    print(B);
    // backward transform, in-place
    boost::math::fft::transform<boost::math::fft::bsl_dft<Complex>>::backward(B.cbegin(),B.cend(),B.begin());
    print(B);
}

#if defined(__GNUC__)
template<class Complex>
void test_fftw() {
    std::cout << "FFTW engine with " << boost::core::demangle(typeid(Complex).name()) << "\n";
    std::vector< Complex > A{1.0,2.0,3.0,4.0},B(A.size());
    // forward transform, out-of-place
    boost::math::fft::transform<boost::math::fft::fftw_dft<Complex>>::forward(A.cbegin(),A.cend(),B.begin());
    print(B);
    // backward transform, in-place
    boost::math::fft::transform<boost::math::fft::fftw_dft<Complex>>::backward(B.cbegin(),B.cend(),B.begin());
    print(B);
}

template<class Complex>
void test_gsl() {
    std::cout << "GSL engine with " << boost::core::demangle(typeid(Complex).name()) << "\n";
    std::vector< Complex > A{1.0,2.0,3.0,4.0},B(A.size());
    // forward transform, out-of-place
    boost::math::fft::transform<boost::math::fft::gsl_dft<Complex>>::forward(A.cbegin(),A.cend(),B.begin());
    print(B);
    // backward transform, in-place
    boost::math::fft::transform<boost::math::fft::gsl_dft<Complex>>::backward(B.cbegin(),B.cend(),B.begin());
    print(B);
}
#endif

int main()
{
    test_bsl<std::complex<float>>();
    test_bsl<std::complex<double>>();
    test_bsl<std::complex<long double>>();
#ifdef BOOST_MATH_USE_FLOAT128
    test_bsl< boost::multiprecision::complex128 >();
#endif
    test_bsl< boost::multiprecision::cpp_complex_50 >();
    test_bsl< boost::multiprecision::cpp_complex_quad >();
#if defined(__GNUC__)
    test_bsl< boost::multiprecision::mpc_complex_50 >();
#endif

#if defined(__GNUC__)
    test_fftw<std::complex<float>>();
    test_fftw<std::complex<double>>();
    test_fftw<std::complex<long double>>();
#endif
#ifdef BOOST_MATH_USE_FLOAT128
    test_fftw<boost::multiprecision::complex128>();
#endif

#if defined(__GNUC__)
    test_gsl<std::complex<double>>();
#endif
    return 0;
}


/**
 * si_function.hpp
 *
 * This file defines functions from a shift invariant space. We call these
 * Shift Invariant Functions (si_functions), which is a slight abuse
 * of naming to mean that they come from shift invariant spaces.
 *
 * @author Joshua Horacsek
 **/

#ifndef _SISL_SI_FUNCTION_H_
#define _SISL_SI_FUNCTION_H_

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sisl/primitives.hpp>
#include <sisl/function/base_function.hpp>

namespace sisl {

	/*! \brief Combines a lattice and a generating function.
	 *
	 */
    template<class L, class BF, int N>
    class si_function : public function {
    public:
        /*! \brief Evaluate the function at a point.
         */
        si_function() : _bForceScale(false), _lattice(nullptr), _dBasisScale(1.), _bUseBasisDerivative(true){
            _mSpaceTransform = Eigen::MatrixXd::Identity(N, N);
        }

        /*! \brief Evaluate the function at a point.
         */
        si_function(L *lattice) : si_function(){
            _lattice = lattice;
        }

        virtual ~si_function() {

        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double operator()(double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return (*this)(V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double operator()(const vector &p) const {
            if(_lattice == nullptr) return 0;

            if(!_bForceScale) {
                return BF::template convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice);
            }
            return BF::template convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        _dBasisScale);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double d(int component, double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return d(component, V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double d(int component, const vector &p) const {
            if(_bUseBasisDerivative && BF::has_derivative()) {
                if(!_bForceScale)
                    return BF::template convolution_sum_deriv<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice,
                            component);
                return BF::template convolution_sum_deriv_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        component,
                        _dBasisScale);

            }
            if(!_d[component]) return 0;
            if(!_bForceScale)
                return BF::template convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_d[component]);
            return BF::template convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_d[component],
                        _dBasisScale);

        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const vector grad(double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return grad(V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const vector grad(const vector &p) const {
            if(_bUseBasisDerivative && BF::has_derivative()) {
                if(!_bForceScale)
                    return BF::template grad_convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice);
                return BF::template grad_convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        _dBasisScale);

            }
            if(!_bForceScale)
                return BF::template grad_convolution_sum<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        (const L**)_d);
            return BF::template grad_convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        (const L**)_d,
                        _dBasisScale);

        }
        /*! \brief Sets a scale for the basis function to use.
        */
        void set_basis_scale(const double &h) {
            _dBasisScale = h;
            _bForceScale = true;
        }

        /*! \brief If the basis has a derivative, this makes the function use
         * that derivative for derivative reconstruction
         */
        void use_basis_gradient() {
            _bUseBasisDerivative = true;
        }

        /*! \brief If the function has derivative lattices, this makes the
         * function use the given basis to reconstruct derivative values.
         */
        void use_derivative_lattice() {
            _bUseBasisDerivative = false;
        }

        /*! \brief Sets the current reconstruction lattice
         */
        void set_lattice(L *new_lat) {
            this->_lattice = new_lat;
        }

        /*! \brief Sets the lattice to be used for derivative reconstruction
         *  use use_derivative_lattice() to use this for reconstruction after
         *  all lattices have been set.
         */
        void set_derivative_lattice(unsigned int component, L *d) {
            if(component < N)
                _d[component] = d;
        }

        /*! \brief Sets the transform for this space,
        */
        void set_transform(const transform &t){
            if(fabs(t.determinant()) > 1e-8)
                _mSpaceTransform = t;
        }

        /*! \bried Gets the transform assiated to this space,
         */
        transform get_transform() const{
            return _mSpaceTransform;
        }

        L *get_lattice() {
            return _lattice;
        }

        void set_scale(const vector &s) {
            m_vUserScale = s;
            _mSpaceTransform *= Eigen::Scaling(s);
        }

        vector get_scale() const {
            return m_vUserScale;
        }
        virtual const int dim() const {
        	return N;
        }

        virtual const double n_d(const int_tuple &order, double d0, ...) const {
        	throw "Not yet implemnented";

        }

        virtual const double n_d(const int_tuple &order, vector &p) const {
        	throw "Not yet implemnented";
        }

    private:
        L *_lattice, *_d[N];
        bool _bUseBasisDerivative;
        bool _bForceScale;
        double _dBasisScale;
        transform _mSpaceTransform;
        sisl::vector m_vUserScale;
    };
}

#endif // _SISL_SI_FUNCTION_H_


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>
#include <chrono>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))

namespace Laplace2D3D {

using EVec3 = Eigen::Vector3d;

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

// real and wave sum of 2D Laplace kernel Ewald

// xm: target, xn: source
inline double realSum(const double xi, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < 1e-14) {
        return 0;
    }
    return ERFC(rnorm * xi) / rnorm;
}

// xm: target, xn: source
inline double realSum2(const double xi, const EVec3 &xn, const EVec3 &xm) {
    double zmn = xm[2] - xn[2];
    double answer =
        exp(-xi * xi * zmn * zmn) / xi + sqrt(PI314) * zmn * ERF(xi * zmn);
    return answer;
}

inline double gkzxi(const double k, double zmn, double xi) {
    double answer = exp(k * zmn) * ERFC(k / (2 * xi) + xi * zmn) +
                    exp(-k * zmn) * ERFC(k / (2 * xi) - xi * zmn);
    return answer;
}

inline double selfTerm(double xi) { return -2 * xi / sqrt(PI314); }

inline double gKernelEwald(const EVec3 &xm, const EVec3 &xn) {
    const double xi = 1.8; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);

    // real sum
    int rLim = 4;
    double Kreal = 0;
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            EVec3 rmn = target - source + EVec3(i, j, 0);
            if (rmn.norm() < 1e-13) {
                continue;
            }
            Kreal += realSum(xi, EVec3(0, 0, 0), rmn);
        }
    }

    // wave sum
    int wLim = 4;
    double Kwave = 0;
    EVec3 rmn = target - source;
    const double rmnnorm = rmn.norm();
    double zmn = rmn[2];
    rmn[2] = 0;
    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            if (i == 0 && j == 0) {
                continue;
            }
            EVec3 kvec = EVec3(i, j, 0) * (2 * PI314);
            double knorm = kvec.norm();
            Kwave += cos(kvec[0] * rmn[0] + kvec[1] * rmn[1]) * (1 / knorm) *
                     gkzxi(knorm, zmn, xi);
        }
    }
    Kwave *= PI314;

    double Kreal2 = 2 * sqrt(PI314) * realSum2(xi, source, target);
    double Kself = rmnnorm < 1e-10 ? -2 * xi / sqrt(PI314) : 0;

    return Kreal + Kwave - Kreal2 + Kself;
}

inline double gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < 1e-14 ? 0 : 1 / rnorm;
}

// Out of Direct Sum Layer, far field part
inline double gKernelFF(const EVec3 &target, const EVec3 &source) {
    double fEwald = gKernelEwald(target, source);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            double gFree = gKernel(target, source - EVec3(i, j, 0));
            fEwald -= gFree;
        }
    }

    //   {
    //     std::cout << "source:" << source << std::endl
    //               << "target:" << target << std::endl
    //               << "gKernalFF" << fEwald << std::endl;
    //   }
    return fEwald;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    // testing Ewald routine
    double Madelung2D =
        gKernelEwald(EVec3(0, 0, 0), EVec3(0.5, 0.5, 0)) * (-1) +
        gKernelEwald(EVec3(0, 0, 0), EVec3(0, 0, 0)) * 1;
    std::cout << std::setprecision(16) << "Madelung2D: " << Madelung2D
              << " Error: " << Madelung2D + 2.2847222932891311 << std::endl;

    //   exit(1);

    std::chrono::high_resolution_clock::time_point t1 =
        std::chrono::high_resolution_clock::now();
    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {
        -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {
        -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(
        pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(equivN, equivN); // Laplace, 1->1

    Eigen::MatrixXd A(1 * checkN, 1 * equivN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            A(k, l) = gKernel(Cpoint, Lpoint);
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        //		std::cout << "debug:" << Mpoint << std::endl;

        // assemble linear system
        Eigen::VectorXd f(checkN);
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            f(k) = gKernelFF(Cpoint, Mpoint);
        }
        //		std::cout << "debug:" << f << std::endl;

        M2L.col(i) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }
    std::chrono::high_resolution_clock::time_point t2 =
        std::chrono::high_resolution_clock::now();
    auto duration =
        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
    std::cout << "Precomputing time:" << duration / 1e6 << std::endl;

    // dump M2L
    for (int i = 0; i < equivN; i++) {
        for (int j = 0; j < equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        chargePoint(2);
    std::vector<double> chargeValue(2);
    chargePoint[0] = Eigen::Vector3d(0.5, 0.5, 0);
    chargeValue[0] = -1;
    chargePoint[1] = Eigen::Vector3d(0, 0, 0);
    chargeValue[1] = 1;

    // solve M
    A.resize(checkN, equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(checkN);
    for (int k = 0; k < checkN; k++) {
        double temp = 0;
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < chargePoint.size(); p++) {
            temp = temp + gKernel(Cpoint, chargePoint[p]) * (chargeValue[p]);
        }
        f(k) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            A(k, l) = gKernel(Mpoint, Cpoint);
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    std::cout << "Msource: " << Msource << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0, 0, 0);
    double Usample = 0;
    double UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (size_t p = 0; p < chargePoint.size(); p++) {
                Usample +=
                    gKernel(samplePoint, chargePoint[p] + EVec3(i, j, 0)) *
                    chargeValue[p];
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "Error : " << UsampleSP + Usample + 2.284722293289131159
              << std::endl;

    samplePoint = EVec3(0.5, 0.5, 0);
    Usample = 0;
    UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (size_t p = 0; p < chargePoint.size(); p++) {
                Usample +=
                    gKernel(samplePoint, chargePoint[p] + EVec3(i, j, 0)) *
                    chargeValue[p];
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "Error : " << UsampleSP + Usample - 2.284722293289131159
              << std::endl;

    return 0;
}

} // namespace Laplace2D3D

#undef DIRECTLAYER
#undef PI314


#pragma once

#include "util.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/algorithms/covered_by.hpp>

#ifndef HABITAT_H
#define HABITAT_H

typedef boost::geometry::model::point<double,2,boost::geometry::cs::cartesian> Point;
typedef boost::geometry::model::ring<Point> Ring;

/*
 The habitat is represented as a ring (a polygon without holes).
 The vertices of the ring are specified by the user in the file `datapath.outer`.
 For example, suppose that `datapath.outer` contains the following six lines:
 0 0
 3 3
 0 6
 12 6
 12 0
 0 0
 Then the habitat is given by (0,0) - (3,3) - (0,6) - (12,6) - (12,0) - (0,0).
 The vertices of the population graph (the demes) will fall inside the habitat by construction.

 However, EEMS will not check that the sampling locations fall inside the habitat -- instead,
 each sample will be assigned to the closest deme. Therefore, the user should specify a habitat
 that is sufficiently large to cover all sampling locations.
 */

class Habitat {
public:

    Habitat( );
    ~Habitat( );

    void generate_outer(const string &datapath);
    bool dlmwrite_outer(const string &mcmcpath) const;
    bool in_point(const double x, const double y) const;

    double get_area( ) const;
    double get_xmin( ) const;
    double get_xmax( ) const;
    double get_ymin( ) const;
    double get_ymax( ) const;
    double get_xspan( ) const;
    double get_yspan( ) const;

private:

    Ring domain;
    double xmin, xmax, xspan;
    double ymin, ymax, yspan;

};

#endif


///////////////////////////////////////////////////////////////////////////////
// main.cpp
//
//  Copyright 2008 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)
#include <iostream>
#include <algorithm>
#include <iterator>
#include <vector>
#include <functional>
#include <fstream>
#include <boost/mpl/size_t.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/delay.hpp>
#include <boost/accumulators/statistics/acvf_moving_average.hpp>
#include <boost/accumulators/statistics/acvf.hpp>
#include <boost/accumulators/statistics/acf.hpp>
#include <boost/accumulators/statistics/integrated_acf.hpp>
#include <boost/accumulators/statistics/integrated_acvf.hpp>
#include <boost/accumulators/statistics/percentage_effective_sample_size.hpp>
#include <boost/accumulators/statistics/standard_error_autocorrelated.hpp>
#include <boost/accumulators/statistics/standard_error_iid.hpp>
#include <boost/accumulators/statistics/acvf_analysis.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/moving_average.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/numeric/conversion/converter.hpp>
int main(){

    const char* filepath = "./acvf_output";
    std::ofstream out(filepath);

    using namespace boost::accumulators;
    typedef boost::mt19937                                    urng_type;
    typedef boost::normal_distribution<>                      nd_type;
    typedef boost::variate_generator<urng_type&,nd_type>      gen_nd_type;
    typedef double                                            value_type;
    typedef boost::random::moving_average<value_type>         ma_type;
    typedef std::vector<value_type>                           ma_vals_type;
    //typedef default_delay_discriminator                       delaydisrc;
    typedef default_delay_discriminator                       discr_t;
    typedef accumulator_set<
        value_type, stats<
            tag::acvf<discr_t>,
            tag::acf<discr_t>,
            tag::integrated_acvf<discr_t>,
            tag::percentage_effective_sample_size<discr_t>,
            tag::standard_error_autocorrelated<discr_t>,
            tag::standard_error_iid<discr_t>
            >
    >            acc_type;

    //model parameters and related quantities
    std::vector<value_type>     coeffs;
    std::vector<unsigned int>   lags;
    std::vector<value_type>     true_acfs;
    value_type                  true_integrated_acvf = 0.0;
    std::size_t                 true_ess = 0;
    //with these coeffs, should expect ess% > 100
    {using namespace boost::assign; coeffs+=1.0,-0.5,0.2; lags+=0,1,2;}
    unsigned int  K = coeffs.size()-1;
    transform(lags.begin(),lags.end(),back_inserter(true_acfs),
        make_acvf_moving_average(coeffs));
    true_integrated_acvf
        = 2*std::accumulate(true_acfs.begin(),true_acfs.end(),0.0);
    true_integrated_acvf -=  *true_acfs.begin();
    true_ess = boost::numeric::converter<std::size_t,value_type>::convert(
        100.0*true_acfs[0]/true_integrated_acvf);

    out << "->true_acvf: ";
    copy(true_acfs.begin(),true_acfs.end(),
        std::ostream_iterator<value_type>(out," "));
    out << "<-" << std::endl;
    {   value_type div = 1.0/true_acfs[0];
        transform(true_acfs.begin(),true_acfs.end(),true_acfs.begin(),
            boost::bind(std::multiplies<value_type>(),_1,div));
    }
    out << "->true_acf: ";
    copy(true_acfs.begin(),true_acfs.end(),
        std::ostream_iterator<value_type>(out," ")); out << "<-" << std::endl;
    out << "->true var: " << true_integrated_acvf << "<-" << std::endl;
    out << "->true ess%: " << true_ess << "<-" << std::endl;

    //generation of a Moving Average of order K process
    const unsigned long N = 100000;
    urng_type urng(0);
    gen_nd_type gen_nd(urng,nd_type());
    ma_type ma(boost::make_iterator_range(coeffs.begin(),coeffs.end()));
    ma_vals_type ma_vals(N);
    for(ma_vals_type::iterator i=ma_vals.begin(); i<ma_vals.end(); i++)
    { (*i) = ma(gen_nd); }

    //estimation
    acc_type acc(tag::delay<discr_t>::cache_size=(K+1));
    for_each(ma_vals.begin(),ma_vals.end(),
        boost::bind<void>(boost::ref(acc),_1));
    out << "->sample size: " << N << std::endl;
    out << "->estimated acvf: ";
    copy(begin(acvf<discr_t>(acc)),end(acvf<discr_t>(acc)),
        std::ostream_iterator<value_type>(out," "));
    out<<"<-"<<std::endl;

    out << "->estimated acf: ";
    copy(begin(acf<discr_t>(acc)),end(acf<discr_t>(acc)),
        std::ostream_iterator<value_type>(out," "));
    out<<"<-"<<std::endl;

    out << "->estimated var: "
        << integrated_acvf<discr_t>(acc) << "<-" << std::endl;

    out << "->estimated ess%: "
        << percentage_effective_sample_size<discr_t>(acc)
        << "<-" << std::endl;

    out << "->estimated standard error assuming iid: "
        << standard_error_iid<discr_t>(acc) << "<-" << std::endl;

    out << "->estimated standard error assuming acf is zero after lag "
        << K << ": "
        << standard_error_autocorrelated<discr_t>(acc) << "<-" << std::endl;

    //the above bundled into one class:
    out << " --------- ";
    out << "output from acvf_analysis:" << std::endl;
    const unsigned int offset = 0;
    const unsigned int stride = 1;
    const unsigned int assumed_lag = 3;
    statistics::acvf_analysis<value_type,discr_t> acvf_x(assumed_lag);
    acvf_x(ma_vals,offset,stride);
    acvf_x.print(out);

    std::cout << "output of libs/accumulators/main.cpp was written to"
        << filepath << std::endl;

    return 0;
}


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include "gps_conversion.h"
#include <cmath>
#include <Eigen/Dense>

namespace mars
{
std::ostream& operator<<(std::ostream& out, const GpsCoordinates& coordinates)
{
  out << "Lat:\t" << coordinates.latitude_ << std::endl;
  out << "Long:\t" << coordinates.longitude_ << std::endl;
  out << "Alt:\t" << coordinates.altitude_ << std::endl;

  return out;
}

Eigen::Matrix<double, 3, 1> mars::GpsConversion::get_enu(mars::GpsCoordinates coordinates)
{
  return WGS84ToENU(coordinates);
}

GpsConversion::GpsConversion(mars::GpsCoordinates coordinates)
{
  ecef_ref_orientation_.setIdentity();
  ecef_ref_point_.setZero();

  set_gps_reference(coordinates);
}

GpsCoordinates mars::GpsConversion::get_gps_reference()
{
  return reference_;
}

void GpsConversion::set_gps_reference(mars::GpsCoordinates coordinates)
{
  // set gps reference coordinates
  reference_ = coordinates;

  // set ecef reference, position and orientation
  const double rad_lat = deg2rad(coordinates.latitude_);
  const double rad_long = deg2rad(coordinates.longitude_);

  const double s_lat = sin(rad_lat);
  const double c_lat = cos(rad_lat);

  const double s_long = sin(rad_long);
  const double c_long = cos(rad_long);

  Eigen::Matrix3d R;
  R(0, 0) = -s_long;
  R(0, 1) = c_long;
  R(0, 2) = 0;

  R(1, 0) = -s_lat * c_long;
  R(1, 1) = -s_lat * s_long;
  R(1, 2) = c_lat;

  R(2, 0) = c_lat * c_long;
  R(2, 1) = c_lat * s_long;
  R(2, 2) = s_lat;

  ecef_ref_orientation_ = R;
  ecef_ref_point_ = WGS84ToECEF(coordinates);
}

double GpsConversion::deg2rad(const double& deg)
{
  return (M_PI / 180) * deg;
}

Eigen::Matrix<double, 3, 1> GpsConversion::WGS84ToENU(const mars::GpsCoordinates& coordinates)
{
  return ECEFToENU(WGS84ToECEF(coordinates));
}

Eigen::Matrix<double, 3, 1> GpsConversion::ECEFToENU(const Eigen::Matrix<double, 3, 1>& ecef)
{
  Eigen::Matrix<double, 3, 1> enu = ecef_ref_orientation_ * (ecef - ecef_ref_point_);
  return enu;
}

Eigen::Matrix<double, 3, 1> GpsConversion::WGS84ToECEF(const mars::GpsCoordinates& coordinates)
{
  // WGS84 ellipsoid constants
  constexpr double a = 6378137.0;             // semi-major axis
  constexpr double ecc = 8.1819190842622e-2;  // eccentricity of this ellipsoid
  constexpr double ecc_sq = ecc * ecc;

  const double rad_lat = deg2rad(coordinates.latitude_);
  const double rad_long = deg2rad(coordinates.longitude_);

  const double s_lat = sin(rad_lat);
  const double c_lat = cos(rad_lat);
  const double s_long = sin(rad_long);
  const double c_long = cos(rad_long);

  const double N = a / sqrt(1 - ecc_sq * s_lat * s_lat);

  Eigen::Matrix<double, 3, 1> ecef;
  const double h = coordinates.altitude_;
  ecef(0) = (N + h) * c_lat * c_long;
  ecef(1) = (N + h) * c_lat * s_long;
  ecef(2) = (N * (1 - ecc_sq) + h) * s_lat;

  return ecef;
}
}


// Copyright (c) 2005-2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation; either version 3 of the License,
// or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Sebastien Loriot, Sylvain Pion

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include<boost/shared_ptr.hpp>
#include <CGAL/CGAL_Ipelet_base.h>
#include<CGAL/create_offset_polygons_2.h>
#include <boost/format.hpp>

namespace CGAL_skeleton{

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

const std::string Slab[] = {
  "Interior skeleton", "Exterior skeleton","Interior offset","Exterior offset","Interior offsets","Exterior offsets", "Help"
};

const std::string Hmsg[] = {
  "Draw the interior skeleton of one polygon",
  "Draw the exterior skeleton of one polygon",
  "Draw an interior offset of one polygon",
  "Draw an exterior offset of one polygon",
  "Draw several interior offsets of one polygon",
  "Draw several exterior offsets of one polygon"
};

class SkeletonIpelet
  : public CGAL::Ipelet_base<Kernel,7>{

  typedef boost::shared_ptr<Polygon_2>        PolygonPtr ;
  typedef std::vector<PolygonPtr>             PolygonPtrVector ;
  typedef CGAL::Straight_skeleton_2<Kernel>   Skeleton ;
  typedef boost::shared_ptr<Skeleton>         SkeletonPtr ;

  void draw_straight_skeleton(const Skeleton& skeleton,double);

public:
  SkeletonIpelet()
    :CGAL::Ipelet_base<Kernel,7>("Skeleton and offset",Slab,Hmsg){}
  void protected_run(int);
};

void SkeletonIpelet::draw_straight_skeleton(const Skeleton& skeleton,double /*max_edge*/)
{
  typedef Skeleton::Vertex_const_handle     Vertex_const_handle ;
  typedef Skeleton::Halfedge_const_handle   Halfedge_const_handle ;
  typedef Skeleton::Halfedge_const_iterator Halfedge_const_iterator ;

  Halfedge_const_handle null_halfedge ;
  Vertex_const_handle   null_vertex ;

  std::list<Segment_2> seglist;
  std::back_insert_iterator< std::list<Segment_2> > out=std::back_inserter(seglist);

  for ( Halfedge_const_iterator i = skeleton.halfedges_begin();
                                i != skeleton.halfedges_end();
                                ++i )
    if ( i->is_bisector() && ((i->id()%2)==0) ){
        out++=Segment_2(i->opposite()->vertex()->point(),i->vertex()->point());
    }
  draw_in_ipe(seglist.begin(),seglist.end());
}

void SkeletonIpelet::protected_run(int fn)
{

  if (fn==6) {
    show_help();
    return;
  }

  std::list<Polygon_2> pol_list;
  Iso_rectangle_2 bbox=
    read_active_objects( CGAL::dispatch_or_drop_output<Polygon_2>( std::back_inserter(pol_list) ) );

  if (pol_list.size()!=1){
    print_error_message("Exactly one polygon must be selected");
    return;
  }

  Polygon_2 polygon=*pol_list.begin();

  if (!polygon.is_simple()){
    print_error_message("Polygon must be simple");
    return;
  }

  if (polygon.orientation()!=CGAL::COUNTERCLOCKWISE)
    polygon.reverse_orientation();

  std::list<double> offsets;
    //~ "Interior skeleton", "Exterior skeleton","Interior offset","Exterior offset","Interior offsets","Exterior offsets", "Help"
  SkeletonPtr ss;
  double max_edge=std::max((bbox.xmax()-bbox.xmin()),(bbox.ymax()-bbox.ymin()));
  double dist=0.;
  int ret_val=-1;
  switch(fn){
    case 3://Exterior offset
    case 5://Exterior offsets
    case 1://Exterior skeleton
      ss = CGAL::create_exterior_straight_skeleton_2(max_edge,polygon);
      break;
    case 2://Interior offset
    case 4://Interior offsets
    case 0://Interior skeleton
      ss = CGAL::create_interior_straight_skeleton_2(polygon);
      break;
  }

  if (fn==0 || fn==1)
    draw_straight_skeleton(*ss,max_edge);
  else{
    boost::tie(ret_val,dist)=
      request_value_from_user<double>(
        (boost::format("Offset value (BBox %1%x%2%)") % (bbox.xmax()-bbox.xmin()) % (bbox.ymax()-bbox.ymin())).str()
      );
    if (ret_val == -1){
      print_error_message("Bad value provided");
      return;
    }

    if (fn==2 || fn==3)
      offsets.push_back(dist);
    else{
      for (int i=1;i<static_cast<int>(ceil(max_edge/dist/2.))+1;++i)
        offsets.push_back(i*dist);
    }

    for (std::list<double>::iterator it=offsets.begin();it!=offsets.end();++it){
      PolygonPtrVector offset_polygons = CGAL::create_offset_polygons_2<Polygon_2>(*it,*ss);
      for( PolygonPtrVector::const_iterator pi = offset_polygons.begin() ; pi != offset_polygons.end() ; ++ pi )
        draw_in_ipe(**pi);
    }

    if (offsets.size()>1)
      group_selected_objects_();
  }
}

}

CGAL_IPELET(CGAL_skeleton::SkeletonIpelet)


/* -*-c++-*--------------------------------------------------------------------
 * 2019 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/logging.h"
#include "tagslam/rpp.h"
#include "tagslam/quartic.h"
#include <boost/range/irange.hpp>
#include <iostream>
#include <math.h>

//
// Implementation of tests for checking if tag can be 'flipped',
// and a second valid pose can be obtained. This gives a measure of
// how well the tag-to-camera transform is established.
//
// See paper: "Robust Pose Estimation from a Planar Target"
// by Gerald Schweighofer and Alex Pinz
//
// Notes:
//
// 1) The paper is a bit nebulous about how to compute \tilde{R}_z^-1.
//    What it means concretely: decompose
//
//    \tilde{R}_1 = Rz(gamma) * Ry(beta) * Rz(gamma'),
//
//    and set \tilde{R}_z^{-1} = Rz(gamma'). Then
//
//    \tilde{R}_1 * \tilde{R}_z = Rz(gamma) * Ry(beta),
//
//    i.e. only z and y rotations.
//
// 2) top of page 6, beta_t = tan(1/(2*beta)) is wrong, it should
//    read beta_t = tan(beta/2)
//
// 3) I cannot reproduce their expression for the gradient. I think it's
//    wrong, but not sure. I derived my own, which works. This affects
//    equations (12) and (13) in the paper:
//
//    (12) becomes:
//
//    t_opt = -G * sum_i (I-\tilde{V}_i)^2 R_z(gamma) R_y(beta) \tilde{p}_i
//
//    with G = (sum_i (I-\tilde{V}_i)^2)^{-1}
//
//    (13) is modified accordingly:
//
//    E_os = sum_i |... - G sum_j(I -\tilde{V}_j)^2 R_z  R_y \tilde{p}_i|^2

// #define DEBUG

namespace tagslam {
  namespace rpp {
    using boost::irange;

    static double eval_poly(double x, const double *a, int n) {
      double p = 1.0;
      double sum = 0;
      for (const auto i: irange(0, n)) {
        sum += p * a[i];
        p = p * x;
      }
      return (sum);
    }

    //
    // computes rotation R_t from the paper: it rotates the optical axis
    // to face to the origin (center) of the tag.
    //
    static Transform rotate_to_z(
      const Eigen::Vector3d &translat, double *ang) {
      const double t_norm = translat.norm();
      const Eigen::Vector3d txz = translat.cross(Eigen::Vector3d::UnitZ());
      const double sin_a_t_norm = txz.norm();
      if (std::abs(sin_a_t_norm) < 1e-8) {
        return Eigen::Isometry3d::Identity();
      }
      const double sin_a = sin_a_t_norm / t_norm;
      const Eigen::Vector3d n = txz / sin_a_t_norm;
      //const Transform tf = Eigen::AngleAxisd(std::asin(sin_a), n);
      *ang = std::asin(sin_a);
      const Transform tf = (Transform) Eigen::AngleAxisd(*ang, n);
      return (tf);
    }

    typedef std::vector<Eigen::Matrix3d,
                        Eigen::aligned_allocator<Eigen::Matrix3d> > M33dVec;

    //
    // make normalized matrices \tilde{V}_i from \tilde{v}_i
    //
    static M33dVec V_from_v(const ImgPointsH &v) {
      M33dVec V(v.rows());
      for (int i = 0; i < v.rows(); i++) {
        const double vnsq = v.row(i).squaredNorm();
        if (vnsq > 1e-12) {
          V[i] = v.row(i).transpose()*v.row(i) / vnsq;
        } else {
          V[i] = Eigen::Matrix3d::Zero();
        }
      }
      return (V);
    }

    //
    // computes G from the paper (but using my formula)
    //
    static Eigen::Matrix3d compute_G(const M33dVec &V_tilde) {
      Eigen::Matrix3d ImV_sum = Eigen::Matrix3d::Zero();
      for (const auto &V_i: V_tilde) {
        const auto ImV = Eigen::Matrix3d::Identity() - V_i;
        ImV_sum = ImV_sum + ImV.transpose() * ImV;
      }
      return (ImV_sum.inverse());
    }

    //
    // little helper matrix K that extracts beta_t to various powers
    // from R_y(beta_t)*p
    //
    static Eigen::Matrix3d make_K(const Eigen::Vector3d &p) {
      Eigen::Matrix3d K;
      K <<
        p(0),  2*p(2), -p(0),
        p(1),     0.0,  p(1),
        p(2), -2*p(0), -p(2);
      return (K);
    }

    //
    // compute helper matrix FTF = F^T * F.
    //
    // The error E can be expressed then as:
    //
    // E_os(beta_t) = (1+beta_t^2)^{-2} * mu^T * (F^T * F) * mu
    //
    // where mu = [1, beta_t, beta_t^2]^T
    //

    static Eigen::Matrix3d compute_FTF(const M33dVec &V_tilde,
                                       const Transform &R_z,
                                       const ObjPoints &p_tilde) {
      const int n = p_tilde.rows(); // number of points
      const auto G = compute_G(V_tilde);
      Eigen::Matrix3d C_sum = Eigen::Matrix3d::Zero();
      M33dVec C(n);
      for (const auto i: irange(0ul, V_tilde.size())) {
        const auto ImV_i = Eigen::Matrix3d::Identity() - V_tilde[i];
        C[i] = ImV_i * R_z * make_K(p_tilde.row(i));
        C_sum = C_sum + ImV_i.transpose() * C[i];
      }
      Eigen::Matrix3d FTF = Eigen::Matrix3d::Zero();
      for (const auto i: irange(0ul, V_tilde.size())) {
        const auto ImV_i = Eigen::Matrix3d::Identity() - V_tilde[i];
        const auto F_i   = C[i] - ImV_i * G * C_sum;
        FTF = FTF + F_i.transpose() * F_i;
      }
      return (FTF);
    }

    //
    // Starting from
    //
    // E_os(beta_t) = (1+beta_t^2)^{-2} * mu^T * (F^T * F) * mu
    // where mu = [1, beta_t, beta_t^2]^T
    //
    // now express E_os(beta_t) and derivates as polynomials in beta_t
    //
    // E_os   = (1+beta^2)^{-2} * (sum_{i=0^n} f[i] beta^i)
    // E_os'  = (1+beta^2)^{-3} * (sum_{i=0^n} g[i] beta^i)
    // E_os'' = (1+beta^2)^{-4} * (sum_{i=0^n} h[i] beta^i)
    //
    //
    static void compute_polynomial(const Eigen::Matrix3d &FTF,
                                   double *f, double *g, double *h) {
      // polynomial coefficients f[0] == zeroth order etc
      //
      // E_os = (f[0] + f[1] * beta_t + ... f[4] * beta_t^4) / (1+beta_t^2)^2
      //
      f[0] = FTF(0, 0);
      f[1] = FTF(0, 1) + FTF(1, 0);
      f[2] = FTF(0, 2) + FTF(1, 1) + FTF(2, 0);
      f[3] = FTF(1, 2) + FTF(2, 1);
      f[4] = FTF(2, 2);

      // first derivate:
      g[4] = -f[3];
      g[3] = 4 * f[4] - 2 * f[2];
      g[2] = 3 * f[3] - 3 * f[1];
      g[1] = 2 * f[2] - 4 * f[0];
      g[0] = f[1];

      // second derivative:
      h[0] =  -4*f[0] +  2*f[2];
      h[1] = -12*f[1] +  6*f[3];
      h[2] =  20*f[0] - 16*f[2] + 12 * f[4];
      h[3] =  12*f[1] - 16*f[3];
      h[4] =   6*f[2] - 12*f[4];
      h[5] =   2*f[3];
    }

    //
    // Finds locations beta_t of real minima and value E there
    // Returns number of minima found.
    //

    static int find_minima(
      const double *f, // poly coeff for E_os
      const double *g, // poly coeff first deriv
      const double *h, // poly coeff second deriv
      double *beta_min, double *beta_max, double *E_min, double *E_max) {
      std::complex<double> root[4];
      // find roots of first derivative
      const int nroots = quartic::solve_quartic(g[4],g[3],g[2],g[1],g[0],root);

      int n_min(0);
      *E_min = 1e90;
      *E_max = -1e90;
      // check all real roots, and evaluate second deriv there
      for (const auto i:irange(0, nroots)) {
        if (std::imag(root[i]) < 1e-8) {
          const double beta_t = std::real(root[i]);
          if (eval_poly(beta_t, h, 6) > 0) {
            const double opbs = (1.0 + beta_t * beta_t);
            const double E_os = eval_poly(beta_t, f, 5)/(opbs * opbs);
            if (E_os < *E_min) {
              *E_min = E_os;
              *beta_min = 2.0 * std::atan(beta_t);
            }
            if (E_os > *E_max) {
              *E_max = E_os;
              *beta_max = 2.0 * std::atan(beta_t);
            }
            n_min++;
          }
        }
      }
      return (n_min);
    }

    //
    // computes ratio of error for lowest/(second lowest) minimum error
    // orientation. The lower the ratio, the better is the pose
    // established, the more robust it is to flipping.
    //
    double check_quality(const ImgPoints &ip, const ObjPoints &op,
                         const Transform &T, double *beta_orig,
                         double *beta_min, double *beta_max) {
      const ImgPointsH iph = ip.rowwise().homogeneous();
      // compute R_t, the matrix that rotates z to the optical axis
      double ang;
      const Transform R_t = rotate_to_z(T.translation(), &ang);
      const auto R1_tilde = R_t * T;
      // decompose R1_tilde = Rz * Ry * Rz0
      const auto   euler_angles = R1_tilde.rotation().eulerAngles(2, 1, 2);
      const double gamma  = euler_angles[0]; // z  rotation
      const double beta   = euler_angles[1]; // y  rotation
      *beta_orig = beta;
      const double alpha  = euler_angles[2]; // z0 rotation
      const Transform R_z = (Transform)
        Eigen::AngleAxisd(gamma, Eigen::Vector3d::UnitZ());
      const Transform R_z0 = (Transform)
        Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ());
      // compute v_tilde from equation (5)
      const ImgPointsH v_tilde = (R_t.rotation()*iph.transpose()).transpose();
      // compute p_tilde
      const ObjPoints p_tilde  = (R_z0.rotation()*op.transpose()).transpose();
      M33dVec V_tilde = V_from_v(v_tilde);
      const Eigen::Matrix3d FTF = compute_FTF(V_tilde, R_z, p_tilde);
      double f[5], g[5], h[6];
      compute_polynomial(FTF, f, g, h);
      double E_min, E_max;
      int n_min = find_minima(f, g, h, beta_min, beta_max, &E_min, &E_max);
      switch (n_min) {
      case 2:
        return (E_min / E_max); // two minima, the usual case
        break;
      case 1:
        return (0.0); // single minimum, assume all is good
        break;
      default:
        ROS_WARN_STREAM("found bad num minima: " << n_min);
        *beta_min = beta;
        *beta_max = beta;
        return (0.0); // close eyes and hope for the best....
        break;
      }
      return (0.0); // should never reach this
    }
  } // end of namespace rpp
}


#define BOOST_TEST_MODULE "test_area"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <periortree/boundary_condition.hpp>
#include <periortree/area.hpp>
#include <test/point_type.hpp>
#include <test/aabb_type.hpp>

BOOST_AUTO_TEST_CASE(test_aabb_unlimited)
{
    {
        const perior::test::xyz  l(0., 0., 0.);
        const perior::test::xyz  u(10., 10., 10.);
        const perior::test::aabb box(l, u);
        const perior::unlimited_boundary<perior::test::xyz> boundary;

        const double area = perior::area(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(area, 1000.0, 1e-12);
    }
}

BOOST_AUTO_TEST_CASE(test_xyz_cubic_periodic)
{
    const perior::test::xyz lw(0., 0., 0.);
    const perior::test::xyz up(10., 10., 10.);
    const perior::cubic_periodic_boundary<perior::test::xyz> boundary(lw, up);

    {
        const perior::test::xyz l(3.0, 3.0, 3.0);
        const perior::test::xyz u(7.0, 7.0, 7.0);
        const perior::test::aabb box(l, u);
        const double area = perior::area(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(area, 64.0, 1e-12);
    }

    {
        const perior::test::xyz l(8.0, 8.0, 8.0);
        const perior::test::xyz u(2.0, 2.0, 2.0);
        const perior::test::aabb box(l, u);
        const double area = perior::area(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(area, 64.0, 1e-12);
    }

    {
        const perior::test::xyz l(8.0, 4.0, 8.0);
        const perior::test::xyz u(2.0, 8.0, 2.0);
        const perior::test::aabb box(l, u);
        const double area = perior::area(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(area, 64.0, 1e-12);
    }
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_
#define SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

namespace smooth::detail {

/**
 * @brief Calculate the value and derivative of the function
 * \f[
 * \phi(\alpha) = \left\| D (J^T J + \alpha D^T D)^{-1} J^T r \right\| - \Delta
 * \f]
 *
 * @param J sparse matrix size MxN
 * @param d vector size N representing diagonal of D
 * @param r vector size M
 * @param Delta scalar
 * @param alpha scalar
 *
 * @return Triplet \f$(x, \phi(\alpha), \phi'(\alpha))\f$ where \f$x\f$ is a solution to \f$ J^T J +
 * \alpha D^T D = -J^T r\f$.
 */
template<int N, int M>
std::tuple<Eigen::Vector<double, N>, double, double> calc_phi(
  const auto & J,
  const Eigen::Vector<double, N> & d,
  const Eigen::Vector<double, M> & r,
  double Delta,
  double alpha)
{
  const auto n = J.cols();

  Eigen::SparseMatrix<double> lhs = J.transpose() * J;

  lhs.reserve(Eigen::Vector<double, N>::Ones(n));
  if (alpha > 0) {
    for (auto i = 0u; i != n; ++i) { lhs.coeffRef(i, i) += alpha * d(i) * d(i); }
  }
  lhs.makeCompressed();

  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;
  ldlt.compute(lhs);

  if (ldlt.info()) {
    // computation failed, add small diagonal to ensure positive definiteness
    for (auto i = 0u; i != n; ++i) { lhs.coeffRef(i, i) += Eigen::NumTraits<double>::epsilon(); }
    ldlt.compute(lhs);
  }

  // calculate q
  const Eigen::Vector<double, N> x = ldlt.solve(-J.transpose() * r);
  const Eigen::Vector<double, N> q = -d.cwiseProduct(x);

  // calculate phi
  const double phi = q.stableNorm() - Delta;

  // calculate dphi
  const Eigen::Vector<double, N> d_q = d.cwiseProduct(q);
  const Eigen::Vector<double, N> y   = ldlt.solve(d_q);
  const double dphi                  = -d.cwiseProduct(q.normalized()).dot(y);

  return std::make_tuple(x, phi, dphi);
}

/**
 * @brief Approximate a Levenberg-Marquardt parameter lambda s.t. if x solves
 *
 *   \| [J; sqrt(lambda) * diag(d)] x  +  [r ; 0] \|^2
 *
 * then either
 *  * lambda = 0 AND \|diag(d) * x\| <= 1.1 Delta
 *    OR
 *  * lambda > 0 AND 0.9 Delta <= \|diag(d) * x\| <= 1.1 Delta
 *
 * @param J sparse matrix MxN
 * @param d vector size Nx1
 * @param r vector size Mx1
 * @param Delta scalar
 *
 * @return pair(lambda, x) where x solves the least-squares problem for lambda
 */
template<int N, int M>
std::pair<double, Eigen::Vector<double, N>> lmpar_sparse(
  const auto & J,
  const Eigen::Vector<double, N> & d,
  const Eigen::Matrix<double, M, 1> & r,
  double Delta)
{
  double alpha = 0;

  auto [x, phi, dphi] = calc_phi(J, d, r, Delta, alpha);

  if (phi <= 0.1 * Delta) {
    return std::make_pair(0, std::move(x));  // alpha = 0 solution fulfills condition
  }

  // initialize bounds
  double l = std::max<double>(0, -phi / dphi);
  double u = (d.cwiseInverse().cwiseProduct(J.transpose() * r)).stableNorm() / Delta;

  // it typically converges in 2 or 3 iterations
  for (auto i = 0u; i != 20; ++i) {
    // ensure alpha stays within bounds (and not equal to zero)
    if (!(l < alpha && alpha < u)) { alpha = std::max<double>(0.001 * u, sqrt(l * u)); }

    std::tie(x, phi, dphi) = calc_phi(J, d, r, Delta, alpha);

    if (std::abs(phi) <= 0.1 * Delta) {
      break;  // condition fulfilled
    }

    // update bounds
    l = std::max<double>(l, alpha - phi / dphi);
    if (phi < 0) { u = alpha; }

    // update alpha
    alpha = alpha - ((phi + Delta) / Delta) * (phi / dphi);
  }

  return std::make_pair(alpha, std::move(x));
}

}  // namespace smooth::detail

#endif  // SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_