/* SUM-MAX MAPS FOR TEMPLATE MATCHING INFERENCE */
# include <stdio.h>
# include <stdlib.h>
# include "mex.h"        
# include "math.h"
# 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 numOrient, numResolution, numElement; /* number of resolutions and number of Gabors */   
double *allSizex, *allSizey;  /* sizes of images at multiple resolutions */
float **MAX1map, **SUM2map, **averageMap; 
int sizex, sizey, subsample, sizexSubsample, sizeySubsample; /* MAX1 maps are smaller than SUM1 maps by subsample */ 
int sizeTemplatex, sizeTemplatey, sizeTemplatexSubsample, sizeTemplateySubsample, halfTemplatexSubsample, halfTemplateySubsample, sizexEnlarge, sizeyEnlarge;
double *selectedOrient, *selectedx, *selectedy, *selectedlambda, *selectedLogZ; /* parameters of learned active basis */  
double saturation; /* for sigmoid transformation */
double thresholdFactor, threshold, sumLogZ; /* match template only on those pixels whose local average is above threshold */
int *iselectedOrient, *iselectedx, *iselectedy; /* integer vector */
/* sigmoid transformation */
float Sigmoid(float r)
{
   return(saturation*(2./(1.+exp(-2.*r/saturation))-1.)); 
}

void Initialize()
{
   int t; 
   
   sumLogZ = 0.; 
   for (t=0; t<numElement; t++)
   {
       sumLogZ += selectedLogZ[t]; 
   }
   
   iselectedOrient = int_vector(numElement); 
   iselectedx = int_vector(numElement); 
   iselectedy = int_vector(numElement);    
   for (t=0; t<numElement; t++)
   {
       iselectedOrient[t] = ROUND(selectedOrient[t]); 
       iselectedx[t] = ROUND(selectedx[t]); 
       iselectedy[t] = ROUND(selectedy[t]); 
   }
}   
   
/* compute the threshold */
void ComputeThreshold(int resolution)
{
   int x, y, here, t; 
   float maxAverage; 
   
   maxAverage = NEGMAX; 
   for (x=1; x<=sizex; x++)
       for (y=1; y<=sizey; y++)
   {
       here = px(x, y, sizex, sizey); 
       if (maxAverage < averageMap[resolution][here])
           maxAverage = averageMap[resolution][here]; 
   }
   threshold = maxAverage*thresholdFactor; 
   

}
/* compute SUM2 maps for image img, the size of SUM2 is the same as that of MAX1 */
void ComputeSUM2map(int resolution)
{
   int t, x, y, here, x1, y1, besto, bestx, besty, x2, y2; 
   double bestlambda, bestLogZ, sumLogZ;
   float averageHere; 
   
   for (x=-sizeTemplatexSubsample+1; x<=sizexSubsample; x++)
       for (y=-sizeTemplateySubsample+1; y<=sizeySubsample; y++)  
      {
       here = px(x+sizeTemplatexSubsample, y+sizeTemplateySubsample, sizexEnlarge, sizeyEnlarge); 
       x2 = (x+halfTemplatexSubsample)*subsample; y2 = (y+halfTemplateySubsample)*subsample; 
       if ((x2>=1)&&(x2<=sizex)&&(y2>=1)&&(y2<=sizey))
             averageHere = averageMap[resolution][px(x2, y2, sizex, sizey)];  
       else
             averageHere = 1.;   
       if (averageHere > threshold)
       {
          for (t=0; t<numElement; t++) 
          {
             x1 = x+iselectedx[t]; y1 = y+iselectedy[t];        
             if ((x1>=1)&&(x1<=sizexSubsample)&&(y1>=1)&&(y1<=sizeySubsample))
                 {      
                  SUM2map[resolution][here] += (selectedlambda[t]*Sigmoid(MAX1map[iselectedOrient[t]*numResolution+resolution][px(x1, y1, sizexSubsample, sizeySubsample)]/averageHere) - selectedLogZ[t]);  
                 }
             else 
                  SUM2map[resolution][here] -= selectedLogZ[t]; 
          }
       }
       else 
           SUM2map[resolution][here] -= sumLogZ; 
      }
}          
/* read in the input and output variables */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int resolution, orient, c; 
 mxArray *f;  
 
 c = 0; 
 numResolution = ROUND(mxGetScalar(prhs[c++]));
 allSizex = mxGetPr(prhs[c++]);
 allSizey = mxGetPr(prhs[c++]); 
 numOrient = ROUND(mxGetScalar(prhs[c++]));
 subsample = ROUND(mxGetScalar(prhs[c++]));
 sizeTemplatex = ROUND(mxGetScalar(prhs[c++]));  
 sizeTemplatey = ROUND(mxGetScalar(prhs[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++]);   
 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++;
 SUM2map = mxCalloc(numResolution, sizeof(float*)); 
 for (resolution=0; resolution<numResolution; resolution++)
   {
     f = mxGetCell(prhs[c], resolution); 
     SUM2map[resolution] = mxGetPr(f);  
  } 
 c++; 
 averageMap = mxCalloc(numResolution, sizeof(float*));   
 for (resolution=0; resolution<numResolution; resolution++)
      {  
       f = mxGetCell(prhs[c], resolution);  
       averageMap[resolution] = mxGetPr(f);       
      }
 c++;
 saturation = mxGetScalar(prhs[c++]);
 thresholdFactor = mxGetScalar(prhs[c++]);
 
 Initialize(); 
 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]); 
 sizexSubsample = floor((double)sizex/subsample); 
 sizeySubsample = floor((double)sizey/subsample); 
 sizexEnlarge = floor((double)(sizeTemplatex+sizex)/subsample); 
 sizeyEnlarge = floor((double)(sizeTemplatey+sizey)/subsample);
 ComputeThreshold(resolution); 
 ComputeSUM2map(resolution);    
 }
 mxFree(iselectedOrient);
 mxFree(iselectedx);
 mxFree(iselectedy);
}





 

                    