/* mex-C: 
 * adaboost
 *
 */

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

#define Lrange 3 /* local perturbation range of Gabor elements */

/* Global variables */
double** posFea;
double** negFea;
int nPos, nNeg;
int numGridPoint; /* number of grid points to search for the threshold of the weak classifier */
double* thresholds;
bool* polars;
double* dataWeights;
double* errRates;
int maxNumFeature;
int* selectedInd;
double* selectedThresholds;
bool* selectedPolars;
double* lambdas;
int dataDim;

/* train the best weak classifier for one data dimension */
void getBestThresholdAndPolar( /* input: feature index */ int j,
                        /* output: */ double* threshold, bool *polar, double *minErrRate )
{
   
    int i, k, tmpPolar;
    double stepSize, candidateThres, errRate;
    double minVal, maxVal, val;
    
    /* find min/max values for this data dimension */
    minVal = posFea[0][j];
    maxVal = minVal;
    for( i = 0; i < nPos; ++i )
    {
        val = posFea[i][j];
        if( val > maxVal )
        {
            maxVal = val;
        }
        if( val < minVal )
        {
            minVal = val;
        }
    }
    for( i = 0; i < nNeg; ++i )
    {
        val = negFea[i][j];
        if( val > maxVal )
        {
            maxVal = val;
        }
        if( val < minVal )
        {
            minVal = val;
        }
    }
    
    *minErrRate = 1;
    /* search over the grid */
    stepSize = ( maxVal - minVal ) / (numGridPoint - 1);
    for( k = 0; k < numGridPoint; ++k )
    {
        candidateThres = minVal + k * stepSize;
        /* compute error rate of the classifier " > candidateThres " */
        errRate = 0;
        for( i = 0; i < nPos; ++i )
        {
            val = posFea[i][j];
            
            if( val <= candidateThres )
            {
                errRate += dataWeights[i];
            }
        }
        for( i = 0; i < nNeg; ++i )
        {
            val = negFea[i][j];
            if( val > candidateThres )
            {
                errRate += dataWeights[nPos+i];
            }
        }
        
        if( errRate > 1 )
            mexErrMsgTxt("warning !! errRate > 1 ");
        
        if( errRate >= 0.5 )
        {
            errRate = 1 - errRate;
            tmpPolar = 0;
        }
        else
        {
            tmpPolar = 1;
        }
        
        
        if( errRate < *minErrRate )
        {
            *minErrRate = errRate;
            if( *minErrRate < 0 )
                mexErrMsgTxt("warning !! *minErrRate < 0");
            *polar = tmpPolar;
            if( threshold == NULL )
                mexErrMsgTxt("warning !! threshold == NULL");
            *threshold = candidateThres;
        }
    }
}

/* adaboost core function */
void adaboost()
{
    int i, j, currentNumFeature;
    double minErrRate, beta, normConst, val;
    int bestFeatureInd;
    
    currentNumFeature = 0;
    while( currentNumFeature < maxNumFeature )
    {
        /* for each data dimension (subsampled), train a best weak classifier under current data weights */
        for( j = 0; j < dataDim; ++j )
		{
			getBestThresholdAndPolar( j, thresholds+j, polars+j, errRates+j );
		}

        /* find the best weak classifier under current data weights */
        minErrRate = 0.51;
        bestFeatureInd = -1;
        j = 0;
        for( j = 0; j < dataDim; ++j )
		{
			if( minErrRate > errRates[j] )
			{
				minErrRate = errRates[j];
				bestFeatureInd = j;
			}
		}
        
        /* add this weak classifier into the strong classifier */
        selectedInd[currentNumFeature] = bestFeatureInd;
        selectedThresholds[currentNumFeature] = thresholds[bestFeatureInd];
        selectedPolars[currentNumFeature] = polars[bestFeatureInd];

        /* compute model parameter for the newly selected feature */
        beta = errRates[bestFeatureInd] / ( 1 - errRates[bestFeatureInd] );
        
        lambdas[currentNumFeature] = - log( beta + 1e-6 );
        
        /* update dataweights */
        normConst = 0;
        for( i = 0; i < nPos; ++i )
        {
            val = posFea[i][bestFeatureInd];
            if( polars[bestFeatureInd] == ( val > thresholds[bestFeatureInd] ) ) /* weak classifier correctly predicts */
                dataWeights[i] *= beta;
            normConst += dataWeights[i];
        }
        for( i = 0; i < nNeg; ++i )
        {
            val = negFea[i][bestFeatureInd];
            if( polars[bestFeatureInd] == ( val <= thresholds[bestFeatureInd] ) ) /* weak classifier correctly predicts */
                dataWeights[i+nPos] *= beta;
            normConst += dataWeights[i+nPos];
        }
        for( i = 0; i < nPos; ++i )
        {
            dataWeights[i] /= normConst;
        }
        for( i = 0; i < nNeg; ++i )
        {
            dataWeights[i+nPos] /= normConst;
        }
        
        /*mexPrintf("%d : select %d-th atom, thres=%.3f, polar=%d, errRate=%.3f, lambda=%.3f, beta=%.3f\n",
            currentNumFeature,bestFeatureInd,thresholds[bestFeatureInd],
            polars[bestFeatureInd],errRates[bestFeatureInd],lambdas[currentNumFeature],beta);
        mexEvalString("drawnow"); */

        /* counter for weak classifier */
        ++currentNumFeature;
    }
}



/* 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 ind, i, x, y, o, j, bytes_to_copy;
    const mxArray *f;
    mwSize ndim;
    const mwSize* dims;
    mwSize dimsOutput[2];
    void* start_of_pr;
    mxClassID datatype;
 
    /*
	 * input variable 0: posFea 
	 */
    nPos = mxGetM( prhs[0] ) * mxGetN( prhs[0] );
    f = mxGetCell(prhs[0], 0); /* get the first cell element */
    
    posFea = mxCalloc(nPos, sizeof(*posFea));   /* fitered positive images */
    for (i=0; i<nPos; ++i)
    {
		f = mxGetCell(prhs[0], i);
		datatype = mxGetClassID(f);
		if (datatype != mxDOUBLE_CLASS)
			mexErrMsgTxt("warning !! double precision required.");
		posFea[i] = mxGetPr(f);    /* get pointers to filtered images */
		dataDim = mxGetM( f ) * mxGetN( f ); /* feature dimensionality */
		/* warning: assignment of pointer to double to pointer to double  */
    }

    /* 
     * input variable 1: negFea
     */
    nNeg = mxGetM( prhs[1] ) * mxGetN( prhs[1] );
    
    negFea = mxCalloc(nNeg, sizeof(*negFea));   /* fitered positive images */
    for (i=0; i<nNeg; ++i)
    {
		f = mxGetCell(prhs[1], i);
		datatype = mxGetClassID(f);
		if (datatype != mxDOUBLE_CLASS)
			mexErrMsgTxt("warning !! double precision required.");
		negFea[i] = mxGetPr(f);    /* get pointers to filtered images */       
    }
    
    /*
     * input variable 2: data weights (will be changed)
     */
    dataWeights = mxGetPr(prhs[2]);
    
    /*
     * input variable 3: number of grid points 
     */
    numGridPoint = (int)mxGetScalar(prhs[3]);
    
    /*
     * input variable 4: maximum number of selected features
     */
    maxNumFeature = (int)mxGetScalar(prhs[4]);
    
    /* mexPrintf("input successful. dataDim = %d\n",dataDim); */
    
    /*
     * Adaboost algorithm.
     */
    thresholds = mxCalloc(dataDim,sizeof(*thresholds));
    polars = mxCalloc(dataDim,sizeof(*polars));
    lambdas =  mxCalloc(maxNumFeature,sizeof(*lambdas));
    selectedInd = mxCalloc(maxNumFeature,sizeof(*selectedInd));
    selectedThresholds = mxCalloc(maxNumFeature,sizeof(*selectedThresholds));
    selectedPolars = mxCalloc(maxNumFeature,sizeof(*selectedPolars));
    errRates =  mxCalloc(dataDim,sizeof(*errRates));

    adaboost();

    /* =============================================
     * Handle output variables.
     * ============================================= 
     */

    dimsOutput[0] = 1; dimsOutput[1] = maxNumFeature;

    /*
     * output variable 0: selectedInd
     */
	plhs[0] = mxCreateNumericArray(2, dimsOutput ,mxINT32_CLASS, mxREAL);
    /* populate the real part of the created array */
    start_of_pr = (int*)mxGetData(plhs[0]);
    bytes_to_copy = maxNumFeature * mxGetElementSize(plhs[0]);
    memcpy(start_of_pr,selectedInd,bytes_to_copy);

    /*
     * output variable 1: selectedThresholds
     */
    plhs[1] = mxCreateNumericArray(2, dimsOutput ,mxDOUBLE_CLASS, mxREAL);
    /* populate the real part of the created array */
    start_of_pr = (double*)mxGetData(plhs[1]);
    bytes_to_copy = maxNumFeature * mxGetElementSize(plhs[1]);
    memcpy(start_of_pr,selectedThresholds,bytes_to_copy);

    /*
     * output variable 2: selectedPolars
     */
    plhs[2] = mxCreateNumericArray(2, dimsOutput ,mxLOGICAL_CLASS, mxREAL);
    /* populate the real part of the created array */
    start_of_pr = (bool*)mxGetData(plhs[2]);
    bytes_to_copy = maxNumFeature * mxGetElementSize(plhs[2]);
    memcpy(start_of_pr,selectedPolars,bytes_to_copy);

    /*
     * output variable 3: lambdas
     */
    plhs[3] = mxCreateNumericArray(2, dimsOutput ,mxDOUBLE_CLASS, mxREAL);
    /* populate the real part of the created array */
    start_of_pr = (double*)mxGetData(plhs[3]);
    bytes_to_copy = maxNumFeature * mxGetElementSize(plhs[3]);
    memcpy(start_of_pr,lambdas,bytes_to_copy);
}

