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

#pragma once

#include <pcl/octree/octree_pointcloud.h>
#include <pcl/point_cloud.h>

namespace pcl {
namespace octree {

/** \brief @b Octree pointcloud search class
 * \note This class provides several methods for spatial neighbor search based on octree
 * structure
 * \tparam PointT type of point used in pointcloud
 * \ingroup octree
 * \author Julius Kammerl (julius@kammerl.de)
 */
template <typename PointT,
          typename LeafContainerT = OctreeContainerPointIndices,
          typename BranchContainerT = OctreeContainerEmpty>
class OctreePointCloudSearch
: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
public:
  // public typedefs
  using IndicesPtr = shared_ptr<Indices>;
  using IndicesConstPtr = shared_ptr<const Indices>;

  using PointCloud = pcl::PointCloud<PointT>;
  using PointCloudPtr = typename PointCloud::Ptr;
  using PointCloudConstPtr = typename PointCloud::ConstPtr;

  // Boost shared pointers
  using Ptr =
      shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
  using ConstPtr = shared_ptr<
      const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;

  // Eigen aligned allocator
  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;

  using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
  using LeafNode = typename OctreeT::LeafNode;
  using BranchNode = typename OctreeT::BranchNode;

  /** \brief Constructor.
   * \param[in] resolution octree resolution at lowest octree level
   */
  OctreePointCloudSearch(const double resolution)
  : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution)
  {}

  /** \brief Search for neighbors within a voxel at given point
   * \param[in] point point addressing a leaf node voxel
   * \param[out] point_idx_data the resultant indices of the neighboring voxel points
   * \return "true" if leaf node exist; "false" otherwise
   */
  bool
  voxelSearch(const PointT& point, Indices& point_idx_data);

  /** \brief Search for neighbors within a voxel at given point referenced by a point
   * index
   * \param[in] index the index in input cloud defining the query point
   * \param[out] point_idx_data the resultant indices of the neighboring voxel points
   * \return "true" if leaf node exist; "false" otherwise
   */
  bool
  voxelSearch(uindex_t index, Indices& point_idx_data);

  /** \brief Search for k-nearest neighbors at the query point.
   * \param[in] cloud the point cloud data
   * \param[in] index the index in \a cloud representing the query point
   * \param[in] k the number of neighbors to search for
   * \param[out] k_indices the resultant indices of the neighboring points (must be
   * resized to \a k a priori!)
   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
   * points (must be resized to \a k a priori!)
   * \return number of neighbors found
   */
  inline uindex_t
  nearestKSearch(const PointCloud& cloud,
                 uindex_t index,
                 uindex_t k,
                 Indices& k_indices,
                 std::vector<float>& k_sqr_distances)
  {
    return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
  }

  /** \brief Search for k-nearest neighbors at given query point.
   * \param[in] p_q the given query point
   * \param[in] k the number of neighbors to search for
   * \param[out] k_indices the resultant indices of the neighboring points (must be
   * resized to k a priori!)
   * \param[out] k_sqr_distances  the resultant squared distances to the neighboring
   * points (must be resized to k a priori!)
   * \return number of neighbors found
   */
  uindex_t
  nearestKSearch(const PointT& p_q,
                 uindex_t k,
                 Indices& k_indices,
                 std::vector<float>& k_sqr_distances);

  /** \brief Search for k-nearest neighbors at query point
   * \param[in] index index representing the query point in the dataset given by \a
   * setInputCloud. If indices were given in setInputCloud, index will be the position
   * in the indices vector.
   * \param[in] k the number of neighbors to search for
   * \param[out] k_indices the resultant indices of the neighboring points (must be
   * resized to \a k a priori!)
   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
   * points (must be resized to \a k a priori!)
   * \return number of neighbors found
   */
  uindex_t
  nearestKSearch(uindex_t index,
                 uindex_t k,
                 Indices& k_indices,
                 std::vector<float>& k_sqr_distances);

  /** \brief Search for approx. nearest neighbor at the query point.
   * \param[in] cloud the point cloud data
   * \param[in] query_index the index in \a cloud representing the query point
   * \param[out] result_index the resultant index of the neighbor point
   * \param[out] sqr_distance the resultant squared distance to the neighboring point
   * \return number of neighbors found
   */
  inline void
  approxNearestSearch(const PointCloud& cloud,
                      uindex_t query_index,
                      index_t& result_index,
                      float& sqr_distance)
  {
    return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
  }

  /** \brief Search for approx. nearest neighbor at the query point.
   * \param[in] p_q the given query point
   * \param[out] result_index the resultant index of the neighbor point
   * \param[out] sqr_distance the resultant squared distance to the neighboring point
   */
  void
  approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);

  /** \brief Search for approx. nearest neighbor at the query point.
   * \param[in] query_index index representing the query point in the dataset given by
   * \a setInputCloud. If indices were given in setInputCloud, index will be the
   * position in the indices vector.
   * \param[out] result_index the resultant index of the neighbor point
   * \param[out] sqr_distance the resultant squared distance to the neighboring point
   * \return number of neighbors found
   */
  void
  approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);

  /** \brief Search for all neighbors of query point that are within a given radius.
   * \param[in] cloud the point cloud data
   * \param[in] index the index in \a cloud representing the query point
   * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
   * \param[out] k_indices the resultant indices of the neighboring points
   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
   * points
   * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
   * \return number of neighbors found in radius
   */
  uindex_t
  radiusSearch(const PointCloud& cloud,
               uindex_t index,
               double radius,
               Indices& k_indices,
               std::vector<float>& k_sqr_distances,
               index_t max_nn = 0)
  {
    return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
  }

  /** \brief Search for all neighbors of query point that are within a given radius.
   * \param[in] p_q the given query point
   * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
   * \param[out] k_indices the resultant indices of the neighboring points
   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
   * points
   * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
   * \return number of neighbors found in radius
   */
  uindex_t
  radiusSearch(const PointT& p_q,
               const double radius,
               Indices& k_indices,
               std::vector<float>& k_sqr_distances,
               uindex_t max_nn = 0) const;

  /** \brief Search for all neighbors of query point that are within a given radius.
   * \param[in] index index representing the query point in the dataset given by \a
   * setInputCloud. If indices were given in setInputCloud, index will be the position
   * in the indices vector
   * \param[in] radius radius of the sphere bounding all of p_q's neighbors
   * \param[out] k_indices the resultant indices of the neighboring points
   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
   * points
   * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
   * \return number of neighbors found in radius
   */
  uindex_t
  radiusSearch(uindex_t index,
               const double radius,
               Indices& k_indices,
               std::vector<float>& k_sqr_distances,
               uindex_t max_nn = 0) const;

  /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
   * (origin, direction).
   * \param[in] origin ray origin
   * \param[in] direction ray direction vector
   * \param[out] voxel_center_list results are written to this vector of PointT elements
   * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
   * disable)
   * \return number of intersected voxels
   */
  uindex_t
  getIntersectedVoxelCenters(Eigen::Vector3f origin,
                             Eigen::Vector3f direction,
                             AlignedPointTVector& voxel_center_list,
                             uindex_t max_voxel_count = 0) const;

  /** \brief Get indices of all voxels that are intersected by a ray (origin,
   * direction).
   * \param[in] origin ray origin \param[in] direction ray direction vector
   * \param[out] k_indices resulting point indices from intersected voxels
   * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
   * disable)
   * \return number of intersected voxels
   */
  uindex_t
  getIntersectedVoxelIndices(Eigen::Vector3f origin,
                             Eigen::Vector3f direction,
                             Indices& k_indices,
                             uindex_t max_voxel_count = 0) const;

  /** \brief Search for points within rectangular search area
   * Points exactly on the edges of the search rectangle are included.
   * \param[in] min_pt lower corner of search area
   * \param[in] max_pt upper corner of search area
   * \param[out] k_indices the resultant point indices
   * \return number of points found within search area
   */
  uindex_t
  boxSearch(const Eigen::Vector3f& min_pt,
            const Eigen::Vector3f& max_pt,
            Indices& k_indices) const;

protected:
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // Octree-based search routines & helpers
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief @b Priority queue entry for branch nodes
   *  \note This class defines priority queue entries for the nearest neighbor search.
   *  \author Julius Kammerl (julius@kammerl.de)
   */
  class prioBranchQueueEntry {
  public:
    /** \brief Empty constructor  */
    prioBranchQueueEntry() : node(), point_distance(0) {}

    /** \brief Constructor for initializing priority queue entry.
     * \param _node pointer to octree node
     * \param _key octree key addressing voxel in octree structure
     * \param[in] _point_distance distance of query point to voxel center
     */
    prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
    : node(_node), point_distance(_point_distance), key(_key)
    {}

    /** \brief Operator< for comparing priority queue entries with each other.
     * \param[in] rhs the priority queue to compare this against
     */
    bool
    operator<(const prioBranchQueueEntry rhs) const
    {
      return (this->point_distance > rhs.point_distance);
    }

    /** \brief Pointer to octree node. */
    const OctreeNode* node;

    /** \brief Distance to query point. */
    float point_distance;

    /** \brief Octree key. */
    OctreeKey key;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief @b Priority queue entry for point candidates
   * \note This class defines priority queue entries for the nearest neighbor point
   * candidates.
   * \author Julius Kammerl (julius@kammerl.de)
   */
  class prioPointQueueEntry {
  public:
    /** \brief Empty constructor  */
    prioPointQueueEntry() : point_idx_(0), point_distance_(0) {}

    /** \brief Constructor for initializing priority queue entry.
     * \param[in] point_idx index for a dataset point given by \a setInputCloud
     * \param[in] point_distance distance of query point to voxel center
     */
    prioPointQueueEntry(uindex_t point_idx, float point_distance)
    : point_idx_(point_idx), point_distance_(point_distance)
    {}

    /** \brief Operator< for comparing priority queue entries with each other.
     * \param[in] rhs priority queue to compare this against
     */
    bool
    operator<(const prioPointQueueEntry& rhs) const
    {
      return (this->point_distance_ < rhs.point_distance_);
    }

    /** \brief Index representing a point in the dataset given by \a setInputCloud. */
    uindex_t point_idx_;

    /** \brief Distance to query point. */
    float point_distance_;
  };

  /** \brief Helper function to calculate the squared distance between two points
   * \param[in] point_a point A
   * \param[in] point_b point B
   * \return squared distance between point A and point B
   */
  float
  pointSquaredDist(const PointT& point_a, const PointT& point_b) const;

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // Recursive search routine methods
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  /** \brief Recursive search method that explores the octree and finds neighbors within
   * a given radius
   * \param[in] point query point \param[in] radiusSquared squared search radius
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[in] tree_depth current depth/level in the octree
   * \param[out] k_indices vector of indices found to be neighbors of query point
   * \param[out] k_sqr_distances squared distances of neighbors to query point
   * \param[in] max_nn maximum of neighbors to be found
   */
  void
  getNeighborsWithinRadiusRecursive(const PointT& point,
                                    const double radiusSquared,
                                    const BranchNode* node,
                                    const OctreeKey& key,
                                    uindex_t tree_depth,
                                    Indices& k_indices,
                                    std::vector<float>& k_sqr_distances,
                                    uindex_t max_nn) const;

  /** \brief Recursive search method that explores the octree and finds the K nearest
   * neighbors
   * \param[in] point query point
   * \param[in] K amount of nearest neighbors to be found
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[in] tree_depth current depth/level in the octree
   * \param[in] squared_search_radius squared search radius distance
   * \param[out] point_candidates priority queue of nearest neigbor point candidates
   * \return squared search radius based on current point candidate set found
   */
  double
  getKNearestNeighborRecursive(
      const PointT& point,
      uindex_t K,
      const BranchNode* node,
      const OctreeKey& key,
      uindex_t tree_depth,
      const double squared_search_radius,
      std::vector<prioPointQueueEntry>& point_candidates) const;

  /** \brief Recursive search method that explores the octree and finds the approximate
   * nearest neighbor
   * \param[in] point query point
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[in] tree_depth current depth/level in the octree
   * \param[out] result_index result index is written to this reference
   * \param[out] sqr_distance squared distance to search
   */
  void
  approxNearestSearchRecursive(const PointT& point,
                               const BranchNode* node,
                               const OctreeKey& key,
                               uindex_t tree_depth,
                               index_t& result_index,
                               float& sqr_distance);

  /** \brief Recursively search the tree for all intersected leaf nodes and return a
   * vector of voxel centers. This algorithm is based off the paper An Efficient
   * Parametric Algorithm for Octree Traversal:
   * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
   * \param[in] min_x octree nodes X coordinate of lower bounding box corner
   * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
   * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
   * \param[in] max_x octree nodes X coordinate of upper bounding box corner
   * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
   * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
   * \param[in] a
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[out] voxel_center_list results are written to this vector of PointT elements
   * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
   * disable)
   * \return number of voxels found
   */
  uindex_t
  getIntersectedVoxelCentersRecursive(double min_x,
                                      double min_y,
                                      double min_z,
                                      double max_x,
                                      double max_y,
                                      double max_z,
                                      unsigned char a,
                                      const OctreeNode* node,
                                      const OctreeKey& key,
                                      AlignedPointTVector& voxel_center_list,
                                      uindex_t max_voxel_count) const;

  /** \brief Recursive search method that explores the octree and finds points within a
   * rectangular search area
   * \param[in] min_pt lower corner of search area
   * \param[in] max_pt upper corner of search area
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[in] tree_depth current depth/level in the octree
   * \param[out] k_indices the resultant point indices
   */
  void
  boxSearchRecursive(const Eigen::Vector3f& min_pt,
                     const Eigen::Vector3f& max_pt,
                     const BranchNode* node,
                     const OctreeKey& key,
                     uindex_t tree_depth,
                     Indices& k_indices) const;

  /** \brief Recursively search the tree for all intersected leaf nodes and return a
   * vector of indices. This algorithm is based off the paper An Efficient Parametric
   * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
   * \param[in] min_x octree nodes X coordinate of lower bounding box corner
   * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
   * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
   * \param[in] max_x octree nodes X coordinate of upper bounding box corner
   * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
   * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
   * \param[in] a
   * \param[in] node current octree node to be explored
   * \param[in] key octree key addressing a leaf node.
   * \param[out] k_indices resulting indices
   * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
   * disable)
   * \return number of voxels found
   */
  uindex_t
  getIntersectedVoxelIndicesRecursive(double min_x,
                                      double min_y,
                                      double min_z,
                                      double max_x,
                                      double max_y,
                                      double max_z,
                                      unsigned char a,
                                      const OctreeNode* node,
                                      const OctreeKey& key,
                                      Indices& k_indices,
                                      uindex_t max_voxel_count) const;

  /** \brief Initialize raytracing algorithm
   * \param origin
   * \param direction
   * \param[in] min_x octree nodes X coordinate of lower bounding box corner
   * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
   * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
   * \param[in] max_x octree nodes X coordinate of upper bounding box corner
   * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
   * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
   * \param a
   */
  inline void
  initIntersectedVoxel(Eigen::Vector3f& origin,
                       Eigen::Vector3f& direction,
                       double& min_x,
                       double& min_y,
                       double& min_z,
                       double& max_x,
                       double& max_y,
                       double& max_z,
                       unsigned char& a) const
  {
    // Account for division by zero when direction vector is 0.0
    const float epsilon = 1e-10f;
    if (direction.x() == 0.0)
      direction.x() = epsilon;
    if (direction.y() == 0.0)
      direction.y() = epsilon;
    if (direction.z() == 0.0)
      direction.z() = epsilon;

    // Voxel childIdx remapping
    a = 0;

    // Handle negative axis direction vector
    if (direction.x() < 0.0) {
      origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
                   origin.x();
      direction.x() = -direction.x();
      a |= 4;
    }
    if (direction.y() < 0.0) {
      origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
                   origin.y();
      direction.y() = -direction.y();
      a |= 2;
    }
    if (direction.z() < 0.0) {
      origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
                   origin.z();
      direction.z() = -direction.z();
      a |= 1;
    }
    min_x = (this->min_x_ - origin.x()) / direction.x();
    max_x = (this->max_x_ - origin.x()) / direction.x();
    min_y = (this->min_y_ - origin.y()) / direction.y();
    max_y = (this->max_y_ - origin.y()) / direction.y();
    min_z = (this->min_z_ - origin.z()) / direction.z();
    max_z = (this->max_z_ - origin.z()) / direction.z();
  }

  /** \brief Find first child node ray will enter
   * \param[in] min_x octree nodes X coordinate of lower bounding box corner
   * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
   * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
   * \param[in] mid_x octree nodes X coordinate of bounding box mid line
   * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
   * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
   * \return the first child node ray will enter
   */
  inline int
  getFirstIntersectedNode(double min_x,
                          double min_y,
                          double min_z,
                          double mid_x,
                          double mid_y,
                          double mid_z) const
  {
    int currNode = 0;

    if (min_x > min_y) {
      if (min_x > min_z) {
        // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
        if (mid_y < min_x)
          currNode |= 2;
        if (mid_z < min_x)
          currNode |= 1;
      }
      else {
        // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
        if (mid_x < min_z)
          currNode |= 4;
        if (mid_y < min_z)
          currNode |= 2;
      }
    }
    else {
      if (min_y > min_z) {
        // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
        if (mid_x < min_y)
          currNode |= 4;
        if (mid_z < min_y)
          currNode |= 1;
      }
      else {
        // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
        if (mid_x < min_z)
          currNode |= 4;
        if (mid_y < min_z)
          currNode |= 2;
      }
    }

    return currNode;
  }

  /** \brief Get the next visited node given the current node upper
   *   bounding box corner. This function accepts three float values, and
   *   three int values. The function returns the ith integer where the
   *   ith float value is the minimum of the three float values.
   * \param[in] x current nodes X coordinate of upper bounding box corner
   * \param[in] y current nodes Y coordinate of upper bounding box corner
   * \param[in] z current nodes Z coordinate of upper bounding box corner
   * \param[in] a next node if exit Plane YZ
   * \param[in] b next node if exit Plane XZ
   * \param[in] c next node if exit Plane XY
   * \return the next child node ray will enter or 8 if exiting
   */
  inline int
  getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
  {
    if (x < y) {
      if (x < z)
        return a;
      return c;
    }
    if (y < z)
      return b;
    return c;
  }
};
} // namespace octree
} // namespace pcl

#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_search.hpp>
#endif