//----------------------------------------------------------------------

// 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

}