/* Roger Jang, 20091205 */

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

#define	VEC	prhs[0]		/* Input Arguments */
#define	X	prhs[1]		/* Input Arguments */
#define	SORTED	prhs[2]		/* Input Arguments */
#define	INDEX	plhs[0]		/* Output Arguments */

/* This comparison function is used in bsearch(). */
int doubleCompFunction(const void *a, const void *b) {
	int value;
	if ((*(double *)a - *(double *)b) > 0)  value = 1;
	if ((*(double *)a - *(double *)b) < 0)  value = -1;
	if ((*(double *)a - *(double *)b) == 0)  value = 0;
	return(value);
}

/* Find elements in a vector */
int vecFind(double *vec, int vecLen, double x, int sorted, int *index){
	int i, count=0, beginIndex, endIndex;
	double *ptr;
	printf("sorted=%d\n", sorted);
	if (sorted==0)		/* Linear search */
		for (i=0; i<vecLen; i++){
			printf("vec[%d]=%f, x=%f\n", i, vec[i], x);
			if (vec[i]==x){
				index[count]=i;
				count++;
			}
		}
	else {			/* Binary search */
		ptr = (double *)bsearch(&x, vec, vecLen, sizeof(double), doubleCompFunction);
		if (ptr==NULL)
			return(0);
		// Find all possible match
		endIndex=beginIndex=(int)(ptr-vec);
		while (*(ptr--)==x){
			beginIndex--;
			if (ptr<vec)
				break;
		}
		while (*(ptr++)==x){
			endIndex++;
			if (ptr>vec+vecLen)
				break;
		}
		for (i=beginIndex; i<=endIndex; i++)
			index[i-beginIndex]=i;
		return(endIndex-beginIndex+1);
	}
	return(count);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	double *vec, x;
	int i, sorted, m, n, count, *index;

	/* 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 */
	vec = mxGetPr(VEC);
	x = mxGetScalar(X);
	sorted = 0;
	if (nrhs<3)
		sorted = (int)mxGetScalar(SORTED);
	m = mxGetM(VEC);
	n = mxGetN(VEC);

	index = (int *)malloc(m*n*sizeof(int));
	count = vecFind(vec, m*n, x, sorted, index);
		
	/* Create a matrix for the return argument */
	if (m==1) INDEX = mxCreateDoubleMatrix(1, count, mxREAL);
	if (n==1) INDEX = mxCreateDoubleMatrix(count, 1, mxREAL);
	for (i=0; i<count; i++)
		mxGetPr(INDEX)[i]=index[i]+1;	// MATLAB index

	free(index);
}