/* SUM-MAX MAPS FOR TEMPLATE MATCHING INFERENCE -- piecewise version */
# 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, numElement; /* number of resolutions and number of Gabors */   
double *allSizex, *allSizey;  /* sizes of images at multiple resolutions */
float **SUM1map, **MAX1map, **SUM2map, *MAX2score;
int halfFilterSize; /* filter size = 2*halfFilterSize + 1 */ 
double *allSymbol; /* symbols of Gabors */              
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 */      
double *selectedx, *selectedy, *selectedlambda, *selectedLogZ; /* parameters of learned active basis */  
float **translatedTemplate; /* templates at the maximum likelihood location */    
/* 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); 
}
/* plot the bar for Gabor at (mx, my, mo) */
void DrawElement(float *Template, int mx, int my, double w) 
{
  int x, y, here; 
  float a; 
          
  for (x=mx-halfFilterSize; x<=mx+halfFilterSize; x++)
     for (y=my-halfFilterSize; y<=my+halfFilterSize; y++)
      if ((x>=1)&&(x<=sizex)&&(y>=1)&&(y<=sizey)) 
        {
         a = allSymbol[px(x-mx+halfFilterSize+1, y-my+halfFilterSize+1, 
                              2*halfFilterSize+1, 2*halfFilterSize+1)]*w; 
         here = px(x, y, sizex, sizey); 
         if (Template[here]<a)
             Template[here] = a;
        }
}
/* compute SUM2 maps for image img, the size of SUM2 is the same as that of MAX1 */
void ComputeSUM2map(int img)
{
   int t, x, y, x1, y1, bestx, besty; 
   double bestlambda, bestLogZ; 
   
   t = 0; 
   do  
   {
     bestx = ROUND(selectedx[t]); besty = ROUND(selectedy[t]);  
     bestlambda = selectedlambda[t]; bestLogZ = selectedLogZ[t]; 
     for (x=1; x<=sizexSubsample; x++)
         for (y=1; y<=sizeySubsample; y++)
          {  
           x1 = x+bestx; y1 = y+besty; 
           if ((x1>=1)&&(x1<=sizexSubsample)&&(y1>=1)&&(y1<=sizeySubsample))
             SUM2map[img][px(x, y, sizexSubsample, sizeySubsample)] +=  
            (bestlambda*MAX1map[img][px(x1, y1, sizexSubsample, sizeySubsample)] - bestLogZ);  
           else 
             SUM2map[img][px(x, y, sizexSubsample, sizeySubsample)] -= bestLogZ; 
         }   
     t++; 
     }
  while (t<numElement);   
}
/* compute the MAX2 score for each resolution */
void ComputeMAX2score(int img)
{
   int x, y, Fx, Fy, t, bestx, besty, shift, trace[2];  
   float F, r, maxResponse; 

   F = NEGMAX; 
   for (x=1; x<=sizexSubsample; x++)
       for (y=1; y<=sizeySubsample; y++)
          {       
             r = SUM2map[img][px(x, y, sizexSubsample, sizeySubsample)]; 
             if (F<r) 
             {
                 F = r; Fx = x; Fy = y; 
             }
          }  
   MAX2score[img] = F;
   /* plot the translated and deformed template */ 
   t = 0; 
   do  
     {
       bestx = ROUND(selectedx[t]); besty = ROUND(selectedy[t]);   
       maxResponse = LocalMaximumPooling(img, (Fx+bestx)*subsample, (Fy+besty)*subsample, trace);
       shift = trace[0]; 
       DrawElement(translatedTemplate[img], 
            (Fx+bestx)*subsample+xShift[shift], (Fy+besty)*subsample+yShift[shift], sqrt(maxResponse)); 
     t++; 
     }
   while (t<numElement); 
} 
/* read in the input and output variables */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int img, orient, 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++]));
 halfFilterSize = 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++;
 allSymbol = mxGetPr(prhs[c++]);    
 numElement = ROUND(mxGetScalar(prhs[c++]));      
 selectedx = mxGetPr(prhs[c++]);         
 selectedy = mxGetPr(prhs[c++]);   
 selectedlambda = mxGetPr(prhs[c++]);  
 selectedLogZ = mxGetPr(prhs[c++]);  
 
 SUM2map = mxCalloc(numImage, sizeof(float*)); 
  for (img=0; img<numImage; img++)
   {
     f = mxGetCell(prhs[c], img); 
     SUM2map[img] = mxGetPr(f);  
  } 
 c++; 
 translatedTemplate = mxCalloc(numImage, sizeof(float*)); 
  for (img=0; img<numImage; img++)
   {
     f = mxGetCell(prhs[c], img); 
     translatedTemplate[img] = mxGetPr(f);  
  } 
 c++; 
 MAX2score = mxGetPr(prhs[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); 
 ComputeSUM2map(img);   
 ComputeMAX2score(img);  
 }
 mxFree(xShift); 
 mxFree(yShift); 
}

     



 

                    