//----------------------------------------------------------------------
// File:			kd_util.cpp
// Programmer:		Sunil Arya and David Mount
// Description:		Common utilities for kd-trees
// Last modified:	01/04/05 (Version 1.0)
//----------------------------------------------------------------------
// 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 0.1  03/04/98
//		Initial release
//----------------------------------------------------------------------

#include "kd_util.h"					// kd-utility declarations

#include <ANN/ANNperf.h>				// performance evaluation

//----------------------------------------------------------------------
// The following routines are utility functions for manipulating
// points sets, used in determining splitting planes for kd-tree
// construction.
//----------------------------------------------------------------------

//----------------------------------------------------------------------
//	NOTE: Virtually all point indexing is done through an index (i.e.
//	permutation) array pidx.  Consequently, a reference to the d-th
//	coordinate of the i-th point is pa[pidx[i]][d].  The macro PA(i,d)
//	is a shorthand for this.
//----------------------------------------------------------------------
										// standard 2-d indirect indexing
#define PA(i,d)			(pa[pidx[(i)]][(d)])
										// accessing a single point
#define PP(i)			(pa[pidx[(i)]])

//----------------------------------------------------------------------
//	annAspectRatio
//		Compute the aspect ratio (ratio of longest to shortest side)
//		of a rectangle.
//----------------------------------------------------------------------

double annAspectRatio(
	int					dim,			// dimension
	const ANNorthRect	&bnd_box)		// bounding cube
{
	ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0];
	ANNcoord min_length = length;				// min side length
	ANNcoord max_length = length;				// max side length
	for (int d = 0; d < dim; d++) {
		length = bnd_box.hi[d] - bnd_box.lo[d];
		if (length < min_length) min_length = length;
		if (length > max_length) max_length = length;
	}
	return max_length/min_length;
}

//----------------------------------------------------------------------
//	annEnclRect, annEnclCube
//		These utilities compute the smallest rectangle and cube enclosing
//		a set of points, respectively.
//----------------------------------------------------------------------

void annEnclRect(
	ANNpointArray		pa,				// point array
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					dim,			// dimension
	ANNorthRect			&bnds)			// bounding cube (returned)
{
	for (int d = 0; d < dim; d++) {		// find smallest enclosing rectangle
		ANNcoord lo_bnd = PA(0,d);		// lower bound on dimension d
		ANNcoord hi_bnd = PA(0,d);		// upper bound on dimension d
		for (int i = 0; i < n; i++) {
			if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d);
			else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d);
		}
		bnds.lo[d] = lo_bnd;
		bnds.hi[d] = hi_bnd;
	}
}

void annEnclCube(						// compute smallest enclosing cube
	ANNpointArray		pa,				// point array
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					dim,			// dimension
	ANNorthRect			&bnds)			// bounding cube (returned)
{
	int d;
										// compute smallest enclosing rect
	annEnclRect(pa, pidx, n, dim, bnds);

	ANNcoord max_len = 0;				// max length of any side
	for (d = 0; d < dim; d++) {			// determine max side length
		ANNcoord len = bnds.hi[d] - bnds.lo[d];
		if (len > max_len) {			// update max_len if longest
			max_len = len;
		}
	}
	for (d = 0; d < dim; d++) {			// grow sides to match max
		ANNcoord len = bnds.hi[d] - bnds.lo[d];
		ANNcoord half_diff = (max_len - len) / 2;
		bnds.lo[d] -= half_diff;
		bnds.hi[d] += half_diff;
	}
}

//----------------------------------------------------------------------
//	annBoxDistance - utility routine which computes distance from point to
//		box (Note: most distances to boxes are computed using incremental
//		distance updates, not this function.)
//----------------------------------------------------------------------

ANNdist annBoxDistance(			// compute distance from point to box
	const ANNpoint		q,				// the point
	const ANNpoint		lo,				// low point of box
	const ANNpoint		hi,				// high point of box
	int					dim)			// dimension of space
{
	register ANNdist dist = 0.0;		// sum of squared distances
	register ANNdist t;

	for (register int d = 0; d < dim; d++) {
		if (q[d] < lo[d]) {				// q is left of box
			t = ANNdist(lo[d]) - ANNdist(q[d]);
		}
		else if (q[d] > hi[d]) {		// q is right of box
			t = ANNdist(q[d]) - ANNdist(hi[d]);
		}
		switch(ANN::MetricType)
		{
		case ANN_METRIC0:
			dist = ANN_SUM0(dist, ANN_POW0(t));
			break;
		case ANN_METRIC1:
			dist = ANN_SUM1(dist, ANN_POW1(t));
			break;
		case ANN_METRIC2:
			dist = ANN_SUM(dist, ANN_POW(t));
			break;
		case ANN_METRICP:
			dist = ANN_SUMp(dist, ANN_POWp(t));
			break;
		}
	}
	ANN_FLOP(4*dim)						// increment floating op count

	return dist;
}

//----------------------------------------------------------------------
//	annSpread - find spread along given dimension
//	annMinMax - find min and max coordinates along given dimension
//	annMaxSpread - find dimension of max spread
//----------------------------------------------------------------------

ANNcoord annSpread(				// compute point spread along dimension
	ANNpointArray		pa,				// point array
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					d)				// dimension to check
{
	ANNcoord min = PA(0,d);				// compute max and min coords
	ANNcoord max = PA(0,d);
	for (int i = 1; i < n; i++) {
		ANNcoord c = PA(i,d);
		if (c < min) min = c;
		else if (c > max) max = c;
	}
	return (max - min);					// total spread is difference
}

void annMinMax(					// compute min and max coordinates along dim
	ANNpointArray		pa,				// point array
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					d,				// dimension to check
	ANNcoord			&min,			// minimum value (returned)
	ANNcoord			&max)			// maximum value (returned)
{
	min = PA(0,d);						// compute max and min coords
	max = PA(0,d);
	for (int i = 1; i < n; i++) {
		ANNcoord c = PA(i,d);
		if (c < min) min = c;
		else if (c > max) max = c;
	}
}

int annMaxSpread(						// compute dimension of max spread
	ANNpointArray		pa,				// point array
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					dim)			// dimension of space
{
	int max_dim = 0;					// dimension of max spread
	ANNcoord max_spr = 0;				// amount of max spread

	if (n == 0) return max_dim;			// no points, who cares?

	for (int d = 0; d < dim; d++) {		// compute spread along each dim
		ANNcoord spr = annSpread(pa, pidx, n, d);
		if (spr > max_spr) {			// bigger than current max
			max_spr = spr;
			max_dim = d;
		}
	}
	return max_dim;
}

//----------------------------------------------------------------------
//	annMedianSplit - split point array about its median
//		Splits a subarray of points pa[0..n] about an element of given
//		rank (median: n_lo = n/2) with respect to dimension d.  It places
//		the element of rank n_lo-1 correctly (because our splitting rule
//		takes the mean of these two).  On exit, the array is permuted so
//		that:
//
//		pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d].
//
//		The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the
//		splitting value.
//
//		All indexing is done indirectly through the index array pidx.
//
//		This function uses the well known selection algorithm due to
//		C.A.R. Hoare.
//----------------------------------------------------------------------

										// swap two points in pa array
#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; }

void annMedianSplit(
	ANNpointArray		pa,				// points to split
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					d,				// dimension along which to split
	ANNcoord			&cv,			// cutting value
	int					n_lo)			// split into n_lo and n-n_lo
{
	int l = 0;							// left end of current subarray
	int r = n-1;						// right end of current subarray
	while (l < r) {
		register int i = (r+l)/2;		// select middle as pivot
		register int k;

		if (PA(i,d) > PA(r,d))			// make sure last > pivot
			PASWAP(i,r)
		PASWAP(l,i);					// move pivot to first position

		ANNcoord c = PA(l,d);			// pivot value
		i = l;
		k = r;
		for(;;) {						// pivot about c
			while (PA(++i,d) < c) ;
			while (PA(--k,d) > c) ;
			if (i < k) PASWAP(i,k) else break;
		}
		PASWAP(l,k);					// pivot winds up in location k

		if (k > n_lo)	   r = k-1;		// recurse on proper subarray
		else if (k < n_lo) l = k+1;
		else break;						// got the median exactly
	}
	if (n_lo > 0) {						// search for next smaller item
		ANNcoord c = PA(0,d);			// candidate for max
		int k = 0;						// candidate's index
		for (int i = 1; i < n_lo; i++) {
			if (PA(i,d) > c) {
				c = PA(i,d);
				k = i;
			}
		}
		PASWAP(n_lo-1, k);				// max among pa[0..n_lo-1] to pa[n_lo-1]
	}
										// cut value is midpoint value
	cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0;
}

//----------------------------------------------------------------------
//	annPlaneSplit - split point array about a cutting plane
//		Split the points in an array about a given plane along a
//		given cutting dimension.  On exit, br1 and br2 are set so
//		that:
//		
//				pa[ 0 ..br1-1] <  cv
//				pa[br1..br2-1] == cv
//				pa[br2.. n -1] >  cv
//
//		All indexing is done indirectly through the index array pidx.
//
//----------------------------------------------------------------------

void annPlaneSplit(				// split points by a plane
	ANNpointArray		pa,				// points to split
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					d,				// dimension along which to split
	ANNcoord			cv,				// cutting value
	int					&br1,			// first break (values < cv)
	int					&br2)			// second break (values == cv)
{
	int l = 0;
	int r = n-1;
	for(;;) {							// partition pa[0..n-1] about cv
		while (l < n && PA(l,d) < cv) l++;
		while (r >= 0 && PA(r,d) >= cv) r--;
		if (l > r) break;
		PASWAP(l,r);
		l++; r--;
	}
	br1 = l;					// now: pa[0..br1-1] < cv <= pa[br1..n-1]
	r = n-1;
	for(;;) {							// partition pa[br1..n-1] about cv
		while (l < n && PA(l,d) <= cv) l++;
		while (r >= br1 && PA(r,d) > cv) r--;
		if (l > r) break;
		PASWAP(l,r);
		l++; r--;
	}
	br2 = l;					// now: pa[br1..br2-1] == cv < pa[br2..n-1]
}


//----------------------------------------------------------------------
//	annBoxSplit - split point array about a orthogonal rectangle
//		Split the points in an array about a given orthogonal
//		rectangle.  On exit, n_in is set to the number of points
//		that are inside (or on the boundary of) the rectangle.
//
//		All indexing is done indirectly through the index array pidx.
//
//----------------------------------------------------------------------

void annBoxSplit(				// split points by a box
	ANNpointArray		pa,				// points to split
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					dim,			// dimension of space
	ANNorthRect			&box,			// the box
	int					&n_in)			// number of points inside (returned)
{
	int l = 0;
	int r = n-1;
	for(;;) {							// partition pa[0..n-1] about box
		while (l < n && box.inside(dim, PP(l))) l++;
		while (r >= 0 && !box.inside(dim, PP(r))) r--;
		if (l > r) break;
		PASWAP(l,r);
		l++; r--;
	}
	n_in = l;					// now: pa[0..n_in-1] inside and rest outside
}

//----------------------------------------------------------------------
//	annSplitBalance - compute balance factor for a given plane split
//		Balance factor is defined as the number of points lying
//		below the splitting value minus n/2 (median).  Thus, a
//		median split has balance 0, left of this is negative and
//		right of this is positive.  (The points are unchanged.)
//----------------------------------------------------------------------

int annSplitBalance(			// determine balance factor of a split
	ANNpointArray		pa,				// points to split
	ANNidxArray			pidx,			// point indices
	int					n,				// number of points
	int					d,				// dimension along which to split
	ANNcoord			cv)				// cutting value
{
	int n_lo = 0;
	for(int i = 0; i < n; i++) {		// count number less than cv
		if (PA(i,d) < cv) n_lo++;
	}
	return n_lo - n/2;
}

//----------------------------------------------------------------------
//	annBox2Bnds - convert bounding box to list of bounds
//		Given two boxes, an inner box enclosed within a bounding
//		box, this routine determines all the sides for which the
//		inner box is strictly contained with the bounding box,
//		and adds an appropriate entry to a list of bounds.  Then
//		we allocate storage for the final list of bounds, and return
//		the resulting list and its size.
//----------------------------------------------------------------------

void annBox2Bnds(						// convert inner box to bounds
	const ANNorthRect	&inner_box,		// inner box
	const ANNorthRect	&bnd_box,		// enclosing box
	int					dim,			// dimension of space
	int					&n_bnds,		// number of bounds (returned)
	ANNorthHSArray		&bnds)			// bounds array (returned)
{
	int i;
	n_bnds = 0;									// count number of bounds
	for (i = 0; i < dim; i++) {
		if (inner_box.lo[i] > bnd_box.lo[i])	// low bound is inside
				n_bnds++;
		if (inner_box.hi[i] < bnd_box.hi[i])	// high bound is inside
				n_bnds++;
	}

	bnds = new ANNorthHalfSpace[n_bnds];		// allocate appropriate size

	int j = 0;
	for (i = 0; i < dim; i++) {					// fill the array
		if (inner_box.lo[i] > bnd_box.lo[i]) {
				bnds[j].cd = i;
				bnds[j].cv = inner_box.lo[i];
				bnds[j].sd = +1;
				j++;
		}
		if (inner_box.hi[i] < bnd_box.hi[i]) {
				bnds[j].cd = i;
				bnds[j].cv = inner_box.hi[i];
				bnds[j].sd = -1;
				j++;
		}
	}
}

//----------------------------------------------------------------------
//	annBnds2Box - convert list of bounds to bounding box
//		Given an enclosing box and a list of bounds, this routine
//		computes the corresponding inner box.  It is assumed that
//		the box points have been allocated already.
//----------------------------------------------------------------------

void annBnds2Box(
	const ANNorthRect	&bnd_box,		// enclosing box
	int					dim,			// dimension of space
	int					n_bnds,			// number of bounds
	ANNorthHSArray		bnds,			// bounds array
	ANNorthRect			&inner_box)		// inner box (returned)
{
	annAssignRect(dim, inner_box, bnd_box);		// copy bounding box to inner

	for (int i = 0; i < n_bnds; i++) {
		bnds[i].project(inner_box.lo);			// project each endpoint
		bnds[i].project(inner_box.hi);
	}
}