/* Roger Jang, 20091205 */

#include <string.h>
#include <math.h>
#include "mex.h"

#define	INPUT	prhs[0]		/* Input Arguments */
#define	OUTPUT	plhs[0]		/* Output Arguments */

int doubleSortFunction(const void *a, const void *b) {
	return(*(double *)a - *(double *)b);
}

/* Median computation, which put nan at end and do quick sort on those non-nan numbers */
/* (Actually there are more efficient ways of finding the median instead of do sorting first.) */
double vecNanMedian(double *vec, int vecLen){
	double *tempVec=(double *)calloc(vecLen, sizeof(double));
	int i, validCount=0;
	double temp, output;
	memcpy(tempVec, vec, vecLen*sizeof(double));

	for (i=0; i<vecLen; i++)
		if (!mxIsNaN(tempVec[i])){		// Push non-nan numbers to the beginning
			temp=tempVec[i];
			tempVec[i]=tempVec[validCount];
			tempVec[validCount]=temp;
			validCount++;
		}
	qsort((void *)tempVec, validCount, sizeof(double), doubleSortFunction);
	output=validCount%2==1?tempVec[(validCount-1)/2]:(tempVec[validCount/2-1]+tempVec[validCount/2])/2;
	free(tempVec);
	return(output);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	double *input, *output;
	int i, m, n;

	/* Check for proper number of arguments */
	if (nrhs<1) {
		char message[100];
		strcpy(message, mexFunctionName());
		strcat(message, " requires 1 input vector.\n");
		mexErrMsgTxt(message);
	}
	
	/* Assign pointers to the various parameters */
	input = mxGetPr(INPUT);
	m = mxGetM(INPUT);
	n = mxGetN(INPUT);

	/* Create a matrix for the return argument */
	if (m==1 | n==1)
		OUTPUT = mxCreateDoubleMatrix(1, 1, mxREAL);
	else
		OUTPUT = mxCreateDoubleMatrix(1, n, mxREAL);
	output = mxGetPr(OUTPUT);
	
	/* Do the actual computation */
	if (m==1 | n==1)	/* input is a vector */
		output[0]=vecNanMedian(input, m*n);
	else			/* input is a matrix */
		for (i=0; i<n; i++)
			output[i]=vecNanMedian(input+i*m, m);
}