# include <stdio.h>
# include <stdlib.h>
# include "mex.h"        
# include "math.h"
# define ROUND(x) (floor((x)+.5))
# define NEGMAX -1e10
# define MAX(x, y) ((x)>(y)? (x):(y))
/* Generating double vector */
double *double_vector(int n)
{
    double *v; 
    v = (double*) mxCalloc (n, sizeof(double));    
    return v; 
}
/* Generating double matrix */
double **double_matrix(int m, int n)
{
    double **mat; 
    int i; 
    mat = (double**) mxCalloc(m, sizeof(double*)); 
    for (i=0; i<m; i++)
        mat[i] = double_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 */
double **SUM1map;    
int numImage, numOrient; 
double windowNormalizeOrNot; /* whether to normalize within scanning window */
double *allSizex, *allSizey;  /* sizes of images at multiple resolutions */        
int sizex, sizey; /* MAX1 maps are smaller than SUM1 maps by subsample */ 
int localHalfx, localHalfy, halfFilterSize; /* half size of the template window and filter */
double **SUM1mapAll, **integralMap, **averageMap;   
double thresholdFactor; 

/* Computer average map for scanning window */
void WindowAverage(int img)
{
   int x, y, here, orient, leftx, rightx, upy, lowy, k, startx[8], endx[8], starty[8], endy[8], copyx[8], copyy[8], fx, fy;  
   double maxAverage; 
   
   if (windowNormalizeOrNot<0)
   {
     for (x=0; x<sizex; x++)
       for (y=0; y<sizey; y++) 
           averageMap[img][px(x+1, y+1, sizex, sizey)] = 1.; 
   }
   else
   {
   /* compute the sum over all the orientations at each pixel */
   SUM1mapAll = double_matrix(sizex, sizey); 
   for (x=0; x<sizex; x++)
       for (y=0; y<sizey; y++)
       {
        SUM1mapAll[x][y] = 0.; 
        here = px(x+1, y+1, sizex, sizey); 
        for (orient=0; orient<numOrient; orient++)
           {    
                SUM1mapAll[x][y] += SUM1map[orient*numImage+img][here]; 
            }
       }
    /* compute the integral map */
    integralMap = double_matrix(sizex, sizey); 
    integralMap[0][0] = SUM1mapAll[0][0];
    for (x=1; x<sizex; x++)
        integralMap[x][0] = integralMap[x-1][0]+SUM1mapAll[x][0];
    for (y=1; y<sizey; y++)
        integralMap[0][y] = integralMap[0][y-1]+SUM1mapAll[0][y]; 
    for (x=1; x<sizex; x++)
	{
       for (y=1; y<sizey; y++)
	   {
			integralMap[x][y] = integralMap[x][y-1]+integralMap[x-1][y]-integralMap[x-1][y-1]+SUM1mapAll[x][y]; 
	   }
	}

    /* compute the local average around each pixel */
    leftx = halfFilterSize+localHalfx; rightx = sizex-halfFilterSize-localHalfx; 
    upy = halfFilterSize+localHalfy; lowy = sizey-halfFilterSize-localHalfy; 
    maxAverage = NEGMAX; 
	if ((leftx<rightx)&&(upy<lowy))
	{
		for (x=leftx; x<rightx; x++)
			for (y=upy; y<lowy; y++)
			{
                here = px(x+1, y+1, sizex, sizey); 
				averageMap[img][here] = (integralMap[x+localHalfx][y+localHalfy] 
				- integralMap[x-localHalfx-1][y+localHalfy] - integralMap[x+localHalfx][y-localHalfy-1]
				+ integralMap[x-localHalfx-1][y-localHalfy-1])/(2.*localHalfx+1.)/(2.*localHalfy+1.)/numOrient; 
                if (maxAverage < averageMap[img][here])
                    maxAverage = averageMap[img][here]; 
			}
        for (x=leftx; x<rightx; x++)
			for (y=upy; y<lowy; y++)
			{
                here = px(x+1, y+1, sizex, sizey); 
                averageMap[img][here] = MAX(averageMap[img][here], thresholdFactor*maxAverage); 
			}
		/* take care of the boundaries */
		k = 0;  
		/* four corners */
		startx[k] = 0; endx[k] = leftx; starty[k] = 0; endy[k] = upy; copyx[k] = leftx; copyy[k] = upy; k++; 
		startx[k] = 0; endx[k] = leftx; starty[k] = lowy; endy[k] = sizey; copyx[k] = leftx; copyy[k] = lowy-1; k++;  
		startx[k] = rightx; endx[k] = sizex; starty[k] = 0; endy[k] = upy; copyx[k] = rightx-1; copyy[k] = upy; k++; 
		startx[k] = rightx; endx[k] = sizex; starty[k] = lowy; endy[k] = sizey; copyx[k] = rightx-1; copyy[k] = lowy-1; k++; 
		/* four sides */
		startx[k] = 0; endx[k] = leftx; starty[k] = upy; endy[k] = lowy; copyx[k] = leftx; copyy[k] = -1; k++; 
		startx[k] = rightx; endx[k] = sizex; starty[k] = upy; endy[k] = lowy; copyx[k] = rightx-1; copyy[k] = -1; k++; 
		startx[k] = leftx; endx[k] = rightx; starty[k] = 0; endy[k] = upy; copyx[k] = -1; copyy[k] = upy; k++; 
		startx[k] = leftx; endx[k] = rightx; starty[k] = lowy; endy[k] = sizey; copyx[k] = -1; copyy[k] = lowy-1; k++; 
		/* propagate the average to the boundaries */
		for (k=0; k<8; k++) 
			for (x=startx[k]; x<endx[k]; x++)
				for (y=starty[k]; y<endy[k]; y++)
				{
					if (copyx[k]<0)
						fx = x; 
					else 
						fx = copyx[k]; 
					if (copyy[k]<0)
						fy = y; 
					else 
						fy = copyy[k]; 
					averageMap[img][px(x+1, y+1, sizex, sizey)] = averageMap[img][px(fx+1, fy+1, sizex, sizey)]; 
				}
	}
	else 
	{
		for (x=0; x<sizex; x++)
			for (y=0; y<sizey; y++)
				averageMap[img][px(x+1, y+1, sizex, sizey)] = 1.; 
	}
    /* free intermediate matrices */
    free_matrix(SUM1mapAll, sizex, sizey);  
    free_matrix(integralMap, sizex, sizey); 
   }
}
/* load in 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++]);
 numOrient = ROUND(mxGetScalar(prhs[c++])); 
 halfFilterSize = ROUND(mxGetScalar(prhs[c++])); 
 SUM1map = mxCalloc(numImage*numOrient, sizeof(double*));   
 for (img=0; img<numImage; img++)
     for (orient=0; orient<numOrient; orient++)
      {  
       f = mxGetCell(prhs[c], orient*numImage+img); 
       SUM1map[orient*numImage+img] = mxGetPr(f);       
      }
 c++;
 averageMap = mxCalloc(numImage, sizeof(double*));   
 for (img=0; img<numImage; img++)
      {  
       f = mxGetCell(prhs[c], img); 
       averageMap[img] = mxGetPr(f);       
      }
 c++;
 localHalfx = ROUND(mxGetScalar(prhs[c++]));
 localHalfy = ROUND(mxGetScalar(prhs[c++]));
 windowNormalizeOrNot = mxGetScalar(prhs[c++]); 
 thresholdFactor = mxGetScalar(prhs[c++]); 
 
 for (img=0; img<numImage; img++)
 {
   sizex = ROUND(allSizex[img]); 
   sizey = ROUND(allSizey[img]); 
   WindowAverage(img); 
 }
}

     



 

                    