# 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 */
 int sx, sy;                 /* current size of image */
 int sub;                    /* subsampling */
 int xR, yR;
 int di; 
 int thisx, thisy; 
 double *thatx, *thaty; 

/* 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 x1, int y1)
{
   int rm[2]; 
   double tmp;  
   
   tmp = shiftmax(x1, y1, rm);   /* local maximum */  
   thatx[di] = rm[0]; thaty[di] = rm[1]; 
}

/* ========================================================================================= */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int c; 
 
 c = 0; /* counter for input pointers and scalars */
 di = floor(mxGetScalar(prhs[c++])+.5)-1; 
 sI = mxGetPr(prhs[c++]); 
 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);
 thisx = floor(mxGetScalar(prhs[c++])+.5);
 thisy = floor(mxGetScalar(prhs[c++])+.5);
 thatx = mxGetPr(prhs[c++]); 
 thaty = mxGetPr(prhs[c++]); 
 
 Cmm(thisx, thisy);      /* local maximum pooling */
}


