#include "mex.h"

typedef double elem_type ;

#define ELEM_SWAP(a,b) { register elem_type t=(a);(a)=(b);(b)=t; }


/*---------------------------------------------------------------------------
   Function :   kth_smallest()
   In       :   array of elements, # of elements in the array, rank k
   Out      :   one element
   Job      :   find the kth smallest element in the array
   Notice   :   use the median() macro defined below to get the median. 

                Reference:

                  Author: Wirth, Niklaus 
                   Title: Algorithms + data structures = programs 
               Publisher: Englewood Cliffs: Prentice-Hall, 1976 
    Physical description: 366 p. 
                  Series: Prentice-Hall Series in Automatic Computation 

 ---------------------------------------------------------------------------*/


elem_type kth_smallest(elem_type a[], int n, int k)
{
    register i,j,l,m ;
    register elem_type x ;

    l=0 ; m=n-1 ;
    while (l<m) {
        x=a[k] ;
        i=l ;
        j=m ;
        do {
            while (a[i]<x) i++ ;
            while (x<a[j]) j-- ;
            if (i<=j) {
                ELEM_SWAP(a[i],a[j]) ;
                i++ ; j-- ;
            }
        } while (i<=j) ;
        if (j<k) l=i ;
        if (k<i) m=j ;
    }
    return a[k] ;
}

#define median(a,n) kth_smallest(a,n,(((n)&1)?((n)/2):(((n)/2)-1)))

void imgMedian(double *in, int m, int n, int hwin, double *out)
{
	memcpy(out, in, m * n * sizeof(double));
}

/* The gateway routine */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
  double *in;
  double *out;
  int status, mrows, ncols, hwin;
  
  /*  Check for proper number of arguments */
  /* NOTE: You do not need an else statement when using
     mexErrMsgTxt within an if statement. It will never
     get to the else statement if mexErrMsgTxt is executed.
     (mexErrMsgTxt breaks you out of the MEX-file) 
  */
  if (nrhs != 2) 
    mexErrMsgTxt("Two inputs required");

  if (nlhs != 1) 
    mexErrMsgTxt("One output required");
  
  /* Create a pointer to the input image */
  in = mxGetPr(prhs[0]);
  
  /* Check to make sure the second input argument is a scalar */
  if (!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) ||
      mxGetN(prhs[1])*mxGetM(prhs[1]) != 1) {
    mexErrMsgTxt("Second input argument must be a scalar");
  }
  
  /* Get the scalar input hwin */
  hwin = (int)mxGetScalar(prhs[1]);

  /* Get the dimensions of the input image */
  mrows = mxGetM(prhs[0]);
  ncols = mxGetN(prhs[0]);
  
  /* Set the output pointer to the output matrix. */
  plhs[0] = mxCreateDoubleMatrix(mrows,ncols, mxREAL);
  
  /* Create a C pointer to a copy of the output matrix. */
  out = mxGetPr(plhs[0]);
  
  /* Call the C subroutine. */
  imgMedian(in, mrows, ncols, hwin, out);
}