/* Nearest neighbor search
 *
 *	K = DSEARCH(X,Y,TRI,XI,YI) returns the index of the nearest (x,y)
 *	point to the point (xi,yi). Requires a triangularization TRI of
 *	the points X,Y obtained from DELAUNAY.
 *
 *	K = DSEARCH(X,Y,TRI,XI,YI,S) uses the sparse matrix S instead of
 *	computing it each time:
 *	
 *	  S = sparse(tri(:,[1 1 2 2 3 3]),tri(:,[2 3 1 3 1 2]),1,nxy,nxy) 
 *
 *	where nxy = prod(size(x)).
 *
 *	See also TSEARCH, DELAUNAY, VORONOI.
 *
 *	Change History (most recent first):
 *
 *		 <4>	 7/30/95	CMT		Walk down triangularization instead of 
 *                                  using the 2-D tree scheme.  Also accept a
 *                                  sparse matrix input in lieu of computing
 *									it.
 *		 <3>	 7/16/95	CMT		Search down natural neighbors to find
 *                                  nearest point.	Also return index to 
 *                                  nearest triangle.
 *		 <2>	 7/16/95	CMT		Convert to point-based scheme instead of
 *                                  triangle based scheme.
 *		 <1>	 7/15/95	CMT		Initial revision.  Doesn't work yet.
 *
 *	Clay M. Thompson 8-15-95
 *	Copyright (c) 1984-98 by The MathWorks, Inc.
 *	$Revision: 1.5 $
 */

#include "mex.h"

#define DOUBLE2INT(x) ((int) x)
#define malloc(siz) mxCalloc(1,siz)
#define free(x) mxFree(x)
#define HandleError mexErrMsgTxt

/* Global variables */
static double 	*x, *xi;
static double 	*y, *yi;
static double	*tri;
static int		nxy, ntri;
static int		*ir,*jc; /* Nearest Neighbor sparse structure */
static bool		warned;

/* Distance from (vx,vy) to x(k) */
static double dist(double vx,double vy, int k)
{
	if (k < 0 || k > nxy)
		mexErrMsgTxt("TRI or S array contains values outside the range of X and Y.");
		
	/* Return squared Euclidean distance */
	return( (x[k]-vx)*(x[k]-vx) + (y[k]-vy)*(y[k]-vy) );
}

/* Search for nearest point to k starting from p */
static int search(int k,int p)
{
	double vx, vy;
	int	prev,minp,j;
	double d,mindist;
	int count = 2*nxy; /* Used to catch non-convergence */
	
	vx = xi[k]; vy = yi[k];
	
	mindist = dist(vx,vy,p);
	minp = p;
	do
	{
		prev = p;
		
		for (j=jc[p]; j<jc[p+1] && count > 0; ++j, --count)
		{
			d = dist(vx,vy,ir[j]);
			if (d < mindist) 
			{
				mindist = d;
				minp = ir[j];
			}
		}
		p = minp;
		
		if (count == 0)
		{
			if (!warned)
			{
				mexWarnMsgTxt("One or more points did not converge.");
				warned = true;
			}
			break;
		}
	} while (prev != p);
	
	return(p);
}

void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
	int 	i;
	int 	n;
	double 	*p,*q;
	mxArray *ins[5];
	mxArray *nearNeigh[1];
	int 	k;
	
	if (nrhs > 6) mexErrMsgTxt("Too many input arguments.");
	if (nrhs < 5) mexErrMsgTxt("Not enough input arguments.");
	if (nlhs > 1) mexErrMsgTxt("Too many output arguments.");
	
	if (mxGetNumberOfElements(prhs[0]) != mxGetNumberOfElements(prhs[1]))
		mexErrMsgTxt("X and Y must be the same length.");
	if ((mxGetM(prhs[3]) != mxGetM(prhs[4])) || (mxGetN(prhs[3]) != mxGetN(prhs[4])))
		mexErrMsgTxt("XI and YI must the same size.");
	if (mxGetN(prhs[2])!=3) HandleError("TRI must be M-by-3.");
	
	nxy = mxGetNumberOfElements(prhs[0]);
	x = mxGetPr(prhs[0]);
	y = mxGetPr(prhs[1]);

	ntri = mxGetM(prhs[2]);
	tri = mxGetPr(prhs[2]);
	
	n = mxGetNumberOfElements(prhs[3]);
	xi = mxGetPr(prhs[3]);
	yi = mxGetPr(prhs[4]);
	
	plhs[0] = mxCreateDoubleMatrix(mxGetM(prhs[3]),mxGetN(prhs[3]),mxREAL);
	
	if (nrhs == 6)
      nearNeigh[0] = (mxArray *)prhs[5];
	else
	{
		/* Call back to MATLAB to create sparse natural neighbor list */
		/* tri(:,[1 1 2 2 3 3]) */
		ins[0] = mxCreateDoubleMatrix(ntri,6,mxREAL);
		p = mxGetPr(ins[0]);
		for (i=0, q = mxGetPr(prhs[2]); i < ntri; ++i) 			/* tri(:,1) */
			*p++ = *q++;
		for (i=0, q = mxGetPr(prhs[2]); i < ntri; ++i) 			/* tri(:,1) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+ntri; i < ntri; ++i) 	/* tri(:,2) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+ntri; i < ntri; ++i) 	/* tri(:,2) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+2*ntri; i < ntri; ++i) 	/* tri(:,3) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+2*ntri; i < ntri; ++i) 	/* tri(:,3) */
			*p++ = *q++;
		
		/* tri(:,[2 3 1 3 1 2]) */
		ins[1] = mxCreateDoubleMatrix(ntri,6,mxREAL);
		p = mxGetPr(ins[1]);
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+ntri; i < ntri; ++i) 	/* tri(:,2) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+2*ntri; i < ntri; ++i) 	/* tri(:,3) */
			*p++ = *q++;
		for (i=0, q = mxGetPr(prhs[2]); i < ntri; ++i) 			/* tri(:,1) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+2*ntri; i < ntri; ++i) 	/* tri(:,3) */
			*p++ = *q++;
		for (i=0, q = mxGetPr(prhs[2]); i < ntri; ++i) 			/* tri(:,1) */
			*p++ = *q++;
		for (i=0, q = ((double *)mxGetPr(prhs[2]))+ntri; i < ntri; ++i) 	/* tri(:,2) */
			*p++ = *q++;
		
		/* 1 */	
		ins[2] = mxCreateDoubleMatrix(1,1,mxREAL);
		((double *)mxGetPr(ins[2]))[0] = 1.0;
		
		/* nxy */
		ins[3] = mxCreateDoubleMatrix(1,1,mxREAL);
		((double *)mxGetPr(ins[3]))[0] = nxy;
		
		/* nxy */
		ins[4] = mxCreateDoubleMatrix(1,1,mxREAL);
		((double *)mxGetPr(ins[4]))[0] = nxy;
		
		/* sparse(tri(:,[1 1 2 2 3 3]),tri(:,[2 3 1 3 1 2]),1,nxy,nxy) */
		nearNeigh[0] = 0L;
		mexCallMATLAB(1,nearNeigh,5,ins,"sparse");
		
		/* Free inputs */
		mxDestroyArray(ins[0]);
		mxDestroyArray(ins[1]);
		mxDestroyArray(ins[2]);
		mxDestroyArray(ins[3]);
		mxDestroyArray(ins[4]);
	}
	
	if (!mxIsSparse(nearNeigh[0]))
		mexErrMsgTxt("Nearest neighbor matrix must be sparse.");
		
	ir = mxGetIr(nearNeigh[0]); /* indexes of points */
	jc = mxGetJc(nearNeigh[0]); /* indexes of points */
	
	warned = false;
	
	/* If possible, start at the first point in the first triangle */
	if (ntri > 0)
		k = ((int) tri[0]) - 1;
	else if (ntri == 0 && n > 0)
		mexErrMsgTxt("No data to search.");
	
	for (i=0, p=mxGetPr(plhs[0]); i<n; ++i)
	{
		k = search(i,k); /* Search starting from point last found */

		/* Return closest point found */
		*p++ = k+1;
	}

	if (nrhs != 6) mxDestroyArray(nearNeigh[0]);
}