/* SUM-MAX MAPS FOR TEMPLATE MATCHING INFERENCE */
# include <stdio.h>
# include <stdlib.h>
# include "mex.h"        
# include "math.h"
# define PI 3.1415926
# define ABS(x) ((x)>0? (x):(-(x)))
# define MAX(x, y) ((x)>(y)? (x):(y))
# define MIN(x, y) ((x)<(y)? (x):(y))
# define ROUND(x) (floor((x)+.5))
# define NEGMAX -1e10
int *int_vector(int n)
{
    int *v; 
    v = (int*) mxCalloc (n, sizeof(int));
    return v; 
}
/* Generating integer matrix */
int **int_matrix(int m, int n)
{
    int **mat; 
    int i; 
    mat = (int**) mxCalloc(m, sizeof(int*)); 
    for (i=0; i<m; i++)
        mat[i] = int_vector(n); 
    return mat; 
}
/* Free matrix space */
void free_matrix(void **mat, int m, int n)
{
        int i;
        for (i=0; i<m; i++)
              mxFree(mat[i]);
        mxFree(mat);
}
/* 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 numOrient, locationShiftLimit, orientShiftLimit; /* key parameters */
int numResolution, numElement; /* number of resolutions and number of Gabors */   
double *allSizex, *allSizey;  /* sizes of images at multiple resolutions */
float **SUM1map, **MAX1map, **trackMap, *MAX2score, **averageMap;
int halfFilterSize; /* filter size = 2*halfFilterSize + 1 */ 
double **Correlation, **allSymbol; /* symbols of Gabors */  
int numShift, **xShift, **yShift, **orientShifted; /* stored shifts in x and y, and the shifted orientations */      
int sizex, sizey, subsample, sizexSubsample, sizeySubsample; /* MAX1 maps are smaller than SUM1 maps by subsample */ 
int sizeTemplatex, sizeTemplatey, sizeTemplatexSubsample, sizeTemplateySubsample, halfTemplatexSubsample, halfTemplateySubsample;
double *selectedOrient, *selectedx, *selectedy, *selectedlambda, *selectedLogZ; /* parameters of learned active basis */  
float **translatedTemplate; /* templates at the maximum likelihood location */    
double *allFx, *allFy; /* detected location over multiple resolutions */
double saturation; /* for sigmoid transformation */
int startx, endx, starty, endy; /* search within the interior of the image */
/* store all the shifts or perturbations in local maximum pooling */
void StoreShift()  
{
    int orient, i, j, shift, ci, cj, si, sj, os;
    double alpha; 
    /* store all the possible shifts for all orientations */
    numShift = (locationShiftLimit*2+1)*(orientShiftLimit*2+1); 
    xShift = int_matrix(numOrient, numShift); 
    yShift = int_matrix(numOrient, numShift); 
    orientShifted = int_matrix(numOrient, numShift);
    /* order the shifts from small to large */
    for (orient=0; orient<numOrient; orient++)        
    {
        alpha = PI*orient/numOrient;
        ci = 0; 
        for (i=0; i<=locationShiftLimit; i++)
           for (si=0; si<=1; si++)
           {
             cj = 0;    
             for (j = 0; j<=orientShiftLimit; j++)
                 for (sj=0; sj<=1; sj++)
                  {
                    shift = ci*(2*orientShiftLimit+1) + cj; 
                    xShift[orient][shift] = ROUND(i*(si*2-1)*subsample*cos(alpha)); 
                    yShift[orient][shift] = ROUND(i*(si*2-1)*subsample*sin(alpha)); 
                    os = orient + j*(sj*2-1);
                    if (os<0)
                        os += numOrient; 
                    else if (os>=numOrient)
                        os -= numOrient; 
                    orientShifted[orient][shift] = os; 
                    
                    if (j>0) 
                        cj ++; 
                    else if (sj==1)
                        cj++; 
                 }
             if (i>0)
                 ci ++; 
             else if (si==1)
                 ci++;                     
             }
    }
}
/* local maximum pooling at (x, y, orient) */
float LocalMaximumPooling(int img, int orient, int x, int y, int *trace) 
{
   float maxResponse, r;
   int shift, x1, y1, orient1, mshift; 
 
   maxResponse = NEGMAX; mshift = 0;
   for (shift=0; shift<numShift; shift++)
   {
       x1 = x + xShift[orient][shift]; 
       y1 = y + yShift[orient][shift]; 
       orient1 = orientShifted[orient][shift];
       if ((x1>=1)&&(x1<=sizex)&&(y1>=1)&&(y1<=sizey)) 
        {            
           r = SUM1map[orient1*numResolution+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); 
}
/* the local maximal Gabor inhibits overlapping Gabors */
void NonMaximumSuppression(int img, int mo, int mx, int my) 
{
   int x, y, orient, x1, y1, orient1, i, here, shift, startx0, endx0, starty0, endy0, trace[2]; 
   float *f, maxResponse, maxResponse1; 
   double *fc; 
   /* inhibit on the SUM1 maps */
   for (orient=0; orient<numOrient; orient++)   
     {
       f = SUM1map[orient*numResolution+img];   
       fc = Correlation[mo+orient*numOrient];   
       for (x=MAX(1, mx-2*halfFilterSize); x<=MIN(sizex, mx+2*halfFilterSize); x++)
         for (y=MAX(1, my-2*halfFilterSize); y<=MIN(sizey, my+2*halfFilterSize); y++)
         {
          f[px(x, y, sizex, sizey)] *= 
          fc[px(x-mx+2*halfFilterSize+1, y-my+2*halfFilterSize+1, 4*halfFilterSize+1, 4*halfFilterSize+1)];
         }
      }
   /* update the MAX1 maps */
   startx0 = floor((mx-2*halfFilterSize)/subsample)-locationShiftLimit+1; 
   starty0 = floor((my-2*halfFilterSize)/subsample)-locationShiftLimit+1; 
   endx0 =   floor((mx+2*halfFilterSize)/subsample)+locationShiftLimit; 
   endy0 =   floor((my+2*halfFilterSize)/subsample)+locationShiftLimit; 
   for (orient=0; orient<numOrient; orient++)   
     {
      i = orient*numResolution+img; 
      for (x=MAX(startx, startx0); x<=MIN(endx, endx0); x++)
         for (y=MAX(starty, starty0); y<=MIN(endy, endy0); y++)
         { /* go over the locations that may be affected */         
           here = px(x, y, sizexSubsample, sizeySubsample);        
           maxResponse = MAX1map[i][here]; 
           shift = ROUND(trackMap[i][here]);
           orient1 = orientShifted[orient][shift];    
           x1 = x*subsample + xShift[orient][shift]; 
           y1 = y*subsample + yShift[orient][shift];            
           if ((x1-mx>=-2*halfFilterSize)&&(x1-mx<=2*halfFilterSize)&&
               (y1-my>=-2*halfFilterSize)&&(y1-my<=2*halfFilterSize)) 
             { /* if the previous local maximum is within the inhibition range */
              if(Correlation[mo+orient1*numOrient]
                [px(x1-mx+2*halfFilterSize+1,y1-my+2*halfFilterSize+1,4*halfFilterSize+1,4*halfFilterSize+1)]==0.) 
                 {   /* if it is indeed inhibited */
                     maxResponse1 = LocalMaximumPooling(img, orient, x*subsample, y*subsample, trace); 
                     trackMap[i][here] = trace[0];  
                     MAX1map[i][here] = maxResponse1;          
                 }
             }         
         }        
      }        
}
/* plot the bar for Gabor at (mx, my, mo) */
void DrawElement(float *Template, int mo, 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[mo][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;
        }
}
/* sigmoid transformation */
float Sigmoid(float r)
{
   return(saturation*(2./(1.+exp(-2.*r/saturation))-1.)); 
}

/* compute the MAX2 score for each resolution */
void DrewTemplate(int resolution)
{
   int Fx, Fy, t, besto, bestx, besty, shift, i, here, Fbx, Fby;
   int x2, y2, mo, mx, my; 
   float averageHere, maxResponse;
   double bestlambda, bestLogZ; 

   Fx = ROUND(allFx[resolution]); Fy = ROUND(allFy[resolution]); 
   x2 = (Fx+halfTemplatexSubsample)*subsample; y2 = (Fy+halfTemplateySubsample)*subsample; 
   if ((x2>=1)&&(x2<=sizex)&&(y2>=1)&&(y2<=sizey))
      averageHere = averageMap[resolution][px(x2, y2, sizex, sizey)];  
   else
      averageHere = 1.; 
   
   /* plot the translated and deformed template */
   t = 0;    
   do  
     {
       besto = ROUND(selectedOrient[t]); bestx = ROUND(selectedx[t]); besty = ROUND(selectedy[t]);  
       bestlambda = selectedlambda[t]; bestLogZ = selectedLogZ[t]; 
       i = besto*numResolution+resolution; 
       Fbx = Fx+bestx; Fby = Fy+besty; 
       if ((Fbx>=1)&&(Fbx<=sizexSubsample)&&(Fby>=1)&&(Fby<=sizeySubsample))
       {
         here = px(Fbx, Fby, sizexSubsample, sizeySubsample); 
         shift = ROUND(trackMap[i][here]);
         mo = orientShifted[besto][shift]; 
         mx = (Fx+bestx)*subsample+xShift[besto][shift]; 
         my = (Fy+besty)*subsample+yShift[besto][shift]; 
         maxResponse = MAX1map[i][here]; 
         if (maxResponse>0.)
            {  
             DrawElement(translatedTemplate[resolution], mo, mx, my, sqrt(Sigmoid(maxResponse/averageHere))); 
             NonMaximumSuppression(resolution, mo, mx, my);  
             MAX2score[resolution] += (bestlambda*Sigmoid(maxResponse/averageHere) - bestLogZ);
           }
       }
     t++; 
     }
   while (t<numElement); 
} 
           
/* read in the input and output variables */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int resolution, orient, c, x, y, here, img, j; 
 mxArray *f;  
 
 c = 0; 
 numResolution = ROUND(mxGetScalar(prhs[c++]));
 allSizex = mxGetPr(prhs[c++]);
 allSizey = mxGetPr(prhs[c++]);
 numOrient = ROUND(mxGetScalar(prhs[c++]));   
 locationShiftLimit = ROUND(mxGetScalar(prhs[c++]));    
 orientShiftLimit = ROUND(mxGetScalar(prhs[c++]));  
 subsample = ROUND(mxGetScalar(prhs[c++]));
 halfFilterSize = ROUND(mxGetScalar(prhs[c++])); 
 allSymbol = mxCalloc(numOrient, sizeof(double*));    
 for (orient=0; orient<numOrient; orient++)
     {  
       f = mxGetCell(prhs[c], orient); 
       allSymbol[orient] = mxGetPr(f);       
     }
 c++; 
 numElement = ROUND(mxGetScalar(prhs[c++])); 
 selectedOrient = mxGetPr(prhs[c++]);              
 selectedx = mxGetPr(prhs[c++]);         
 selectedy = mxGetPr(prhs[c++]); 
 selectedlambda = mxGetPr(prhs[c++]);  
 selectedLogZ = mxGetPr(prhs[c++]);   
 SUM1map = mxCalloc(numResolution*numOrient, sizeof(float*));   
 for (img=0; img<numResolution; img++)
     for (orient=0; orient<numOrient; orient++)
      {  
       f = mxGetCell(prhs[c], orient*numResolution+img); 
       SUM1map[orient*numResolution+img] = mxGetPr(f);       
      }
 c++;
 MAX1map = mxCalloc(numResolution*numOrient, sizeof(float*));   
 for (resolution=0; resolution<numResolution; resolution++)
     for (orient=0; orient<numOrient; orient++)
      {  
       f = mxGetCell(prhs[c], orient*numResolution+resolution); 
       MAX1map[orient*numResolution+resolution] = mxGetPr(f);         
      }
 c++;
 trackMap = mxCalloc(numResolution*numOrient, sizeof(float*));   
 for (resolution=0; resolution<numResolution; resolution++)
     for (orient=0; orient<numOrient; orient++)
      {  
       f = mxGetCell(prhs[c], orient*numResolution+resolution); 
       trackMap[orient*numResolution+resolution] = mxGetPr(f);         
      }
 c++;
 translatedTemplate = mxCalloc(numResolution, sizeof(float*)); 
 for (resolution=0; resolution<numResolution; resolution++)
   {
     f = mxGetCell(prhs[c], resolution); 
     translatedTemplate[resolution] = mxGetPr(f);  
  } 
 c++; 
 allFx =  mxGetPr(prhs[c++]);
 allFy =  mxGetPr(prhs[c++]);
 saturation = mxGetScalar(prhs[c++]);
 
 sizeTemplatex = ROUND(mxGetScalar(prhs[c++]));  
 sizeTemplatey = ROUND(mxGetScalar(prhs[c++])); 
 Correlation = mxCalloc(numOrient*numOrient, sizeof(double*));   
 for (orient=0; orient<numOrient; orient++)
     {  
       for (j=0; j<numOrient; j++)
        {
         f = mxGetCell(prhs[c], j*numOrient+orient); 
         Correlation[j*numOrient+orient] = mxGetPr(f); 
        }   
     }
 c++;  
 MAX2score = mxGetPr(prhs[c++]);
 averageMap = mxCalloc(numResolution, sizeof(float*));   
 for (resolution=0; resolution<numResolution; resolution++)
      {  
       f = mxGetCell(prhs[c], resolution);  
       averageMap[resolution] = mxGetPr(f);       
      }
 c++;
 
 StoreShift();  
 sizeTemplatexSubsample = floor((double)sizeTemplatex/subsample); 
 sizeTemplateySubsample = floor((double)sizeTemplatey/subsample); 
 halfTemplatexSubsample = floor((double)sizeTemplatex/2./subsample); 
 halfTemplateySubsample = floor((double)sizeTemplatey/2./subsample); 
 for (resolution=0; resolution<numResolution; resolution++)
 {
 sizex = ROUND(allSizex[resolution]); 
 sizey = ROUND(allSizey[resolution]); 
   startx = floor((halfFilterSize+1)/subsample)+1+locationShiftLimit; 
   endx = floor((sizex-halfFilterSize)/subsample)-1-locationShiftLimit; 
   starty = floor((halfFilterSize+1)/subsample)+1+locationShiftLimit; 
   endy = floor((sizey-halfFilterSize)/subsample)-1-locationShiftLimit; 
 for (x=1; x<=sizex; x++)
    for (y=1; y<=sizey; y++)
         {  
           here = px(x, y, sizex, sizey);            
           translatedTemplate[resolution][here] = 0.; 
         }  
 sizexSubsample = floor((double)sizex/subsample); 
 sizeySubsample = floor((double)sizey/subsample); 
 DrewTemplate(resolution);  
 }
 free_matrix(xShift, numOrient, numShift); 
 free_matrix(yShift, numOrient, numShift); 
 free_matrix(orientShifted, numOrient, numShift);
}

     



 

                    