# include <stdio.h>
# include <stdlib.h>
# include "mex.h"        
# include "math.h"
# define ABS(x) ((x)>0? (x):(-(x)))
# define ROUND(x) (floor((x)+.5))
# define NEGMAX -1e10
/* Generating integer vector */
int *int_vector(int n)
{
    int *v; 
    v = (int*) mxCalloc (n, sizeof(int));
    return v; 
}
/* Compute pixel index in the vector that stores image */
int px(int x, int y, int lengthx, int lengthy)  /* the image is lengthx*lengthy */
{            
   return (x + (y-1)*lengthx - 1); 
 }
 /* variables */
int locationShiftLimit; /* key parameters */
int numImage; /* number of resolutions */   
double *allSizex, *allSizey;  /* sizes of images at multiple resolutions */
float **SUM1map, **MAX1map, **trackMap;            
int sizex, sizey, subsample, sizexSubsample, sizeySubsample; /* MAX1 maps are smaller than SUM1 maps by subsample */ 
int numShift, *xShift, *yShift; /* stored shifts in x and y, and the shifted orientations */      
/* store all the shifts or perturbations in local maximum pooling */
void StoreShift()  
{
    int shift, r, x, y;
    /* store all the possible shifts for all orientations */
    numShift = (locationShiftLimit*2+1)*(locationShiftLimit*2+1); 
    xShift = int_vector(numShift); 
    yShift = int_vector(numShift); 
    shift = 0; 
    /* order the shifts from small to large */
    for (r=0; r<=2*locationShiftLimit*locationShiftLimit; r++)        
    {
        for (x=-locationShiftLimit; x<=locationShiftLimit; x++)
            for (y=-locationShiftLimit; y<=locationShiftLimit; y++)
              {
                if (ABS(x*x+y*y-r)<.1)
                {
                    xShift[shift] = x*subsample; 
                    yShift[shift] = y*subsample; 
                    shift ++;
                }
             }
    }
 }
/* local maximum pooling at (x, y, orient) */
float LocalMaximumPooling(int img, int x, int y, int *trace) 
{
   float maxResponse, r;
   int shift, x1, y1, mshift; 
 
   maxResponse = NEGMAX; mshift = 0;  
   for (shift=0; shift<numShift; shift++)
   {
       x1 = x + xShift[shift]; 
       y1 = y + yShift[shift]; 
       if ((x1>=1)&&(x1<=sizex)&&(y1>=1)&&(y1<=sizey))
        {            
           r = SUM1map[img][px(x1, y1, sizex, sizey)]; 
           if (r>maxResponse)
                {
                   maxResponse = r;   
                   mshift = shift; 
                }
         }
     }
   trace[0] = mshift;  /* return the shift in local maximum pooling */
   return(maxResponse); 
}
   
/* computer MAX1 maps for image img */
void ComputeMAX1map(int img)
{
   int x, y, here, trace[2];  
   
   for (x=1; x<=sizexSubsample; x++)
      for (y=1; y<=sizeySubsample; y++)
       {
        here = px(x, y, sizexSubsample, sizeySubsample); 
        MAX1map[img][here] = LocalMaximumPooling(img, x*subsample, y*subsample, trace); 
        trackMap[img][here] = trace[0]; 
       }
}

void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int img, c; 
 mxArray *f;  
 
 c = 0; 
 numImage = ROUND(mxGetScalar(prhs[c++]));
 allSizex = mxGetPr(prhs[c++]);
 allSizey = mxGetPr(prhs[c++]);
 locationShiftLimit = ROUND(mxGetScalar(prhs[c++]));    
 subsample = ROUND(mxGetScalar(prhs[c++]));
 SUM1map = mxCalloc(numImage, sizeof(float*));   
 for (img=0; img<numImage; img++)
      {  
       f = mxGetCell(prhs[c], img); 
       SUM1map[img] = mxGetPr(f);       
      }
 c++;
 MAX1map = mxCalloc(numImage, sizeof(float*));   
 for (img=0; img<numImage; img++)
   {
       f = mxGetCell(prhs[c], img); 
       MAX1map[img] = mxGetPr(f);         
    }
 c++;
 trackMap = mxCalloc(numImage, sizeof(float*));   
 for (img=0; img<numImage; img++)
   {
       f = mxGetCell(prhs[c], img); 
       trackMap[img] = mxGetPr(f);         
    }
 c++;
 StoreShift();  
 for (img=0; img<numImage; img++)
 {
 sizex = ROUND(allSizex[img]); 
 sizey = ROUND(allSizey[img]); 
 sizexSubsample = floor((double)sizex/subsample); 
 sizeySubsample = floor((double)sizey/subsample); 
 ComputeMAX1map(img);  
 }
 mxFree(xShift); 
 mxFree(yShift); 
}
 


     



 

                    