//----------------------------------------------------------------------
//	File:		kd_util.cc
//	Programmer:	Sunil Arya and David Mount
//	Last modified:	03/04/98 (Release 0.1)
//	Description:	Common utilities for kd-trees
//
//	The following routines are utility functions for manipulating
//	points sets, used in determining splitting planes for kd-tree
//	construction.
//----------------------------------------------------------------------
// Copyright (c) 1997-1998 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).
// 
// Permission to use, copy, and distribute this software and its 
// documentation is hereby granted free of charge, provided that 
// (1) it is not a component of a commercial product, and 
// (2) this notice appears in all copies of the software and
//     related documentation. 
// 
// 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.
//----------------------------------------------------------------------

#include "kd_util.h"			// kd-utility declarations
#include <ANN/ANNperf.h>		// performance evaluation

//----------------------------------------------------------------------
//	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]);
	    dist = ANN_SUM(dist, ANN_POW(t));
	}
	else if (q[d] > hi[d]) {	// q is right of box
	    t = ANNdist(q[d]) - ANNdist(hi[d]);
	    dist = ANN_SUM(dist, ANN_POW(t));
	}
    }
    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);
    }
}
