# include <stdio.h>
# include <stdlib.h>
# include "mex.h"        /* the algorithm is connect to matlab */
# 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 NEGMAX -1e10

/* Using sigmoid model. */

/*
* Local Maximizatio, more general version.
* Input: Response map, neighborhood.
* Output: Local-maxed response map.
*/

double *double_vector(int n)
{
    double *v; 
    v = (double*) mxCalloc (n, sizeof(double));
    return v; 
}

int *int_vector(int n)
{
    int *v; 
    v = (int*) mxCalloc (n, sizeof(int));
    return v;
}

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;
}

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;
}

/* getting the index of matlab image, (x,y) location, (sx, sy) sizes */
int px(int x, int y, int bx, int by)
{
   return (x + (y-1)*bx - 1);
}

 /* variables */
 double *sI;                /* filtered images (data) with multiple channels */
 double *mI;                 /* local maximum pooling for filtered images */
 int sx, sy;                 /* current size of image */
 int ax, ay;                 /* half size of bounding box of learned model */
 int sub;                    /* subsampling */
 int xR, yR;
 

/* ind: index for channel */
double shiftmax(int x, int y, int *rm)
{
   double tmp, m;
   int x1, y1, dx, dy; 
   
   m = NEGMAX;  
   for (dx =-xR; dx<=xR; dx++)   
      for (dy=-yR; dy<=yR; dy++)
      {
	   x1 = x + dx*sub;
	   y1 = y + dy*sub;
	   if(x1<1 || x1>sx || y1<1 || y1>sy) 
		   continue;
	   tmp = sI[px(x1, y1, sx, sy)];
	   if(tmp>m)
	   {
		   m = tmp;
		   rm[0] = x1;
		   rm[1] = y1;
	   }
     }
   return(m);
}

/* local maximization */
void Cmm()
{
   int x, y, x1, y1, rm[2];  
   
   for(x=1; x<=floor(sx/sub); x++)
      for(y=1; y<=floor(sy/sub); y++)
      {
         x1 = x*sub; y1 = y*sub;  
         mI[px(x1, y1, sx, sy)] = shiftmax(x1, y1, rm);   /* local maximum */  
      }
}

/* ========================================================================================= */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int c; 
 
 c = 0; /* counter for input pointers and scalars */
 sI = mxGetPr(prhs[c++]); 
 mI = mxGetPr(prhs[c++]);    /* local-maxed response (output) */

 sub = floor(mxGetScalar(prhs[c++])+.5);   /* sub-sampling of Gabor filters to improve speed */
 sx = floor(mxGetScalar(prhs[c++])+.5);
 sy = floor(mxGetScalar(prhs[c++])+.5);
 xR = floor(mxGetScalar(prhs[c++])+.5);
 yR = floor(mxGetScalar(prhs[c++])+.5);

 Cmm();      /* local maximum pooling */
}