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

/* Input Arguments */
#define	IN	prhs[0]
/* Output Arguments */
#define	SORTED	plhs[0]
#define	INDEX	plhs[1]

/// In-place quicksort for conventional C-style vector
/// This should not be called directly. It should be called by mySort() only.
template <class T>
void quickSort(T *vec, int low, int up, int *index){
	int i, j, temp1;
	T pkey, temp2;
	if(low<up) {
		i=low;
		j=up+1;
		pkey=vec[low];
		do {
			do {i++;} while((vec[i]<pkey)&&(i<up));
			do {j--;} while((vec[j]>pkey)&&(j>low));
			if (i<j) {
				temp2=vec[i]; vec[i]=vec[j]; vec[j]=temp2;
				temp1=index[i]; index[i]=index[j]; index[j]=temp1;
			}
		} while(i<j);
		temp2=vec[j]; vec[j]=vec[low]; vec[low]=temp2;
		temp1=index[j]; index[j]=index[low]; index[low]=temp1;
		quickSort(vec, low, j-1, index);
		quickSort(vec, j+1, up, index);
	}
}
/// Sorting for C-style vector, just like MATLAB: [sorted, index]=sort(vec)	
/// If "sorted" is the same as "vec", then it's in-place sorting
template <class T>
void mySort(const T *vec, int vecSize, T *sorted, int *index){
	for (int i=0; i<vecSize; i++){
		index[i]=i;
		sorted[i]=vec[i];
	}
	quickSort(sorted, 0, vecSize-1, index);
}

void mexFunction(
	int nlhs,	mxArray *plhs[],
	int nrhs, const mxArray *prhs[])
{
	double	*in, *sorted, *index;
	int m, n, i, j;
	char message[200];

	/* Check for proper number of arguments */
	if (nrhs!=1){
		sprintf(message, "Usage: [sorted, index] = %s(vector)", mexFunctionName());
		mexErrMsgTxt(message);
	}

	/* Dimensions of the input matrix */
	m = mxGetM(IN);
	n = mxGetN(IN);
	
	if ((m!=1) && (n!=1))
		mexErrMsgTxt("Input should be a vector, not a matrix!");

	if (m==1){
		SORTED = mxCreateDoubleMatrix(1, m*n, mxREAL);
		INDEX = mxCreateDoubleMatrix(1, m*n, mxREAL);
	}else{
		SORTED = mxCreateDoubleMatrix(m*n, 1, mxREAL);
		INDEX = mxCreateDoubleMatrix(m*n, 1, mxREAL);
	}
	in = mxGetPr(IN);
	sorted = mxGetPr(SORTED);
	index = mxGetPr(INDEX);
	int *indexInt = (int *)malloc(m*n*sizeof(int));
	int *inInt =    (int *)malloc(m*n*sizeof(int));
	int *sortedInt =    (int *)malloc(m*n*sizeof(int));
	double2intVec(in, m*n, inInt);
	mySort(inInt, m*n, sortedInt, indexInt);
	for (i=0; i<m*n; i++)
		index[i]=indexInt[i]+1;
	int2doubleVec(sortedInt, m*n, sorted);

	free(inInt);
	free(sortedInt);
	free(indexInt);
}