# 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))

/*
* Input: Local-maxed response map
* Output: Response map of template at this level
*/

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 */
 int nbasis;                 /* number of sub-templates */
 double **M2;                /* M2 maps for sub-templates */
 double *Fx, *Fy;            /* relative locations of the sub-templates */ 
 double *outResp;            /* scores assigned to out of view sub-templates */
 int sub;                    /* subsampling */
 double *S3;                 /* S3 map */
 int sx, sy;                 /* image size */
 
/* Template matching by summation */
void Cmp()
{
   int x0, y0, x, y, x1, y1, mi, mx, my, t; 
   double resp; 
   
   t = 0; 
   do  
   {
    mx = floor(Fx[t]+.5); my = floor(Fy[t]+.5);  
    for (x=1; x<=floor(sx/sub); x++)
         for (y=1; y<=floor(sy/sub); y++)
          {            
           x0 = x*sub; y0 = y*sub;   
           x1 = x0+mx; y1 = y0+my; 
          
           if ((x1>=1)&&(x1<=sx)&&(y1>=1)&&(y1<=sy))
               resp = M2[t][px(x1, y1, sx, sy)]; 
           else
               resp = outResp[t]; 
              
           S3[px(x0, y0, sx, sy)] +=  resp;  
          }    
     t++; 
     }
  while (t<nbasis);   /* stopping criterion */  
}


/* mex function is used to pass on the pointers and scalars from matlab, 
   so that heavy computation can be done by C, which puts the results into 
   some of the pointers. After that, matlab can then use these results. 
   
   So matlab is very much like a managing platform for organizing the 
   experiments, and mex C is like a work enginee for fast computation. */

void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int c, i; 
 mxArray *f;  
 
 c = 0; /* counter for input pointers and scalars */
 nbasis = floor(mxGetScalar(prhs[c++])+.5);  /* number of sub-templates */
 M2 = mxCalloc(nbasis, sizeof(double*));   /* M2 maps*/
 for(i=0; i<nbasis; i++)
 {
    f = mxGetCell(prhs[c], i); 
    M2[i] = mxGetPr(f);    /* get pointers to M2 maps */       
 }
 c++;        
 Fx = mxGetPr(prhs[c++]);         
 Fy = mxGetPr(prhs[c++]);   /* (x, y) positions of sub-templates */
 S3 = mxGetPr(prhs[c++]);
 sx = floor(mxGetScalar(prhs[c++])+.5); 
 sy = floor(mxGetScalar(prhs[c++])+.5);
 sub = floor(mxGetScalar(prhs[c++])+.5);
 outResp = mxGetPr(prhs[c++]); /* scores for out of view sub-templates */
 
 Cmp();      /* filtering */
}

