// Test of closest pair algorithms
// David Eppstein, UC Irvine, 19 Apr 1997
//
// Fast pair algorithm: hybrid of conga line and nearest neighbors

#include "FastPair.h"
#include "Error.h"

// Subroutine to find nearest neighbor of a given point
void FastPair::FindNeighbor(point p)
{
	// if no neighbors avail, set flag for UpdatePoint to find
	if (npoints == 1) {
		neighbors[p] = p;
		nbr_dist[p] = MAX_DISTANCE;
		return;
	}

	// find first point unequal to p itself
	int first_nbr = 0;
	if (p == points[first_nbr]) first_nbr = 1;
	neighbors[p] = points[first_nbr];
	nbr_dist[p] = dist(p, neighbors[p]);

	// now test whether each other point is closer
	for (long i = first_nbr + 1; i < npoints; i++)
	{
		point q = points[i];
		if (q != p) {
			double d = dist(p,q);
			if (d < nbr_dist[p]) {
				nbr_dist[p] = d;
				neighbors[p] = q;
			}
		}
	}
}

// Set up
FastPair::FastPair(long np, long mp, Distance & d)
	: ClosestPairs(np,mp,d), points(new point[mp]), neighbors(new point[mp]),
	  where_are_the_points(new point[mp]), nbr_dist(new double[mp]), dist(d)
{
	if (points == 0 || where_are_the_points == 0 || neighbors == 0 ||
		nbr_dist == 0) error("NeighborCP: unable to create neighbor arrays");
		
	gInsertions += np;
	npoints = np;
	
	// set up points array for conga line
	unsigned long i;
	for (i = 0; i < np; i++) points[i] = i;

	// Find all neighbors. We use a conga line rather than calling FindNeighbor.
	point * p = points;
	while (np > 1) {
	
		// find neighbor to p[0]
		point nbr = 1;
		double nbd = dist(p[0], p[1]);
		for (i = 2; i < np; i++) {
			double d = dist(p[0],p[i]);
			if (d < nbd) {
				nbr = i;
				nbd = d;
			}
		}
		
		// add that edge, move nbr to p[1], increment p
		neighbors[p[0]] = p[nbr];
		nbr_dist[p[0]] = nbd;
		p[nbr] = p[1];
		p[1] = neighbors[p[0]];
		p++;
		np--;
	}
	
	// No more neighbors, terminate conga line
	neighbors[p[0]] = p[0];
	nbr_dist[p[0]] = MAX_DISTANCE;
	
	// set where_are...
	for (i = 0; i < npoints; i++)
		where_are_the_points[points[i]] = i;
}

// Clean up
FastPair::~FastPair()
{
	delete points;
	delete where_are_the_points;
	delete neighbors;
	delete nbr_dist;
}

// Add a point and find its nearest neighbor
void FastPair::operator += (point p)
{
	gInsertions++;
	FindNeighbor(p);
	points[where_are_the_points[p] = npoints++] = p;
}

// Remove a point and update neighbors of points for which it had been nearest
void FastPair::operator -= (point p)
{
	gDeletions++;
	npoints--;
	unsigned long q = where_are_the_points[p];
	where_are_the_points[points[q] = points[npoints]] = q;

	for (long i = 0; i < npoints; i++)
		if (neighbors[points[i]] == p)
			FindNeighbor(points[i]);
}

// Find closest pair by scanning list of nearest neighbors
double FastPair::operator () (point & a, point & b)
{
	if (npoints < 2) error("FastPair: not enough points to form pair");
	gPairs++;
	double d = nbr_dist[points[0]];
	point r = 0;
	for (long i = 1; i < npoints; i++) {
		if (nbr_dist[points[i]] < d) {
			d = nbr_dist[points[i]];
			r = i;
		}
	}
	a = points[r];
	b = neighbors[a];
	return d;
}

// All distances to point have changed, check if our structures are ok
// Note that although we completely recompute the neighbors of p,
// we don't explicitly call FindNeighbor, since that would double
// the number of distance computations made by this routine.
// Also, like deletion, we don't change any other point's neighbor to p.
void FastPair::UpdatePoint(point p)
{
	neighbors[p] = p;	// flag for not yet found any
	nbr_dist[p] = MAX_DISTANCE;
	for (long i = 0; i < npoints; i++) {
		point q = points[i];
		if (q != p) {
			double d = dist(p,q);
			if (d < nbr_dist[p]) {
				nbr_dist[p] = d;
				neighbors[p] = q;
			}
			if (neighbors[q] == p) {
				if (d > nbr_dist[q]) FindNeighbor(q);
				else nbr_dist[q] = d;
			}
		}
	}
}

// Single distance has changed, check if our structures are ok
void FastPair::UpdateDistance(point p, point q)
{
	double d = dist(p,q);

	if (d < nbr_dist[p]) {
		nbr_dist[p] = q;
		neighbors[p] = q;
	} else if (neighbors[p] == q && d > nbr_dist[p]) FindNeighbor(p);

	if (d < nbr_dist[q]) {
		nbr_dist[q] = p;
		neighbors[q] = p;
	} else if (neighbors[q] == p && d > nbr_dist[q]) FindNeighbor(q);
}