// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2023.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Johannes Veit $
// $Authors: Johannes Veit $
// --------------------------------------------------------------------------

/* Note: This file contains a concatenation of the 6 header files comprising
 *       libkdtree++ (node.hpp, function.hpp, allocator.hpp, iterator.hpp,
 *       region.hpp, kdtree.hpp).
 *
 *       Johannes Veit (2016)
 */

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

/** \file
 * Defines interfaces for nodes as used by the KDTree class.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 */

#ifndef OPENMS_DATASTRUCTURES_KDTREE_H
#define OPENMS_DATASTRUCTURES_KDTREE_H

#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
#  include <ostream>
#endif

#include <cstddef>
#include <cmath>

namespace KDTree
{
struct _Node_base
{
  typedef _Node_base* _Base_ptr;
  typedef _Node_base const* _Base_const_ptr;

  _Base_ptr _M_parent;
  _Base_ptr _M_left;
  _Base_ptr _M_right;

  _Node_base(_Base_ptr const __PARENT = nullptr,
             _Base_ptr const __LEFT = nullptr,
             _Base_ptr const __RIGHT = nullptr)
    : _M_parent(__PARENT), _M_left(__LEFT), _M_right(__RIGHT) {}

  static _Base_ptr
  _S_minimum(_Base_ptr __x)
  {
    while (__x->_M_left) __x = __x->_M_left;
    return __x;
  }

  static _Base_ptr
  _S_maximum(_Base_ptr __x)
  {
    while (__x->_M_right) __x = __x->_M_right;
    return __x;
  }
};

template <typename _Val>
struct _Node : public _Node_base
{
  using _Node_base::_Base_ptr;
  typedef _Node* _Link_type;

  _Val _M_value;

  _Node(_Val const& __VALUE = _Val(),
        _Base_ptr const __PARENT = nullptr,
        _Base_ptr const __LEFT = nullptr,
        _Base_ptr const __RIGHT = nullptr)
    : _Node_base(__PARENT, __LEFT, __RIGHT), _M_value(__VALUE) {}

#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS

  template <typename Char, typename Traits>
  friend
  std::basic_ostream<Char, Traits>&
  operator<<(typename std::basic_ostream<Char, Traits>& out,
             _Node_base const& node)
  {
    out << &node;
    out << " parent: " << node._M_parent;
    out << "; left: " << node._M_left;
    out << "; right: " << node._M_right;
    return out;
  }

  template <typename Char, typename Traits>
  friend
  std::basic_ostream<Char, Traits>&
  operator<<(typename std::basic_ostream<Char, Traits>& out,
             _Node<_Val> const& node)
  {
    out << &node;
    out << ' ' << node._M_value;
    out << "; parent: " << node._M_parent;
    out << "; left: " << node._M_left;
    out << "; right: " << node._M_right;
    return out;
  }

#endif
};

template <typename _Val, typename _Acc, typename _Cmp>
class _Node_compare
{
public:
  _Node_compare(size_t const __DIM, _Acc const& acc, _Cmp const& cmp)
    : _M_DIM(__DIM), _M_acc(acc), _M_cmp(cmp) {}

  bool
  operator()(_Val const& __A, _Val const& __B) const
  {
    return _M_cmp(_M_acc(__A, _M_DIM), _M_acc(__B, _M_DIM));
  }

private:
  size_t _M_DIM;	// don't make this const so that an assignment operator can be auto-generated
  _Acc _M_acc;
  _Cmp _M_cmp;
};

/*! Compare two values on the same dimension using a comparison functor _Cmp
      and an accessor _Acc.

      The comparison functor and the accessor are references to the template
      parameters of the KDTree.
   */
template <typename _ValA, typename _ValB, typename _Cmp,
          typename _Acc>
inline
bool
_S_node_compare (const size_t __dim,
                 const _Cmp& __cmp, const _Acc& __acc,
                 const _ValA& __a, const _ValB& __b)
{
  return __cmp(__acc(__a, __dim), __acc(__b, __dim));
}

/*! Compute the distance between two values for one dimension only.

      The distance functor and the accessor are references to the template
      parameters of the KDTree.
   */
template <typename _ValA, typename _ValB, typename _Dist,
          typename _Acc>
inline
typename _Dist::distance_type
_S_node_distance (const size_t __dim,
                  const _Dist& __dist, const _Acc& __acc,
                  const _ValA& __a, const _ValB& __b)
{
  return __dist(__acc(__a, __dim), __acc(__b, __dim));
}

/*! Compute the distance between two values and accumulate the result for all
      dimensions.

      The distance functor and the accessor are references to the template
      parameters of the KDTree.
   */
template <typename _ValA, typename _ValB, typename _Dist,
          typename _Acc>
inline
typename _Dist::distance_type
_S_accumulate_node_distance (const size_t __dim,
                             const _Dist& __dist, const _Acc& __acc,
                             const _ValA& __a, const _ValB& __b)
{
  typename _Dist::distance_type d = 0;
  for (size_t i=0; i<__dim; ++i)
    d += __dist(__acc(__a, i), __acc(__b, i));
  return d;
}

/*! Descend on the left or the right of the node according to the comparison
      between the node's value and the value.

      \note it's the caller responsibility to check if node is NULL.
   */
template <typename _Val, typename _Cmp, typename _Acc, typename NodeType>
inline
const NodeType*
_S_node_descend (const size_t __dim,
                 const _Cmp& __cmp, const _Acc& __acc,
                 const _Val& __val, const NodeType* __node)
{
  if (_S_node_compare(__dim, __cmp, __acc, __val,  __node->_M_value))
    return static_cast<const NodeType *>(__node->_M_left);
  return static_cast<const NodeType *>(__node->_M_right);
}

/*! Find the nearest node to __val from __node

    If many nodes are equidistant to __val, the node with the lowest memory
    address is returned.

    \return the nearest node of __end node if no nearest node was found for the
    given arguments.
   */
template <class SearchVal,
          typename NodeType, typename _Cmp,
          typename _Acc, typename _Dist,
          typename _Predicate>
inline
std::pair<const NodeType*,
std::pair<size_t, typename _Dist::distance_type> >
_S_node_nearest (const size_t __k, size_t __dim, SearchVal const& __val,
                 const NodeType* __node, const _Node_base* __end,
                 const NodeType* __best, typename _Dist::distance_type __max,
                 const _Cmp& __cmp, const _Acc& __acc, const _Dist& __dist,
                 _Predicate __p)
{
  typedef const NodeType* NodePtr;
  NodePtr pcur = __node;
  NodePtr cur = _S_node_descend(__dim % __k, __cmp, __acc, __val, __node);
  size_t cur_dim = __dim+1;
  // find the smallest __max distance in direct descent
  while (cur)
  {
    if (__p(cur->_M_value))
    {
      typename _Dist::distance_type d = 0;
      for (size_t i=0; i != __k; ++i)
        d += _S_node_distance(i, __dist, __acc, __val, cur->_M_value);
      d = std::sqrt(d);
      if (d <= __max)
        // ("bad candidate notes")
        // Changed: removed this test: || ( d == __max && cur < __best ))
        // Can't do this optimisation without checking that the current 'best' is not the root AND is not a valid candidate...
        // This is because find_nearest() etc will call this function with the best set to _M_root EVEN IF _M_root is not a valid answer (eg too far away or doesn't pass the predicate test)
      {
        __best = cur;
        __max = d;
        __dim = cur_dim;
      }
    }
    pcur = cur;
    cur = _S_node_descend(cur_dim % __k, __cmp, __acc, __val, cur);
    ++cur_dim;
  }
  // Swap cur to prev, only prev is a valid node.
  cur = pcur;
  --cur_dim;
  pcur = NULL;
  // Probe all node's children not visited yet (siblings of the visited nodes).
  NodePtr probe = cur;
  NodePtr pprobe = probe;
  NodePtr near_node;
  NodePtr far_node;
  size_t probe_dim = cur_dim;
  if (_S_node_compare(probe_dim % __k, __cmp, __acc, __val, probe->_M_value))
    near_node = static_cast<NodePtr>(probe->_M_right);
  else
    near_node = static_cast<NodePtr>(probe->_M_left);
  if (near_node
      // only visit node's children if node's plane intersect hypersphere
      && (std::sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, probe->_M_value)) <= __max))
  {
    probe = near_node;
    ++probe_dim;
  }
  while (cur != __end)
  {
    while (probe != cur)
    {
      if (_S_node_compare(probe_dim % __k, __cmp, __acc, __val, probe->_M_value))
      {
        near_node = static_cast<NodePtr>(probe->_M_left);
        far_node = static_cast<NodePtr>(probe->_M_right);
      }
      else
      {
        near_node = static_cast<NodePtr>(probe->_M_right);
        far_node = static_cast<NodePtr>(probe->_M_left);
      }
      if (pprobe == probe->_M_parent) // going downward ...
      {
        if (__p(probe->_M_value))
        {
          typename _Dist::distance_type d = 0;
          for (size_t i=0; i < __k; ++i)
            d += _S_node_distance(i, __dist, __acc, __val, probe->_M_value);
          d = std::sqrt(d);
          if (d <= __max)  // CHANGED, see the above notes ("bad candidate notes")
          {
            __best = probe;
            __max = d;
            __dim = probe_dim;
          }
        }
        pprobe = probe;
        if (near_node)
        {
          probe = near_node;
          ++probe_dim;
        }
        else if (far_node &&
                 // only visit node's children if node's plane intersect hypersphere
                 std::sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, probe->_M_value)) <= __max)
        {
          probe = far_node;
          ++probe_dim;
        }
        else
        {
          probe = static_cast<NodePtr>(probe->_M_parent);
          --probe_dim;
        }
      }
      else // ... and going upward.
      {
        if (pprobe == near_node && far_node
            // only visit node's children if node's plane intersect hypersphere
            && std::sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, probe->_M_value)) <= __max)
        {
          pprobe = probe;
          probe = far_node;
          ++probe_dim;
        }
        else
        {
          pprobe = probe;
          probe = static_cast<NodePtr>(probe->_M_parent);
          --probe_dim;
        }
      }
    }
    pcur = cur;
    cur = static_cast<NodePtr>(cur->_M_parent);
    --cur_dim;
    pprobe = cur;
    probe = cur;
    probe_dim = cur_dim;
    if (cur != __end)
    {
      if (pcur == cur->_M_left)
        near_node = static_cast<NodePtr>(cur->_M_right);
      else
        near_node = static_cast<NodePtr>(cur->_M_left);
      if (near_node
          // only visit node's children if node's plane intersect hypersphere
          && (std::sqrt(_S_node_distance(cur_dim % __k, __dist, __acc, __val, cur->_M_value)) <= __max))
      {
        probe = near_node;
        ++probe_dim;
      }
    }
  }
  return std::pair<NodePtr,
      std::pair<size_t, typename _Dist::distance_type> >
      (__best, std::pair<size_t, typename _Dist::distance_type>
       (__dim, __max));
}


} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
/** \file
 * Defines the various functors and interfaces used for KDTree.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 * \author Sylvain Bougerel <sylvain.bougerel.devel@gmail.com>
 */

#ifndef INCLUDE_KDTREE_ACCESSOR_HPP
#define INCLUDE_KDTREE_ACCESSOR_HPP

#include <cstddef>

namespace KDTree
{
template <typename _Val>
struct _Bracket_accessor
{
  typedef typename _Val::value_type result_type;

  result_type
  operator()(_Val const& V, size_t const N) const
  {
    return V[N];
  }
};

template <typename _Tp>
struct always_true
{
  bool operator() (const _Tp& ) const { return true; }
};

template <typename _Tp, typename _Dist>
struct squared_difference
{
  typedef _Dist distance_type;

  distance_type
  operator() (const _Tp& __a, const _Tp& __b) const
  {
    distance_type d=__a - __b;
    return d*d;
  }
};

template <typename _Tp, typename _Dist>
struct squared_difference_counted
{
  typedef _Dist distance_type;

  squared_difference_counted()
    : _M_count(0)
  { }

  void reset ()
  { _M_count = 0; }

  long&
  count () const
  { return _M_count; }

  distance_type
  operator() (const _Tp& __a, const _Tp& __b) const
  {
    distance_type d=__a - __b;
    ++_M_count;
    return d*d;
  }

private:
  mutable long _M_count;
};

} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
/** \file
 * Defines the allocator interface as used by the KDTree class.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 */

#ifndef INCLUDE_KDTREE_ALLOCATOR_HPP
#define INCLUDE_KDTREE_ALLOCATOR_HPP

#include <cstddef>

namespace KDTree
{

template <typename _Tp, typename _Alloc>
class _Alloc_base
{
public:
  typedef _Node<_Tp> _Node_;
  typedef typename _Node_::_Base_ptr _Base_ptr;
  typedef _Alloc allocator_type;

  _Alloc_base(allocator_type const& __A)
    : _M_node_allocator(__A) {}

  allocator_type
  get_allocator() const
  {
    return _M_node_allocator;
  }


  class NoLeakAlloc
  {
    _Alloc_base * base;
    _Node_ * new_node;

  public:
    NoLeakAlloc(_Alloc_base * b) : base(b), new_node(base->_M_allocate_node()) {}

    _Node_ * get() { return new_node; }
    void disconnect() { new_node = nullptr; }

    ~NoLeakAlloc() { if (new_node) base->_M_deallocate_node(new_node); }
  };


protected:
  allocator_type _M_node_allocator;

  _Node_*
  _M_allocate_node()
  {
    return _M_node_allocator.allocate(1);
  }

  void
  _M_deallocate_node(_Node_* const __P)
  {
    return _M_node_allocator.deallocate(__P, 1);
  }

  void
  _M_construct_node(_Node_* __p, _Tp const __V = _Tp(),
                    _Base_ptr const __PARENT = NULL,
                    _Base_ptr const __LEFT = NULL,
                    _Base_ptr const __RIGHT = NULL)
  {
    new (__p) _Node_(__V, __PARENT, __LEFT, __RIGHT);
  }

  void
  _M_destroy_node(_Node_* __p)
  {
    _M_node_allocator.destroy(__p);
  }
};

} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
/** \file
 * Defines interfaces for iterators as used by the KDTree class.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 */

#ifndef INCLUDE_KDTREE_ITERATOR_HPP
#define INCLUDE_KDTREE_ITERATOR_HPP

#include <iterator>

namespace KDTree
{
template <typename _Val, typename _Ref, typename _Ptr>
class _Iterator;

template<typename _Val, typename _Ref, typename _Ptr>
inline bool
operator==(_Iterator<_Val, _Ref, _Ptr> const&,
           _Iterator<_Val, _Ref, _Ptr> const&);

template<typename _Val>
inline bool
operator==(_Iterator<_Val, const _Val&, const _Val*> const&,
           _Iterator<_Val, _Val&, _Val*> const&);

template<typename _Val>
inline bool
operator==(_Iterator<_Val, _Val&, _Val*> const&,
           _Iterator<_Val, const _Val&, const _Val*> const&);

template<typename _Val, typename _Ref, typename _Ptr>
inline bool
operator!=(_Iterator<_Val, _Ref, _Ptr> const&,
           _Iterator<_Val, _Ref, _Ptr> const&);

template<typename _Val>
inline bool
operator!=(_Iterator<_Val, const _Val&, const _Val*> const&,
           _Iterator<_Val, _Val&, _Val*> const&);

template<typename _Val>
inline bool
operator!=(_Iterator<_Val, _Val&, _Val*> const&,
           _Iterator<_Val, const _Val&, const _Val*> const&);

class _Base_iterator
{
protected:
  typedef _Node_base::_Base_const_ptr _Base_const_ptr;
  _Base_const_ptr _M_node;

  inline _Base_iterator(_Base_const_ptr const __N = nullptr)
    : _M_node(__N) {}
  inline _Base_iterator(_Base_iterator const& __THAT)
    : _M_node(__THAT._M_node) {}

  inline void
  _M_increment()
  {
    if (_M_node->_M_right)
    {
      _M_node = _M_node->_M_right;
      while (_M_node->_M_left) _M_node = _M_node->_M_left;
    }
    else
    {
      _Base_const_ptr __p = _M_node->_M_parent;
      while (__p && _M_node == __p->_M_right)
      {
        _M_node = __p;
        __p = _M_node->_M_parent;
      }
      if (__p) // (__p) provide undetermined behavior on end()++ rather
        // than a segfault, similar to standard iterator.
        _M_node = __p;
    }
  }

  inline void
  _M_decrement()
  {
    if (!_M_node->_M_parent) // clearly identify the header node
    {
      _M_node = _M_node->_M_right;
    }
    else if (_M_node->_M_left)
    {
      _Base_const_ptr x = _M_node->_M_left;
      while (x->_M_right) x = x->_M_right;
      _M_node = x;
    }
    else
    {
      _Base_const_ptr __p = _M_node->_M_parent;
      while (__p && _M_node == __p->_M_left) // see below
      {
        _M_node = __p;
        __p = _M_node->_M_parent;
      }
      if (__p) // (__p) provide undetermined behavior on rend()++ rather
        // than a segfault, similar to standard iterator.
        _M_node = __p;
    }
  }

  template <size_t const __K, typename _Val, typename _Acc,
            typename _Dist, typename _Cmp, typename _Alloc>
  friend class KDTree;
};

template <typename _Val, typename _Ref, typename _Ptr>
class _Iterator : protected _Base_iterator
{
public:
  typedef _Val value_type;
  typedef _Ref reference;
  typedef _Ptr pointer;
  typedef _Iterator<_Val, _Val&, _Val*> iterator;
  typedef _Iterator<_Val, _Val const&, _Val const*> const_iterator;
  typedef _Iterator<_Val, _Ref, _Ptr> _Self;
  typedef _Node<_Val> const* _Link_const_type;
  typedef std::bidirectional_iterator_tag iterator_category;
  typedef ptrdiff_t difference_type;

  inline _Iterator()
    : _Base_iterator() {}
  inline _Iterator(_Link_const_type const __N)
    : _Base_iterator(__N) {}
  inline _Iterator(iterator const& __THAT)
    : _Base_iterator(__THAT) {}

  _Link_const_type get_raw_node() const
  {
    return _Link_const_type(_M_node);
  }

  reference
  operator*() const
  {
    return _Link_const_type(_M_node)->_M_value;
  }

  pointer
  operator->() const
  {
    return &(operator*());
  }

  _Self
  operator++()
  {
    _M_increment();
    return *this;
  }

  _Self
  operator++(int)
  {
    _Self ret = *this;
    _M_increment();
    return ret;
  }

  _Self&
  operator--()
  {
    _M_decrement();
    return *this;
  }

  _Self
  operator--(int)
  {
    _Self ret = *this;
    _M_decrement();
    return ret;
  }

  friend bool
  operator== <>(_Iterator<_Val, _Ref, _Ptr> const&,
                _Iterator<_Val, _Ref, _Ptr> const&);

  friend bool
  operator== <>(_Iterator<_Val, const _Val&, const _Val*> const&,
                _Iterator<_Val, _Val&, _Val*> const&);

  friend bool
  operator== <>(_Iterator<_Val, _Val&, _Val*> const&,
                _Iterator<_Val, const _Val&, const _Val*> const&);

  friend bool
  operator!= <>(_Iterator<_Val, _Ref, _Ptr> const&,
                _Iterator<_Val, _Ref, _Ptr> const&);

  friend bool
  operator!= <>(_Iterator<_Val, const _Val&, const _Val*> const&,
                _Iterator<_Val, _Val&, _Val*> const&);

  friend bool
  operator!= <>(_Iterator<_Val, _Val&, _Val*> const&,
                _Iterator<_Val, const _Val&, const _Val*> const&);
};

template<typename _Val, typename _Ref, typename _Ptr>
bool
operator==(_Iterator<_Val, _Ref, _Ptr> const& __X,
           _Iterator<_Val, _Ref, _Ptr> const& __Y)
{ return __X._M_node == __Y._M_node; }

template<typename _Val>
bool
operator==(_Iterator<_Val, const _Val&, const _Val*> const& __X,
           _Iterator<_Val, _Val&, _Val*> const& __Y)
{ return __X._M_node == __Y._M_node; }

template<typename _Val>
bool
operator==(_Iterator<_Val, _Val&, _Val*> const& __X,
           _Iterator<_Val, const _Val&, const _Val*> const& __Y)
{ return __X._M_node == __Y._M_node; }

template<typename _Val, typename _Ref, typename _Ptr>
bool
operator!=(_Iterator<_Val, _Ref, _Ptr> const& __X,
           _Iterator<_Val, _Ref, _Ptr> const& __Y)
{ return __X._M_node != __Y._M_node; }

template<typename _Val>
bool
operator!=(_Iterator<_Val, const _Val&, const _Val*> const& __X,
           _Iterator<_Val, _Val&, _Val*> const& __Y)
{ return __X._M_node != __Y._M_node; }

template<typename _Val>
bool
operator!=(_Iterator<_Val, _Val&, _Val*> const& __X,
           _Iterator<_Val, const _Val&, const _Val*> const& __Y)
{ return __X._M_node != __Y._M_node; }

} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
/** \file
 * Defines the interface of the _Region class.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 */

#ifndef INCLUDE_KDTREE_REGION_HPP
#define INCLUDE_KDTREE_REGION_HPP

#include <cstddef>

namespace KDTree
{

template <size_t const __K, typename _Val, typename _SubVal,
          typename _Acc, typename _Cmp>
struct _Region
{
  typedef _Val value_type;
  typedef _SubVal subvalue_type;

  // special typedef for checking against a fuzzy point (for find_nearest)
  // Note the region (first) component is not supposed to have an area, its
  // bounds should all be set to a specific point.
  typedef std::pair<_Region,_SubVal> _CenterPt;

  _Region(_Acc const& __acc=_Acc(), const _Cmp& __cmp=_Cmp())
    : _M_acc(__acc), _M_cmp(__cmp) {}

  template <typename Val>
  _Region(Val const& __V,
          _Acc const& __acc=_Acc(), const _Cmp& __cmp=_Cmp())
    : _M_acc(__acc), _M_cmp(__cmp)
  {
    for (size_t __i = 0; __i != __K; ++__i)
    {
      _M_low_bounds[__i] = _M_high_bounds[__i] = _M_acc(__V,__i);
    }
  }

  template <typename Val>
  _Region(Val const& __V, subvalue_type const& __R,
          _Acc const& __acc=_Acc(), const _Cmp& __cmp=_Cmp())
    : _M_acc(__acc), _M_cmp(__cmp)
  {
    for (size_t __i = 0; __i != __K; ++__i)
    {
      _M_low_bounds[__i] = _M_acc(__V,__i) - __R;
      _M_high_bounds[__i] = _M_acc(__V,__i) + __R;
    }
  }

  bool
  intersects_with(_CenterPt const& __THAT) const
  {
    for (size_t __i = 0; __i != __K; ++__i)
    {
      // does it fall outside the bounds?
      // ! low-tolerance <= x <= high+tolerance
      // ! (low-tol <= x and x <= high+tol)
      // !low-tol<=x or !x<=high+tol
      // low-tol>x or x>high+tol
      // x<low-tol or high+tol<x
      if (_M_cmp(__THAT.first._M_low_bounds[__i], _M_low_bounds[__i] - __THAT.second)
          || _M_cmp(_M_high_bounds[__i] + __THAT.second, __THAT.first._M_low_bounds[__i]))
        return false;
    }
    return true;
  }

  bool
  intersects_with(_Region const& __THAT) const
  {
    for (size_t __i = 0; __i != __K; ++__i)
    {
      if (_M_cmp(__THAT._M_high_bounds[__i], _M_low_bounds[__i])
          || _M_cmp(_M_high_bounds[__i], __THAT._M_low_bounds[__i]))
        return false;
    }
    return true;
  }

  bool
  encloses(value_type const& __V) const
  {
    for (size_t __i = 0; __i != __K; ++__i)
    {
      if (_M_cmp(_M_acc(__V, __i), _M_low_bounds[__i])
          || _M_cmp(_M_high_bounds[__i], _M_acc(__V, __i)))
        return false;
    }
    return true;
  }

  _Region&
  set_high_bound(value_type const& __V, size_t const __L)
  {
    _M_high_bounds[__L % __K] = _M_acc(__V, __L % __K);
    return *this;
  }

  _Region&
  set_low_bound(value_type const& __V, size_t const __L)
  {
    _M_low_bounds[__L % __K] = _M_acc(__V, __L % __K);
    return *this;
  }

  subvalue_type _M_low_bounds[__K], _M_high_bounds[__K];
  _Acc _M_acc;
  _Cmp _M_cmp;
};

} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
/** \file
 * Defines the interface for the KDTree class.
 *
 * \author Martin F. Krafft <libkdtree@pobox.madduck.net>
 *
 * Paul Harris figured this stuff out (below)
 * Notes:
 * This is similar to a binary tree, but its not the same.
 * There are a few important differences:
 *
 *  * Each level is sorted by a different criteria (this is fundamental to the design).
 *
 *  * It is possible to have children IDENTICAL to its parent in BOTH branches
 *    This is different to a binary tree, where identical children are always to the right
 *    So, KDTree has the relationships:
 *    * The left branch is <= its parent (in binary tree, this relationship is a plain < )
 *    * The right branch is <= its parent (same as binary tree)
 *
 *    This is done for mostly for performance.
 *    Its a LOT easier to maintain a consistent tree if we use the <= relationship.
 *    Note that this relationship only makes a difference when searching for an exact
 *    item with find() or find_exact, other search, erase and insert functions don't notice
 *    the difference.
 *
 *    In the case of binary trees, you can safely assume that the next identical item
 *    will be the child leaf,
 *    but in the case of KDTree, the next identical item might
 *    be a long way down a subtree, because of the various different sort criteria.
 *
 *    So to erase() a node from a KDTree could require serious and complicated
 *    tree rebalancing to maintain consistency... IF we required binary-tree-like relationships.
 *
 *    This has no effect on insert()s, a < test is good enough to keep consistency.
 *
 *    It has an effect on find() searches:
 *      * Instead of using compare(child,node) for a < relationship and following 1 branch,
 *        we must use !compare(node,child) for a <= relationship, and test BOTH branches, as
 *        we could potentially go down both branches.
 *
 *    It has no real effect on bounds-based searches (like find_nearest, find_within_range)
 *    as it compares vs a boundary and would follow both branches if required.
 *
 *    This has no real effect on erase()s, a < test is good enough to keep consistency.
 */

#ifndef INCLUDE_KDTREE_KDTREE_HPP
#define INCLUDE_KDTREE_KDTREE_HPP


//
//  This number is guaranteed to change with every release.
//
//  KDTREE_VERSION % 100 is the patch level
//  KDTREE_VERSION / 100 % 1000 is the minor version
//  KDTREE_VERSION / 100000 is the major version
#define KDTREE_VERSION 701
//
//  KDTREE_LIB_VERSION must be defined to be the same as KDTREE_VERSION
//  but as a *string* in the form "x_y[_z]" where x is the major version
//  number, y is the minor version number, and z is the patch level if not 0.
#define KDTREE_LIB_VERSION "0_7_1"


#include <vector>

#ifdef KDTREE_CHECK_PERFORMANCE_COUNTERS
#  include <map>
#endif
#include <algorithm>
#include <functional>

#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
#  include <ostream>
#  include <stack>
#endif

#include <cmath>
#include <cstddef>
#include <cassert>

namespace KDTree
{

#ifdef KDTREE_CHECK_PERFORMANCE
unsigned long long num_dist_calcs = 0;
#endif

template <size_t const __K, typename _Val,
          typename _Acc = _Bracket_accessor<_Val>,
          typename _Dist = squared_difference<typename _Acc::result_type,
                                              typename _Acc::result_type>,
          typename _Cmp = std::less<typename _Acc::result_type>,
          typename _Alloc = std::allocator<_Node<_Val> > >
class KDTree : protected _Alloc_base<_Val, _Alloc>
{
protected:
  typedef _Alloc_base<_Val, _Alloc> _Base;
  typedef typename _Base::allocator_type allocator_type;

  typedef _Node_base* _Base_ptr;
  typedef _Node_base const* _Base_const_ptr;
  typedef _Node<_Val>* _Link_type;
  typedef _Node<_Val> const* _Link_const_type;

  typedef _Node_compare<_Val, _Acc, _Cmp> _Node_compare_;

public:
  typedef _Region<__K, _Val, typename _Acc::result_type, _Acc, _Cmp> _Region_;
  typedef _Val value_type;
  typedef value_type* pointer;
  typedef value_type const* const_pointer;
  typedef value_type& reference;
  typedef value_type const& const_reference;
  typedef typename _Acc::result_type subvalue_type;
  typedef typename _Dist::distance_type distance_type;
  typedef size_t size_type;
  typedef ptrdiff_t difference_type;

  KDTree(_Acc const& __acc = _Acc(), _Dist const& __dist = _Dist(),
         _Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
    : _Base(__a), _M_header(),
      _M_count(0), _M_acc(__acc), _M_cmp(__cmp), _M_dist(__dist)
  {
    _M_empty_initialise();
  }

  KDTree(const KDTree& __x)
    : _Base(__x.get_allocator()), _M_header(), _M_count(0),
      _M_acc(__x._M_acc), _M_cmp(__x._M_cmp), _M_dist(__x._M_dist)
  {
    _M_empty_initialise();
    // this is slow:
    // this->insert(begin(), __x.begin(), __x.end());
    // this->optimise();

    // this is much faster, as it skips a lot of useless work
    // do the optimisation before inserting
    // Needs to be stored in a vector first as _M_optimise()
    // sorts the data in the passed iterators directly.
    std::vector<value_type> temp;
    temp.reserve(__x.size());
    std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
    _M_optimise(temp.begin(), temp.end(), 0);
  }

  template<typename _InputIterator>
  KDTree(_InputIterator __first, _InputIterator __last,
         _Acc const& acc = _Acc(), _Dist const& __dist = _Dist(),
         _Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
    : _Base(__a), _M_header(), _M_count(0),
      _M_acc(acc), _M_cmp(__cmp), _M_dist(__dist)
  {
    _M_empty_initialise();
    // this is slow:
    // this->insert(begin(), __first, __last);
    // this->optimise();

    // this is much faster, as it skips a lot of useless work
    // do the optimisation before inserting
    // Needs to be stored in a vector first as _M_optimise()
    // sorts the data in the passed iterators directly.
    std::vector<value_type> temp;
    temp.reserve(std::distance(__first,__last));
    std::copy(__first,__last,std::back_inserter(temp));
    _M_optimise(temp.begin(), temp.end(), 0);

    // NOTE: this will BREAK users that are passing in
    // read-once data via the iterator...
    // We increment __first all the way to __last once within
    // the distance() call, and again within the copy() call.
    //
    // This should end up using some funky C++ concepts or
    // type traits to check that the iterators can be used in this way...
  }


  // this will CLEAR the tree and fill it with the contents
  // of 'writable_vector'.  it will use the passed vector directly,
  // and will basically resort the vector many times over while
  // optimising the tree.
  //
  // Paul: I use this when I have already built up a vector of data
  // that I want to add, and I don't mind if its contents get shuffled
  // by the kdtree optimise routine.
  void efficient_replace_and_optimise( std::vector<value_type> & writable_vector )
  {
    this->clear();
    _M_optimise(writable_vector.begin(), writable_vector.end(), 0);
  }



  KDTree&
  operator=(const KDTree& __x)
  {
    if (this != &__x)
    {
      _M_acc = __x._M_acc;
      _M_dist = __x._M_dist;
      _M_cmp = __x._M_cmp;
      // this is slow:
      // this->insert(begin(), __x.begin(), __x.end());
      // this->optimise();

      // this is much faster, as it skips a lot of useless work
      // do the optimisation before inserting
      // Needs to be stored in a vector first as _M_optimise()
      // sorts the data in the passed iterators directly.
      std::vector<value_type> temp;
      temp.reserve(__x.size());
      std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
      efficient_replace_and_optimise(temp);
    }
    return *this;
  }

  ~KDTree()
  {
    this->clear();
  }

  allocator_type
  get_allocator() const
  {
    return _Base::get_allocator();
  }

  size_type
  size() const
  {
    return _M_count;
  }

  size_type
  max_size() const
  {
    return size_type(-1);
  }

  bool
  empty() const
  {
    return this->size() == 0;
  }

  void
  clear()
  {
    _M_erase_subtree(_M_get_root());
    _M_set_leftmost(&_M_header);
    _M_set_rightmost(&_M_header);
    _M_set_root(nullptr);
    _M_count = 0;
  }

  /*! \brief Comparator for the values in the KDTree.

  The comparator shall not be modified, it could invalidate the tree.
  \return a copy of the comparator used by the KDTree.
       */
  _Cmp
  value_comp() const
  { return _M_cmp; }

  /*! \brief Accessor to the value's elements.

  This accessor shall not be modified, it could invalidate the tree.
  \return a copy of the accessor used by the KDTree.
       */
  _Acc
  value_acc() const
  { return _M_acc; }

  /*! \brief Distance calculator between 2 value's element.

  This functor can be modified. It's modification will only affect the
  behavior of the find and find_nearest functions.
  \return a reference to the distance calculator used by the KDTree.
       */
  const _Dist&
  value_distance() const
  { return _M_dist; }

  _Dist&
  value_distance()
  { return _M_dist; }

  // typedef _Iterator<_Val, reference, pointer> iterator;
  typedef _Iterator<_Val, const_reference, const_pointer> const_iterator;
  // No mutable iterator at this stage
  typedef const_iterator iterator;
  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
  typedef std::reverse_iterator<iterator> reverse_iterator;

  // Note: the static_cast in end() is invalid (_M_header is not convertible to a _Link_type), but
  // that's ok as it just means undefined behaviour if the user dereferences the end() iterator.

  const_iterator begin() const { return const_iterator(_M_get_leftmost()); }
  const_iterator end() const { return const_iterator(static_cast<_Link_const_type>(&_M_header)); }
  const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
  const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }

  iterator
  insert(iterator /* ignored */, const_reference __V)
  {
    return this->insert(__V);
  }

  iterator
  insert(const_reference __V)
  {
    if (!_M_get_root())
    {
      _Link_type __n = _M_new_node(__V, &_M_header);
      ++_M_count;
      _M_set_root(__n);
      _M_set_leftmost(__n);
      _M_set_rightmost(__n);
      return iterator(__n);
    }
    return _M_insert(_M_get_root(), __V, 0);
  }

  template <class _InputIterator>
  void insert(_InputIterator __first, _InputIterator __last) {
    for (; __first != __last; ++__first)
      this->insert(*__first);
  }

  void
  insert(iterator __pos, size_type __n, const value_type& __x)
  {
    for (; __n > 0; --__n)
      this->insert(__pos, __x);
  }

  template<typename _InputIterator>
  void
  insert(iterator __pos, _InputIterator __first, _InputIterator __last) {
    for (; __first != __last; ++__first)
      this->insert(__pos, *__first);
  }

  // Note: this uses the find() to location the item you want to erase.
  // find() compares by equivalence of location ONLY.  See the comments
  // above find_exact() for why you may not want this.
  //
  // If you want to erase ANY item that has the same location as __V,
  // then use this function.
  //
  // If you want to erase a PARTICULAR item, and not any other item
  // that might happen to have the same location, then you should use
  // erase_exact().
  void
  erase(const_reference __V) {
    const_iterator b =  this->find(__V);
    this->erase(b);
  }

  void
  erase_exact(const_reference __V) {
    this->erase(this->find_exact(__V));
  }

  // note: kept as const because its easier to const-cast it away
  void
  erase(const_iterator const& __IT)
  {
    assert(__IT != this->end());
    _Link_const_type target = __IT.get_raw_node();
    _Link_const_type n = target;
    size_type level = 0;
    while ((n = _S_parent(n)) != &_M_header)
      ++level;
    _M_erase( const_cast<_Link_type>(target), level );
    _M_delete_node( const_cast<_Link_type>(target) );
    --_M_count;
  }

  /* this does not work since erasure changes sort order
      void
      erase(const_iterator __A, const_iterator const& __B)
      {
        if (0 && __A == this->begin() && __B == this->end())
          {
            this->clear();
          }
        else
          {
            while (__A != __B)
              this->erase(__A++);
          }
      }
*/

  // compares via equivalence
  // so if you are looking for any item with the same location,
  // according to the standard accessor comparisons,
  // then this is the function for you.
  template <class SearchVal>
  const_iterator
  find(SearchVal const& __V) const
  {
    if (!_M_get_root()) return this->end();
    return _M_find(_M_get_root(), __V, 0);
  }

  // compares via equality
  // if you are looking for a particular item in the tree,
  // and (for example) it has an ID that is checked via an == comparison
  // e.g.
  // struct Item
  // {
  //    size_type unique_id;
  //    bool operator==(Item const& a, Item const& b) { return a.unique_id == b.unique_id; }
  //    Location location;
  // };
  // Two items may be equivalent in location.  find() would return
  // either one of them.  But no two items have the same ID, so
  // find_exact() would always return the item with the same location AND id.
  //
  template <class SearchVal>
  const_iterator
  find_exact(SearchVal const& __V) const
  {
    if (!_M_get_root()) return this->end();
    return _M_find_exact(_M_get_root(), __V, 0);
  }

  // NOTE: see notes on find_within_range().
  size_type
  count_within_range(const_reference __V, subvalue_type const __R) const
  {
    if (!_M_get_root()) return 0;
    _Region_ __region(__V, __R, _M_acc, _M_cmp);
    return this->count_within_range(__region);
  }

  size_type
  count_within_range(_Region_ const& __REGION) const
  {
    if (!_M_get_root()) return 0;

    _Region_ __bounds(__REGION);
    return _M_count_within_range(_M_get_root(),
                                 __REGION, __bounds, 0);
  }

  // NOTE: see notes on find_within_range().
  template <typename SearchVal, class Visitor>
  Visitor
  visit_within_range(SearchVal const& V, subvalue_type const R, Visitor visitor) const
  {
    if (!_M_get_root()) return visitor;
    _Region_ region(V, R, _M_acc, _M_cmp);
    return this->visit_within_range(region, visitor);
  }

  template <class Visitor>
  Visitor
  visit_within_range(_Region_ const& REGION, Visitor visitor) const
  {
    if (_M_get_root())
    {
      _Region_ bounds(REGION);
      return _M_visit_within_range(visitor, _M_get_root(), REGION, bounds, 0);
    }
    return visitor;
  }

  // NOTE: this will visit points based on 'Manhattan distance' aka city-block distance
  // aka taxicab metric. Meaning it will find all points within:
  //    max(x_dist,max(y_dist,z_dist));
  //  AND NOT than what you would expect: sqrt(x_dist*x_dist + y_dist*y_dist + z_dist*z_dist)
  //
  // This is because it converts the distance into a bounding-box 'region' and compares
  // against that.
  //
  // If you want the sqrt() behaviour, ask on the mailing list for different options.
  //
  template <typename SearchVal, typename _OutputIterator>
  _OutputIterator
  find_within_range(SearchVal const& val, subvalue_type const range,
                    _OutputIterator out) const
  {
    if (!_M_get_root()) return out;
    _Region_ region(val, range, _M_acc, _M_cmp);
    return this->find_within_range(region, out);
  }

  template <typename _OutputIterator>
  _OutputIterator
  find_within_range(_Region_ const& region,
                    _OutputIterator out) const
  {
    if (_M_get_root())
    {
      _Region_ bounds(region);
      out = _M_find_within_range(out, _M_get_root(),
                                 region, bounds, 0);
    }
    return out;
  }

  template <class SearchVal>
  std::pair<const_iterator, distance_type>
  find_nearest (SearchVal const& __val) const
  {
    if (_M_get_root())
    {
      std::pair<const _Node<_Val>*,
          std::pair<size_type, typename _Acc::result_type> >
          best = _S_node_nearest (__K, 0, __val,
                                  _M_get_root(), &_M_header, _M_get_root(),
                                  std::sqrt(_S_accumulate_node_distance
                                            (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val)),
                                  _M_cmp, _M_acc, _M_dist,
                                  always_true<value_type>());
      return std::pair<const_iterator, distance_type>
          (best.first, best.second.second);
    }
    return std::pair<const_iterator, distance_type>(end(), 0);
  }

  template <class SearchVal>
  std::pair<const_iterator, distance_type>
  find_nearest (SearchVal const& __val, distance_type __max) const
  {
    if (_M_get_root())
    {
      bool root_is_candidate = false;
      const _Node<_Val>* node = _M_get_root();
      { // scope to ensure we don't use 'root_dist' anywhere else
        distance_type root_dist = std::sqrt(_S_accumulate_node_distance
                                            (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
        if (root_dist <= __max)
        {
          root_is_candidate = true;
          __max = root_dist;
        }
      }
      std::pair<const _Node<_Val>*,
          std::pair<size_type, typename _Acc::result_type> >
          best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
                                  node, __max, _M_cmp, _M_acc, _M_dist,
                                  always_true<value_type>());
      // make sure we didn't just get stuck with the root node...
      if (root_is_candidate || best.first != _M_get_root())
        return std::pair<const_iterator, distance_type>
            (best.first, best.second.second);
    }
    return std::pair<const_iterator, distance_type>(end(), __max);
  }

  template <class SearchVal, class _Predicate>
  std::pair<const_iterator, distance_type>
  find_nearest_if (SearchVal const& __val, distance_type __max,
                   _Predicate __p) const
  {
    if (_M_get_root())
    {
      bool root_is_candidate = false;
      const _Node<_Val>* node = _M_get_root();
      if (__p(_M_get_root()->_M_value))
      {
        { // scope to ensure we don't use root_dist anywhere else
          distance_type root_dist = std::sqrt(_S_accumulate_node_distance
                                              (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
          if (root_dist <= __max)
          {
            root_is_candidate = true;
            root_dist = __max;
          }
        }
      }
      std::pair<const _Node<_Val>*,
          std::pair<size_type, typename _Acc::result_type> >
          best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
                                  node, __max, _M_cmp, _M_acc, _M_dist, __p);
      // make sure we didn't just get stuck with the root node...
      if (root_is_candidate || best.first != _M_get_root())
        return std::pair<const_iterator, distance_type>
            (best.first, best.second.second);
    }
    return std::pair<const_iterator, distance_type>(end(), __max);
  }

  void
  optimise()
  {
    std::vector<value_type> __v(this->begin(),this->end());
    this->clear();
    _M_optimise(__v.begin(), __v.end(), 0);
  }

  void
  optimize()
  { // cater for people who cannot spell :)
    this->optimise();
  }

  void check_tree()
  {
    _M_check_node(_M_get_root(),0);
  }

protected:

  void _M_check_children( _Link_const_type child, _Link_const_type parent, size_type const level, bool to_the_left )
  {
    assert(parent);
    if (child)
    {
      _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
      // REMEMBER! its a <= relationship for BOTH branches
      // for left-case (true), child<=node --> !(node<child)
      // for right-case (false), node<=child --> !(child<node)
      assert(!to_the_left || !compare(parent->_M_value,child->_M_value));  // check the left
      assert(to_the_left || !compare(child->_M_value,parent->_M_value));   // check the right
      // and recurse down the tree, checking everything
      _M_check_children(_S_left(child),parent,level,to_the_left);
      _M_check_children(_S_right(child),parent,level,to_the_left);
    }
  }

  void _M_check_node( _Link_const_type node, size_type const level )
  {
    if (node)
    {
      // (comparing on this level)
      // everything to the left of this node must be smaller than this
      _M_check_children( _S_left(node), node, level, true );
      // everything to the right of this node must be larger than this
      _M_check_children( _S_right(node), node, level, false );

      _M_check_node( _S_left(node), level+1 );
      _M_check_node( _S_right(node), level+1 );
    }
  }

  void _M_empty_initialise()
  {
    _M_set_leftmost(&_M_header);
    _M_set_rightmost(&_M_header);
    _M_header._M_parent = nullptr;
    _M_set_root(nullptr);
  }

  iterator
  _M_insert_left(_Link_type __N, const_reference __V)
  {
    _S_set_left(__N, _M_new_node(__V)); ++_M_count;
    _S_set_parent( _S_left(__N), __N );
    if (__N == _M_get_leftmost())
      _M_set_leftmost( _S_left(__N) );
    return iterator(_S_left(__N));
  }

  iterator
  _M_insert_right(_Link_type __N, const_reference __V)
  {
    _S_set_right(__N, _M_new_node(__V)); ++_M_count;
    _S_set_parent( _S_right(__N), __N );
    if (__N == _M_get_rightmost())
      _M_set_rightmost( _S_right(__N) );
    return iterator(_S_right(__N));
  }

  iterator
  _M_insert(_Link_type __N, const_reference __V,
            size_type const __L)
  {
    if (_Node_compare_(__L % __K, _M_acc, _M_cmp)(__V, __N->_M_value))
    {
      if (!_S_left(__N))
        return _M_insert_left(__N, __V);
      return _M_insert(_S_left(__N), __V, __L+1);
    }
    else
    {
      if (!_S_right(__N) || __N == _M_get_rightmost())
        return _M_insert_right(__N, __V);
      return _M_insert(_S_right(__N), __V, __L+1);
    }
  }

  _Link_type
  _M_erase(_Link_type dead_dad, size_type const level)
  {
    // find a new step_dad, he will become a drop-in replacement.
    _Link_type step_dad = _M_get_erase_replacement(dead_dad, level);

    // tell dead_dad's parent that his new child is step_dad
    if (dead_dad == _M_get_root())
      _M_set_root(step_dad);
    else if (_S_left(_S_parent(dead_dad)) == dead_dad)
      _S_set_left(_S_parent(dead_dad), step_dad);
    else
      _S_set_right(_S_parent(dead_dad), step_dad);

    // deal with the left and right edges of the tree...
    // if the dead_dad was at the edge, then substitute...
    // but if there IS no new dead, then left_most is the dead_dad's parent
    if (dead_dad == _M_get_leftmost())
      _M_set_leftmost( (step_dad ? step_dad : _S_parent(dead_dad)) );
    if (dead_dad == _M_get_rightmost())
      _M_set_rightmost( (step_dad ? step_dad : _S_parent(dead_dad)) );

    if (step_dad)
    {
      // step_dad gets dead_dad's parent
      _S_set_parent(step_dad, _S_parent(dead_dad));

      // first tell the children that step_dad is their new dad
      if (_S_left(dead_dad))
        _S_set_parent(_S_left(dead_dad), step_dad);
      if (_S_right(dead_dad))
        _S_set_parent(_S_right(dead_dad), step_dad);

      // step_dad gets dead_dad's children
      _S_set_left(step_dad, _S_left(dead_dad));
      _S_set_right(step_dad, _S_right(dead_dad));
    }

    return step_dad;
  }



  _Link_type
  _M_get_erase_replacement(_Link_type node, size_type const level)
  {
    // if 'node' is null, then we can't do any better
    if (_S_is_leaf(node))
      return NULL;

    std::pair<_Link_type,size_type> candidate;
    // if there is nothing to the left, find a candidate on the right tree
    if (!_S_left(node))
      candidate = _M_get_j_min( std::pair<_Link_type,size_type>(_S_right(node),level), level+1);
    // ditto for the right
    else if ((!_S_right(node)))
      candidate = _M_get_j_max( std::pair<_Link_type,size_type>(_S_left(node),level), level+1);
    // we have both children ...
    else
    {
      // we need to do a little more work in order to find a good candidate
      // this is actually a technique used to choose a node from either the
      // left or right branch RANDOMLY, so that the tree has a greater change of
      // staying balanced.
      // If this were a true binary tree, we would always hunt down the right branch.
      // See top for notes.
      _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
      // compare the children based on this level's criteria...
      // (this gives virtually random results)
      if (compare(_S_right(node)->_M_value, _S_left(node)->_M_value))
        // the right is smaller, get our replacement from the SMALLEST on the right
        candidate = _M_get_j_min(std::pair<_Link_type,size_type>(_S_right(node),level), level+1);
      else
        candidate = _M_get_j_max( std::pair<_Link_type,size_type>(_S_left(node),level), level+1);
    }

    // we have a candidate replacement by now.
    // remove it from the tree, but don't delete it.
    // it must be disconnected before it can be reconnected.
    _Link_type parent = _S_parent(candidate.first);
    if (_S_left(parent) == candidate.first)
      _S_set_left(parent, _M_erase(candidate.first, candidate.second));
    else
      _S_set_right(parent, _M_erase(candidate.first, candidate.second));

    return candidate.first;
  }



  // TODO: why not pass node by const-ref?
  std::pair<_Link_type,size_type>
  _M_get_j_min( std::pair<_Link_type,size_type> const node, size_type const level)
  {
    typedef std::pair<_Link_type,size_type> Result;
    if (_S_is_leaf(node.first))
      return Result(node.first,level);

    _Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
    Result candidate = node;
    if (_S_left(node.first))
    {
      Result left = _M_get_j_min(Result(_S_left(node.first), node.second), level+1);
      if (compare(left.first->_M_value, candidate.first->_M_value))
        candidate = left;
    }
    if (_S_right(node.first))
    {
      Result right = _M_get_j_min( Result(_S_right(node.first),node.second), level+1);
      if (compare(right.first->_M_value, candidate.first->_M_value))
        candidate = right;
    }
    if (candidate.first == node.first)
      return Result(candidate.first,level);

    return candidate;
  }



  // TODO: why not pass node by const-ref?
  std::pair<_Link_type,size_type>
  _M_get_j_max( std::pair<_Link_type,size_type> const node, size_type const level)
  {
    typedef std::pair<_Link_type,size_type> Result;

    if (_S_is_leaf(node.first))
      return Result(node.first,level);

    _Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
    Result candidate = node;
    if (_S_left(node.first))
    {
      Result left = _M_get_j_max( Result(_S_left(node.first),node.second), level+1);
      if (compare(candidate.first->_M_value, left.first->_M_value))
        candidate = left;
    }
    if (_S_right(node.first))
    {
      Result right = _M_get_j_max(Result(_S_right(node.first),node.second), level+1);
      if (compare(candidate.first->_M_value, right.first->_M_value))
        candidate = right;
    }

    if (candidate.first == node.first)
      return Result(candidate.first,level);

    return candidate;
  }


  void
  _M_erase_subtree(_Link_type __n)
  {
    while (__n)
    {
      _M_erase_subtree(_S_right(__n));
      _Link_type __t = _S_left(__n);
      _M_delete_node(__n);
      __n = __t;
    }
  }

  const_iterator
  _M_find(_Link_const_type node, const_reference value, size_type const level) const
  {
    // be aware! This is very different to normal binary searches, because of the <=
    // relationship used. See top for notes.
    // Basically we have to check ALL branches, as we may have an identical node
    // in different branches.
    const_iterator found = this->end();

    _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
    if (!compare(node->_M_value,value))   // note, this is a <= test
    {
      // this line is the only difference between _M_find_exact() and _M_find()
      if (_M_matches_node(node, value, level))
        return const_iterator(node);   // return right away
      if (_S_left(node))
        found = _M_find(_S_left(node), value, level+1);
    }
    if ( _S_right(node) && found == this->end() && !compare(value,node->_M_value))   // note, this is a <= test
      found = _M_find(_S_right(node), value, level+1);
    return found;
  }

  const_iterator
  _M_find_exact(_Link_const_type node, const_reference value, size_type const level) const
  {
    // be aware! This is very different to normal binary searches, because of the <=
    // relationship used. See top for notes.
    // Basically we have to check ALL branches, as we may have an identical node
    // in different branches.
    const_iterator found = this->end();

    _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
    if (!compare(node->_M_value,value))  // note, this is a <= test
    {
      // this line is the only difference between _M_find_exact() and _M_find()
      if (value == *const_iterator(node))
        return const_iterator(node);   // return right away
      if (_S_left(node))
        found = _M_find_exact(_S_left(node), value, level+1);
    }

    // note: no else!  items that are identical can be down both branches
    if ( _S_right(node) && found == this->end() && !compare(value,node->_M_value))   // note, this is a <= test
      found = _M_find_exact(_S_right(node), value, level+1);
    return found;
  }

  bool
  _M_matches_node_in_d(_Link_const_type __N, const_reference __V,
                       size_type const __L) const
  {
    _Node_compare_ compare(__L % __K, _M_acc, _M_cmp);
    return !(compare(__N->_M_value, __V) || compare(__V, __N->_M_value));
  }

  bool
  _M_matches_node_in_other_ds(_Link_const_type __N, const_reference __V,
                              size_type const __L = 0) const
  {
    size_type __i = __L;
    while ((__i = (__i + 1) % __K) != __L % __K)
      if (!_M_matches_node_in_d(__N, __V, __i)) return false;
    return true;
  }

  bool
  _M_matches_node(_Link_const_type __N, const_reference __V,
                  size_type __L = 0) const
  {
    return _M_matches_node_in_d(__N, __V, __L)
        && _M_matches_node_in_other_ds(__N, __V, __L);
  }

  size_type
  _M_count_within_range(_Link_const_type __N, _Region_ const& __REGION,
                        _Region_ const& __BOUNDS,
                        size_type const __L) const
  {
    size_type count = 0;
    if (__REGION.encloses(_S_value(__N)))
    {
      ++count;
    }
    if (_S_left(__N))
    {
      _Region_ __bounds(__BOUNDS);
      __bounds.set_high_bound(_S_value(__N), __L);
      if (__REGION.intersects_with(__bounds))
        count += _M_count_within_range(_S_left(__N),
                                       __REGION, __bounds, __L+1);
    }
    if (_S_right(__N))
    {
      _Region_ __bounds(__BOUNDS);
      __bounds.set_low_bound(_S_value(__N), __L);
      if (__REGION.intersects_with(__bounds))
        count += _M_count_within_range(_S_right(__N),
                                       __REGION, __bounds, __L+1);
    }

    return count;
  }


  template <class Visitor>
  Visitor
  _M_visit_within_range(Visitor visitor,
                        _Link_const_type N, _Region_ const& REGION,
                        _Region_ const& BOUNDS,
                        size_type const L) const
  {
    if (REGION.encloses(_S_value(N)))
    {
      visitor(_S_value(N));
    }
    if (_S_left(N))
    {
      _Region_ bounds(BOUNDS);
      bounds.set_high_bound(_S_value(N), L);
      if (REGION.intersects_with(bounds))
        visitor = _M_visit_within_range(visitor, _S_left(N),
                                        REGION, bounds, L+1);
    }
    if (_S_right(N))
    {
      _Region_ bounds(BOUNDS);
      bounds.set_low_bound(_S_value(N), L);
      if (REGION.intersects_with(bounds))
        visitor = _M_visit_within_range(visitor, _S_right(N),
                                        REGION, bounds, L+1);
    }

    return visitor;
  }



  template <typename _OutputIterator>
  _OutputIterator
  _M_find_within_range(_OutputIterator out,
                       _Link_const_type __N, _Region_ const& __REGION,
                       _Region_ const& __BOUNDS,
                       size_type const __L) const
  {
    if (__REGION.encloses(_S_value(__N)))
    {
      *out++ = _S_value(__N);
    }
    if (_S_left(__N))
    {
      _Region_ __bounds(__BOUNDS);
      __bounds.set_high_bound(_S_value(__N), __L);
      if (__REGION.intersects_with(__bounds))
        out = _M_find_within_range(out, _S_left(__N),
                                   __REGION, __bounds, __L+1);
    }
    if (_S_right(__N))
    {
      _Region_ __bounds(__BOUNDS);
      __bounds.set_low_bound(_S_value(__N), __L);
      if (__REGION.intersects_with(__bounds))
        out = _M_find_within_range(out, _S_right(__N),
                                   __REGION, __bounds, __L+1);
    }

    return out;
  }


  template <typename _Iter>
  void
  _M_optimise(_Iter const& __A, _Iter const& __B,
              size_type const __L)
  {
    if (__A == __B) return;
    _Node_compare_ compare(__L % __K, _M_acc, _M_cmp);
    _Iter __m = __A + (__B - __A) / 2;
    std::nth_element(__A, __m, __B, compare);
    this->insert(*__m);
    if (__m != __A) _M_optimise(__A, __m, __L+1);
    if (++__m != __B) _M_optimise(__m, __B, __L+1);
  }

  _Link_const_type
  _M_get_root() const
  {
    return const_cast<_Link_const_type>(_M_root);
  }

  _Link_type
  _M_get_root()
  {
    return _M_root;
  }

  void _M_set_root(_Link_type n)
  {
    _M_root = n;
  }

  _Link_const_type
  _M_get_leftmost() const
  {
    return static_cast<_Link_type>(_M_header._M_left);
  }

  void
  _M_set_leftmost( _Node_base * a )
  {
    _M_header._M_left = a;
  }

  _Link_const_type
  _M_get_rightmost() const
  {
    return static_cast<_Link_type>( _M_header._M_right );
  }

  void
  _M_set_rightmost( _Node_base * a )
  {
    _M_header._M_right = a;
  }

  static _Link_type
  _S_parent(_Base_ptr N)
  {
    return static_cast<_Link_type>( N->_M_parent );
  }

  static _Link_const_type
  _S_parent(_Base_const_ptr N)
  {
    return static_cast<_Link_const_type>( N->_M_parent );
  }

  static void
  _S_set_parent(_Base_ptr N, _Base_ptr p)
  {
    N->_M_parent = p;
  }

  static void
  _S_set_left(_Base_ptr N, _Base_ptr l)
  {
    N->_M_left = l;
  }

  static _Link_type
  _S_left(_Base_ptr N)
  {
    return static_cast<_Link_type>( N->_M_left );
  }

  static _Link_const_type
  _S_left(_Base_const_ptr N)
  {
    return static_cast<_Link_const_type>( N->_M_left );
  }

  static void
  _S_set_right(_Base_ptr N, _Base_ptr r)
  {
    N->_M_right = r;
  }

  static _Link_type
  _S_right(_Base_ptr N)
  {
    return static_cast<_Link_type>( N->_M_right );
  }

  static _Link_const_type
  _S_right(_Base_const_ptr N)
  {
    return static_cast<_Link_const_type>( N->_M_right );
  }

  static bool
  _S_is_leaf(_Base_const_ptr N)
  {
    return !_S_left(N) && !_S_right(N);
  }

  static const_reference
  _S_value(_Link_const_type N)
  {
    return N->_M_value;
  }

  static const_reference
  _S_value(_Base_const_ptr N)
  {
    return static_cast<_Link_const_type>(N)->_M_value;
  }

  static _Link_const_type
  _S_minimum(_Link_const_type __X)
  {
    return static_cast<_Link_const_type> ( _Node_base::_S_minimum(__X) );
  }

  static _Link_const_type
  _S_maximum(_Link_const_type __X)
  {
    return static_cast<_Link_const_type>( _Node_base::_S_maximum(__X) );
  }


  _Link_type
  _M_new_node(const_reference __V, //  = value_type(),
              _Base_ptr const __PARENT = nullptr,
              _Base_ptr const __LEFT = nullptr,
              _Base_ptr const __RIGHT = nullptr)
  {
    typename _Base::NoLeakAlloc noleak(this);
    _Link_type new_node = noleak.get();
    _Base::_M_construct_node(new_node, __V, __PARENT, __LEFT, __RIGHT);
    noleak.disconnect();
    return new_node;
  }

  /* WHAT was this for?
      _Link_type
      _M_clone_node(_Link_const_type __X)
      {
        _Link_type __ret = _M_allocate_node(__X->_M_value);
        // TODO
        return __ret;
      }
      */

  void
  _M_delete_node(_Link_type __p)
  {
    _Base::_M_destroy_node(__p);
    _Base::_M_deallocate_node(__p);
  }

  _Link_type _M_root;
  _Node_base _M_header;
  size_type _M_count;
  _Acc _M_acc;
  _Cmp _M_cmp;
  _Dist _M_dist;

#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
  friend std::ostream&
  operator<<(std::ostream& o,
             KDTree<__K, _Val, _Acc, _Dist, _Cmp, _Alloc> const& tree)
  {
    o << "meta node:   " << tree._M_header << std::endl;
    o << "root node:   " << tree._M_root << std::endl;

    if (tree.empty())
      return o << "[empty " << __K << "d-tree " << &tree << "]";

    o << "nodes total: " << tree.size() << std::endl;
    o << "dimensions:  " << __K << std::endl;

    typedef KDTree<__K, _Val, _Acc, _Dist, _Cmp, _Alloc> _Tree;
    typedef typename _Tree::_Link_type _Link_type;

    std::stack<_Link_const_type> s;
    s.push(tree._M_get_root());

    while (!s.empty())
    {
      _Link_const_type n = s.top();
      s.pop();
      o << *n << std::endl;
      if (_Tree::_S_left(n)) s.push(_Tree::_S_left(n));
      if (_Tree::_S_right(n)) s.push(_Tree::_S_right(n));
    }

    return o;
  }
#endif

};


} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 * Parts of this file are (c) 2004-2007 Paul Harris <paulharris@computer.org>.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */
