/ani/mrses

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>


#include <mex.h>

#include "mrses.h"
#include "msg.h"

#define MAX_FILE_NAME 2048
/*#define USE_UNDOCUMENTED*/

#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 */


typedef enum {
    ACTION_SETUP = 1,
    ACTION_COMPUTE = 10,
    ACTION_ITERATE = 15,
    ACTION_GET_HIST = 16
} TAction;


struct MEXContextT {
    MRSESContext mrses;
    mxArray *res; 
    mxArray *hist;
    unsigned int initialized;
    unsigned int width, max_block;
};
typedef struct MEXContextT MEXContextS;
typedef struct MEXContextT *MEXContext;

static MEXContext pstate = NULL;

MEXContext mexCreateContext() {
    pstate = (MEXContext)malloc(sizeof(MEXContextS));
    if (pstate) {
	MRSESContext ctx = mrses_create_context();
	if (!ctx) {
	    free(pstate);
	    return NULL;
	}
	
	memset(pstate, 0, sizeof(MEXContextS));
	pstate->mrses = ctx;
    }
    return pstate;
}

void mexDestroyContext(MEXContext pstate) {
    if (pstate) {
	if (pstate->res) {
	    mxDestroyArray(pstate->res);
	}
	if (pstate->hist) {
	    mxDestroyArray(pstate->hist);
	}
	if (pstate->mrses) {
	    mrses_destroy_context(pstate->mrses);
	}
	free(pstate);
    }
}

static void selfClean() {
    if (pstate) {
	reportMessage("Cleaning hw instance");

	mexDestroyContext(pstate);
	pstate = NULL;
    }

}

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

    mxArray *idMatrix;
    int32_t *idPtr;

    static int32_t id = 0;
    
    MRSESDistance dist;
    MRSESIntType *ires;
    const mxArray *A, *B, *index;
    mxArray *res;
    unsigned int iterations;
    unsigned int properties;
    unsigned int block_size;
    unsigned int width;

    MEXContext ps;
    TAction action;


    if (!nrhs) {
	reportMessage("Initializing 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;
	}

	if (pstate) mexDestroyContext(pstate);
	pstate = mexCreateContext();
	
	if (pstate) {
	    mexAtExit(selfClean);
	    id = 1;
	} 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_SETUP:
        if ((nrhs < 6)||(nrhs > 7)) {
	    reportError("Initialization call expects following arguments: 'width', 'block_size', matrices 'A' and 'B', distance_type");
	    return;
	}
	
	width = (int)mxGetScalar(prhs[2]);
	block_size = (int)mxGetScalar(prhs[3]);
	if (nrhs == 7) {
	    dist = (int)mxGetScalar(prhs[6]) - 1;
	    if (dist >= MRSES_DISTANCE_LAST) {
		reportError("Invalid distance mode is requested: %i", dist);
		return;
	    }
	} else dist = BHATTACHARYYA;
	A = prhs[4];
	B = prhs[5];
	
	properties = mxGetN(A);
	if (properties != mxGetN(B)) {
	    reportError("Dimenssions of matrices A and B does not match: %i, %i", properties, mxGetN(B));
	    return;
	}
	
	if ((mxGetClassID(A) == mxSINGLE_CLASS)&&(mxGetClassID(B) == mxSINGLE_CLASS)) {
	    err = mrses_init_context(ps->mrses, properties, mxGetM(A), mxGetData(A), mxGetM(B), mxGetData(B));
	} else if ((mxGetClassID(A) == mxDOUBLE_CLASS)&&(mxGetClassID(B) == mxDOUBLE_CLASS)) {
	    err = mrses_init_context_from_double(ps->mrses, properties, mxGetM(A), mxGetData(A), mxGetM(B), mxGetData(B));
	} else {
	    reportError("Only single-precision floating point numbers are supported");
	    return;
	}
	
	if (err) {
	    reportError("Initialization is failed with error, %i", err);
	    return;
	}
	
	err = mrses_set_distance_mode(ps->mrses, dist);
	if (err) {
	    reportError("Failed to set distance mode, error %i", err);
	    return;
	}
	
	err = mrses_prepare(ps->mrses, width, block_size);
	if (err) {
	    reportError("Failed to prepare for execution, error %i", err);
	    return;
	}

	if (ps->res) mxDestroyArray(ps->res);
	ps->res = mxCreateNumericMatrix(block_size, 1, mxSINGLE_CLASS, mxREAL);
	if (ps->res) mexMakeArrayPersistent(ps->res);
	else {
	    reportError("Allocation of result matrix of size %u*float bytes is failed", block_size);
	    return;
	}
	
	if (ps->hist) mxDestroyArray(ps->hist);
	ps->hist = mxCreateNumericMatrix(1, properties, mxUINT32_CLASS, mxREAL);
	if (ps->hist) mexMakeArrayPersistent(ps->hist);
	else {
	    reportError("Allocation of result matrix of size %u*uint32_t bytes is failed", properties);
	    return;
	}

	ps->width = width;
	ps->max_block = block_size;
	ps->initialized = 1;
     break;
     case ACTION_COMPUTE:
	if (!pstate->initialized) {
	    reportError("We are not initialized yet, issue intialize call first");
	    return;
	}

        if ((nlhs != 1)||((nrhs < 3)||(nrhs > 4))) {
	    reportError("Compute call expects following arguments: 'block_size', 'index'. It should accept a single result matrix as well.");
	    return;
	}
	
	if (nrhs > 3) {
	    block_size = (int)mxGetScalar(prhs[2]);
	    index = prhs[3];

	    if (mxGetN(index) < block_size) {
		reportError("Index matrix does not contain specified number of blocks");
		return;
	    }
	} else {
	    index = prhs[2];
	    block_size = mxGetN(index);
	}

	if (block_size > pstate->max_block) {
	    reportError("The block is too big: maximum configured size is %i, but %i supplied", pstate->max_block, block_size);
	    return;
	}
	
	width = mxGetM(index);
	if (pstate->width != width) {
	    reportError("The index matrix dimension does not fit requirement: width = %i, but index side is %i", pstate->width, width);
	    return;
	}
	
	if (mxGetClassID(index) != mxINT16_CLASS) {
	    reportError("Only 16 bit integers are supported for index");
	    return;
	}
	
	res = ps->res;
	mxSetN(res, block_size);
	mxSetM(res, 1);

	err = mrses_compute(ps->mrses, block_size, mxGetData(index), mxGetData(res));
	if (err) {
	    reportError("Error executing compute, failed with code: %i", err);
	    return;
	}

#ifdef USE_UNDOCUMENTED
	plhs[0] = mxCreateSharedDataCopy(res);
#else /* USE_UNDOCUMENTED */
	plhs[0] = mxDuplicateArray(res);
#endif /* USE_UNDOCUMENTED */
	
     break;
     case ACTION_ITERATE:
	if (!pstate->initialized) {
	    reportError("We are not initialized yet, issue intialize call first");
	    return;
	}

        if (nrhs != 4) {
	    reportError("Iterate call expects following arguments: 'iterations', 'block_size'.");
	    return;
	}
	
	iterations = (int)mxGetScalar(prhs[2]);
	block_size = (int)mxGetScalar(prhs[3]);

	if (block_size > pstate->max_block) {
	    reportError("The block is too big: maximum configured size is %i, but %i supplied", pstate->max_block, block_size);
	    return;
	}

	if (nlhs == 1) {
	    idMatrix = mxCreateNumericMatrix(pstate->width, pstate->max_block, mxUINT16_CLASS, mxREAL);
	    mxSetN(idMatrix, block_size);
	    ires = mxGetData(idMatrix);
	    plhs[0] = idMatrix;
	} else {
	    ires = NULL;
	}
	err = mrses_iterate(ps->mrses, iterations, block_size, ires);
	if (err) {
	    reportError("Error executing iterate, failed with code: %i", err);
	    return;
	}
     break;
     case ACTION_GET_HIST:
	if (!pstate->initialized) {
	    reportError("We are not initialized yet, issue intialize call first");
	    return;
	}

        if ((nlhs != 1)&&(nrhs != 2)) {
	    reportError("GetHist call returns histogram vector and expects no arguments.");
	    return;
	}

	res = ps->hist;
	err = mrses_get_results(ps->mrses, mxGetData(res));
	if (err) {
	    reportError("Error executing iterate, failed with code: %i", err);
	    return;
	}

	plhs[0] = mxDuplicateArray(res);
     break;
     default:
        reportError("Unknown request %i", action);
    }
}