#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/zernike.h>
#include <scitbx/math/2d_zernike_moments.h>

namespace scitbx { namespace math {
namespace {

  struct two_d_voxel_wrapper
  {
    typedef voxel_2d <double> w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("two_d_voxel", no_init)
        .def( init<
                   int const&,
                   double const&,
                   double const&,
                   double const&,
                   scitbx::af::const_ref< scitbx::vec3<double> >
                  >
             ((
                arg("splat_range"),
                arg("external_rmax"),
                arg("dx"),
                arg("fraction"),
                arg("xyz")
             ))
            )
        .def("rmax", &w_t::rmax)
        .def("np", &w_t::np)
        .def("get_image", &w_t::get_image)
        .def("get_value", &w_t::get_value)
      ;
    }
  };

//
//
  struct two_d_grid_wrapper
  {
    typedef grid_2d  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("two_d_grid", no_init)
        .def( init<
                   int const&,
                   int const&
                  >
             ((
                arg("np"),
                arg("n_max")
             ))
            )
        .def("get_ss", &w_t::get_ss)
        .def("get_ss", &w_t::get_all_ss)
        .def("clean_space", &w_t::clean_space)
        .def("construct_space_sum",&w_t::construct_space_sum)
      ;
    }
  };

//
//
  struct two_d_moments_wrapper
  {
    typedef zernike_2d_moments  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("two_d_zernike_moments", no_init)
        .def( init<
                   grid_2d< double >,
                   int const&
                  >
             ((
                arg("grid"),
                arg("nmax")
             ))
            )
        .def("moments", &w_t::all_moments)
        .def("nm", &w_t::nm)
        .def("get_moment",&w_t::get_moment)
        .def("calc_moments",&w_t::calc_moments)
        .def("update_ss",&w_t::update_ss)
        .def("print_bnmk",&w_t::print_Bnmk)
        .def("zernike_poly",&w_t::zernike_poly)
        .def("zernike_map",&w_t::zernike_map)
      ;
    }
  };

//
//

} //namespace <anonymous>

namespace boost_python {

  void wrap_2d_zernike_mom()
  {
    two_d_voxel_wrapper::wrap();
    two_d_moments_wrapper::wrap();
    two_d_grid_wrapper::wrap();
  }

}}}


/*!
 *
 * \file affine_transform.hpp
 *
 * \brief File containing n-dimensional affine transformation functions
 *        that can be combined with various interpolation and boundary
 *        functions.
 *
 *
 */
#pragma once
#include <array>
#include <tuple>

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include "interpolation.hpp"

/*! \brief Namespace containing affine transform functionality
 *         for transforming n-dimnesional data.
 */
namespace affine_transform
{
/*! \brief Namespace containing implementation details for the affine
 *         transformation functionality.
 */
namespace detail
{
/*! \brief Inner For loop of the transform function.
 *
 *  Loops over the dimensions of the output image and fills it
 *  using the given interpolation and boundary functions to look
 *  up values in the given input image.
 *
 *  @param[in] point            The point from which to start to loop
 *                              in the input image coordinate system
 *  @param[in] input_image      The image from which to extract values
 *  @param[in,out] output_image The image to write to
 *  @param[in] dx               The vectors along which to iterate to
 *                              fill the output image
 *  @param[in,out] chunk        A temporary memory object which is used
 *                              to call the interpolation function
 *  @param[in] background_value A background value to use when accessing
 *                              points outside the input image. (might get
 *                              ignored by some boundary functions)
 *  @param[in] begin            The starting index of the first dimension
 *                              (can be used to parallelise the transform)
 *  @param[in] end              The end index of the first dimension
 *                              (can be used to parallelise the transform)
 *  @param[in] xs               Loop variables used to access the n-dimensional
 *                              data
 *
 *  @tparam Dim                 The dimensions of the images
 *  @tparam T                   The data type of the images
 *  @tparam Func                The interpolation order function
 *  @tparam BoundaryFunc        The boundary function to use
 *  @tparam Xs                  The loop indices, will be int
 */
template <int Dim, typename T, typename Func, typename BoundaryFunc,
          typename... Xs>
constexpr void transform_loop(
    Eigen::Matrix<double, Dim, 1> point,
    const pybind11::detail::unchecked_reference<T, Dim>& input_image,
    pybind11::detail::unchecked_mutable_reference<T, Dim>& output_image,
    const std::array<Eigen::Matrix<double, Dim, 1>, Dim>& dx,
    interpolation::Data<Func, Dim>& chunk, T background_value, int begin,
    int end, Xs... xs)
{
    for (int i = begin; i < end; ++i)
    {
        /* We are in the inner-most loop*/
        if constexpr (Dim == sizeof...(xs) + 1)
        {
            auto x_lower = std::array<int, Dim>{};
            auto x_relative = std::array<double, Dim>{};
            for (size_t l = 0; l < Dim; ++l)
            {
                x_lower[l] = point(l) - (point(l) < 0);
                x_relative[l] = point(l) - x_lower[l];
            }

            interpolation::extract<Func, BoundaryFunc, Dim>(
                chunk, input_image, x_lower, background_value);

            auto interpolate = [&chunk](auto... args) {
                return interpolation::apply_func(chunk, args...);
            };

            output_image(xs..., i) = std::apply(interpolate, x_relative);
        }
        /* Start more loops to iterate over the N-dimensional data*/
        else
        {
            transform_loop<Dim, T, Func, BoundaryFunc>(
                point, input_image, output_image, dx, chunk, background_value,
                0, output_image.shape(sizeof...(xs) + 1), xs..., i);
        }
        point += dx[sizeof...(xs)];
    }
}
}; // namespace detail

/*! \brief Extracts an image from a given one with a given
 *         coordinate system.
 *
 *  @param[in] origin            The origin of the coordinate system for the
 *                               resulting image in the coordinate system of
 *                               the input system.
 *  @param[in] dx                The vectors of the coordinate system of the
 *                               resulting image in the coordinate system of the
 *                               input system.
 *  @param[in] input_image       The image from which to extract data
 *  @param[in,out] output_image  The image in which to store the results
 *  @param[in] background_value  The value to use for points outside the input
 *                               image's domain. Might be ignored by some
 *                               boundary functions.
 *
 *  @tparam Dim                  The dimensionality of the image data
 *  @tparam T                    The datatype of the image data
 *  @tparam Func                 The interpolation order to use
 *  @tparam BoundaryFunc         The boundary function to use
 */
template <int Dim, typename T, template <typename> typename Func,
          typename BoundaryFunc>
void transform(const Eigen::Matrix<double, Dim, 1>& origin,
               const std::array<Eigen::Matrix<double, Dim, 1>, Dim>& dx,
               const pybind11::array_t<T>& input_image,
               pybind11::array_t<T>& output_image, T background_value)
{
    auto input = input_image.template unchecked<Dim>();
    auto output = output_image.template mutable_unchecked<Dim>();

#pragma omp parallel
    {
        typedef Func<T> _Func;

        interpolation::Data<_Func, Dim> chunk;

        int x_len = output.shape(0) / omp_get_num_threads();
        int x_start = x_len * omp_get_thread_num();
        int x_end = x_start + x_len;

        Eigen::Matrix<double, Dim, 1> local_origin = origin + x_start * dx[0];
        if (omp_get_thread_num() == omp_get_num_threads() - 1)
        {
            x_end = output.shape(0);
        }
        detail::transform_loop<Dim, T, _Func, BoundaryFunc>(
            local_origin, input, output, dx, chunk, background_value, x_start,
            x_end);
    }
}

}; // namespace affine_transform


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

#include <boost/math/tools/fraction.hpp>
#include <iostream>
#include <complex>

//[golden_ratio_1
template <class T>
struct golden_ratio_fraction
{
   typedef T result_type;

   result_type operator()()
   {
      return 1;
   }
};
//]

//[cf_tan_fraction
template <class T>
struct tan_fraction
{
private:
   T a, b;
public:
   tan_fraction(T v)
      : a(-v * v), b(-1)
   {}

   typedef std::pair<T, T> result_type;

   std::pair<T, T> operator()()
   {
      b += 2;
      return std::make_pair(a, b);
   }
};
//]
//[cf_tan
template <class T>
T tan(T a)
{
   tan_fraction<T> fract(a);
   return a / continued_fraction_b(fract, std::numeric_limits<T>::epsilon());
}
//]
//[cf_expint_fraction
template <class T>
struct expint_fraction
{
   typedef std::pair<T, T> result_type;
   expint_fraction(unsigned n_, T z_) : b(z_ + T(n_)), i(-1), n(n_) {}
   std::pair<T, T> operator()()
   {
      std::pair<T, T> result = std::make_pair(-static_cast<T>((i + 1) * (n + i)), b);
      b += 2;
      ++i;
      return result;
   }
private:
   T b;
   int i;
   unsigned n;
};
//]
//[cf_expint
template <class T>
inline std::complex<T> expint_as_fraction(unsigned n, std::complex<T> const& z)
{
   boost::uintmax_t max_iter = 1000;
   expint_fraction<std::complex<T> > f(n, z);
   std::complex<T> result = boost::math::tools::continued_fraction_b(
      f,
      std::complex<T>(std::numeric_limits<T>::epsilon()),
      max_iter);
   result = exp(-z) / result;
   return result;
}
//]
//[cf_upper_gamma_fraction
template <class T>
struct upper_incomplete_gamma_fract
{
private:
   typedef typename T::value_type scalar_type;
   T z, a;
   int k;
public:
   typedef std::pair<T, T> result_type;

   upper_incomplete_gamma_fract(T a1, T z1)
      : z(z1 - a1 + scalar_type(1)), a(a1), k(0)
   {
   }

   result_type operator()()
   {
      ++k;
      z += scalar_type(2);
      return result_type(scalar_type(k) * (a - scalar_type(k)), z);
   }
};
//]
//[cf_gamma_Q
template <class T>
inline std::complex<T> gamma_Q_as_fraction(const std::complex<T>& a, const std::complex<T>& z)
{
   upper_incomplete_gamma_fract<std::complex<T> > f(a, z);
   std::complex<T> eps(std::numeric_limits<T>::epsilon());
   return pow(z, a) / (exp(z) *(z - a + T(1) + boost::math::tools::continued_fraction_a(f, eps)));
}
//]

int main()
{
   using namespace boost::math::tools;

   //[cf_gr
   golden_ratio_fraction<double> func;
   double gr = continued_fraction_a(
      func,
      std::numeric_limits<double>::epsilon());
   std::cout << "The golden ratio is: " << gr << std::endl;
   //]

   std::cout << tan(0.5) << std::endl;

   std::complex<double> arg(3, 2);
   std::cout << expint_as_fraction(5, arg) << std::endl;

   std::complex<double> a(3, 3), z(3, 2);
   std::cout << gamma_Q_as_fraction(a, z) << std::endl;

   return 0;
}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of variable class.
 */
#include "num_collect/auto_diff/backward/variable.h"

#include <Eigen/Core>
#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::variable", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("construct with a node") {
        constexpr auto value = static_cast<scalar_type>(1.234);
        const auto node =
            num_collect::auto_diff::backward::graph::create_node<scalar_type>();

        const auto var = variable_type(value, node);

        REQUIRE(var.value() == value);
        REQUIRE(var.node() == node);
    }

    SECTION("construct a constant") {
        constexpr auto value = static_cast<scalar_type>(1.234);

        const auto var = variable_type(value, constant_tag());

        REQUIRE(var.value() == value);
        REQUIRE(var.node() == nullptr);
    }

    SECTION("construct a variable") {
        constexpr auto value = static_cast<scalar_type>(1.234);

        const auto var = variable_type(value, variable_tag());

        REQUIRE(var.value() == value);
        REQUIRE(var.node() != nullptr);
    }

    SECTION("construct a constant without tags") {
        constexpr auto value = static_cast<scalar_type>(1.234);

        const auto var = variable_type(value);

        REQUIRE(var.value() == value);
        REQUIRE(var.node() == nullptr);
    }

    SECTION("construct without arguments") {
        const auto var = variable_type();

        REQUIRE(var.value() == static_cast<scalar_type>(0));
        REQUIRE(var.node() == nullptr);
    }

    SECTION("construct with a node") {
        const auto orig =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());

        const variable_type res = -orig;
        REQUIRE_THAT(res.value(), Catch::Matchers::WithinRel(-orig.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == orig.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(-1)));
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::operator+", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("variable + variable") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left + right;

        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() + right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 2);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE(res.node()->children()[1].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(1)));
        REQUIRE_THAT(res.node()->children()[1].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(1)));
    }

    SECTION("scalar + variable") {
        const auto left = static_cast<scalar_type>(1.234);
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left + right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left + right.value()));
        REQUIRE(res.node() == right.node());
    }

    SECTION("variable + scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left + right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() + right));
        REQUIRE(res.node() == left.node());
    }

    SECTION("scalar + scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), constant_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left + right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() + right));
        REQUIRE(res.node() == nullptr);
    }

    SECTION("self assignment") {
        const auto orig =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        auto var = orig;

        var += var;

        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(orig.value() + orig.value()));
        REQUIRE(var.node());
        REQUIRE(var.node()->children().size() == 1);
        REQUIRE(var.node()->children()[0].node() == orig.node());
        REQUIRE_THAT(var.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(2)));
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::operator-", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("variable - variable") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left - right;

        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() - right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 2);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE(res.node()->children()[1].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(1)));
        REQUIRE_THAT(res.node()->children()[1].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(-1)));
    }

    SECTION("scalar - variable") {
        const auto left = static_cast<scalar_type>(1.234);
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left - right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left - right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(-1)));
    }

    SECTION("variable - scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left - right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() - right));
        REQUIRE(res.node() == left.node());
    }

    SECTION("scalar - scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), constant_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left - right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() - right));
        REQUIRE(res.node() == nullptr);
    }

    SECTION("self assignment") {
        const auto orig =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        auto var = orig;

        var -= var;  // NOLINT

        REQUIRE(var.value() == static_cast<scalar_type>(0));
        REQUIRE(var.node() == nullptr);
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::operator*", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("variable * variable") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left * right;

        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() * right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 2);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE(res.node()->children()[1].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(right.value()));
        REQUIRE_THAT(res.node()->children()[1].sensitivity(),
            Catch::Matchers::WithinRel(left.value()));
    }

    SECTION("scalar * variable") {
        const auto left = static_cast<scalar_type>(1.234);
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left * right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left * right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(left));
    }

    SECTION("variable * scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left * right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() * right));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(right));
    }

    SECTION("scalar * scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), constant_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left * right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() * right));
        REQUIRE(res.node() == nullptr);
    }

    SECTION("self assignment") {
        const auto orig =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        auto var = orig;

        var *= var;

        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(orig.value() * orig.value()));
        REQUIRE(var.node());
        REQUIRE(var.node()->children().size() == 1);
        REQUIRE(var.node()->children()[0].node() == orig.node());
        REQUIRE_THAT(var.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(
                static_cast<scalar_type>(2) * orig.value()));
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::operator/", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("variable / variable") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left / right;

        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() / right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 2);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE(res.node()->children()[1].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(
                static_cast<scalar_type>(1) / right.value()));
        REQUIRE_THAT(res.node()->children()[1].sensitivity(),
            Catch::Matchers::WithinRel(
                -left.value() / (right.value() * right.value())));
    }

    SECTION("scalar / variable") {
        const auto left = static_cast<scalar_type>(1.234);
        const auto right =
            variable_type(static_cast<scalar_type>(2.345), variable_tag());

        const variable_type res = left / right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left / right.value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == right.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(
                -left / (right.value() * right.value())));
    }

    SECTION("variable / scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left / right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() / right));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 1);
        REQUIRE(res.node()->children()[0].node() == left.node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(static_cast<scalar_type>(1) / right));
    }

    SECTION("scalar / scalar") {
        const auto left =
            variable_type(static_cast<scalar_type>(1.234), constant_tag());
        const auto right = static_cast<scalar_type>(2.345);

        const variable_type res = left / right;

        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(left.value() / right));
        REQUIRE(res.node() == nullptr);
    }

    SECTION("self assignment") {
        const auto orig =
            variable_type(static_cast<scalar_type>(1.234), variable_tag());
        auto var = orig;

        var /= var;  // NOLINT

        REQUIRE(var.value() == static_cast<scalar_type>(1));
        REQUIRE(var.node() == nullptr);
    }
}

TEST_CASE("Eigen::Matrix<num_collect::auto_diff::backward::variable>") {
    using variable_type = num_collect::auto_diff::backward::variable<double>;
    using vector_type = Eigen::Matrix<variable_type, 2, 1>;
    using num_collect::auto_diff::backward::variable_tag;

    const auto vec = vector_type(variable_type(1.234, variable_tag()),
        variable_type(2.345, variable_tag()));

    SECTION("prod") {
        const variable_type res = vec.prod();
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(vec(0).value() * vec(1).value()));
        REQUIRE(res.node());
        REQUIRE(res.node()->children().size() == 2);
        REQUIRE(res.node()->children()[0].node() == vec[0].node());
        REQUIRE(res.node()->children()[1].node() == vec[1].node());
        REQUIRE_THAT(res.node()->children()[0].sensitivity(),
            Catch::Matchers::WithinRel(vec[1].value()));
        REQUIRE_THAT(res.node()->children()[1].sensitivity(),
            Catch::Matchers::WithinRel(vec[0].value()));
    }
}


#ifndef SEQUENTIAL_LINE_SEARCH_UTILS_HPP
#define SEQUENTIAL_LINE_SEARCH_UTILS_HPP

#include <Eigen/Core>
#include <Eigen/LU>
#include <cassert>
#include <cmath>
#include <string>

namespace sequential_line_search
{
    namespace utils
    {
        ////////////////////////////////////////////////
        // Random
        ////////////////////////////////////////////////

        // Uniform sampling from [0, 1]^n
        Eigen::VectorXd GenerateRandomVector(unsigned n);

        ////////////////////////////////////////////////
        // Bradley-Terry-Luce Model
        ////////////////////////////////////////////////

        inline double CalcBtl(const Eigen::VectorXd& f, double scale = 1.0)
        {
            const auto exp_rep = ((1.0 / scale) * f).array().exp();
            return exp_rep(0) / exp_rep.sum();
        }

        inline Eigen::VectorXd CalcBtlDerivative(const Eigen::VectorXd& f, double scale = 1.0)
        {
            const unsigned dim = f.rows();
            const double   btl = CalcBtl(f, scale);
            const double   tmp = -btl * btl / scale;

            Eigen::VectorXd d(dim);

            double sum = 0.0;
            for (unsigned i = 1; i < dim; ++i)
            {
                sum += std::exp((f(i) - f(0)) / scale);
            }
            d(0) = -sum;

            for (unsigned i = 1; i < dim; ++i)
            {
                d(i) = std::exp((f(i) - f(0)) / scale);
            }

            return tmp * d;
        }

        ////////////////////////////////////////////////
        // File IO
        ////////////////////////////////////////////////

        void ExportMatrixToCsv(const std::string& file_path, const Eigen::MatrixXd& X);
    } // namespace utils
} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_UTILS_HPP


/**
 * @file tests/range_search_test.cpp
 * @author Ryan Curtin
 *
 * Test file for RangeSearch<> class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/range_search/range_search.hpp>
#include <mlpack/core/tree/cover_tree.hpp>
#include <mlpack/methods/range_search/rs_model.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::range;
using namespace mlpack::math;
using namespace mlpack::tree;
using namespace mlpack::bound;
using namespace mlpack::metric;
using namespace std;

BOOST_AUTO_TEST_SUITE(RangeSearchTest);

// Get our results into a sorted format, so we can actually then test for
// correctness.
void SortResults(const vector<vector<size_t>>& neighbors,
                 const vector<vector<double>>& distances,
                 vector<vector<pair<double, size_t>>>& output)
{
  output.resize(neighbors.size());
  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    output[i].resize(neighbors[i].size());
    for (size_t j = 0; j < neighbors[i].size(); ++j)
      output[i][j] = make_pair(distances[i][j], neighbors[i][j]);

    // Now that it's constructed, sort it.
    sort(output[i].begin(), output[i].end());
  }
}

// Clean a tree's statistics.
template<typename TreeType>
void CleanTree(TreeType& node)
{
  node.Stat().LastDistance() = 0.0;

  for (size_t i = 0; i < node.NumChildren(); ++i)
    CleanTree(node.Child(i));
}

/**
 * Simple range-search test with small, synthetic dataset.  This is an
 * exhaustive test, which checks that each method for performing the calculation
 * (dual-tree, single-tree, naive) produces the correct results.  An
 * eleven-point dataset and the points within three ranges are taken.  The
 * dataset is in one dimension for simplicity -- the correct functionality of
 * distance functions is not tested here.
 */
BOOST_AUTO_TEST_CASE(ExhaustiveSyntheticTest)
{
  // Set up our data.
  arma::mat data(1, 11);
  data[0] = 0.05; // Row addressing is unnecessary (they are all 0).
  data[1] = 0.35;
  data[2] = 0.15;
  data[3] = 1.25;
  data[4] = 5.05;
  data[5] = -0.22;
  data[6] = -2.00;
  data[7] = -1.30;
  data[8] = 0.45;
  data[9] = 0.90;
  data[10] = 1.00;

  typedef KDTree<EuclideanDistance, RangeSearchStat, arma::mat> TreeType;

  // We will loop through three times, one for each method of performing the
  // calculation.
  std::vector<size_t> oldFromNew;
  std::vector<size_t> newFromOld;
  TreeType* tree = new TreeType(data, oldFromNew, newFromOld, 1);
  for (int i = 0; i < 3; ++i)
  {
    RangeSearch<>* rs;

    switch (i)
    {
      case 0: // Use the naive method.
        rs = new RangeSearch<>(tree->Dataset(), true);
        break;
      case 1: // Use the single-tree method.
        rs = new RangeSearch<>(tree, true);
        break;
      case 2: // Use the dual-tree method.
        rs = new RangeSearch<>(tree);
        break;
    }

    // Now perform the first calculation.  Points within 0.50.
    vector<vector<size_t>> neighbors;
    vector<vector<double>> distances;
    rs->Search(Range(0.0, sqrt(0.5)), neighbors, distances);

    // Now the exhaustive check for correctness.  This will be long.
    vector<vector<pair<double, size_t>>> sortedOutput;
    SortResults(neighbors, distances, sortedOutput);

    BOOST_REQUIRE(sortedOutput[newFromOld[0]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][0].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][1].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][1].first, 0.27, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][2].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][2].first, 0.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][3].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][3].first, 0.40, 1e-5);

    // Neighbors of point 1.
    BOOST_REQUIRE(sortedOutput[newFromOld[1]].size() == 6);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][0].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][1].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][1].first, 0.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][2].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][2].first, 0.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][3].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][3].first, 0.55, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][4].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][4].first, 0.57, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][5].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][5].first, 0.65, 1e-5);

    // Neighbors of point 2.
    BOOST_REQUIRE(sortedOutput[newFromOld[2]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][0].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][1].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][1].first, 0.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][2].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][2].first, 0.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][3].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][3].first, 0.37, 1e-5);

    // Neighbors of point 3.
    BOOST_REQUIRE(sortedOutput[newFromOld[3]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][0].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][0].first, 0.25, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][1].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][1].first, 0.35, 1e-5);

    // Neighbors of point 4.
    BOOST_REQUIRE(sortedOutput[newFromOld[4]].size() == 0);

    // Neighbors of point 5.
    BOOST_REQUIRE(sortedOutput[newFromOld[5]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][0].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][0].first, 0.27, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][1].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][1].first, 0.37, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][2].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][2].first, 0.57, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][3].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][3].first, 0.67, 1e-5);

    // Neighbors of point 6.
    BOOST_REQUIRE(sortedOutput[newFromOld[6]].size() == 1);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][0].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][0].first, 0.70, 1e-5);

    // Neighbors of point 7.
    BOOST_REQUIRE(sortedOutput[newFromOld[7]].size() == 1);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][0].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][0].first, 0.70, 1e-5);

    // Neighbors of point 8.
    BOOST_REQUIRE(sortedOutput[newFromOld[8]].size() == 6);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][0].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][1].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][1].first, 0.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][2].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][2].first, 0.40, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][3].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][3].first, 0.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][4].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][4].first, 0.55, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][5].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][5].first, 0.67, 1e-5);

    // Neighbors of point 9.
    BOOST_REQUIRE(sortedOutput[newFromOld[9]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][0].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][1].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][1].first, 0.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][2].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][2].first, 0.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][3].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][3].first, 0.55, 1e-5);

    // Neighbors of point 10.
    BOOST_REQUIRE(sortedOutput[newFromOld[10]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][0].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][0].first, 0.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][1].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][1].first, 0.25, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][2].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][2].first, 0.55, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][3].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][3].first, 0.65, 1e-5);

    // Now do it again with a different range: [sqrt(0.5) 1.0].
    if (rs->ReferenceTree())
      CleanTree(*rs->ReferenceTree());
    rs->Search(Range(sqrt(0.5), 1.0), neighbors, distances);
    SortResults(neighbors, distances, sortedOutput);

    // Neighbors of point 0.
    BOOST_REQUIRE(sortedOutput[newFromOld[0]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][0].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][0].first, 0.85, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][1].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][1].first, 0.95, 1e-5);

    // Neighbors of point 1.
    BOOST_REQUIRE(sortedOutput[newFromOld[1]].size() == 1);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][0].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][0].first, 0.90, 1e-5);

    // Neighbors of point 2.
    BOOST_REQUIRE(sortedOutput[newFromOld[2]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][0].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][0].first, 0.75, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][1].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][1].first, 0.85, 1e-5);

    // Neighbors of point 3.
    BOOST_REQUIRE(sortedOutput[newFromOld[3]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][0].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][0].first, 0.80, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][1].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][1].first, 0.90, 1e-5);

    // Neighbors of point 4.
    BOOST_REQUIRE(sortedOutput[newFromOld[4]].size() == 0);

    // Neighbors of point 5.
    BOOST_REQUIRE(sortedOutput[newFromOld[5]].size() == 0);

    // Neighbors of point 6.
    BOOST_REQUIRE(sortedOutput[newFromOld[6]].size() == 0);

    // Neighbors of point 7.
    BOOST_REQUIRE(sortedOutput[newFromOld[7]].size() == 0);

    // Neighbors of point 8.
    BOOST_REQUIRE(sortedOutput[newFromOld[8]].size() == 1);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][0].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][0].first, 0.80, 1e-5);

    // Neighbors of point 9.
    BOOST_REQUIRE(sortedOutput[newFromOld[9]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][0].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][0].first, 0.75, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][1].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][1].first, 0.85, 1e-5);

    // Neighbors of point 10.
    BOOST_REQUIRE(sortedOutput[newFromOld[10]].size() == 2);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][0].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][0].first, 0.85, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][1].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][1].first, 0.95, 1e-5);

    // Now do it again with a different range: [1.0 inf].
    if (rs->ReferenceTree())
      CleanTree(*rs->ReferenceTree());
    rs->Search(Range(1.0, numeric_limits<double>::infinity()), neighbors,
        distances);
    SortResults(neighbors, distances, sortedOutput);

    // Neighbors of point 0.
    BOOST_REQUIRE(sortedOutput[newFromOld[0]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][0].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][0].first, 1.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][1].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][1].first, 1.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][2].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][2].first, 2.05, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[0]][3].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[0]][3].first, 5.00, 1e-5);

    // Neighbors of point 1.
    BOOST_REQUIRE(sortedOutput[newFromOld[1]].size() == 3);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][0].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][0].first, 1.65, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][1].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][1].first, 2.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[1]][2].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[1]][2].first, 4.70, 1e-5);

    // Neighbors of point 2.
    BOOST_REQUIRE(sortedOutput[newFromOld[2]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][0].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][0].first, 1.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][1].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][1].first, 1.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][2].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][2].first, 2.15, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[2]][3].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[2]][3].first, 4.90, 1e-5);

    // Neighbors of point 3.
    BOOST_REQUIRE(sortedOutput[newFromOld[3]].size() == 6);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][0].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][0].first, 1.10, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][1].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][1].first, 1.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][2].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][2].first, 1.47, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][3].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][3].first, 2.55, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][4].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][4].first, 3.25, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[3]][5].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[3]][5].first, 3.80, 1e-5);

    // Neighbors of point 4.
    BOOST_REQUIRE(sortedOutput[newFromOld[4]].size() == 10);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][0].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][0].first, 3.80, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][1].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][1].first, 4.05, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][2].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][2].first, 4.15, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][3].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][3].first, 4.60, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][4].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][4].first, 4.70, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][5].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][5].first, 4.90, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][6].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][6].first, 5.00, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][7].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][7].first, 5.27, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][8].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][8].first, 6.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[4]][9].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[4]][9].first, 7.05, 1e-5);

    // Neighbors of point 5.
    BOOST_REQUIRE(sortedOutput[newFromOld[5]].size() == 6);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][0].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][0].first, 1.08, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][1].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][1].first, 1.12, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][2].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][2].first, 1.22, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][3].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][3].first, 1.47, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][4].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][4].first, 1.78, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[5]][5].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[5]][5].first, 5.27, 1e-5);

    // Neighbors of point 6.
    BOOST_REQUIRE(sortedOutput[newFromOld[6]].size() == 9);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][0].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][0].first, 1.78, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][1].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][1].first, 2.05, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][2].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][2].first, 2.15, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][3].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][3].first, 2.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][4].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][4].first, 2.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][5].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][5].first, 2.90, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][6].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][6].first, 3.00, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][7].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][7].first, 3.25, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[6]][8].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[6]][8].first, 7.05, 1e-5);

    // Neighbors of point 7.
    BOOST_REQUIRE(sortedOutput[newFromOld[7]].size() == 9);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][0].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][0].first, 1.08, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][1].second == newFromOld[0]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][1].first, 1.35, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][2].second == newFromOld[2]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][2].first, 1.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][3].second == newFromOld[1]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][3].first, 1.65, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][4].second == newFromOld[8]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][4].first, 1.75, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][5].second == newFromOld[9]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][5].first, 2.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][6].second == newFromOld[10]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][6].first, 2.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][7].second == newFromOld[3]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][7].first, 2.55, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[7]][8].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[7]][8].first, 6.35, 1e-5);

    // Neighbors of point 8.
    BOOST_REQUIRE(sortedOutput[newFromOld[8]].size() == 3);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][0].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][0].first, 1.75, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][1].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][1].first, 2.45, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[8]][2].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[8]][2].first, 4.60, 1e-5);

    // Neighbors of point 9.
    BOOST_REQUIRE(sortedOutput[newFromOld[9]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][0].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][0].first, 1.12, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][1].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][1].first, 2.20, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][2].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][2].first, 2.90, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[9]][3].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[9]][3].first, 4.15, 1e-5);

    // Neighbors of point 10.
    BOOST_REQUIRE(sortedOutput[newFromOld[10]].size() == 4);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][0].second == newFromOld[5]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][0].first, 1.22, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][1].second == newFromOld[7]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][1].first, 2.30, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][2].second == newFromOld[6]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][2].first, 3.00, 1e-5);
    BOOST_REQUIRE(sortedOutput[newFromOld[10]][3].second == newFromOld[4]);
    BOOST_REQUIRE_CLOSE(sortedOutput[newFromOld[10]][3].first, 4.05, 1e-5);

    // Clean the memory.
    delete rs;
  }

  delete tree;
}

/**
 * Test the dual-tree range search method with the naive method.  This
 * uses both a query and reference dataset.
 *
 * Errors are produced if the results are not identical.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsNaive1)
{
  arma::mat dataForTree;

  // Hard-coded filename: bad!
  if (!data::Load("test_data_3_1000.csv", dataForTree))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Set up matrices to work with.
  arma::mat dualQuery(dataForTree);
  arma::mat dualReferences(dataForTree);
  arma::mat naiveQuery(dataForTree);
  arma::mat naiveReferences(dataForTree);

  RangeSearch<> rs(dualReferences);

  RangeSearch<> naive(naiveReferences, true);

  vector<vector<size_t>> neighborsTree;
  vector<vector<double>> distancesTree;
  rs.Search(dualQuery, Range(0.25, 1.05), neighborsTree, distancesTree);
  vector<vector<pair<double, size_t>>> sortedTree;
  SortResults(neighborsTree, distancesTree, sortedTree);

  vector<vector<size_t>> neighborsNaive;
  vector<vector<double>> distancesNaive;
  naive.Search(naiveQuery, Range(0.25, 1.05), neighborsNaive, distancesNaive);
  vector<vector<pair<double, size_t>>> sortedNaive;
  SortResults(neighborsNaive, distancesNaive, sortedNaive);

  for (size_t i = 0; i < sortedTree.size(); ++i)
  {
    BOOST_REQUIRE(sortedTree[i].size() == sortedNaive[i].size());

    for (size_t j = 0; j < sortedTree[i].size(); ++j)
    {
      BOOST_REQUIRE(sortedTree[i][j].second == sortedNaive[i][j].second);
      BOOST_REQUIRE_CLOSE(sortedTree[i][j].first, sortedNaive[i][j].first,
          1e-5);
    }
  }
}

/**
 * Test the dual-tree range search method with the naive method.  This uses
 * only a reference dataset.
 *
 * Errors are produced if the results are not identical.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsNaive2)
{
  arma::mat dataForTree;

  // Hard-coded filename: bad!
  // Code duplication: also bad!
  if (!data::Load("test_data_3_1000.csv", dataForTree))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Set up matrices to work with.
  arma::mat dualQuery(dataForTree);
  arma::mat naiveQuery(dataForTree);

  RangeSearch<> rs(dualQuery);

  // Set naive mode.
  RangeSearch<> naive(naiveQuery, true);

  vector<vector<size_t>> neighborsTree;
  vector<vector<double>> distancesTree;
  rs.Search(Range(0.25, 1.05), neighborsTree, distancesTree);
  vector<vector<pair<double, size_t>>> sortedTree;
  SortResults(neighborsTree, distancesTree, sortedTree);

  vector<vector<size_t>> neighborsNaive;
  vector<vector<double>> distancesNaive;
  naive.Search(Range(0.25, 1.05), neighborsNaive, distancesNaive);
  vector<vector<pair<double, size_t>>> sortedNaive;
  SortResults(neighborsNaive, distancesNaive, sortedNaive);

  for (size_t i = 0; i < sortedTree.size(); ++i)
  {
    BOOST_REQUIRE(sortedTree[i].size() == sortedNaive[i].size());

    for (size_t j = 0; j < sortedTree[i].size(); ++j)
    {
      BOOST_REQUIRE(sortedTree[i][j].second == sortedNaive[i][j].second);
      BOOST_REQUIRE_CLOSE(sortedTree[i][j].first, sortedNaive[i][j].first,
          1e-5);
    }
  }
}

/**
 * Test the single-tree range search method with the naive method.  This
 * uses only a reference dataset.
 *
 * Errors are produced if the results are not identical.
 */
BOOST_AUTO_TEST_CASE(SingleTreeVsNaive)
{
  arma::mat dataForTree;

  // Hard-coded filename: bad!
  // Code duplication: also bad!
  if (!data::Load("test_data_3_1000.csv", dataForTree))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Set up matrices to work with (may not be necessary with no ALIAS_MATRIX?).
  arma::mat singleQuery(dataForTree);
  arma::mat naiveQuery(dataForTree);

  RangeSearch<> single(singleQuery, false, true);

  // Set up computation for naive mode.
  RangeSearch<> naive(naiveQuery, true);

  vector<vector<size_t>> neighborsSingle;
  vector<vector<double>> distancesSingle;
  single.Search(Range(0.25, 1.05), neighborsSingle, distancesSingle);
  vector<vector<pair<double, size_t>>> sortedTree;
  SortResults(neighborsSingle, distancesSingle, sortedTree);

  vector<vector<size_t>> neighborsNaive;
  vector<vector<double>> distancesNaive;
  naive.Search(Range(0.25, 1.05), neighborsNaive, distancesNaive);
  vector<vector<pair<double, size_t>>> sortedNaive;
  SortResults(neighborsNaive, distancesNaive, sortedNaive);

  for (size_t i = 0; i < sortedTree.size(); ++i)
  {
    BOOST_REQUIRE(sortedTree[i].size() == sortedNaive[i].size());

    for (size_t j = 0; j < sortedTree[i].size(); ++j)
    {
      BOOST_REQUIRE(sortedTree[i][j].second == sortedNaive[i][j].second);
      BOOST_REQUIRE_CLOSE(sortedTree[i][j].first, sortedNaive[i][j].first,
          1e-5);
    }
  }
}

/**
 * Ensure that dual tree range search with cover trees works by comparing
 * with the kd-tree implementation.
 */
BOOST_AUTO_TEST_CASE(CoverTreeTest)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.

  // Set up cover tree range search.
  RangeSearch<EuclideanDistance, arma::mat, StandardCoverTree>
      coversearch(data);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.5, 1.5);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(0.8, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for cover tree search.
    vector<vector<size_t>> coverNeighbors;
    vector<vector<double>> coverDistances;

    // Clean the tree statistics.
    CleanTree(*coversearch.ReferenceTree());

    // Run the searches.
    kdsearch.Search(range, kdNeighbors, kdDistances);
    coversearch.Search(range, coverNeighbors, coverDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> coverSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(coverNeighbors, coverDistances, coverSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, coverSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, coverSorted[i][j].first,
            1e-5);
      }
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), coverSorted[i].size());
    }
  }
}

/**
 * Ensure that dual tree range search with cover trees works when using
 * two datasets.
 */
BOOST_AUTO_TEST_CASE(CoverTreeTwoDatasetsTest)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::mat queries;
  queries.randu(8, 350); // 350 points in 8 dimensions.

  // Set up cover tree range search.
  RangeSearch<EuclideanDistance, arma::mat, StandardCoverTree>
      coversearch(data);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.  We don't have an easy way to rebuild the
    // tree, so we'll just reinstantiate it here each loop time.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.85, 1.05);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(1.35, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for cover tree search.
    vector<vector<size_t>> coverNeighbors;
    vector<vector<double>> coverDistances;

    // Clean the trees.
    CleanTree(*coversearch.ReferenceTree());

    // Run the searches.
    coversearch.Search(queries, range, coverNeighbors, coverDistances);
    kdsearch.Search(queries, range, kdNeighbors, kdDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> coverSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(coverNeighbors, coverDistances, coverSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, coverSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, coverSorted[i][j].first,
            1e-5);
      }
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), coverSorted[i].size());
    }
  }
}

/**
 * Ensure that single-tree cover tree range search works.
 */
BOOST_AUTO_TEST_CASE(CoverTreeSingleTreeTest)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.

  // Set up cover tree range search.
  RangeSearch<EuclideanDistance, arma::mat, StandardCoverTree>
      coversearch(data, false, true);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.5, 1.5);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(0.8, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for cover tree search.
    vector<vector<size_t>> coverNeighbors;
    vector<vector<double>> coverDistances;

    // Clean the tree statistics.
    CleanTree(*coversearch.ReferenceTree());

    // Run the searches.
    kdsearch.Search(range, kdNeighbors, kdDistances);
    coversearch.Search(range, coverNeighbors, coverDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> coverSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(coverNeighbors, coverDistances, coverSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, coverSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, coverSorted[i][j].first,
            1e-5);
      }
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), coverSorted[i].size());
    }
  }
}

/**
 * Ensure that single-tree ball tree range search works.
 */
BOOST_AUTO_TEST_CASE(SingleBallTreeTest)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.

  // Set up ball tree range search.
  RangeSearch<EuclideanDistance, arma::mat, BallTree> ballsearch(data, false,
      true);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.5, 1.5);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(0.8, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for ball tree search.
    vector<vector<size_t>> ballNeighbors;
    vector<vector<double>> ballDistances;

    // Clean the tree statistics.
    CleanTree(*ballsearch.ReferenceTree());

    // Run the searches.
    kdsearch.Search(range, kdNeighbors, kdDistances);
    ballsearch.Search(range, ballNeighbors, ballDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> ballSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(ballNeighbors, ballDistances, ballSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, ballSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, ballSorted[i][j].first,
            1e-5);
      }
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), ballSorted[i].size());
    }
  }
}

/**
 * Ensure that dual tree range search with ball trees works by comparing
 * with the kd-tree implementation.
 */
BOOST_AUTO_TEST_CASE(DualBallTreeTest)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.

  // Set up ball tree range search.
  RangeSearch<EuclideanDistance, arma::mat, BallTree> ballsearch(data);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.5, 1.5);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(0.8, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for ball tree search.
    vector<vector<size_t>> ballNeighbors;
    vector<vector<double>> ballDistances;

    // Clean the tree statistics.
    CleanTree(*ballsearch.ReferenceTree());

    // Run the searches.
    kdsearch.Search(range, kdNeighbors, kdDistances);
    ballsearch.Search(range, ballNeighbors, ballDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> ballSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(ballNeighbors, ballDistances, ballSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, ballSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, ballSorted[i][j].first,
            1e-5);
      }
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), ballSorted[i].size());
    }
  }
}

/**
 * Ensure that dual tree range search with ball trees works when using
 * two datasets.
 */
BOOST_AUTO_TEST_CASE(DualBallTreeTest2)
{
  arma::mat data;
  data.randu(8, 1000); // 1000 points in 8 dimensions.

  arma::mat queries;
  queries.randu(8, 350); // 350 points in 8 dimensions.

  // Set up ball tree range search.
  RangeSearch<EuclideanDistance, arma::mat, BallTree> ballsearch(data);

  // Four trials with different ranges.
  for (size_t r = 0; r < 4; ++r)
  {
    // Set up kd-tree range search.  We don't have an easy way to rebuild the
    // tree, so we'll just reinstantiate it here each loop time.
    RangeSearch<> kdsearch(data);

    Range range;
    switch (r)
    {
      case 0:
        // Includes zero distance.
        range = Range(0.0, 0.75);
        break;
      case 1:
        // A bounded range on both sides.
        range = Range(0.85, 1.05);
        break;
      case 2:
        // A range with no upper bound.
        range = Range(1.35, DBL_MAX);
        break;
      case 3:
        // A range which should have no results.
        range = Range(15.6, 15.7);
        break;
    }

    // Results for kd-tree search.
    vector<vector<size_t>> kdNeighbors;
    vector<vector<double>> kdDistances;

    // Results for ball tree search.
    vector<vector<size_t>> ballNeighbors;
    vector<vector<double>> ballDistances;

    // Clean the trees.
    CleanTree(*ballsearch.ReferenceTree());

    // Run the searches.
    ballsearch.Search(queries, range, ballNeighbors, ballDistances);
    kdsearch.Search(queries, range, kdNeighbors, kdDistances);

    // Sort before comparison.
    vector<vector<pair<double, size_t>>> kdSorted;
    vector<vector<pair<double, size_t>>> ballSorted;
    SortResults(kdNeighbors, kdDistances, kdSorted);
    SortResults(ballNeighbors, ballDistances, ballSorted);

    // Now compare the results.
    for (size_t i = 0; i < kdSorted.size(); ++i)
    {
      BOOST_REQUIRE_EQUAL(kdSorted[i].size(), ballSorted[i].size());
      for (size_t j = 0; j < kdSorted[i].size(); ++j)
      {
        BOOST_REQUIRE_EQUAL(kdSorted[i][j].second, ballSorted[i][j].second);
        BOOST_REQUIRE_CLOSE(kdSorted[i][j].first, ballSorted[i][j].first,
            1e-5);
      }
    }
  }
}

/**
 * Make sure that no results are returned when we build a range search object
 * with no reference set.
 */
BOOST_AUTO_TEST_CASE(EmptySearchTest)
{
  RangeSearch<EuclideanDistance, arma::mat, KDTree> rs;

  vector<vector<size_t>> neighbors;
  vector<vector<double>> distances;

  rs.Search(math::Range(0.0, 10.0), neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.size(), 0);
  BOOST_REQUIRE_EQUAL(distances.size(), 0);

  // Now check with a query set.
  arma::mat querySet = arma::randu<arma::mat>(3, 100);

  BOOST_REQUIRE_THROW(rs.Search(querySet, math::Range(0.0, 10.0), neighbors,
      distances), std::invalid_argument);
}

/**
 * Make sure things work right after Train() is called.
 */
BOOST_AUTO_TEST_CASE(TrainTest)
{
  RangeSearch<> empty;

  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  RangeSearch<> baseline(dataset);

  vector<vector<size_t>> neighbors, baselineNeighbors;
  vector<vector<double>> distances, baselineDistances;

  empty.Train(dataset);

  empty.Search(math::Range(0.5, 0.7), neighbors, distances);
  baseline.Search(math::Range(0.5, 0.7), baselineNeighbors, baselineDistances);

  BOOST_REQUIRE_EQUAL(neighbors.size(), baselineNeighbors.size());
  BOOST_REQUIRE_EQUAL(distances.size(), baselineDistances.size());

  // Sort the results before comparing.
  vector<vector<pair<double, size_t>>> sorted;
  vector<vector<pair<double, size_t>>> baselineSorted;
  SortResults(neighbors, distances, sorted);
  SortResults(baselineNeighbors, baselineDistances, baselineSorted);

  for (size_t i = 0; i < sorted.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(sorted[i].size(), baselineSorted[i].size());
    for (size_t j = 0; j < sorted[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(sorted[i][j].second, baselineSorted[i][j].second);
      BOOST_REQUIRE_CLOSE(sorted[i][j].first, baselineSorted[i][j].first, 1e-5);
    }
  }
}

/**
 * Test training when a tree is given.
 */
BOOST_AUTO_TEST_CASE(TrainTreeTest)
{
  // Avoid mappings by using the cover tree.
  typedef RangeSearch<EuclideanDistance, arma::mat, StandardCoverTree> RSType;
  RSType empty;

  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  RSType baseline(dataset);

  vector<vector<size_t>> neighbors, baselineNeighbors;
  vector<vector<double>> distances, baselineDistances;

  RSType::Tree tree(dataset);
  empty.Train(&tree);

  empty.Search(math::Range(0.5, 0.7), neighbors, distances);
  baseline.Search(math::Range(0.5, 0.7), baselineNeighbors, baselineDistances);

  BOOST_REQUIRE_EQUAL(neighbors.size(), baselineNeighbors.size());
  BOOST_REQUIRE_EQUAL(distances.size(), baselineDistances.size());

  // Sort the results before comparing.
  vector<vector<pair<double, size_t>>> sorted;
  vector<vector<pair<double, size_t>>> baselineSorted;
  SortResults(neighbors, distances, sorted);
  SortResults(baselineNeighbors, baselineDistances, baselineSorted);

  for (size_t i = 0; i < sorted.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(sorted[i].size(), baselineSorted[i].size());
    for (size_t j = 0; j < sorted[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(sorted[i][j].second, baselineSorted[i][j].second);
      BOOST_REQUIRE_CLOSE(sorted[i][j].first, baselineSorted[i][j].first, 1e-5);
    }
  }
}

/**
 * Test that training with a tree throws an exception when in naive mode.
 */
BOOST_AUTO_TEST_CASE(NaiveTrainTreeTest)
{
  RangeSearch<> empty(true);

  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  RangeSearch<>::Tree tree(dataset);

  BOOST_REQUIRE_THROW(empty.Train(&tree), std::invalid_argument);
}

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

  RangeSearch<> movers(std::move(copy));
  RangeSearch<> rs(dataset);

  BOOST_REQUIRE_EQUAL(copy.n_elem, 0);
  BOOST_REQUIRE_EQUAL(movers.ReferenceSet().n_rows, 3);
  BOOST_REQUIRE_EQUAL(movers.ReferenceSet().n_cols, 100);

  vector<vector<size_t>> moveNeighbors, neighbors;
  vector<vector<double>> moveDistances, distances;

  movers.Search(math::Range(0.5, 0.7), moveNeighbors, moveDistances);
  rs.Search(math::Range(0.5, 0.7), neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.size(), moveNeighbors.size());
  BOOST_REQUIRE_EQUAL(distances.size(), moveDistances.size());

  // Sort the results before comparing.
  vector<vector<pair<double, size_t>>> sorted;
  vector<vector<pair<double, size_t>>> moveSorted;
  SortResults(neighbors, distances, sorted);
  SortResults(moveNeighbors, moveDistances, moveSorted);

  for (size_t i = 0; i < sorted.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(sorted[i].size(), moveSorted[i].size());
    for (size_t j = 0; j < sorted[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(sorted[i][j].second, moveSorted[i][j].second);
      BOOST_REQUIRE_CLOSE(sorted[i][j].first, moveSorted[i][j].first, 1e-5);
    }
  }
}

/**
 * Test that the std::move() Train() function works.
 */
BOOST_AUTO_TEST_CASE(MoveTrainTest)
{
  arma::mat dataset = arma::randu<arma::mat>(3, 100);
  arma::mat copy(dataset);

  RangeSearch<> movers;
  movers.Train(std::move(copy));
  RangeSearch<> rs(dataset);

  BOOST_REQUIRE_EQUAL(copy.n_elem, 0);
  BOOST_REQUIRE_EQUAL(movers.ReferenceSet().n_rows, 3);
  BOOST_REQUIRE_EQUAL(movers.ReferenceSet().n_cols, 100);

  vector<vector<size_t>> moveNeighbors, neighbors;
  vector<vector<double>> moveDistances, distances;

  movers.Search(math::Range(0.5, 0.7), moveNeighbors, moveDistances);
  rs.Search(math::Range(0.5, 0.7), neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.size(), moveNeighbors.size());
  BOOST_REQUIRE_EQUAL(distances.size(), moveDistances.size());

  // Sort the results before comparing.
  vector<vector<pair<double, size_t>>> sorted;
  vector<vector<pair<double, size_t>>> moveSorted;
  SortResults(neighbors, distances, sorted);
  SortResults(moveNeighbors, moveDistances, moveSorted);

  for (size_t i = 0; i < sorted.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(sorted[i].size(), moveSorted[i].size());
    for (size_t j = 0; j < sorted[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(sorted[i][j].second, moveSorted[i][j].second);
      BOOST_REQUIRE_CLOSE(sorted[i][j].first, moveSorted[i][j].first, 1e-5);
    }
  }
}

BOOST_AUTO_TEST_CASE(RSModelTest)
{
  // Ensure that we can build an RSModel and get correct results.
  arma::mat queryData = arma::randu<arma::mat>(10, 50);
  arma::mat referenceData = arma::randu<arma::mat>(10, 200);

  // Build all the possible models.
  RSModel models[28];
  models[0] = RSModel(RSModel::TreeTypes::KD_TREE, true);
  models[1] = RSModel(RSModel::TreeTypes::KD_TREE, false);
  models[2] = RSModel(RSModel::TreeTypes::COVER_TREE, true);
  models[3] = RSModel(RSModel::TreeTypes::COVER_TREE, false);
  models[4] = RSModel(RSModel::TreeTypes::R_TREE, true);
  models[5] = RSModel(RSModel::TreeTypes::R_TREE, false);
  models[6] = RSModel(RSModel::TreeTypes::R_STAR_TREE, true);
  models[7] = RSModel(RSModel::TreeTypes::R_STAR_TREE, false);
  models[8] = RSModel(RSModel::TreeTypes::X_TREE, true);
  models[9] = RSModel(RSModel::TreeTypes::X_TREE, false);
  models[10] = RSModel(RSModel::TreeTypes::BALL_TREE, true);
  models[11] = RSModel(RSModel::TreeTypes::BALL_TREE, false);
  models[12] = RSModel(RSModel::TreeTypes::HILBERT_R_TREE, true);
  models[13] = RSModel(RSModel::TreeTypes::HILBERT_R_TREE, false);
  models[14] = RSModel(RSModel::TreeTypes::R_PLUS_TREE, true);
  models[15] = RSModel(RSModel::TreeTypes::R_PLUS_TREE, false);
  models[16] = RSModel(RSModel::TreeTypes::R_PLUS_PLUS_TREE, true);
  models[17] = RSModel(RSModel::TreeTypes::R_PLUS_PLUS_TREE, false);
  models[18] = RSModel(RSModel::TreeTypes::VP_TREE, true);
  models[19] = RSModel(RSModel::TreeTypes::VP_TREE, false);
  models[20] = RSModel(RSModel::TreeTypes::RP_TREE, true);
  models[21] = RSModel(RSModel::TreeTypes::RP_TREE, false);
  models[22] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, true);
  models[23] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, false);
  models[24] = RSModel(RSModel::TreeTypes::UB_TREE, true);
  models[25] = RSModel(RSModel::TreeTypes::UB_TREE, false);
  models[26] = RSModel(RSModel::TreeTypes::OCTREE, true);
  models[27] = RSModel(RSModel::TreeTypes::OCTREE, false);

  for (size_t j = 0; j < 2; ++j)
  {
    // Get a baseline.
    RangeSearch<> rs(referenceData);
    vector<vector<size_t>> baselineNeighbors;
    vector<vector<double>> baselineDistances;
    rs.Search(queryData, math::Range(0.25, 0.75), baselineNeighbors,
        baselineDistances);

    vector<vector<pair<double, size_t>>> baselineSorted;
    SortResults(baselineNeighbors, baselineDistances, baselineSorted);

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

      vector<vector<size_t>> neighbors;
      vector<vector<double>> distances;

      models[i].Search(std::move(queryCopy), math::Range(0.25, 0.75), neighbors,
          distances);

      BOOST_REQUIRE_EQUAL(neighbors.size(), baselineNeighbors.size());
      BOOST_REQUIRE_EQUAL(distances.size(), baselineDistances.size());

      vector<vector<pair<double, size_t>>> sorted;
      SortResults(neighbors, distances, sorted);

      for (size_t k = 0; k < sorted.size(); ++k)
      {
        BOOST_REQUIRE_EQUAL(sorted[k].size(), baselineSorted[k].size());
        for (size_t l = 0; l < sorted[k].size(); ++l)
        {
          BOOST_REQUIRE_EQUAL(sorted[k][l].second, baselineSorted[k][l].second);
          BOOST_REQUIRE_CLOSE(sorted[k][l].first, baselineSorted[k][l].first,
              1e-5);
        }
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(RSModelMonochromaticTest)
{
  // Ensure that we can build an RSModel and get correct results.
  arma::mat referenceData = arma::randu<arma::mat>(10, 200);

  // Build all the possible models.
  RSModel models[28];
  models[0] = RSModel(RSModel::TreeTypes::KD_TREE, true);
  models[1] = RSModel(RSModel::TreeTypes::KD_TREE, false);
  models[2] = RSModel(RSModel::TreeTypes::COVER_TREE, true);
  models[3] = RSModel(RSModel::TreeTypes::COVER_TREE, false);
  models[4] = RSModel(RSModel::TreeTypes::R_TREE, true);
  models[5] = RSModel(RSModel::TreeTypes::R_TREE, false);
  models[6] = RSModel(RSModel::TreeTypes::R_STAR_TREE, true);
  models[7] = RSModel(RSModel::TreeTypes::R_STAR_TREE, false);
  models[8] = RSModel(RSModel::TreeTypes::X_TREE, true);
  models[9] = RSModel(RSModel::TreeTypes::X_TREE, false);
  models[10] = RSModel(RSModel::TreeTypes::BALL_TREE, true);
  models[11] = RSModel(RSModel::TreeTypes::BALL_TREE, false);
  models[12] = RSModel(RSModel::TreeTypes::HILBERT_R_TREE, true);
  models[13] = RSModel(RSModel::TreeTypes::HILBERT_R_TREE, false);
  models[14] = RSModel(RSModel::TreeTypes::R_PLUS_TREE, true);
  models[15] = RSModel(RSModel::TreeTypes::R_PLUS_TREE, false);
  models[16] = RSModel(RSModel::TreeTypes::R_PLUS_PLUS_TREE, true);
  models[17] = RSModel(RSModel::TreeTypes::R_PLUS_PLUS_TREE, false);
  models[18] = RSModel(RSModel::TreeTypes::VP_TREE, true);
  models[19] = RSModel(RSModel::TreeTypes::VP_TREE, false);
  models[20] = RSModel(RSModel::TreeTypes::RP_TREE, true);
  models[21] = RSModel(RSModel::TreeTypes::RP_TREE, false);
  models[22] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, true);
  models[23] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, false);
  models[24] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, true);
  models[25] = RSModel(RSModel::TreeTypes::MAX_RP_TREE, false);
  models[26] = RSModel(RSModel::TreeTypes::OCTREE, true);
  models[27] = RSModel(RSModel::TreeTypes::OCTREE, false);

  for (size_t j = 0; j < 2; ++j)
  {
    // Get a baseline.
    RangeSearch<> rs(referenceData);
    vector<vector<size_t>> baselineNeighbors;
    vector<vector<double>> baselineDistances;
    rs.Search(math::Range(0.25, 0.5), baselineNeighbors, baselineDistances);

    vector<vector<pair<double, size_t>>> baselineSorted;
    SortResults(baselineNeighbors, baselineDistances, baselineSorted);

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

      vector<vector<size_t>> neighbors;
      vector<vector<double>> distances;

      models[i].Search(math::Range(0.25, 0.5), neighbors, distances);

      BOOST_REQUIRE_EQUAL(neighbors.size(), baselineNeighbors.size());
      BOOST_REQUIRE_EQUAL(distances.size(), baselineDistances.size());

      vector<vector<pair<double, size_t>>> sorted;
      SortResults(neighbors, distances, sorted);

      for (size_t k = 0; k < sorted.size(); ++k)
      {
        BOOST_REQUIRE_EQUAL(sorted[k].size(), baselineSorted[k].size());
        for (size_t l = 0; l < sorted[k].size(); ++l)
        {
          BOOST_REQUIRE_EQUAL(sorted[k][l].second, baselineSorted[k][l].second);
          BOOST_REQUIRE_CLOSE(sorted[k][l].first, baselineSorted[k][l].first,
              1e-5);
        }
      }
    }
  }
}

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

  // Build the tree ourselves.
  vector<size_t> oldFromNewReferences;
  RangeSearch<>::Tree tree(dataset);
  RangeSearch<> ra(&tree);

  // Now make a query set.
  arma::mat queryset = arma::randu<arma::mat>(5, 50);
  vector<vector<double>> distances;
  vector<vector<size_t>> neighbors;
  ra.Search(queryset, math::Range(0.2, 0.5), neighbors, distances);

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

/**
 * Test copy constructor and copy operator.
 */
BOOST_AUTO_TEST_CASE(CopyConstructorAndOperatorTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<> rs(std::move(dataset));

  // Copy constructor and operator.
  RangeSearch<> rs2(rs);
  RangeSearch<> rs3 = rs;

  // Get results.
  vector<vector<double>> distances, distances2, distances3;
  vector<vector<size_t>> neighbors, neighbors2, neighbors3;

  rs.Search(math::Range(0.2, 0.3), neighbors, distances);
  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);
  rs3.Search(math::Range(0.2, 0.3), neighbors3, distances3);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(distances.size(), distances3.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors3.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances3[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors3[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors3[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
      BOOST_REQUIRE_CLOSE(distances[i][j], distances3[i][j], 1e-5);
    }
  }
}

/**
 * Test move constructor.
 */
BOOST_AUTO_TEST_CASE(MoveConstructorTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<>* rs = new RangeSearch<>(std::move(dataset));

  // Get results.
  vector<vector<double>> distances, distances2;
  vector<vector<size_t>> neighbors, neighbors2;

  rs->Search(math::Range(0.2, 0.3), neighbors, distances);

  RangeSearch<> rs2(std::move(*rs));

  delete rs;

  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
    }
  }
}

/**
 * Test move operator.
 */
BOOST_AUTO_TEST_CASE(MoveOperatorTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<>* rs = new RangeSearch<>(std::move(dataset));

  // Get results.
  vector<vector<double>> distances, distances2;
  vector<vector<size_t>> neighbors, neighbors2;

  rs->Search(math::Range(0.2, 0.3), neighbors, distances);

  RangeSearch<> rs2 = std::move(*rs);

  delete rs;

  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
    }
  }
}

/**
 * Test copy constructor and copy operator in naive mode (so there are no
 * trees).
 */
BOOST_AUTO_TEST_CASE(CopyConstructorAndOperatorNaiveTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<> rs(std::move(dataset), true);

  // Copy constructor and operator.
  RangeSearch<> rs2(rs);
  RangeSearch<> rs3 = rs;

  BOOST_REQUIRE_EQUAL(rs2.Naive(), true);
  BOOST_REQUIRE_EQUAL(rs3.Naive(), true);

  // Get results.
  vector<vector<double>> distances, distances2, distances3;
  vector<vector<size_t>> neighbors, neighbors2, neighbors3;

  rs.Search(math::Range(0.2, 0.3), neighbors, distances);
  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);
  rs3.Search(math::Range(0.2, 0.3), neighbors3, distances3);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(distances.size(), distances3.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors3.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances3[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors3[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors3[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
      BOOST_REQUIRE_CLOSE(distances[i][j], distances3[i][j], 1e-5);
    }
  }
}

/**
 * Test move constructor.
 */
BOOST_AUTO_TEST_CASE(MoveConstructorNaiveTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<>* rs = new RangeSearch<>(std::move(dataset), true);

  // Get results.
  vector<vector<double>> distances, distances2;
  vector<vector<size_t>> neighbors, neighbors2;

  rs->Search(math::Range(0.2, 0.3), neighbors, distances);

  RangeSearch<> rs2(std::move(*rs));

  BOOST_REQUIRE_EQUAL(rs2.Naive(), true);

  delete rs;

  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
    }
  }
}

/**
 * Test move operator.
 */
BOOST_AUTO_TEST_CASE(MoveOperatorNaiveTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 500);
  RangeSearch<>* rs = new RangeSearch<>(std::move(dataset), true);

  // Get results.
  vector<vector<double>> distances, distances2;
  vector<vector<size_t>> neighbors, neighbors2;

  rs->Search(math::Range(0.2, 0.3), neighbors, distances);

  RangeSearch<> rs2 = std::move(*rs);

  BOOST_REQUIRE_EQUAL(rs2.Naive(), true);

  delete rs;

  rs2.Search(math::Range(0.2, 0.3), neighbors2, distances2);

  // Check results.
  BOOST_REQUIRE_EQUAL(distances.size(), distances2.size());
  BOOST_REQUIRE_EQUAL(neighbors.size(), neighbors2.size());

  for (size_t i = 0; i < neighbors.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(distances[i].size(), distances2[i].size());
    BOOST_REQUIRE_EQUAL(neighbors[i].size(), neighbors2[i].size());

    for (size_t j = 0; j < neighbors[i].size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(neighbors[i][j], neighbors2[i][j]);

      // Distances will always be between 0.2 and 0.3.
      BOOST_REQUIRE_CLOSE(distances[i][j], distances2[i][j], 1e-5);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <kdl/frames.hpp>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace dr {

/// Convert a KDL vector to an Eigen vector.
Eigen::Vector3d toEigen(KDL::Vector const & input) {
	return Eigen::Vector3d{input[0], input[1], input[2]};
}

/// Convert an Eigen vector to a KDL vector.
KDL::Vector toKdlVector(Eigen::Vector3d const & input) {
	return KDL::Vector{input.x(), input.y(), input.z()};
}

/// Convert a KDL rotation to an Eigen quaternion.
Eigen::Quaterniond toEigen(KDL::Rotation const & input) {
	Eigen::Quaterniond result;
	input.GetQuaternion(result.x(), result.y(), result.z(), result.w());
	return result;
}

/// Convert a KDL rotation to an Eigen rotation matrix.
Eigen::Matrix3d toEigenMatrix(KDL::Rotation const & input) {
	return (Eigen::Matrix3d{} <<
		input(0, 0),  input(0, 1), input(0, 2),
		input(1, 0),  input(1, 1), input(1, 2),
		input(2, 0),  input(2, 1), input(2, 2)
	).finished();
}

/// Convert an Eigen quaternion to a KDL rotation.
KDL::Rotation toKdlRotation(Eigen::Quaterniond const & input) {
	return KDL::Rotation::Quaternion(input.x(), input.y(), input.z(), input.w());
}

/// Convert an Eigen rotation matrix to a KDL rotation.
KDL::Rotation toKdlRotation(Eigen::Matrix3d const & input) {
	KDL::Rotation result;
	for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) {
		result(i, j) = input(i, j);
	}
	return result;
}

/// Convert a KDL frame to an Eigen isometry.
Eigen::Isometry3d toEigen(KDL::Frame const & input) {
	return Eigen::Translation3d(toEigen(input.p)) * toEigen(input.M);
}

/// Convert an Eigen isometry to a KDL frame.
KDL::Frame toKdlFrame(Eigen::Isometry3d const & input) {
	return {toKdlRotation(input.rotation()), toKdlVector(Eigen::Vector3d{input.translation()})};
}

}