//----------------------------------------------------------------------
// File:      kd_fix_rad_search.cpp
// Programmer:    Sunil Arya and David Mount
// Description:   Standard kd-tree fixed-radius kNN search
// Last modified: 05/03/05 (Version 1.1)
//----------------------------------------------------------------------
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
// David Mount.  All Rights Reserved.
// 
// This software and related documentation is part of the Approximate
// Nearest Neighbor Library (ANN).  This software is provided under
// the provisions of the Lesser GNU Public License (LGPL).  See the
// file ../ReadMe.txt for further information.
// 
// The University of Maryland (U.M.) and the authors make no
// representations about the suitability or fitness of this software for
// any purpose.  It is provided "as is" without express or implied
// warranty.
//----------------------------------------------------------------------
// History:
//  Revision 1.1  05/03/05
//    Initial release
//----------------------------------------------------------------------

#include "kd_fix_rad_search.h"      // kd fixed-radius search decls

//----------------------------------------------------------------------
//  Approximate fixed-radius k nearest neighbor search
//    The squared radius is provided, and this procedure finds the
//    k nearest neighbors within the radius, and returns the total
//    number of points lying within the radius.
//
//    The method used for searching the kd-tree is a variation of the
//    nearest neighbor search used in kd_search.cpp, except that the
//    radius of the search ball is known.  We refer the reader to that
//    file for the explanation of the recursive search procedure.
//----------------------------------------------------------------------

//----------------------------------------------------------------------
//    To keep argument lists short, a number of global variables
//    are maintained which are common to all the recursive calls.
//    These are given below.
//----------------------------------------------------------------------

int       ANNkdFRDim;       // dimension of space
ANNpoint    ANNkdFRQ;       // query point
ANNdist     ANNkdFRSqRad;     // squared radius search bound
double      ANNkdFRMaxErr;      // max tolerable squared error
ANNpointArray ANNkdFRPts;       // the points
ANNmin_k*   ANNkdFRPointMK;     // set of k closest points
int       ANNkdFRPtsVisited;    // total points visited
int       ANNkdFRPtsInRange;    // number of points in the range

//----------------------------------------------------------------------
//  annkFRSearch - fixed radius search for k nearest neighbors
//----------------------------------------------------------------------

int ANNkd_tree::annkFRSearch(
  ANNpoint      q,        // the query point
  ANNdist       sqRad,      // squared radius search bound
  int         k,        // number of near neighbors to return
  ANNidxArray     nn_idx,     // nearest neighbor indices (returned)
  ANNdistArray    dd,       // the approximate nearest neighbor
  double        eps)      // the error bound
{
  ANNkdFRDim = dim;         // copy arguments to static equivs
  ANNkdFRQ = q;
  ANNkdFRSqRad = sqRad;
  ANNkdFRPts = pts;
  ANNkdFRPtsVisited = 0;        // initialize count of points visited
  ANNkdFRPtsInRange = 0;        // ...and points in the range

  ANNkdFRMaxErr = ANN_POW(1.0 + eps);
  ANN_FLOP(2)             // increment floating op count

  ANNkdFRPointMK = new ANNmin_k(k); // create set for closest k points
                    // search starting at the root
  root->ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim));

  for (int i = 0; i < k; i++) {   // extract the k-th closest points
    if (dd != NULL)
      dd[i] = ANNkdFRPointMK->ith_smallest_key(i);
    if (nn_idx != NULL)
      nn_idx[i] = ANNkdFRPointMK->ith_smallest_info(i);
  }

  delete ANNkdFRPointMK;        // deallocate closest point set
  return ANNkdFRPtsInRange;     // return final point count
}

//----------------------------------------------------------------------
//  kd_split::ann_FR_search - search a splitting node
//    Note: This routine is similar in structure to the standard kNN
//    search.  It visits the subtree that is closer to the query point
//    first.  For fixed-radius search, there is no benefit in visiting
//    one subtree before the other, but we maintain the same basic
//    code structure for the sake of uniformity.
//----------------------------------------------------------------------

void ANNkd_split::ann_FR_search(ANNdist box_dist)
{
                    // check dist calc term condition
  if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return;

                    // distance to cutting plane
  ANNcoord cut_diff = ANNkdFRQ[cut_dim] - cut_val;

  if (cut_diff < 0) {         // left of cutting plane
    child[ANN_LO]->ann_FR_search(box_dist);// visit closer child first

    ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdFRQ[cut_dim];
    if (box_diff < 0)       // within bounds - ignore
      box_diff = 0;
                    // distance to further box
    box_dist = (ANNdist) ANN_SUM(box_dist,
        ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                    // visit further child if in range
    if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
      child[ANN_HI]->ann_FR_search(box_dist);

  }
  else {                // right of cutting plane
    child[ANN_HI]->ann_FR_search(box_dist);// visit closer child first

    ANNcoord box_diff = ANNkdFRQ[cut_dim] - cd_bnds[ANN_HI];
    if (box_diff < 0)       // within bounds - ignore
      box_diff = 0;
                    // distance to further box
    box_dist = (ANNdist) ANN_SUM(box_dist,
        ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                    // visit further child if close enough
    if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
      child[ANN_LO]->ann_FR_search(box_dist);

  }
  ANN_FLOP(13)            // increment floating ops
  ANN_SPL(1)              // one more splitting node visited
}

//----------------------------------------------------------------------
//  kd_leaf::ann_FR_search - search points in a leaf node
//    Note: The unreadability of this code is the result of
//    some fine tuning to replace indexing by pointer operations.
//----------------------------------------------------------------------

void ANNkd_leaf::ann_FR_search(ANNdist box_dist)
{
  register ANNdist dist;        // distance to data point
  register ANNcoord* pp;        // data coordinate pointer
  register ANNcoord* qq;        // query coordinate pointer
  register ANNcoord t;
  register int d;

  for (int i = 0; i < n_pts; i++) { // check points in bucket

    pp = ANNkdFRPts[bkt[i]];    // first coord of next data point
    qq = ANNkdFRQ;          // first coord of query point
    dist = 0;

    for(d = 0; d < ANNkdFRDim; d++) {
      ANN_COORD(1)        // one more coordinate hit
      ANN_FLOP(5)         // increment floating ops

      t = *(qq++) - *(pp++);    // compute length and adv coordinate
                    // exceeds dist to k-th smallest?
      if( (dist = ANN_SUM(dist, ANN_POW(t))) > ANNkdFRSqRad) {
        break;
      }
    }

    if (d >= ANNkdFRDim &&          // among the k best?
       (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem
                        // add it to the list
      ANNkdFRPointMK->insert(dist, bkt[i]);
      ANNkdFRPtsInRange++;        // increment point count
    }
  }
  ANN_LEAF(1)             // one more leaf node visited
  ANN_PTS(n_pts)            // increment points visited
  ANNkdFRPtsVisited += n_pts;     // increment number of points visited
}
