/* mex-C: 
 * Adaboost Testing.
 * 
 * Image are assumed to be resized to template size, thus no window scanning is needed.
 *
 */

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

/*
 * Global variables
 */

double** feaMaps;                /* Gabor feature response maps */
int n;                          /* number of testing examples */
int nOri;                       /* number of orientations */
int sx, sy;                     /* image size (template size) */
/* the trained strong classifier begins --- */
int numFea;
int* selectedXs;
int* selectedYs;
int* selectedOs;
double* selectedThresholds;
bool* selectedPolars;
double* lambdas;
/* the trained strong classifier ends --- */
double* scores;                  /* <scores> in matlab is a #example * #feature matrix. */

/* compute results of the strong classifier */
void classify()
{
    int i, j, x, y, o;
    bool val;
    
    /* 
     * form of strong classifier:
     *  \sum_j lambdas_j * c_j - \sum_j lambdas_j / 2  
     */
    
    /* the first selected weak classifier */
    j = 0;
    x = selectedXs[j]; y = selectedYs[j]; o = selectedOs[j];
    for( i = 0; i < n; ++i )
    {
        val = selectedPolars[j] == ( feaMaps[o*n+i][y*sx+x] > selectedThresholds[j] );
        scores[j*n+i] = lambdas[j] * (double)val - lambdas[j]/2;
    }
    
    for( j = 1; j < numFea; ++j )
    {
        x = selectedXs[j];
        y = selectedYs[j];
        o = selectedOs[j];
        i = 0;
        val = feaMaps[o*n+i][y*sx+x] > 0;
        
        for( i = 0; i < n; ++i )
        {
            val = selectedPolars[j] == ( feaMaps[o*n+i][y*sx+x] > selectedThresholds[j] );
            scores[j*n+i] = scores[(j-1)*n+i] + lambdas[j] * (double)val - lambdas[j]/2;
        }
    }
}

/* main function */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])                
{
    int ind, i, bytes_to_copy;
    const mxArray *f;
    mwSize ndim;
    const mwSize* dims;
    mwSize dimsOutput[2];
    void* start_of_pr;
    mxClassID datatype;
 
    /*
	 * input variable 0: feaMaps 
	 */
    ndim = mxGetNumberOfDimensions(prhs[0]);
    dims = mxGetDimensions(prhs[0]);
    n = dims[0]; /* number of testing examples */
    nOri = dims[1]; /* number of orientations */
 
    f = mxGetCell(prhs[0], 0); /* get the first cell element */
    sx = mxGetM(f);
    sy = mxGetN(f);
    
    feaMaps = mxCalloc(n*nOri, sizeof(*feaMaps));   /* fitered positive images */
    for (i=0; i<n; ++i)
    {
        for (ind=0; ind<nOri; ++ind)
        {
            f = mxGetCell(prhs[0], ind*n+i);
            datatype = mxGetClassID(f);
            if (datatype != mxDOUBLE_CLASS)
                mexErrMsgTxt("warning !! double precision required.");
            feaMaps[ind*n+i] = mxGetPr(f);    /* get pointers to filtered images */   
        }
    }

    /*
     * input variable 1: selected features - X
     */
    selectedXs = mxGetPr(prhs[1]);
    numFea = mxGetM(prhs[1]) * mxGetN(prhs[1]);
    
    /*
     * input variable 2: selected features - Y
     */
    selectedYs = mxGetPr(prhs[2]);
    
    /*
     * input variable 3: selected features - O
     */
    selectedOs = mxGetPr(prhs[3]);
    
    /*
     * input variable 4: selected features - thresholds
     */
    selectedThresholds = mxGetPr(prhs[4]);
    
    /*
     * input variable 5: selected features - polars
     */
    selectedPolars = mxGetPr(prhs[5]);
    
    /*
     * input variable 6: selected features - lambdas
     */
    lambdas = mxGetPr(prhs[6]);

    /* mexPrintf("input successful.\n"); */
    
    /* =========================================
     * Testing the linear classifier.
     * =========================================
     */
    
    scores = mxCalloc(n*numFea,sizeof(*scores));
    for (i=0; i<n*numFea; ++i)
        scores[i] = 0;
    classify();
    
    /* =============================================
     * Handle output variables.
     * ============================================= 
     */
    
    dimsOutput[0] = n; dimsOutput[1] = numFea;
    
    /*
     * output variable 0: scores
     */
	plhs[0] = mxCreateNumericArray(2, dimsOutput ,mxDOUBLE_CLASS, mxREAL);
    /* populate the real part of the created array */
    start_of_pr = (double*)mxGetData(plhs[0]);
    bytes_to_copy = n*numFea * mxGetElementSize(plhs[0]);
    memcpy(start_of_pr,scores,bytes_to_copy);
}

