/*   Copyright 1984-2000 The MathWorks, Inc.  */
/* $Revision: 1.8 $ */
/*
 * HISTC.MEX
 *
 * Usage:
 *          Y = histc(X,edges)
 *          Y = histc(X,edges,dim)
 *
 * Output is always double
 */

static char rcsid[] = "$Id: histc.c,v 1.8 2000/06/01 02:10:46 joeya Exp $";

#include "mex.h"

/* Defines */
#define NOBIN -1
#define PRIVATE static
#define MIN(a,b) (a < b ? a : b)
#define MAX(a,b) (a > b ? a : b)

PRIVATE
/*
 * Allocate reduction output.
 */
mxArray *mfCreateHistResult(
    int          ndims,
    const int    *siz,
    int          dim,
    int		 nbins,
    mxClassID    classid,
    mxComplexity cmplxFlag
    )
{
    int     i;
    int     *newsiz;
    mxArray *res;
    
    /* Output is the same size as the input with siz[dim] replaced by nbins */
    newsiz = (int *)mxCalloc(MAX(ndims,dim+1),sizeof(int));
    for (i=0; i < ndims; i++)
	newsiz[i]=siz[i];
    for (i=ndims; i < dim; i++)
    	newsiz[i] = 1;

    newsiz[dim] = nbins;

    res = mxCreateNumericArray(MAX(ndims,dim+1),newsiz,classid,cmplxFlag);
    mxFree(newsiz);
    
    return res;
}


/*
 * Return index of first non-singleton dimension.
 */
PRIVATE int mfFindFirstNonSingleton(
    int ndims,
    const int *siz
    )
{
    int i;
    int dim;
    
    /* Find first non-singleton dimension */
    for (i=0, dim=ndims-1; i < ndims; i++) 
    {
	if (siz[i] != 1)
	{
	    dim = i;
	    break;
	}
    }
    return dim;
}


/*
 * GetElementSizeFromClassID
 *
 * Purpose: Given MATLAB class ID, return corresponding element size
 *
 * Inputs:  classID --- MATLAB class ID
 * Outputs: none
 * Return:  # of bytes for an element of this class
 */
PRIVATE int GetElementSizeFromClassID(mxClassID classID)
{
    int result;

    switch (classID)
    {

    case mxCHAR_CLASS:
        result = sizeof(mxChar);
        break;

    case mxDOUBLE_CLASS:
        result = sizeof(double);
        break;

    case mxSINGLE_CLASS:
        result = sizeof(real32_T);
        break;

    case mxINT8_CLASS:
        result = sizeof(int8_T);
        break;

    case mxUINT8_CLASS:
        result = sizeof(uint8_T);
        break;

    case mxINT16_CLASS:
        result = sizeof(int16_T);
        break;

    case mxUINT16_CLASS:
        result = sizeof(uint16_T);
        break;

    case mxINT32_CLASS:
        result = sizeof(int32_T);
        break;

    case mxUINT32_CLASS:
        result = sizeof(uint32_T);
        break;

    default:
        mexErrMsgTxt("Input array must be numeric.");
    }

    return(result);
}


/*
 * GetDoubleValue
 *
 * Purpose: Return value as a double
 *
 * Inputs:  x -- array element, classID --- MATLAB class ID
 * Outputs: none
 * Return:  value as a double
 */
PRIVATE double GetDoubleValue(void *x, mxClassID classID)
{
    double result;

    switch (classID)
    {

    case mxCHAR_CLASS:
        result = ((double) *((mxChar *)x));
        break;

    case mxDOUBLE_CLASS:
        result = ((double) *((double *)x));
        break;

    case mxSINGLE_CLASS:
        result = ((double) *((real32_T *)x));
        break;

    case mxINT8_CLASS:
        result = ((double) *((int8_T *)x));
        break;

    case mxUINT8_CLASS:
        result = ((double) *((uint8_T *)x));
        break;

    case mxINT16_CLASS:
        result = ((double) *((int16_T *)x));
        break;

    case mxUINT16_CLASS:
        result = ((double) *((uint16_T *)x));
        break;

    case mxINT32_CLASS:
        result = ((double) *((int32_T *)x));
        break;

    case mxUINT32_CLASS:
        result = ((double) *((uint32_T *)x));
        break;

    default:
        mexErrMsgTxt("Input array must be numeric.");
    }

    return(result);
}

PRIVATE mxArray *muMustBeDouble(const mxArray *x)
{
    mxArray *result;
    double *pr;
    mxClassID classid = mxGetClassID(x);
    int i;
    int n = mxGetNumberOfElements(x);

    if (classid == mxDOUBLE_CLASS)
    	return (mxArray *)x;
 
    result = mxCreateNumericArray(mxGetNumberOfDimensions(x),mxGetDimensions(x),
                                  mxDOUBLE_CLASS,mxREAL);
    pr = mxGetPr(result);
                                  
    switch (classid)
    {

    case mxCHAR_CLASS:
    {
    	mxChar *val;
    	
    	val = (mxChar *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxSINGLE_CLASS:
    {
    	real32_T *val;
    	
    	val = (real32_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxINT8_CLASS:
    {
    	int8_T *val;
    	
    	val = (int8_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxUINT8_CLASS:
    {
    	uint8_T *val;
    	
    	val = (uint8_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxINT16_CLASS:
    {
    	int16_T *val;
    	
    	val = (int16_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxUINT16_CLASS:
    {
    	uint16_T *val;
    	
    	val = (uint16_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxINT32_CLASS:
    {
    	int32_T *val;
    	
    	val = (int32_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    case mxUINT32_CLASS:
    {
    	uint32_T *val;
    	
    	val = (uint32_T *)mxGetData(x);
    	
    	for (i=0; i<n; i++)
    	  *pr++ = (double) (*val++);
        break;
    }
    default:
        mexErrMsgTxt("Input array must be numeric.");
    }

    return(result);
}

/*
 * mfGetDimArg
 *
 * Purpose: Get an integer value from MATLAB array; error out if array is
 *          empty or not an integer.
 *
 * Inputs:  A --- MATLAB array
 * Outputs: none
 * Return:  integer value
 *
 */ 
PRIVATE int mfGetDimArg(const mxArray *A)
{
    double d;
    int result;
    static char msg[] = "Dimension argument must be a positive integer scalar.";

    if ((mxGetNumberOfElements(A)!=1) || mxIsComplex(A))
        mexErrMsgTxt(msg);

    d = GetDoubleValue(mxGetPr(A),mxGetClassID(A));
    result = (int)d;
	
    if (((double) result) != d || result < 1)
        mexErrMsgTxt(msg);
        
    return(result - 1); /* return 0 based index */
}


/*
 * Return index of bin the x.  x is in bin k if  bin_edges[k] <= x < bin_edges[k+1].
 *
 * Special case: x is in the last bin if x == bin_edges[nbins].
 */
PRIVATE int findBin(
    double x,
    double *bin_edges, /* Bin edges */
    int nbins	/* Number of edges */
    )
{
    int k = NOBIN;

    /* Check for NaN and empty bin_edges */    
    if (! mxIsNaN(x) && bin_edges != NULL)
	{
		/* Use a binary search */
		{
		int k0 = 0;
		int k1 = nbins-1;
		
		if (x >= bin_edges[0] && x < bin_edges[nbins-1])
		{
		  k = (k0+k1)/2;
		  while (k0 < k1-1)
		  {
			if (x >= bin_edges[k]) k0 = k;
			else k1 = k;
			k = (k0+k1)/2;
		  }
		  k = k0;
		}
		}
    
		/* Check for special case */
		if (x == bin_edges[nbins-1])
		k = nbins-1;
	}
	
    return k;
}

/*
 * Return index of bin the x.  x is in bin k if  bin_edges[k] <= x < bin_edges[k+1].
 *
 * Special case: x is in the last bin also if x == bin_edges[nbins].
 */
PRIVATE int findNonDoubleBin(
    void *px,
    mxClassID classid,
    double *bin_edges, /* Bin edges */
    int nbins	/* Number of edges */
    )
{
    int k = NOBIN;
    double val = GetDoubleValue(px,classid);
    
    /* Check for NaN */    
    if (mxIsNaN(val))
        return NOBIN;

    /* Use a binary search */
    {
	int k0 = 0;
	int k1 = nbins-1;
	
	if (val >= bin_edges[0] && val < bin_edges[nbins-1])
	{
	  	  k = (k0+k1)/2;
	  while (k0 < k1-1)
	  {
	    if (val >= bin_edges[k]) k0 = k;
	    else k1 = k;
	    k = (k0+k1)/2;
	  }
	  k = k0;
	}
    }
    
    /* Check for special case */
    if (val == bin_edges[nbins-1])
	k = nbins-1;
	
    return k;
}


/*
 * Inner HIST loop: y = HIST(x,edges)
 */
PRIVATE void _HistLoop(
    const mxArray *x, /* input array */
    const mxArray *edges, /* input array */
    mxArray *y,       /* output array */
    mxArray *bin_output, /* bin index output array */
    int stride,       /* stride along active dimension */
    int mx,	      /* number of elements along active dimension in x */
    int my            /* number of elements along active dimension in y */
    )
{
    double *xr = mxGetPr(x);
    double *yr = mxGetPr(y);
    double *pbin = NULL;
    double *bin_edges = mxGetPr(edges);
    int i,j,k,n1,n2;
    int xoffset,yoffset;
    int bin;
    
    if (bin_output != NULL)
    	pbin = mxGetPr(bin_output);
    	
    if (mx < 1) {
	return;
    }

    /* If stride <= 1 choose the roles of n1 and n2 so the loop is faster */
    if (stride <= 1)
    {
    	n1 = stride;
	n2 = mxGetNumberOfElements(x)/mx;
    	xoffset = 0;
    	yoffset = 0;
    }
    else
    {
	n1 = mxGetNumberOfElements(x)/stride/mx;
	n2 = stride;
	xoffset = stride*mx - 1;
	yoffset = stride*my - 1;
    }
    	    
    /* real loop */
    for (i=0; i < n1; i++)
    {
	for (j=0; j < n2; j++)
	{
	    for (k=0; k < mx; k++)
	    {
		bin = findBin(*xr,bin_edges,my);
		if (bin != NOBIN)
		    yr[bin*stride]++;
		if (pbin != NULL)
		{
		    *pbin = bin+1; /* MATLAB indices are 1-based */
		    pbin += stride;
		}
		xr += stride;
	    }
	    if (pbin != NULL) pbin -= xoffset;
	    xr -= xoffset;
	    yr += my*stride - yoffset;
	}
	/* go to next page of input and output arrays */
	if (pbin != NULL) pbin += xoffset - stride + 1;
	xr += xoffset - stride + 1;
	yr += yoffset - stride + 1;
    }
}


/*
 * Inner HIST loop for non-doubles: y = HIST(x,edges)
 */
PRIVATE void _NonDoubleHistLoop(
    const mxArray *x, /* input array */
    const mxArray *edges, /* input array */
    mxArray *y,       /* output array */
    mxArray *bin_output, /* bin index output array */
    int stride,       /* stride along active dimension */
    int mx,	      /* number of elements along active dimension in x */
    int my            /* number of elements along active dimension in y */
    )
{
    uint8_T *xr = (uint8_T *) mxGetData(x); /* Treat x as a bunch of bytes */
    double *yr = mxGetPr(y);
    double *pbin = NULL;
    double *bin_edges = mxGetPr(edges);
    int i,j,k,n1,n2;
    int xoffset,yoffset;
    int bin;
    mxClassID classid = mxGetClassID(x);
    int elem_size = GetElementSizeFromClassID(classid);
    
    if (mx < 1) {
	return;
    }

    if (bin_output != NULL)
    	pbin = mxGetPr(bin_output);

    /* If stride <= 1 choose the roles of n1 and n2 so the loop is faster */
    if (stride <= 1)
    {
    	n1 = stride;
	n2 = mxGetNumberOfElements(x)/mx;
    	xoffset = 0;
    	yoffset = 0;
    }
    else
    {
	n1 = mxGetNumberOfElements(x)/stride/mx;
	n2 = stride;
	xoffset = stride*mx - 1;
	yoffset = stride*my - 1;
    }
    	    
    /* real loop */
    for (i=0; i < n1; i++)
    {
	for (j=0; j < n2; j++)
	{
	    for (k=0; k < mx; k++)
	    {
		bin = findNonDoubleBin(xr,classid,bin_edges,my);
		if (bin != NOBIN)
		    yr[bin*stride]++;
		if (pbin != NULL)
		{
		    *pbin = bin+1; /* MATLAB indices are 1-based */
		    pbin += stride;
		}
		xr += stride*elem_size;
	    }
	    if (pbin != NULL) pbin -= xoffset;
	    xr -= xoffset*elem_size;
	    yr += my*stride - yoffset;
	}
	/* go to next page of input and output arrays */
	if (pbin != NULL) pbin += xoffset - stride + 1;
	xr += (xoffset - stride + 1)*elem_size;
	yr += yoffset - stride + 1;
    }
}


/*
 * Full HISTC function
 */
void mexFunction(
    int		  nlhs, 	/* number of expected outputs */
    mxArray	  *plhs[],	/* mxArray output pointer array */
    int		  nrhs, 	/* number of inputs */
    const mxArray	  *prhs[]	/* mxArray input pointer array */
   )
{
    const mxArray	*x;
    mxArray	*edges;
    mxArray     *bin_output;
    double *bin_edges;
    int nbins;
    int i;
    
    if (nlhs > 2)
    	mexErrMsgTxt("Too many output arguments.");
    if (nrhs < 2)
    	mexErrMsgTxt("Not enough input arguments.");
    if (nrhs > 3)
    	mexErrMsgTxt("Too many input arguments.");
    	
    x = prhs[0];
    if (mxIsEmpty(x))
    	mexErrMsgTxt("First input must be non-empty numeric array.");

    if (!(mxIsNumeric(x) || mxIsChar(x)) || mxIsSparse(x))
    	mexErrMsgTxt("First input must be non-sparse numeric array.");
    	

    edges = muMustBeDouble(prhs[1]);

    nbins = mxGetNumberOfElements(edges);

    /* Make sure the edges vector is monotonically non-decreasing */
    bin_edges = mxGetPr(edges);
    for (i=0; i<nbins-1; i++)
    {
    	if (bin_edges[i] > bin_edges[i+1])
    	   mexErrMsgTxt("Edges vector must be monotonically non-decreasing.");
    }
    
    if (nrhs == 2 && mxGetM(x)==0 && mxGetN(x)==0) 
    {
	/*
	 * Special case:
	 * 
	 * hist([],edges) is nbins-by-0
	 */
         plhs[0] = mxCreateDoubleMatrix(nbins, 0, mxREAL);
         if (nlhs > 1)
           plhs[1] = mxCreateNumericArray(mxGetNumberOfDimensions(x),mxGetDimensions(x),
                                          mxDOUBLE_CLASS,mxREAL);
    }
    else
    {
	int	  ndims = mxGetNumberOfDimensions(x);
	const int *siz = mxGetDimensions(x);
	int 	  dim;
	int	  stride;
	int       m;
    	
    	/* Determine active dimension -- dim */
    	if (nrhs == 3)
    	    dim = mfGetDimArg(prhs[2]);
    	else
    	    dim = mfFindFirstNonSingleton(ndims,siz);

	/* Compute stride along active dimension */
	for (i=0, stride=1; i < MIN(ndims,dim); i++)
	    stride *= siz[i];
	    
	/* Number of elements along active dimension */
	m = (dim < ndims ? siz[dim] : 1);
	
	if (mxGetPi(x) != NULL)
	    mexErrMsgTxt("All inputs must be real.");

	plhs[0] = mfCreateHistResult(ndims,siz,dim,nbins,mxDOUBLE_CLASS,mxREAL);
	if (nlhs > 1)
	     bin_output = mxCreateNumericArray(mxGetNumberOfDimensions(x),mxGetDimensions(x),
                                               mxDOUBLE_CLASS,mxREAL);
	else
	     bin_output = NULL;
	        
	if (mxGetClassID(x) == mxDOUBLE_CLASS)
	   _HistLoop(x,edges,plhs[0],bin_output,stride,m,nbins);
	else
	   _NonDoubleHistLoop(x,edges,plhs[0],bin_output,stride,m,nbins);
    }
    
    if (edges != prhs[1])
    	mxDestroyArray(edges);
    if (nlhs > 1)
        plhs[1] = bin_output;
}