#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "mex.h"        /* the algorithm is connect to matlab */
#include "math.h"
#include "matrix.h"
#define ABS(x) ((x)>0? (x):(-(x)))
#define MAX(x, y) ((x)>(y)? (x):(y))
#define MIN(x, y) ((x)<(y)? (x):(y))

/*
[invB'B, B'I, coefficient, Image_projected]=ComputeProjection(Corr,template,single(halfFilterSize),filters,img);
*/

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

int px(int x, int y, int bx, int by)    
{            
   return (x + (y-1)*bx - 1); 
 }

void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
  int c, N,i,j,bytes_to_copy,g;
  const mxArray *pFilter;
  const float **FilterMap;            /* Filter maps */
  int* heightFilterMap,* widthFilterMap; 
  int h,w,row,col;
  int imageStartingRow,imageStartingCol;
  int iLocation;
  int iRow, iCol;
  float temp_sum;
  int heightImage, widthImage, nFilter;
  float* arrayFilteringResponse;
  
  const float *image;                 /* image */
  const mxArray *f;
  const mwSize* dims;
  mxClassID datatype;
  float **Corr;
  const mxArray* Template;      /* template */
  const float *selectedRow, *selectedCol, *selectedFilter;
  int numSelectedFeature;
  float* halfFilterSize;
  float * result;
  
  mwSize dimsOutput[2];
  void* start_of_pr;
  int i_ind, i_row, i_col, j_ind, j_row, j_col, hi, hj;
   
  mxArray *invBB;
  mxArray *Coefficient;
  float *coeff;
  mxArray *multiplication[2];          /* input to plot function */
  float * projectedI;
          
  /* input variable 0: correlation matrix  */
  dims = mxGetDimensions(prhs[0]);
  N=dims[0];
  Corr = (float**) mxCalloc(N*N, sizeof(float*));    /* C: correlation/inhibition between filters */
  for (i=0; i<N; i++)
  {  
     for (j=0; j<N; j++)
     {
         f = mxGetCell(prhs[0], j*N+i); 
         datatype = mxGetClassID(f);
         if (datatype != mxSINGLE_CLASS)
            mexErrMsgTxt("warning !! single precision required for correlation matrix.");
         Corr[j*N+i] = (const float*)mxGetPr(f);         
       
     }   
   }
  
  /*
     * input variable 1: template
     */
    Template = prhs[1];
    f = mxGetField( Template, 0, "selectedRow" );
	if ( f == NULL )
	{
	  mexErrMsgTxt( "no such field 1 in struct" );
	}
    selectedRow = (const float*)mxGetPr(f);
       
    
    f = mxGetField( Template, 0, "selectedCol" );
    if ( f == NULL )
	{
	  mexErrMsgTxt( "no such field 2 in struct" );
	}
    selectedCol = (const float*)mxGetPr(f);
    
    f = ( mxGetField( Template, 0, "selectedFilter" ) );
    if ( f == NULL )
	{
	  mexErrMsgTxt( "no such field 3 in struct" );
    }
    selectedFilter = (const float*)mxGetPr(f);
    
    numSelectedFeature= mxGetM(f)*mxGetN(f);
    
    /*
     * input variable 2: halfFilterSize
     */
    datatype = mxGetClassID(prhs[2]);
    if (datatype != mxSINGLE_CLASS)
        mexErrMsgTxt("warning !! single precision required.");
    halfFilterSize = (const float*)mxGetPr(prhs[2]); 
    
    
    /*
	 * input variable 3: filter maps
	 */
    pFilter = prhs[3];
    dims = mxGetDimensions(pFilter);
    nFilter = dims[0] * dims[1];
     
    
    FilterMap = (const float**)mxCalloc( nFilter, sizeof(*FilterMap) );   /* Filter maps */
    heightFilterMap = (int*)mxCalloc( nFilter, sizeof(int) );
    widthFilterMap = (int*)mxCalloc( nFilter, sizeof(int) );
       
    
    for (i=0; i<nFilter; ++i)
    {
        f = mxGetCell(pFilter, i);
        datatype = mxGetClassID(f);
        if (datatype != mxSINGLE_CLASS)
            mexErrMsgTxt("warning !! single precision required.");
        FilterMap[i] = (const float*)mxGetPr(f);    /* get the pointer to cell content */
        heightFilterMap[i] = mxGetM(f);    
        widthFilterMap[i] = mxGetN(f);
    }
    
    /*
	 * input variable 4: image
	 */
	datatype = mxGetClassID(prhs[4]);
    if (datatype != mxSINGLE_CLASS)
        mexErrMsgTxt("warning !! single precision required.");
    image = (const float*)mxGetPr(prhs[4]); 
    heightImage = mxGetM(prhs[4]);    
    widthImage = mxGetN(prhs[4]);
    
    /* =============================================
     * Compute the B'B
     * ============================================= 
     */
     
    /*  initialization */
    result = (float*) mxCalloc(numSelectedFeature*numSelectedFeature, sizeof(float));
    
    for(i=0; i<numSelectedFeature; i++ ){
         for(j=0; j<numSelectedFeature; j++ ){
             result[i+j*numSelectedFeature]=0.0;
         }
    }   
    
    
    /* compute B'B*/
    for(i=0; i<numSelectedFeature; i++ ){    
    
        i_ind=selectedFilter[i]-1;   /*  start from 0 */
        i_row=selectedRow[i];    /*  start from 1 */
        i_col=selectedCol[i];
        hi=halfFilterSize[i_ind];
    
        result[i+i*numSelectedFeature]=1;
        for(j=i+1; j<numSelectedFeature; j++ ){        
        
            j_ind=selectedFilter[j]-1;
            j_row=selectedRow[j];
            j_col=selectedCol[j];
            hj=halfFilterSize[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*numSelectedFeature]= Corr[i_ind+j_ind*N][g]; 
                result[j+i*numSelectedFeature]= result[i+j*numSelectedFeature];   
           }   
   
        }      
    
    }
    
    /* compute B'I  */
   
    arrayFilteringResponse = (float*)mxCalloc( numSelectedFeature, sizeof(float) );
    for (i=0; i<numSelectedFeature; i++)
    {
        
        i_ind=(int)selectedFilter[i]-1;
        h=heightFilterMap[i_ind];
        w=widthFilterMap[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++)
          {
               /*printf("%d of %d, %d of %d, %d of %d\n",i+1, numSelectedFeature, iCol,imageStartingCol+w-1, iRow, imageStartingRow+h-1);*/
               if( (iRow>=0) && (iRow<heightImage) && (iCol>=0) && (iCol<widthImage)){
                   
                   temp_sum = temp_sum + image[iRow+iCol*heightImage] * FilterMap[i_ind][iLocation];  
               }
               
               iLocation++;
          }
          
        }        
               
           
      arrayFilteringResponse[i]=temp_sum;    
      
    }
    
     /* =============================================
     * Handle output variables.
     * ============================================= 
     */
    
    dimsOutput[0] = numSelectedFeature; dimsOutput[1] = numSelectedFeature;
    invBB = mxCreateNumericArray( 2, dimsOutput, mxSINGLE_CLASS, mxREAL);
    start_of_pr = (float*)mxGetData(invBB);
    bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(invBB);    
    memcpy( start_of_pr, result, bytes_to_copy );       
    mexCallMATLAB(1, &plhs[0], 1, &invBB, "pinv");
    
    
    dimsOutput[0] = numSelectedFeature; dimsOutput[1] = 1;
    plhs[1] = mxCreateNumericArray( 2, dimsOutput, mxSINGLE_CLASS, mxREAL );
    start_of_pr = (float*)mxGetData(plhs[1]);
    bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(plhs[1]);
    memcpy( start_of_pr, arrayFilteringResponse, bytes_to_copy );  
    
    multiplication[0]=plhs[0];
    multiplication[1]=plhs[1];
    mexCallMATLAB(1, &Coefficient, 2, multiplication, "*");
    plhs[2]=Coefficient;
    
    /* prepare the reconstructed result by intializing it with zeros value*/
    
    projectedI = (float*)mxCalloc( heightImage * widthImage, sizeof(float) );
        
    for( iCol = 0; iCol < widthImage; ++iCol )
    {
        for( iRow = 0; iRow < heightImage; ++iRow )
        {
             projectedI[iRow + iCol*heightImage] = 0.0;
        }
    }
    
    /*  compute: I_reconstruct = B*C, where C = B* inv(B'B)B' I_syn */
    coeff = (float*)mxGetPr(Coefficient); 
    for (i=0; i<numSelectedFeature; i++)
    { 
        
    
        i_ind=(int)selectedFilter[i]-1;
        h=heightFilterMap[i_ind];
        w=widthFilterMap[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);
                
               
        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)){
                   
                   projectedI[iRow+iCol*heightImage] += coeff[i]* FilterMap[i_ind][iLocation];  
               
               }           
            
               iLocation++;            
           }
   
        }
    
    
    }
    
    dimsOutput[0] =  heightImage; dimsOutput[1] = widthImage;
    plhs[3] = mxCreateNumericArray( 2, dimsOutput, mxSINGLE_CLASS, mxREAL);
    start_of_pr = (float*)mxGetData(plhs[3]);
    bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(plhs[3]);
    memcpy( start_of_pr, projectedI, bytes_to_copy );
    
    
}