/********************************************************************************************
  The Orthogonal Matching Pursuit Algorithm
*******************************************/
# include "time.h"
# include "mex.h"        /* the algorithm is connect to matlab */
# include "math.h"
# include "matrix.h"
# include "stdlib.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

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



void free_matrix(void **a, int nrow, int ncol)
{
	int i;
	for(i = 0; i < nrow; i++)
		mxFree(a[i]);
	mxFree(a);
}


double ROUND(double d){
    
    return floor(d+0.5);
}

/* 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); 
 }

const double** ImageMap;    /* images */
double **Corr;               /* correlation matrix for projection*/ 
int nElement;               /* height (or length) in correlation matrix */
int n;                      /* number of images */
int N;                      /* number of filters */
int M;                      /* number of orientations */
int LAYER;                  /* number of scales */
int K;                      /* number of DoGs */
double **fIr, **fIi, **fI;                /* filtered images and max image */
double **copyfIr, **copyfIi, **copyfI;

double **Crr, **Cri, **Cir, **Cii;    /* inhibition coefficients */
double *half;                      /* halfsizes of filters */
int sx, sy;                 /* sizes of image and range of max pooling */ 
double **allsymbol, **allfilterr, **allfilteri;
int* heightFilterMap,* widthFilterMap;
double *syma, **sym, **Asym, **Asym0, **Asyma;
int L, ore; 
double TOTAL; 
double epsilon, Upperbound;
double SHUTUP; 
int **sinsh, **cossh;       /* store the shift  */
int **pI;                /* indicator */
int** MAX1mapML; /*relative perturbation in location */
int** MAX1mapMX; /*x position */
int** MAX1mapMY; /*y position */
int** MAX1mapMO; /*template index */
int** MAX1mapMD; /*relative perturbation in orientation */
double** MAX1mapMC; /* MAX1 map*/

int** copyMAX1mapML; /*relative perturbation in location */
int** copyMAX1mapMX; /*x position */
int** copyMAX1mapMY; /*y position */
int** copyMAX1mapMO; /*template index */
int** copyMAX1mapMD; /*relative perturbation in orientation */
double** copyMAX1mapMC; /* MAX1 map*/

double* filterSelected;
double* xSelected;
double* ySelected;
double numSketchSelected;


double* deformed_filterSelected;
double* deformed_xSelected;
double* deformed_ySelected;

double *arList, *aiList;  /* save the least squared coefficients*/
int subsampling;
 
/* store the shift values so that we do not need to repeat the sin and cos computation */ 
void storeshift()
{
    int ind, l;
    double theta; 
    sinsh = int_matrix(M, L+L+1); 
    cossh = int_matrix(M, L+L+1); 
    for (ind=0; ind<M; ind++)        
    {
        theta = PI*ind/M; 
        for (l=-L; l<=L; l++)
         {
            sinsh[ind][l+L] = floor(l*sin(theta)+.5); 
            cossh[ind][l+L] = floor(l*cos(theta)+.5); 
        }   
    }
}

/* local max in X or Y direction */
double shiftmax(int i, int ind, int x, int y, int *rm)  
{
   double m, en;
   int layer, ind1, x1, y1, l, ml, mx, my, de, d, d1, g, r, mo, md; 
   ml = 0;
   mx = 0;
   my = 0;
   mo = 0;
   md = 0;
   layer = floor(ind/M);  /* ind: index of filter; M: number of orientation */
   ind1 = ind - layer*M;  /* orientation */
   m = NEGMAX;  
   for (l=-L; l<=L; l++)
     {
      x1 = x + cossh[ind1][l+L]; 
      y1 = y + sinsh[ind1][l+L]; 
      if ((x1>=1)&&(x1<=sx)&&(y1>=1)&&(y1<=sy))
      {
        g = px(x1, y1, sx, sy);
        for (de=-ore; de<=ore; de++)
            {
              d = de+ind1;
              d1 = d;
              if (d<0)
                 d1 = d+M;
              if (d>=M)
                  d1 = d-M;
              r = (d1+layer*M)*n+i;
              en = fI[r][g];   
              if (en>m)
                {
                   m = en; 
                   ml = l; mx = x1; my = y1; mo = d1+layer*M; md = de; 
                }
             }
        }
     }
   rm[0] = ml; rm[1] = mx; rm[2] = my; rm[3] = mo; rm[4] = md; 
   return(m); 
   /*return(Upperbound*(2./(1.+exp(-2.*m/Upperbound))-1.)); */
}



/* local max in orientation */
double shiftmaxo(int i, int ind, int x, int y, int *rm)  
{
    double m, en;
    int x1, y1, lx, ly, ml, mx, my, mo, md, g; 
    ml = 0;
    mx = 0;
    my = 0;
    mo = 0;
    md = 0;
    m = NEGMAX;  
    for (lx=-L; lx<=L; lx++)
        for (ly=-L; ly<=L; ly++)
        {
            x1 = x + lx; 
            y1 = y + ly; 
            if ((x1>=1)&&(x1<=sx)&&(y1>=1)&&(y1<=sy))
            {
                g = px(x1, y1, sx, sy);
                en = fI[ind*n+i][g];   
                if (en>m)
                {
                    m = en; 
                    ml = lx; mx = x1; my = y1; mo = ind; md = ly; 
                }
            }
        }
   rm[0] = ml; rm[1] = mx; rm[2] = my; rm[3] = mo; rm[4] = md; 
   return(m); 
    /* return(Upperbound*(2./(1.+exp(-2.*m/Upperbound))-1.)); */
}


void resetMAXmap(int i)
{
  int ind, ii,jj, h;
  int sizeX, sizeY, x, y;
  /*  reset MAX1map */  
 
  for (ind=0; ind<N; ind++)  /* number of filters */         
  {
         
          h = floor(half[ind]+.5);
          sizeX = sx-h-L-(h+1+L)+1;
          sizeY = sy-h-L-(h+1+L)+1;
             
          for (y=0; y<sizeY; y++)
          {
             for (x=0; x<sizeX; x++)
             {
                MAX1mapML[i*N+ind][x+sizeX*y] = copyMAX1mapML[i*N+ind][x+sizeX*y];
                MAX1mapMX[i*N+ind][x+sizeX*y] = copyMAX1mapMX[i*N+ind][x+sizeX*y];
                MAX1mapMY[i*N+ind][x+sizeX*y] = copyMAX1mapMY[i*N+ind][x+sizeX*y];
                MAX1mapMO[i*N+ind][x+sizeX*y] = copyMAX1mapMO[i*N+ind][x+sizeX*y];
                MAX1mapMD[i*N+ind][x+sizeX*y] = copyMAX1mapMD[i*N+ind][x+sizeX*y];
                MAX1mapMC[i*N+ind][x+sizeX*y] = copyMAX1mapMC[i*N+ind][x+sizeX*y];
             }             
          }
  }
}  

void resetSUMmap(int i)
{
  int ind, ii,jj, h;
  int sizeX, sizeY, x, y;
  /*  reset SUMmap */    
   /*  fIr, fIi, and fI */  
  for (ind=0; ind<N; ind++)
  {                
         for (jj=0; jj<sy; jj++){
           for (ii=0; ii<sx; ii++){
               fIr[ind*n+i][ii+sx*jj]=copyfIr[ind*n+i][ii+sx*jj];
               fIi[ind*n+i][ii+sx*jj]=copyfIi[ind*n+i][ii+sx*jj];
               fI[ind*n+i][ii+sx*jj]=copyfI[ind*n+i][ii+sx*jj];       
           }       
         }          
                  
  }
  
}

void recoverSUMmap(int i, int num)
{
    /*  fast version to rest the SUM1map */
   int iFilter, ind, x, y, r, h; 
   int mi, mx, my, mh;
    
    for (iFilter=0; iFilter<num; iFilter++){     /* number of filters that have been selected */  
           
       mi = (int)deformed_filterSelected[i+n*iFilter]-1;  
       mx = (int)deformed_xSelected[i+n*iFilter]; 
       my = (int)deformed_ySelected[i+n*iFilter]; 
       
       mh = floor(half[mi]+.5); 
       
       for (ind=0; ind<N; ind++)   /* number of filters */
       {
           h = floor(half[ind]+.5); 
           for (x=MAX(1, mx-mh-h); x<=MIN(sx, mx+mh+h); x++)
           for (y=MAX(1, my-mh-h); y<=MIN(sy, my+mh+h); y++)
           {
              r = px(x, y, sx, sy);              
              fIr[ind*n+i][r] = copyfIr[ind*n+i][r];
              fIi[ind*n+i][r] = copyfIi[ind*n+i][r];
              fI[ind*n+i][r] = copyfI[ind*n+i][r];
           }
           
       }
     
    }
}


/* you can update the parameter alpha to adjust inhabit neighbors */
void updateMAX1map(int i, int mi, int orgX, int orgY)
{
    int halfX, halfY;
    float alpha = 0.7;
    int k,j, x, y, h, here, rm[5], ind, sizeX, sizeY, mh;
    double mm;
    mh = floor(half[mi]+.5); 
  
    for (ind=0; ind<N; ind++)  /* all filters */
    {
            h = floor(half[ind]+.5);
            sizeX = sx-h-L-(h+1+L)+1;
            sizeY = sy-h-L-(h+1+L)+1;
            
            /* neighbor sizes. */
            /* option 1: using a parameter alpha to control the size of the update region */
            /*
            halfX = floor(h*alpha); 
            halfY = floor(h*alpha); 
            */
            /* option 2: go over all the locations that may be affected */ 
            
            halfX = floor(h+mh+L); 
            halfY = floor(h+mh+L); 
            
            for (x = MAX(orgX-halfX,h+1+L); x<=MIN(sx-h-L,orgX+halfX);x++)
            {
                for (y = MAX(orgY-halfY,h+1+L); y<=MIN(sy-h-L,orgY+halfY);y++)
                {
                    if (ind<N-K)
                        mm = shiftmax(i, ind, x, y, rm);    
                    else 
                        mm = shiftmaxo(i, ind, x, y, rm);
                    j = x-h-1-L;
                    k = y-h-1-L; 
                    MAX1mapML[i*N+ind][j*sizeY+k] = rm[0];
                    MAX1mapMX[i*N+ind][j*sizeY+k] = rm[1];
                    MAX1mapMY[i*N+ind][j*sizeY+k] = rm[2];
                    MAX1mapMO[i*N+ind][j*sizeY+k] = rm[3];
                    MAX1mapMD[i*N+ind][j*sizeY+k] = rm[4];
                    MAX1mapMC[i*N+ind][j*sizeY+k] = mm;
                }
            }
     }
     
}





void computeLSCoefficient(int indImage, int numSelectedFeature, double* arList, double* aiList)
{
    
    /* =============================================
     * Compute the B'B
     * ============================================= 
     */
     
    double *result;        /* B'B */
    int i,j;   
    int i_ind, i_row, i_col, hi; 
    int j_ind, j_row, j_col, hj; 
    int numFeatureReconstruction;
    int *selectedFilter, *selectedRow, *selectedCol;
    int g;
    double* arrayFilteringResponse;
    
    int h,w,row,col;
    int imageStartingRow,imageStartingCol;
    int iLocation;
    int iRow, iCol;
    double temp_sum;
    int heightImage, widthImage;
    int adjusted_i_ind;
    double **FilterMap;
    double *coeff;
    
    const mxArray* pA;
    mwSize dimsOutput[2];
    void* start_of_pr;
    int bytes_to_copy;
    mxArray *multiplication[2];  
    mxArray *invBB, *BB, *filteringResponse;
    mxArray *Coefficient;
    
    
    
    numFeatureReconstruction = 2*numSelectedFeature;  /* each selected feature will generate sin part and cos part*/
    
    /* create a new selected features array that includes sin and cos parts together */
    /* the form of the array is that all the sin parts are placed first, then all the cos: [sin1, sin2, ..., sinn, cos1, cos2, ..., cosn] */
    selectedFilter = (int*) mxCalloc(numFeatureReconstruction, sizeof(int));
    selectedRow = (int*) mxCalloc(numFeatureReconstruction, sizeof(int));
    selectedCol = (int*) mxCalloc(numFeatureReconstruction, sizeof(int));
    
       
    for(i=0; i<numSelectedFeature; i++ ){ 
        selectedFilter[i] = (int)deformed_filterSelected[indImage+n*i];   /*  start from 1 so far, save the index of the sin part of the selected Gabor */
        selectedFilter[i+numSelectedFeature] = (int)deformed_filterSelected[indImage+n*i] + N;   /* save the index of the cos part of the selected Gabor */
        
        selectedRow[i] = (int)deformed_xSelected[indImage+n*i];    /*  start from 1 */
        selectedRow[i+numSelectedFeature] = (int)deformed_xSelected[indImage+n*i];    /*  start from 1 */
        
        selectedCol[i] = (int)deformed_ySelected[indImage+n*i];    /*  start from 1 */
        selectedCol[i+numSelectedFeature] = (int)deformed_ySelected[indImage+n*i];    /*  start from 1 */
        
    }
    
    
    /*  initialization */
    result = (double*) mxCalloc(numFeatureReconstruction*numFeatureReconstruction, sizeof(double));
    
    for(i=0; i<numFeatureReconstruction; i++ ){
         for(j=0; j<numFeatureReconstruction; j++ ){
             result[i+j*numFeatureReconstruction]=0.0;
         }
    }
        
           
      
    /* compute B'B*/
    for(i=0; i<numFeatureReconstruction; i++ ){    
    
        i_ind=selectedFilter[i]-1;   /*  start from 0 */
        i_row=selectedRow[i];    /*  start from 1 */
        i_col=selectedCol[i];
        
        if (i_ind >= N){
           hi=half[i_ind-N];  
        }else{
           hi=half[i_ind]; 
        }
        
        result[i+i*numFeatureReconstruction]=1;
        for(j=i+1; j<numFeatureReconstruction; j++ ){        
        
            j_ind=selectedFilter[j]-1;
            j_row=selectedRow[j];
            j_col=selectedCol[j];
            
            if (j_ind >= N){
               hj=half[j_ind-N];
            }else{
               hj=half[j_ind]; 
            }
     
           if( ABS(j_row-i_row)<=(hi+hj) && ABS(j_col-i_col)<=(hi+hj)  ){
               
                g = px(j_row-i_row+hi+hj+1, j_col-i_col+hi+hj+1, 2*(hi+hj)+1, 2*(hi+hj)+1);
                result[i+j*numFeatureReconstruction]= Corr[i_ind+j_ind*nElement][g]; 
                result[j+i*numFeatureReconstruction]= result[i+j*numFeatureReconstruction];   
           }   
   
        }      
    
    }
            
   
    /* compute B'I  */
     
    heightImage = sx;
    widthImage = sy;
    
    arrayFilteringResponse = (double*)mxCalloc( numFeatureReconstruction, sizeof(double) );
    for (i=0; i<numFeatureReconstruction; i++)
    {
        
        i_ind=(int)selectedFilter[i]-1;
       
        
        if (i_ind >= N){
           adjusted_i_ind = i_ind-N; 
           FilterMap = allfilteri;    /* using cos filters*/
        }else{
           adjusted_i_ind = i_ind;  
           FilterMap = allfilterr;    /* using sin filters*/
        }
        
        h=heightFilterMap[adjusted_i_ind];
        w=widthFilterMap[adjusted_i_ind];
        
        
        row=(int)selectedRow[i]-1;
        col=(int)selectedCol[i]-1;
        
        imageStartingRow=row-ROUND(h/2.0-1);
        imageStartingCol=col-ROUND(w/2.0-1);
                       
        temp_sum=0.0;
        iLocation=0;
        
        for (iCol=imageStartingCol; iCol<=imageStartingCol+w-1; iCol++)
        {
                    
          for(iRow=imageStartingRow; iRow<=imageStartingRow+h-1; iRow++)
          {
              
               if( (iRow>=0) && (iRow<heightImage) && (iCol>=0) && (iCol<widthImage)){                                      
                   temp_sum = temp_sum + ImageMap[indImage][iRow+iCol*heightImage] * FilterMap[adjusted_i_ind][iLocation];                
                   
               }
               
               iLocation++;
          }
          
        }               
           
      arrayFilteringResponse[i]=temp_sum;    
      
    }
    
   
    
    /* compute inv(B'B) */
    dimsOutput[0] = numFeatureReconstruction; dimsOutput[1] = numFeatureReconstruction;
    BB = mxCreateNumericArray( 2, dimsOutput, mxDOUBLE_CLASS, mxREAL);
    start_of_pr = (double*)mxGetData(BB);
    bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(BB);
    memcpy( start_of_pr, result, bytes_to_copy );
       
    mexCallMATLAB(1, &invBB, 1, &BB, "pinv");
    
    /* compute inv(B'B)*(B'I) */
    dimsOutput[0] = numFeatureReconstruction; dimsOutput[1] = 1;
    filteringResponse = mxCreateNumericArray( 2, dimsOutput, mxDOUBLE_CLASS, mxREAL );
    start_of_pr = (double*)mxGetData(filteringResponse);
    bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(filteringResponse);
    memcpy( start_of_pr, arrayFilteringResponse, bytes_to_copy );  
    
    multiplication[0]=invBB;
    multiplication[1]=filteringResponse;
    mexCallMATLAB(1, &Coefficient, 2, multiplication, "*");
   
    coeff = (double*)mxGetPr(Coefficient); 
    
    /* assign least square coefficient (sin coefficient and cos coefficient) to the arList and aiList*/
    for (i=0; i<numSelectedFeature; i++){        
       arList[indImage+n*i] = coeff[i];
       aiList[indImage+n*i] = coeff[i+numSelectedFeature];
    }
    
       
    mxFree(selectedFilter);
    mxFree(selectedRow);
    mxFree(selectedCol);
    mxFree(arrayFilteringResponse);
    mxFree(result);
    
}





void inhibit(int i, int numSelectedFeature, int mi, int mx, int my)   
{
   int x, y, ind, mh, h, g, r; 
   double *fr, *fi, *f, *fcrr, *fcri, *fcir, *fcii, ar, ai, mlayer, layer;
   /* mlayer = floor(mi/M); */
   mh = floor(half[mi]+.5); 
   ar = fIr[mi*n+i][px(mx, my, sx, sy)]; 
   ai = fIi[mi*n+i][px(mx, my, sx, sy)]; 
   
   arList[i+n*(numSelectedFeature-1)] = ar;
   aiList[i+n*(numSelectedFeature-1)] = ai; 
    
   /*printf("image=%d, ar=%lf, ai=%lf/n",i,ar,ai);*/
   for (ind=0; ind<N; ind++)   /* number of filters */
     {
     /*  layer = floor(ind/M); */
       h = floor(half[ind]+.5);  
       fr = fIr[ind*n+i]; 
       fi = fIi[ind*n+i]; 
       f = fI[ind*n+i];
       fcrr = Crr[mi+ind*N]; fcri = Cri[mi+ind*N];  /* (mi, ind)*/
       fcir = Cir[mi+ind*N]; fcii = Cii[mi+ind*N];
       for (x=MAX(1, mx-mh-h); x<=MIN(sx, mx+mh+h); x++)
         for (y=MAX(1, my-mh-h); y<=MIN(sy, my+mh+h); y++)
           {
              r = px(x, y, sx, sy);
              g = px(x-mx+mh+h+1, y-my+mh+h+1, 2*(mh+h)+1, 2*(mh+h)+1);
              fr[r] -= (fcrr[g]*ar+fcir[g]*ai);
              fi[r] -= (fcri[g]*ar+fcii[g]*ai);
                          
              {
                   f[r] = (fr[r]*fr[r] +fi[r]*fi[r])/2;
                    
                   /*if (fcrr[g]*fcrr[g]+fcri[g]*fcri[g]+fcir[g]*fcir[g]+fcii[g]*fcii[g]>epsilon) f[r] = 0.; */
             
              }
           }
      }  
}



void OrthogonalInhibitAndMAX1Update(int i, int num)   
{
   int x, y, ind, mh, h, g, r, iFilter; 
   double *fr, *fi, *f, *fcrr, *fcri, *fcir, *fcii, ar, ai, mlayer, layer;
   int mi, mx, my;
   clock_t time;   
   
   /* mlayer = floor(mi/M); */
   /*time = clock();*/
  
   resetMAXmap(i); /* reset MAX1map*/
   /*time = clock()-time;
  mexPrintf("time used: %f seconds\n", (float)(time)/CLOCKS_PER_SEC ); */
   
  /*  results are returned by arList, aiList */
  
  
   /* image i, coefficients are saved in arList and aiList, one is for sin part, the other is for cos part */
  computeLSCoefficient(i, num, arList, aiList); 
 
  resetSUMmap(i);
  /*computeNormalCoefficient(i, num, arList, aiList);*/
  
  for (iFilter=0; iFilter<num; iFilter++){     /* number of filters that have been selected */  
           
    mi = (int)deformed_filterSelected[i+n*iFilter]-1;  
    mx = (int)deformed_xSelected[i+n*iFilter]; 
    my = (int)deformed_ySelected[i+n*iFilter];            
    
    mh = floor(half[mi]+.5); 
    ar = arList[i+n*iFilter];
    ai = aiList[i+n*iFilter];
    
    for (ind=0; ind<N; ind++)   /* number of filters */
    {
     /*  layer = floor(ind/M); */
       h = floor(half[ind]+.5);  
       fr = fIr[ind*n+i];  /* after reseting, fIr, fIi, and fI are the same as the original maps without selecting any basis */
       fi = fIi[ind*n+i]; 
       f = fI[ind*n+i];
       fcrr = Crr[mi+ind*N]; fcri = Cri[mi+ind*N];
       fcir = Cir[mi+ind*N]; fcii = Cii[mi+ind*N];
       for (x=MAX(1, mx-mh-h); x<=MIN(sx, mx+mh+h); x++)
         for (y=MAX(1, my-mh-h); y<=MIN(sy, my+mh+h); y++)
           {
              r = px(x, y, sx, sy);
              g = px(x-mx+mh+h+1, y-my+mh+h+1, 2*(mh+h)+1, 2*(mh+h)+1);
              fr[r] -= (fcrr[g]*ar+fcir[g]*ai);
              fi[r] -= (fcri[g]*ar+fcii[g]*ai);
              
              f[r] = (fr[r]*fr[r] +fi[r]*fi[r])/2;
           }
     }
     
     updateMAX1map(i, (int)filterSelected[iFilter]-1, (int)xSelected[iFilter], (int)ySelected[iFilter]); 
   
  }
  
  
  
/*  time = clock();
  recoverSUMmap(i, num); 
  time = clock()-time;
  mexPrintf("fast time used: %f seconds\n", (float)(time)/CLOCKS_PER_SEC ); */
 /* resetSUMmap(i);  reset fIr, fIi, fI, and MAX1map*/
  
}



void draw(double *symo, int x0, int y0, int ind)
{
  int x, y, h; 
   
  h = floor(half[ind]+.5);
  for (x=MAX(1, x0-h); x<=MIN(sx, x0+h); x++)
     for (y=MAX(1, y0-h); y<=MIN(sy, y0+h); y++)
       {
          symo[px(x, y, sx, sy)] = MAX(symo[px(x, y, sx, sy)], allsymbol[ind][px(x-x0+h+1, y-y0+h+1, 2*h+1, 2*h+1)]);
       }
}


void draw1(double *symo, int x0, int y0, int ind, int i)
{
  int x, y, h, r;
  double ar, ai; 

  ar = fIr[ind*n+i][px(x0, y0, sx, sy)]; 
  ai = fIi[ind*n+i][px(x0, y0, sx, sy)]; 
  h = floor(half[ind]+.5);
  for (x=MAX(1, x0-h); x<=MIN(sx, x0+h); x++)
     for (y=MAX(1, y0-h); y<=MIN(sy, y0+h); y++)
       {
          r = px(x-x0+h+1, y-y0+h+1, 2*h+1, 2*h+1); 
          symo[px(x, y, sx, sy)] += ar*allfilterr[ind][r]+ai*allfilteri[ind][r];
       }
}


void drawReconstruction(double **symAll, int numSelectedFeature)
{
  int x, y, h, r,i;
  double ar, ai; 
  int iF;
  int x0,y0,ind;
  
 for (i=0; i<n; i++){ 
      
  for (iF=0; iF<numSelectedFeature; iF++){
      
    x0 = (int)deformed_xSelected[i+n*iF];
    y0 = (int)deformed_ySelected[i+n*iF];
    ind = (int)deformed_filterSelected[i+n*iF]-1;
   
    ar = arList[i+n*iF];  
    ai = aiList[i+n*iF];  
            
 /*   ar = fIr[ind*n+i][px(x0, y0, sx, sy)]; 
    ai = fIi[ind*n+i][px(x0, y0, sx, sy)]; */
    
    h = floor(half[ind]+.5);
    for (x=MAX(1, x0-h); x<=MIN(sx, x0+h); x++)
     for (y=MAX(1, y0-h); y<=MIN(sy, y0+h); y++)
     {
          r = px(x-x0+h+1, y-y0+h+1, 2*h+1, 2*h+1); 
          symAll[i][px(x, y, sx, sy)] += ar*allfilterr[ind][r]+ai*allfilteri[ind][r];
     }
  }
 }
}


/* new code here, calculate MAX1map */
void calculatemax()
{
    int k,i, j, x, y, h, here, rm[5], ind, sizeX, sizeY;
    double mm;
    MAX1mapML = (int**) mxCalloc(n*N, sizeof(int*));/*relative perturbation in location */
    MAX1mapMX = (int**) mxCalloc(n*N, sizeof(int*));/*x position */
    MAX1mapMY = (int**) mxCalloc(n*N, sizeof(int*));/*y position */
    MAX1mapMO = (int**) mxCalloc(n*N, sizeof(int*));/*template index */
    MAX1mapMD = (int**) mxCalloc(n*N, sizeof(int*));/*relative perturbation in orientation */
    MAX1mapMC = (double**) mxCalloc(n*N, sizeof(double*)); /* maximal MAX1 score */
    
    /* make one copy of the MAX1map  */
    copyMAX1mapML = (int**) mxCalloc(n*N, sizeof(int*));/*relative perturbation in location */
    copyMAX1mapMX = (int**) mxCalloc(n*N, sizeof(int*));/*x position */
    copyMAX1mapMY = (int**) mxCalloc(n*N, sizeof(int*));/*y position */
    copyMAX1mapMO = (int**) mxCalloc(n*N, sizeof(int*));/*template index */
    copyMAX1mapMD = (int**) mxCalloc(n*N, sizeof(int*));/*relative perturbation in orientation */
    copyMAX1mapMC = (double**) mxCalloc(n*N, sizeof(double*)); /* maximal MAX1 score */
    
    
    for (i=0; i<n; i++)  /* number of images */
    {
        for (ind=0; ind<N; ind++)  /* number of filters */
        {
            h = floor(half[ind]+.5);
            sizeX = sx-h-L-(h+1+L)+1;
            sizeY = sy-h-L-(h+1+L)+1;
            
            if ((sizeX<=0)||(sizeY<=0)){    
              mexPrintf("The valid region for matching pursuit is non-positive: sizeX=%d, sizeY=%d\n",sizeX, sizeY);
              mexErrMsgTxt("Error! Please use larger template size or smaller filter size!\n"); 
            }
            
            MAX1mapML[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            MAX1mapMX[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            MAX1mapMY[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            MAX1mapMO[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            MAX1mapMD[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            MAX1mapMC[i*N+ind] = (double*) mxCalloc(sizeX*sizeY, sizeof(double));
            
            /* make one copy of the MAX1map  */
            copyMAX1mapML[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            copyMAX1mapMX[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            copyMAX1mapMY[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            copyMAX1mapMO[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            copyMAX1mapMD[i*N+ind] = (int*) mxCalloc(sizeX*sizeY, sizeof(int));
            copyMAX1mapMC[i*N+ind] = (double*) mxCalloc(sizeX*sizeY, sizeof(double));
            
            
            for (x=h+1+L; x<=sx-h-L; x++)
            {
                for (y=h+1+L; y<=sy-h-L; y++)
                {
                    if (ind<N-K)
                        mm = shiftmax(i, ind, x, y, rm);   /* for Gabor filter */ 
                    else 
                        mm = shiftmaxo(i, ind, x, y, rm);  /* for DoG*/
                    j = x-h-1-L;
                    k = y-h-1-L; 
                    MAX1mapML[i*N+ind][j*sizeY+k] = rm[0];
                    MAX1mapMX[i*N+ind][j*sizeY+k] = rm[1];
                    MAX1mapMY[i*N+ind][j*sizeY+k] = rm[2];
                    MAX1mapMO[i*N+ind][j*sizeY+k] = rm[3];
                    MAX1mapMD[i*N+ind][j*sizeY+k] = rm[4];
                    MAX1mapMC[i*N+ind][j*sizeY+k] = mm;
                    
                     /* make one copy of the MAX1map  */
                    copyMAX1mapML[i*N+ind][j*sizeY+k] = rm[0];
                    copyMAX1mapMX[i*N+ind][j*sizeY+k] = rm[1];
                    copyMAX1mapMY[i*N+ind][j*sizeY+k] = rm[2];
                    copyMAX1mapMO[i*N+ind][j*sizeY+k] = rm[3];
                    copyMAX1mapMD[i*N+ind][j*sizeY+k] = rm[4];
                    copyMAX1mapMC[i*N+ind][j*sizeY+k] = mm;
                    
                    
                }
            }
        }
    }

}


/* the match pursuit algorithm */
void Cmp()     
{
   int i, j, k, ind, x, y, mi, mx, my, mj, mk, t, rm[5], h, here, orie, layer, mlayer,sizeX, sizeY; 
   double m, s, mm; 
   clock_t time;
   
   
   pI = int_matrix(N, sx*sy);  
   for (ind=0; ind<N; ind++)
     for (here=0; here<sx*sy; here++)
         pI[ind][here] = 1; 
  /* storeshift();
   calculatemax();
   */
   t = 0; 
   do
     {
      m = NEGMAX;  
      for (ind=0; ind<N; ind++)  /* for all filters*/
      { 
        h = floor(half[ind]+.5);
        sizeX = sx-h-L-(h+1+L)+1;
        sizeY = sy-h-L-(h+1+L)+1;
        for (x=h+1+L; x<=sx-h-L; x++)
           for (y=h+1+L; y<=sy-h-L; y++)
            {
             here = px(x, y, sx, sy); 
             if (pI[ind][here])
             {
               s = 0.; 
               for (i=0; i<n; i++)  /* for all images */
                 {  
                   /* original code here*/
                   /*
                     if (ind<N-K)
                       mm = shiftmax(i, ind, x, y, rm);    
                     else 
                       mm = shiftmaxo(i, ind, x, y, rm);
                   */
                   j = x-h-1-L;
                   k = y-h-1-L; 
                   mm = MAX1mapMC[i*N+ind][j*sizeY+k];
                   
                   s += mm; 
                  }   
                if (m<s)
                  {
                    m = s; 
                    mi = ind; 
                    mx = x; 
                    my = y; 
                    mj = j;   /* j is the actual index in MAX1mapMC, which can be computed by j=x-h-1-L   */
                    mk = k;   /* k is the actual index in MAX1mapMC, which can be computed by k=y-h-1-L;  */
                   }
                if (s/n<SHUTUP)
                   pI[ind][here] = 0; 
              }
             }
      }
    
      if (m/n>SHUTUP)     
      {
          
          t++; 
          
          mexPrintf("No. %d:  (filter = %d   x = %d   y = %d) --> average response = %f\n", t, mi+1, mx, my, m/n); 
          mexEvalString("drawnow;");
          
          filterSelected[t-1]=(double)(mi+1);
          xSelected[t-1]=(double)mx;
          ySelected[t-1]=(double)my;
          
          mlayer = floor(mi/M);      
          draw(syma, mx, my, mi);        
          draw(sym[mlayer], mx, my, mi); 
    
         /* time = clock(); */         
                
            for (i=0; i<n; i++)     /* go through all images */
            { 
              /*original code here*/
              /*if (mi<N-K)
                   mm = shiftmax(i, mi, mx, my, rm);   
                else 
                   mm = shiftmaxo(i, mi, mx, my, rm); 
              */
         
              /* new code here*/
              h = floor(half[mi]+.5);
              sizeX = sx-h-L-(h+1+L)+1;  /* effective size of x */
              sizeY = sy-h-L-(h+1+L)+1;  /* effective size of y */
              rm[0] = MAX1mapML[i*N+mi][mj*sizeY+mk];
              rm[1] = MAX1mapMX[i*N+mi][mj*sizeY+mk];
              rm[2] = MAX1mapMY[i*N+mi][mj*sizeY+mk];
              rm[3] = MAX1mapMO[i*N+mi][mj*sizeY+mk];
              rm[4] = MAX1mapMD[i*N+mi][mj*sizeY+mk];
              
              deformed_filterSelected[i+n*(t-1)]=(double)(rm[3]+1);
              deformed_xSelected[i+n*(t-1)]=(double)rm[1];
              deformed_ySelected[i+n*(t-1)]=(double)rm[2];
                      
            /*  draw1(Asym[i], rm[1], rm[2], rm[3], i); */ /* draw constructed image */
              draw(Asym0[i*(LAYER+1)+mlayer], rm[1], rm[2], rm[3]); 
              draw(Asyma[i], rm[1], rm[2], rm[3]);  /* draw sketches */
              
              if (t % subsampling==0){
                OrthogonalInhibitAndMAX1Update(i, t);
              }  /* number of filters that have been selected */
              else{
                inhibit(i, t, rm[3], rm[1], rm[2]);  /* need to save the coefficient in arList and aiList*/ 
                updateMAX1map(i, mi,mx,my);
              }
              
            }
            
          
         /*  time = clock()-time;
           mexPrintf("time used: %f seconds\n", (float)(time)/CLOCKS_PER_SEC ); */
          
      }              
   }
   while ((m/n>SHUTUP)&&(t<TOTAL));  /* stopping criterion */  
   
   drawReconstruction(Asym, t);  /* draw all the reconstructed images */
   
   numSketchSelected=(double)t; 
}




void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
 int ind, i, j, c, ii, jj; 
 mxArray *f;  
 const mwSize* dims;
 mxClassID datatype;

/*
#pragma omp parallel
printf("Hello from thread %d, nthreads %d\n", omp_get_thread_num(), omp_get_num_threads());
*/
 
 c=0;
  /* images */
 dims = mxGetDimensions(prhs[c]);
 n = dims[0] * dims[1];  /* number of images */
 
 ImageMap = (const double**)mxCalloc( n, sizeof(*ImageMap) );   /* Images maps */
 for (i=0; i<n; ++i)
 {
     f = mxGetCell(prhs[c], i);
     datatype = mxGetClassID(f);
     if (datatype != mxDOUBLE_CLASS)
     mexErrMsgTxt("warning !! double precision required.");
     ImageMap[i] = (const double*)mxGetPr(f);    /* get the pointer to cell content */    
 }
 c++;
  
 N = floor(mxGetScalar(prhs[c++])+.5);  /* number of total filters*/
 M = floor(mxGetScalar(prhs[c++])+.5);  /* number of orientation */
 LAYER = floor((N+1.0)/M); 
 K = floor(mxGetScalar(prhs[c++])+.5);  /* number of DoG*/
 
 fIr = (double**) mxCalloc(n*N, sizeof(double*));    
 fIi = (double**) mxCalloc(n*N, sizeof(double*)); 
 fI =  (double**) mxCalloc(n*N, sizeof(double*));  
 for (i=0; i<n; i++)
   {
     for (ind=0; ind<N; ind++)
      {  
       f = mxGetCell(prhs[c], ind*n+i); 
       fIr[ind*n+i] = mxGetPr(f); 
       f = mxGetCell(prhs[c+1], ind*n+i); 
       fIi[ind*n+i] = mxGetPr(f);   
       f = mxGetCell(prhs[c+2], ind*n+i); 
       fI[ind*n+i] = mxGetPr(f);       
      }
    }
 c += 3;
 
 
 Crr = (double**) mxCalloc(N*N, sizeof(double*));    /* C: correlation/inhibition between filters */
 Cri = (double**) mxCalloc(N*N, sizeof(double*)); 
 Cir = (double**) mxCalloc(N*N, sizeof(double*)); 
 Cii = (double**) mxCalloc(N*N, sizeof(double*)); 
 for (ind=0; ind<N; ind++)
     {  
       for (j=0; j<N; j++)
        {
         f = mxGetCell(prhs[c], j*N+ind); 
         Crr[j*N+ind] = mxGetPr(f);         /* get correlation/inhibition */
         f = mxGetCell(prhs[c+1], j*N+ind); 
         Cri[j*N+ind] = mxGetPr(f);   
         f = mxGetCell(prhs[c+2], j*N+ind); 
         Cir[j*N+ind] = mxGetPr(f);   
         f = mxGetCell(prhs[c+3], j*N+ind); 
         Cii[j*N+ind] = mxGetPr(f);   
        }   
     }
 c += 4;     
 half = mxGetPr(prhs[c++]); 
 sx = floor(mxGetScalar(prhs[c++])+.5); 
 sy = floor(mxGetScalar(prhs[c++])+.5); 

 allfilterr = (double**) mxCalloc(N, sizeof(double*));  
 allfilteri = (double**) mxCalloc(N, sizeof(double*));  
 allsymbol = (double**) mxCalloc(N, sizeof(double*));    

 heightFilterMap = (int*)mxCalloc( N, sizeof(int) );
 widthFilterMap = (int*)mxCalloc( N, sizeof(int) );
 
 for (ind=0; ind<N; ind++)
     {  
       f = mxGetCell(prhs[c], ind); 
       allfilterr[ind] = mxGetPr(f);  
       
       heightFilterMap[ind] = mxGetM(f);  
       widthFilterMap[ind] = mxGetN(f);
       
       f = mxGetCell(prhs[c+1], ind); 
       allfilteri[ind] = mxGetPr(f);        
       f = mxGetCell(prhs[c+2], ind); 
       allsymbol[ind] = mxGetPr(f);        
     }
 c += 3; 
 
 syma = mxGetPr(prhs[c++]); 
 
 sym = (double**) mxCalloc(LAYER+1, sizeof(double*)); 
 for (i=0; i<LAYER+1; i++)
 {
        f = mxGetCell(prhs[c], i);
        sym[i] = mxGetPr(f);  
 }
 c++; 
 
 Asym = (double**) mxCalloc(n, sizeof(double*)); 
 for (i=0; i<n; i++)
 {
        f = mxGetCell(prhs[c], i);
        Asym[i] = mxGetPr(f);  
 }
 c++; 
 
 Asym0 = (double**) mxCalloc(n*(LAYER+1), sizeof(double*)); 
 for (i=0; i<n*(LAYER+1); i++)
    {
        f = mxGetCell(prhs[c], i);
        Asym0[i] = mxGetPr(f);  
     }
 c++;
 
 Asyma = (double**) mxCalloc(n, sizeof(double*)); 
 for (i=0; i<n; i++)
 {
        f = mxGetCell(prhs[c], i);
        Asyma[i] = mxGetPr(f);  
 }
 c++; 
 
 L = floor(mxGetScalar(prhs[c++])+.5);    /* location limit */
 ore = floor(mxGetScalar(prhs[c++])+.5);  /* orientation limit  */
 TOTAL = mxGetScalar(prhs[c++]);          /* number of sketches */
 epsilon = mxGetScalar(prhs[c++]); 
 
 SHUTUP = mxGetScalar(prhs[c++]);  
 Upperbound = mxGetScalar(prhs[c++]);  
 
 filterSelected = mxGetPr(prhs[c++]); 
 xSelected = mxGetPr(prhs[c++]); 
 ySelected = mxGetPr(prhs[c++]); 
 
 
 deformed_filterSelected = mxGetPr(prhs[c++]); 
 deformed_xSelected = mxGetPr(prhs[c++]); 
 deformed_ySelected = mxGetPr(prhs[c++]); 
 
 arList = mxGetPr(prhs[c++]); 
 aiList = mxGetPr(prhs[c++]);
         
 /*numSketchSelected = mxGetPr(prhs[c++]);*/ 
 
 /* correlation matrix  */
 dims = mxGetDimensions(prhs[c]);
 nElement=dims[0];
 Corr = (double**) mxCalloc(nElement*nElement, sizeof(double*));    /* C: correlation/inhibition between filters */
 for (i=0; i<nElement; i++)
 {  
     for (j=0; j<nElement; j++)
     {
         f = mxGetCell(prhs[c], j*nElement+i); 
         datatype = mxGetClassID(f);
         if (datatype != mxDOUBLE_CLASS)
            mexErrMsgTxt("warning !! double precision required for correlation matrix.");
         Corr[j*nElement+i] = (const double*)mxGetPr(f);         
       
     }   
  } 
 c++;
 
 /* subsampling  */
 subsampling = floor(mxGetScalar(prhs[c++])+0.5);  
 if  (subsampling<=0 ){
     mexErrMsgTxt("warning !! The value of subsampling is less than 0 or equal to 0.");
 }
 
 
 /* copy of fIr and fIi. The value of them will not be modified in the whole process */
 copyfIr = (double**) mxCalloc(n*N, sizeof(double*));    
 copyfIi = (double**) mxCalloc(n*N, sizeof(double*)); 
 copyfI = (double**) mxCalloc(n*N, sizeof(double*)); 
 for (i=0; i<n; i++)
 {
     for (ind=0; ind<N; ind++)
     {  
              
       copyfIr[ind*n+i] = (double*) mxCalloc(sx*sy, sizeof(double)); 
       copyfIi[ind*n+i] = (double*) mxCalloc(sx*sy, sizeof(double)); 
       copyfI[ind*n+i] = (double*) mxCalloc(sx*sy, sizeof(double));
       for (jj=0; jj<sy; jj++){
       for (ii=0; ii<sx; ii++){
               copyfIr[ind*n+i][ii+sx*jj]=fIr[ind*n+i][ii+sx*jj];
               copyfIi[ind*n+i][ii+sx*jj]=fIi[ind*n+i][ii+sx*jj];
               copyfI[ind*n+i][ii+sx*jj]=fI[ind*n+i][ii+sx*jj];
       }
       }          
                  
     }
  }
 
  
 
 
 storeshift();
 calculatemax();
 
 Cmp();
 
 /* output the actual numer of selected filters */ 
 plhs[0] = mxCreateDoubleScalar(numSketchSelected);
 
 /*
 dimsOutput[0] = n; dimsOutput[1] = N;
 plhs[0] = mxCreateCellArray( 2, dimsOutput );
 dimsOutput[0] = sx; dimsOutput[1] = sy;
 for( i = 0; i < n*N; ++i )
 {
        pA = mxCreateNumericArray( 2, dimsOutput, mxDOUBLE_CLASS, mxREAL );
        
        start_of_pr = (double*)mxGetData(pA);
        bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(pA);
        memcpy( start_of_pr, copyfIr[i], bytes_to_copy );
        mxSetCell( plhs[0], i, pA );
 }
 

 dimsOutput[0] = n; dimsOutput[1] = N;
 plhs[1] = mxCreateCellArray( 2, dimsOutput );
 dimsOutput[0] = sx; dimsOutput[1] = sy;
 for( i = 0; i < n*N; ++i )
 {
        pA = mxCreateNumericArray( 2, dimsOutput, mxDOUBLE_CLASS, mxREAL );
        
        start_of_pr = (double*)mxGetData(pA);
        bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(pA);
        memcpy( start_of_pr, copyfIi[i], bytes_to_copy );
        mxSetCell( plhs[1], i, pA );
 }
 */   
 
 /* release memory */
 

 mxFree(fIr);
 mxFree(fIi);
 mxFree(fI);
 for (i=0; i<n*N; i++)
 {
     mxFree(copyfIr[i]);
     mxFree(copyfIi[i]);
     mxFree(copyfI[i]);
 }
 mxFree(copyfIr);
 mxFree(copyfIi);
 mxFree(copyfI);
 
 mxFree(Crr);
 mxFree(Cri);
 mxFree(Cir);
 mxFree(Cii);
 mxFree(allfilterr);
 mxFree(allfilteri);
 mxFree(allsymbol);
 mxFree(sym);
 mxFree(Asym);
 mxFree(Asym0);
 mxFree(Asyma);
 for (i=0; i<n*N; i++)
 {
     mxFree(MAX1mapML[i]);
     mxFree(MAX1mapMX[i]);
     mxFree(MAX1mapMY[i]);
     mxFree(MAX1mapMO[i]);
     mxFree(MAX1mapMD[i]);
     mxFree(MAX1mapMC[i]);  
 }
mxFree(MAX1mapML);
mxFree(MAX1mapMX);
mxFree(MAX1mapMY);
mxFree(MAX1mapMO);
mxFree(MAX1mapMD);
mxFree(MAX1mapMC);

for (i=0; i<n*N; i++)
 {
     mxFree(copyMAX1mapML[i]);
     mxFree(copyMAX1mapMX[i]);
     mxFree(copyMAX1mapMY[i]);
     mxFree(copyMAX1mapMO[i]);
     mxFree(copyMAX1mapMD[i]);
     mxFree(copyMAX1mapMC[i]);  
 }
mxFree(copyMAX1mapML);
mxFree(copyMAX1mapMX);
mxFree(copyMAX1mapMY);
mxFree(copyMAX1mapMO);
mxFree(copyMAX1mapMD);
mxFree(copyMAX1mapMC);


for (i=0; i<M; i++)
{
	mxFree(sinsh[i]);
	mxFree(cossh[i]);
}
mxFree(sinsh);
mxFree(cossh);
for (i=0; i<N; i++)
{
	mxFree(pI[i]);
}
mxFree(pI); 


}
                 