void findknnmtree(double *outknn, double *outdist, const double *pX,const double *pTESTX,const struct tree *t1,const int Ni,const int N,const int DIM, const int k){
int node;
// here should be some checkups if all the variables have the right size//
poppush_t stack=poppush();
stack.init(t1->MAXTREE);
double double_max=2147483647;
double double_max2=double_max*2;
while(double_max<double_max2){
double_max=double_max2;
double_max2=double_max*2;
}
// Init heaptree
heaptree_t heap=heaptree();
heap.initheaptree(N,k);
for(int input=0;input<N;input++) {
// initialize abstract datatype of stack
int indim=input*DIM;
stack.push(0,0); // push first tree onto stack
while(1){
double mindist;
int fb;
do{
fb=stack.pop(&node,&mindist);
} while(heap.heapsize[input]==k && fb>=0 && mindist>heap.heapnodes[input][0]);
if(fb==-1) break;
int kid1=(int) t1->pKIDS[node*2]-1;
if(kid1<0) { // leaf
double thresh=heap.heapnodes[input][0]*heap.heapnodes[input][0]; // avoid some sqrt() operations
if(heap.heapsize[input]<k) thresh=double_max;
for(int i=(int) t1->pJI[node*2]-1;i<=(int) t1->pJI[node*2+1]-1;i++){
double dist=distance(&pX[DIM*i],&pTESTX[indim],DIM,thresh);
if(dist<thresh) {
heap.heapupdate(input,sqrt(dist),i+1);
thresh=heap.heapnodes[input][0]*heap.heapnodes[input][0];
if(heap.heapsize[input]<k) thresh=double_max;
}
}
}
else
{ // no leaf
int kid2=(int) t1->pKIDS[node*2+1]-1;
double d1=sqrt(distance(&t1->pPIVX[DIM*kid1],&pTESTX[indim],DIM));
double d2=sqrt(distance(&t1->pPIVX[DIM*kid2],&pTESTX[indim],DIM));
if(d1<d2) { // if kid1 is the closer child
stack.push(kid2,max(d2-t1->pRADIUS[kid2],0));
stack.push(kid1,max(d1-t1->pRADIUS[kid1],0));
} else{ // if kid2 is the closer child
stack.push(kid1,max(d1-t1->pRADIUS[kid1],0));
stack.push(kid2,max(d2-t1->pRADIUS[kid2],0));
}
}
}
}
// go through heap trees and pop value by value
int i=N*k-1;
for(int n=N-1;n>=0;n--)
for(int m=k-1;m>=0;m--){
outknn[i]=heap.heapdata[n][0];
outdist[i]=heap.heapnodes[n][0];
heap.heappoproot(n);
i--;
}
stack.cleanup();
heap.cleanup(N);
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* Declare variables. */
int DIM,N,Ni,MAXTREE,node,MAXIMPS;
double *pX, *pTESTX, *pPIVX,*pRADI;
double *pJI,*pKIDS, *pNITR, *pNITE;
int oc=0;
/* Check for proper number of input and output arguments. */
if (nrhs != EXACTINPUTS) {
mexErrMsgTxt("Not the right number of inputs.\nPlease call findknnmtree(x,testx,k,tree.pivots,tree.radius,tree.jumpindex,tree.kids);");
}
if (nlhs > 3) {
mexErrMsgTxt("Too many output arguments.");
}
/* Get the data. */
pX = (double *)mxGetPr(INPUTX);
pTESTX = (double *)mxGetPr(TESTX);
pPIVX = (double *)mxGetPr(PIVOTSX);
pRADI = (double *)mxGetPr(RADIUS);
pJI = (double *)mxGetPr(JUMPIND);
pKIDS = (double *)mxGetData(KIDS);
pNITR = (double *)mxGetData(NITR);
pNITE = (double *)mxGetData(NITE);
// printf("K=%i\n",k);
/* Get the number of elements in the input argument. */
DIM = mxGetM(INPUTX);
N = mxGetN(TESTX);
Ni = mxGetN(INPUTX);
MAXTREE = mxGetN(PIVOTSX);
if (mxGetN(NITE)!=mxGetN(TESTX)) {
mexErrMsgTxt("There must be one cutoff value for each testpoint.");
}
if (mxGetN(NITR)!=mxGetN(INPUTX)) {
mexErrMsgTxt("There must be one cutoff value for each trainingpoint.");
}
// define output matrix
MAXIMPS=10*N*2;
plhs[0]=mxCreateDoubleMatrix(2,MAXIMPS,mxREAL);
double * po=mxGetPr(plhs[0]);
// here should be some checkups if all the variables have the right size//
poppush_t stack=poppush();
stack.init(MAXTREE);
// compute MAXNI
double* MAXNI = new double[MAXTREE];
maxnirecurse(MAXNI,pKIDS, pJI, pNITR,0);
double rejected=0,taken=0;
for(int input=0;input<N;input++) {
// initialize abstract datatype of stack
int indim=input*DIM;
double sni=pNITE[input];
double sqni=sqrt(pNITE[input]);
// compute first radius
double md0=max(sqrt(distance(&pPIVX[0],&pTESTX[indim],DIM))-pRADI[0],0);
if(md0<sqni || (md0*md0)<MAXNI[0]){
stack.push(0,0); // push first tree onto stack
while(1){
double mindist;
int fb;
// do{
fb=stack.pop(&node,&mindist);
// } while(fb>=0 && mindist>sqni && mindist>MAXNI[node]);
if(fb==-1) break;
int kid1=(int) pKIDS[node*2]-1;
if(kid1<0) { // leaf
for(int i=(int) pJI[node*2]-1;i<=(int) pJI[node*2+1]-1;i++){
double cutoff=max(sni,pNITR[i]);
double dist=distance(&pX[DIM*i],&pTESTX[indim],DIM,cutoff);
if(dist<cutoff) {
po[oc]=input+1;
po[oc+1]=i+1;
oc+=2;
if(oc>=MAXIMPS-1){
printf(".");
double *temp=po;
MAXIMPS=(int) ceil((((double) MAXIMPS)/2)*(N+1)/(input+1)*1.1)*2;
mxArray *mtemp=plhs[0];
plhs[0]=mxCreateDoubleMatrix(2,MAXIMPS,mxREAL);
po=mxGetPr(plhs[0]);
memcpy(po,temp,oc*sizeof(double));
mxDestroyArray(mtemp);
}
}
}
}
else
{ // no leaf
int kid2=(int) pKIDS[node*2+1]-1;
double md1=max(sqrt(distance(&pPIVX[DIM*kid1],&pTESTX[indim],DIM))-pRADI[kid1],0);
if(md1<sqni || md1*md1<MAXNI[kid1]) stack.push(kid1,md1);
double md2=max(sqrt(distance(&pPIVX[DIM*kid2],&pTESTX[indim],DIM))-pRADI[kid2],0);
// push children onto stack
if(md2<sqni || md2*md2<MAXNI[kid2]) stack.push(kid2,md2);
}
}
}
}
stack.cleanup();
mxSetN(plhs[0],(int) ceil ((double)oc/2));
}
