#include "mex.h"
#include <stdio.h>
#include <math.h>

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	double *speed1,*speed2,*lsetout,*lsetin;
	double *level0_list;
	double level0[1000][2];
	double imin,jmin;
	double *nb;
	unsigned int m,n,i,j,level0_sz,k,kmin,kkmin,k1,k2;
	double d,dmin;

	m=mxGetM(prhs[1]); n=mxGetN(prhs[1]);
	level0_sz=mxGetM(prhs[0]);
	plhs[0]=mxCreateDoubleMatrix(m,n,mxREAL);
	plhs[1]=mxCreateDoubleMatrix(m,n,mxREAL);
	plhs[2]=mxCreateDoubleMatrix(m,n,mxREAL);

	speed1=mxGetPr(plhs[0]);
	speed2=mxGetPr(plhs[1]);
	lsetout=mxGetPr(plhs[2]);

	level0_list=mxGetPr(prhs[0]);
	lsetin=mxGetPr(prhs[1]);
	nb=mxGetPr(prhs[2]);

	for (i=0;i<level0_sz;i++) {
		level0[i][1]=(double) *(level0_list+i);
		level0[i][2]=(double) *(level0_list+level0_sz+i);
	}
	for (i=1;i<=m;i++) {
		for (j=1;j<=n;j++) {
		   if (*(nb+m*(j-1)+i-1)!=0.0) {
			dmin=1000000; kkmin=10000;
			for (k=0;k<level0_sz;k=k+3) {
				d=(level0[k][1]-i)*(level0[k][1]-i)+
					(level0[k][2]-j)*(level0[k][2]-j);
				if (d<dmin) {
					imin=level0[k][1];
					jmin=level0[k][2];
					dmin=d; kmin=k; kkmin=k;
				}
			}
			for (k=1;k<3;k++) {
				d=(level0[kmin+k][1]-i)*(level0[kmin+k][1]-i)+
				   (level0[kmin+k][2]-j)*(level0[kmin+k][2]-j);
				if (d<dmin) {
					imin=level0[kmin+k][1];
					jmin=level0[kmin+k][2];
					dmin=d; kkmin=kmin+k;
				}
			}
			if (kmin==0) {
				k1=level0_sz-2; k2=level0_sz-1;
			} else {
				k1=kmin-2; k2=kmin-1;
			}
			for (k=k1;k<=k2;k++) {
				d=(level0[k][1]-i)*(level0[k][1]-i)+
					(level0[k][2]-j)*(level0[k][2]-j);
				if (d<dmin) {
					imin=level0[k][1];
					jmin=level0[k][2];
					dmin=d; kkmin=k;
				}
			}
			*(speed1+m*(j-1)+i-1)=kkmin;
			*(speed2+m*(j-1)+i-1)=jmin;
			if (*(lsetin+m*(j-1)+i-1)>=0)
				*(lsetout+m*(j-1)+i-1)=sqrt(dmin);
			else
				*(lsetout+m*(j-1)+i-1)=-sqrt(dmin);
		   }
		}
	}
}