/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);

    }

}