/normxcorr/trunk

#include <stdio.h>
#include <stdlib.h>

#include <mex.h>
#include <dict_hw.h>

#if defined(_WIN32) || defined(_WIN64)
# include <windows.h>
    typedef UINT8 uint8_t;
    typedef UINT16 uint16_t;
    typedef UINT32 uint32_t;
    typedef UINT64 uint64_t;
    typedef INT8 int8_t;
    typedef INT16 int16_t;
    typedef INT32 int32_t;
    typedef INT64 int64_t;
#else
# include <stdint.h>
#endif

#include "normxcorr_hw_msg.h"

#define MAX_FILE_NAME 2048

#define USE_UNDOCUMENTED
//#define VALIDATE_LSUM
//#define VALIDATE_PEAK

typedef enum {
    ACTION_SETUP = 1,
#ifdef VALIDATE_LSUM
    ACTION_COMPUTE_BASE_FRAGMENT = 2,
#endif /* VALIDAT_LSUM */
    ACTION_SET_BASE_POINTS = 3,
    ACTION_COMPUTE_BASE = 4,
#ifdef VALIDATE_PEAK
    ACTION_COMPUTE_FRAGMENT = 11,
    ACTION_GET_CORRECTIONS = 15,
#endif /* VALIDATE_PEAK */
    ACTION_SET_POINTS = 12,
    ACTION_COMPUTE = 13,
    ACTION_GET_POINTS = 14
} TAction;


static DICTContext pstate = NULL;
static mxArray *coords = NULL;			// Matlab array with current coordinates

#ifndef EXTERN_C
# ifdef __cplusplus
   #define EXTERN_C extern "C"
# else
   #define EXTERN_C extern
# endif
#endif

#ifdef USE_UNDOCUMENTED
EXTERN_C mxArray *mxCreateSharedDataCopy(const mxArray *pr);
#endif /* USE_UNDOCUMENTED */


static void selfClean() {
    if (pstate) {
	reportMessage("cleaning normxcorr_hw instance");

	dictDestroyContext(pstate);
	pstate = NULL;
    }
    
    if (coords) {
	mxDestroyArray(coords);
	coords = NULL;
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    int err;

    int i;
    mxArray *idMatrix;
    int32_t *idPtr;
    int64_t *errPtr;

    static int32_t id = 0;
    static int ncp;
    static int width, height;
    static int corr_size;
    
    float *points;
    

    DICTContext ps;
    TAction action;
    int class_id;

    int iprop;
    
    const mxArray *input;
    const mxArray *base;

#ifdef VALIDATE_LSUM
    const mxArray *lsum;
    const mxArray *denom;
#endif /* VALIDATE_LSUM */

    const mxArray *x, *y;
    
    int length;
    mxChar *chars;
    char name[MAX_FILE_NAME+1];

    if (!nrhs) {
	reportMessage("Initializing normxcorr_hw instance");

	if (nlhs != 1) {
	    reportError("You should accept a single result from initialization call");
	    return;
	}
	
	idMatrix = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
	if (!idMatrix) {
	    reportError("Initialization is failed");
	    return;
	}
	
	dictSetLogger(reportError, reportMessage);

	if (pstate) {
	    dictDestroyContext(pstate);
	    pstate = dictCreateContext();
	} else {
	    id = dictDetectHardware();
	    if (id > 0) {
		pstate = dictCreateContext();
	    }
	}
	
	if (pstate) {
	    mexAtExit(selfClean);
	} else if (id > 0) {
	    reportError("Context initialization has failed");
	    id = -1;
	}

	idPtr = (int32_t*)mxGetData(idMatrix);
	idPtr[0] = id;
	
	plhs[0] = idMatrix;
	return;
    } else {
	if (!pstate) {
	    reportError("normxcorr_hw should be initialized first");
	    return;
	}
    }

	// Clean request
    if (nrhs == 1) {
	selfClean();
	return;
    }

    ps = pstate;

    action = (TAction)(mxGetScalar((mxArray*)prhs[1]));

//    reportMessage("Executing normxcorr_hw action: %u", action);

    switch (action) {
     case ACTION_COMPUTE:
	if (nrhs != 3) {
	    reportError("Compute action expects 1 argument, but %i is passed", nrhs - 2);
	    return;
        }

	input = prhs[2];

	class_id = mxGetClassID(input);
	switch (class_id) {
	    case mxUINT8_CLASS:
		if (mxGetNumberOfDimensions(input) != 2) {
		    reportError("Invalid dimensionality of base matrix, 2D matrix is expected");
		    return;
		}
	
		if ((mxGetN(input) != width)||(mxGetM(input) != height)) {
		    reportError("Invalid size of image (%ix%i), but (%ix%i) is expected", mxGetN(input), mxGetM(input), width, height);
		    return;
		}

		dictLoadImage(ps, (unsigned char*)mxGetData(input));
	    break;
	    case mxCHAR_CLASS:
    		if ((mxGetNumberOfDimensions(input) != 2)&&(mxGetM(input) != 1)) {
		    reportError("Only a single string is supported");
		    return;
		}

		length = mxGetN(input);
		chars = mxGetChars(input);
		
		if (length > MAX_FILE_NAME) {
		    reportError("File name is too long, limit is %i", MAX_FILE_NAME);
		    return;
		}
		
		for (i = 0; i < length; i++) {
		    if (chars[i] > 255) {
			reportError("Unicode file names are not supported yet");
			return;
		    }
		    name[i] = chars[i];
		}
		name[length] = 0;

		err = dictLoadImageFile(ps, name);
		if (err) {
		    reportError("Failed to load image: %s", name);
		    return;
		}
	    break;
	    default:
		reportError("Invalid type of image data, should be 8bit integers");
		return;
	}
     break;
     case ACTION_COMPUTE_BASE:
	if (nrhs != 3) {
	    reportError("ComputeBase action expects 1 argument, but %i is passed", nrhs - 2);
	    return;
        }

	base = prhs[2];
	
	class_id = mxGetClassID(base);
	
	switch (class_id) {
	    case mxUINT8_CLASS:
    		if (mxGetNumberOfDimensions(base) != 2) {
		    reportError("Invalid dimensionality of base matrix, 2D matrix is expected");
		    return;
		}
		
		width = mxGetN(base);
		height = mxGetM(base);
	
		dictLoadTemplateImage(ps, (unsigned char*)mxGetData(base), width, height);
	    break;
	    case mxCHAR_CLASS:
    		if ((mxGetNumberOfDimensions(base) != 2)&&(mxGetM(base) != 1)) {
		    reportError("Only a single string is supported");
		    return;
		}

		length = mxGetN(base);
		chars = mxGetChars(base);
		
		if (length > MAX_FILE_NAME) {
		    reportError("File name is too long, limit is %i", MAX_FILE_NAME);
		    return;
		}
		
		for (i = 0; i < length; i++) {
		    if (chars[i] > 255) {
			reportError("Unicode file names are not supported yet");
			return;
		    }
		    name[i] = chars[i];
		}
		name[length] = 0;

		err = dictLoadTemplateImageFile(ps, name);
		if (err) {
		    reportError("Failed to load image: %s", name);
		    return;
		}
	    break;
	    default:
	        reportError("Invalid matrix. The data type (%s) is not supported", mxGetClassName(base));
		return;
	}
     break;
     case ACTION_SET_BASE_POINTS:
	if (nrhs != 4) {
	    reportError("SET_POINTS action expects two arrays with 'x' and 'y' coordinates of control points");
	    return;
	}

        x = prhs[2];
	y = prhs[3];
	
	if (	(mxGetClassID(x) != mxSINGLE_CLASS)||
		(mxGetClassID(y) != mxSINGLE_CLASS)||
		(mxGetN(x)*mxGetM(x) != ncp)||
		(mxGetN(y)*mxGetM(y) != ncp)
	) {
	    reportError("Invalid control points are specified");
	    return;
	}
	
	dictSetTemplatePoints(ps,  (float*)mxGetData(x),  (float*)mxGetData(y));
     break;
     case ACTION_SET_POINTS:
	if (nrhs != 4) {
	    reportError("SET_POINTS action expects two arrays with 'x' and 'y' coordinates of control points");
	    return;
	}

        x = prhs[2];
	y = prhs[3];
	
	if (	(mxGetClassID(x) != mxSINGLE_CLASS)||
		(mxGetClassID(y) != mxSINGLE_CLASS)||
		(mxGetN(x)*mxGetM(x) != ncp)||
		(mxGetN(y)*mxGetM(y) != ncp)
	) {
	    reportError("Invalid control points are specified");
	    return;
	}
	
	dictSetCurrentPoints(ps,  (float*)mxGetData(x),  (float*)mxGetData(y));
     break;
     case ACTION_GET_POINTS:
        if (nrhs != 2) {
	    reportError("GetPoints action do not expect any arguments");
	    return;
	}
        if (nlhs != 1) {
	    reportError("GetPoints action returns a single matrix");
	    return;
	}

	if (!coords) {
	    reportError("normxcorr is not properly initialized, the result matrix is not allocated");
	    return;
	}
	
	dictCompute(ps);
	
#ifdef USE_UNDOCUMENTED
	plhs[0] = mxCreateSharedDataCopy(coords);
//    mxArray *mxCreateSharedDataCopy(const mxArray *pr);
//    bool mxUnshareArray(const mxArray *pr, const bool noDeepCopy);    // true if not successful
//    mxArray *mxUnreference(const mxArray *pr);
#else /* USE_UNDOCUMENTED */
	plhs[0] = mxDuplicateArray(coords);
#endif /* USE_UNDOCUMENTED */
     break;     
     case ACTION_SETUP:
	if (nrhs != 6) {
	    reportError("SETUP action expects 'ncp', 'corrsize', 'precision', and 'optimization level' parameters");
	    return;
	}

	ncp = (int)mxGetScalar(prhs[2]);
	corr_size = (int)mxGetScalar(prhs[3]);
	iprop = (int)mxGetScalar(prhs[5]);
	
    	err = dictSetup(ps, ncp, corr_size, (int)mxGetScalar(prhs[4]), ((iprop>3)?0:DICT_FLAGS_FIXED_FFT_SIZE)|DICT_FLAGS_MATLAB_MODE);

	if (!err) {
	    if (coords) mxDestroyArray(coords);
	    coords = mxCreateNumericMatrix(ncp, 2, mxSINGLE_CLASS, mxREAL);
	    if (coords) mexMakeArrayPersistent(coords);
	    else {
		reportError("Allocation of result matrix of size %u*float bytes is failed", ncp);
		err = DICT_ERROR_MALLOC;
	    }
	}
	
	if (!err) {
	    points = (float*)mxGetData(coords);
	    dictSetPointsBuffer(ps, points, points + ncp);
	}
	
	//mexMakeMemoryPersistent(ps->coords);
	//mexLock() mexUnlock()

	if (nlhs == 1) {
	    idMatrix = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
	    if (idMatrix) {
		errPtr = (int64_t*)mxGetData(idMatrix);
		errPtr[0] = err;
		plhs[0] = idMatrix;
	    } else {
		reportError("Initialization of result matrix is failed");
	        return;
	    }
	}
     break;
#ifdef VALIDATE_PEAK
     case ACTION_COMPUTE_FRAGMENT:
        if (nlhs > 0) {
	    icp = (unsigned int)mxGetScalar(prhs[2]) - 1;
	    idMatrix = mxCreateNumericMatrix(size, size, mxSINGLE_CLASS, mxREAL);

	    dictProcessFragment(ps, icp, 1, prhs[3]);
	    dictGetCorrelations(ps, icp, (float*)mxGetPr(idMatrix));
	
	    plhs[0] = idMatrix;
	}
     break;
     case ACTION_GET_CORRECTIONS:
        if (nlhs > 0) {
	    idMatrix = mxCreateNumericMatrix(ncp, 2, mxSINGLE_CLASS, mxREAL);
	    float *points = (float*)mxGetData(idMatrix); 
	
	    dictGetCorrections(ps, points, points + ncp);
	
	    plhs[0] = idMatrix
	}
     break;
#endif /* VALIDATE_PEAK */
#ifdef VALIDATE_LSUM
     case ACTION_COMPUTE_BASE_FRAGMENT:
	if (nrhs != 4) {
	    reportError("ComputeBaseFragment action expects 2 arguments, but %i is passed", nrhs - 2);
	    return;
        }

	icp = (unsigned int)mxGetScalar(prhs[2]) - 1;
	if (icp >= ps->ncp) {
	    reportError("The control point (%i) is out of range (0-%u)", icp, ps->ncp - 1);
	    return;
	}

	base = prhs[3];
    
	if (mxGetNumberOfDimensions(base) != 2) {
	    reportError("Invalid dimensionality of base matrix, 2D matrix is expected");
	    return;
	}

	if (mxGetClassID(base) != mxUINT8_CLASS) {
	    reportError("Invalid matrix. The data type (%s) is not supported", mxGetClassName(base));
	    return;
	}

	fft_size = 6 * corr_size + 1;
	if (nlhs > 0) lsum = mxCreateNumericMatrix(fft_size, fft_size, mxSINGLE_CLASS, mxREAL);
	else lsum = NULL;
        if (nlhs > 1) denom = mxCreateNumericMatrix(fft_size, fft_size, mxSINGLE_CLASS, mxREAL);
	else denom = NULL;
	
	dictSetDimensions(ps, GetN(base), GetM(base));
	dictLoadTemplateFragment(ps, icp, 1, base);
	dictGetLocalSum(ps, icp, lsum, denom);

	if (nlhs > 0) {
	    plhs[0] = lsum;
	    if (nlhs > 1) plhs[1] = denom;
	}
     break;
#endif /* VALIDATE_LSUM */

     default:
        reportError("Unknown request %i", action);
    }
}