// gridResample3Dc.c
// Accepts a 3D data grid along with its coordinate system information, and resamples
// that grid onto another grid, specified by another coordiate system.

#include <stdlib.h>
#include <stdio.h>
#include "mex.h"

// macros for grid indexing
#define GRIDIND3D(x,y,z,dim) ((x) + dim[0]*(y) + dim[0]*dim[1]*(z))

// Input Arguments 
#define X         (prhs[0])  // coordinate vector for first dimension of original data
#define Y         (prhs[1])  // coordinate vector for second dimension of original data
#define Z         (prhs[2])  // coordinate vector for third dimension of original data
#define DATA      (prhs[3])  // original data
#define XNEW      (prhs[4])  // coordinate vector for first dimension of resampled data
#define YNEW      (prhs[5])  // coordinate vector for second dimension of resampled data
#define ZNEW      (prhs[6])  // coordinate vector for third dimension of resampled data
#define MODE	  (prhs[7])  // interpolation mode ('linear' or 'nearest')
#define DATANEW   (plhs[0])  // resampled data

// Notes on inputs
// Original coordinate vectors must be monotonically increasing, but this
// does not have to be true for the new coordinate vectors.

// Prototypes
int binSearch(float *, float, int);

void mexFunction(int nlhs, mxArray  *plhs[], int nrhs, const mxArray  *prhs[])
{
  // pointers to input/output mxArray numbers of dimensions
  int xDim, yDim, zDim, dataDim, xNewDim, yNewDim, zNewDim, modeDim;

  // pointers to input/output mxArray dimensions
  const int *xDimPtr, *yDimPtr, *zDimPtr, *dataDimPtr;
  const int *xNewDimPtr, *yNewDimPtr, *zNewDimPtr, *modeDimPtr;
  int *dataNewDimPtr;

  // pointers to input/output data
  float *x, *y, *z, *dataPtr, *xNew, *yNew, *zNew, *dataNewPtr;
  char *modePtr;

  // utility variables
  int i,j,k,iNew,jNew,kNew,M,N,Q,Mnew,Nnew,Qnew;
  float t, u, v, f, one;
  int nearestNeighborFlag, linearFlag;  // interpolation type flags

  float x0,y0,z0;
  float dx,dy,dz;

  int xUniformSpacingFlag, yUniformSpacingFlag,zUniformSpacingFlag;

  nearestNeighborFlag = 0;
  linearFlag = 0;
  one = 1.0;

  xDim = mxGetNumberOfDimensions(X);
  yDim = mxGetNumberOfDimensions(Y);
  zDim = mxGetNumberOfDimensions(Z);
  dataDim = mxGetNumberOfDimensions(DATA);
  xNewDim = mxGetNumberOfDimensions(XNEW);
  yNewDim = mxGetNumberOfDimensions(YNEW);
  zNewDim = mxGetNumberOfDimensions(ZNEW);
  modeDim = mxGetNumberOfDimensions(MODE);

  xDimPtr = mxGetDimensions(X);
  yDimPtr = mxGetDimensions(Y);
  zDimPtr = mxGetDimensions(Z);
  dataDimPtr = mxGetDimensions(DATA);
  xNewDimPtr = mxGetDimensions(XNEW);
  yNewDimPtr = mxGetDimensions(YNEW);
  zNewDimPtr = mxGetDimensions(ZNEW);
  modeDimPtr = mxGetDimensions(MODE);

  // check input arguments
  if (  (xDim != 2) || (yDim != 2) || (zDim != 2) 
	 || (xNewDim != 2) || (yNewDim != 2) || (zNewDim != 2)
	 || (modeDim != 2))
	 mexErrMsgTxt("X, Y, Z, XNEW, YNEW, ZNEW, and MODE inputs must all be vectors.");

  /*
  printf("xDim = (%d,%d)\n",xDimPtr[0],xDimPtr[1]);
  printf("yDim = (%d,%d)\n",yDimPtr[0],yDimPtr[1]);
  printf("zDim = (%d,%d)\n",zDimPtr[0],zDimPtr[1]); */

  // get the lengths of the vectors, as they are related to coordinate system sizes
  M = xDimPtr[0]*xDimPtr[1];
  N = yDimPtr[0]*yDimPtr[1];
  Q = zDimPtr[0]*zDimPtr[1];

  Mnew = xNewDimPtr[0]*xNewDimPtr[1];
  Nnew = yNewDimPtr[0]*yNewDimPtr[1];
  Qnew = zNewDimPtr[0]*zNewDimPtr[1];

  if (   !mxIsSingle(X) || !mxIsSingle(Y) || !mxIsSingle(Z)
	  || !mxIsSingle(DATA) 
	  || !mxIsSingle(XNEW) || !mxIsSingle(YNEW) || !mxIsSingle(ZNEW))
	  mexErrMsgTxt("X, Y, Z, DATA, XNEW, YNEW, ZNEW must be single arrays.");

  /* 
  printf("DataDim = (%d,%d,%d)\n",dataDimPtr[0],dataDimPtr[1],dataDimPtr[2]);
  printf("X,Y,Z = (%d,%d,%d)\n",M,N,Q); */

  // check the size of the original data array against the original coordinate system
  if (  ((dataDim == 2) && ((M != dataDimPtr[0]) || (N != dataDimPtr[1]) || (Q != 1)))
     || ((dataDim == 3) && ((M != dataDimPtr[0]) || (N != dataDimPtr[1]) || (Q != dataDimPtr[2])))
	 ||  (dataDim > 3))
	  mexErrMsgTxt("DATA must be sized to match the lengths of X, Y, and Z.\n");

  if (!mxIsChar(MODE))
	  mexErrMsgTxt("MODE must be a character array");

  // get coordinate system data pointers
  x = (float *)mxGetData(X);
  y = (float *)mxGetData(Y);
  z = (float *)mxGetData(Z);

  xNew = (float *)mxGetData(XNEW);
  yNew = (float *)mxGetData(YNEW);
  zNew = (float *)mxGetData(ZNEW);

  // ensure that the original coordinate system vectors are monotonically increasing
  for (i=1;i<M;i++)
	  if ((x[i] - x[i-1]) <= 0.0)
		  mexErrMsgTxt("X must be a monotonically increasing vector.");

  for (j=1;j<N;j++)
	  if ((y[j] - y[j-1]) <= 0.0)
		  mexErrMsgTxt("Y must be a monotonically increasing vector.");

  for (k=1;k<Q;k++)
	  if ((z[k] - z[k-1]) <= 0.0)
		  mexErrMsgTxt("Z must be a monotonically increasing vector.");

  xUniformSpacingFlag = 1;
  yUniformSpacingFlag = 1;
  zUniformSpacingFlag = 1;

  // centers of first elements in each axis
  x0 = x[0];
  y0 = y[0];
  z0 = z[0];

  // check to see if any axes are uniformly-spaced
  dx = x[1] - x[0];
  for (i=1;i<M;i++)
	  if (fabs(fabs(x[i] - x[i-1]) - fabs(dx))/fabs(dx) > 1e-4)
		  xUniformSpacingFlag = 0;

  dy = y[1] - y[0];
  for (j=1;j<N;j++)
	  if (fabs(fabs(y[j] - y[j-1]) - fabs(dy))/fabs(dy) > 1e-4)
		  yUniformSpacingFlag = 0;

  dz = z[1] - z[0];
  for (k=1;k<Q;k++)
	  if (fabs(fabs(z[k] - z[k-1]) - fabs(dz))/fabs(dz) > 1e-4)
		  zUniformSpacingFlag = 0;
  
  // set pointer to original data
  dataPtr = (float *)mxGetData(DATA);

  // read in the mode
  modePtr = (char *)malloc(sizeof(char)*modeDimPtr[0]*modeDimPtr[1]);
  mxGetString(MODE,modePtr,modeDimPtr[0]*modeDimPtr[1]+1);

  // determine the interpolation mode
  if (strcmp(modePtr,"nearest") == 0)
	  nearestNeighborFlag = 1;
  else if (strcmp(modePtr,"linear") == 0)
	  linearFlag = 1;
  else
	  mexErrMsgTxt("Unknown interpolation mode.  Must be either 'linear' or 'nearest'.");

  // set up the output grid
  dataNewDimPtr = (int *)malloc(sizeof(int)*3);

  dataNewDimPtr[0] = Mnew;
  dataNewDimPtr[1] = Nnew;
  dataNewDimPtr[2] = Qnew;

  if((DATANEW = mxCreateNumericArray(3,dataNewDimPtr,mxSINGLE_CLASS,mxREAL)) == NULL)
	  mexErrMsgTxt("Unable to create DATANEW array.");

  dataNewPtr = (float *)mxGetData(DATANEW);

  // interpolate the old grid onto the new one
  for (kNew=0;kNew<Qnew;kNew++) for (jNew=0;jNew<Nnew;jNew++) for (iNew=0;iNew<Mnew;iNew++)
  {
	// mexPrintf("(iNew,jNew,kNew) = (%d,%d,%d)\n",iNew,jNew,kNew);
    // find the bordering indices in the old grid that contain the current point
	// in the new grid
	if (xUniformSpacingFlag == 0)
		i = binSearch(x,xNew[iNew],M);
	else
		i = (int)((xNew[iNew]-x0)/dx);

	if (yUniformSpacingFlag == 0)
		j = binSearch(y,yNew[jNew],N);
	else
		j = (int)((yNew[jNew]-y0)/dy);

	if (zUniformSpacingFlag == 0)
		k = binSearch(z,zNew[kNew],Q);
	else
		k = (int)((zNew[kNew]-z0)/dz);
	
	// mexPrintf("(i,j,k) = (%d,%d,%d)\n",i,j,k);

	// if the current point falls outside of the original grid, place a zero there
	if (   (i < 0) || (i+1 >= M)
		|| (j < 0) || (j+1 >= N)
		|| (k < 0) || (k+1 >= Q))
		dataNewPtr[GRIDIND3D(iNew,jNew,kNew,dataNewDimPtr)] = 0.0;
	else
	{
		// the divisions here are safe, since x, y, and z are monotonically increasing
		t = (xNew[iNew] - x[i])/(x[i+1] - x[i]);
		u = (yNew[jNew] - y[j])/(y[j+1] - y[j]);
		v = (zNew[kNew] - z[k])/(z[k+1] - z[k]);

		// printf("(i,j,k) = (%d,%d,%d), (t,u,v) = (%f,%f,%f)\n",i,j,k,t,u,v);

		if (linearFlag == 1)
		// linear interpolation
			f = dataPtr[GRIDIND3D(i,j,k,dataDimPtr)]*(one-t)*(one-u)*(one-v)
			  + dataPtr[GRIDIND3D(i,j,k+1,dataDimPtr)]*(one-t)*(one-u)*v
			  + dataPtr[GRIDIND3D(i,j+1,k+1,dataDimPtr)]*(one-t)*u*v
		      + dataPtr[GRIDIND3D(i+1,j+1,k+1,dataDimPtr)]*t*u*v
		      + dataPtr[GRIDIND3D(i,j+1,k,dataDimPtr)]*(one-t)*u*(one-v)
		      + dataPtr[GRIDIND3D(i+1,j+1,k,dataDimPtr)]*t*u*(one-v)
		      + dataPtr[GRIDIND3D(i+1,j,k+1,dataDimPtr)]*t*(one-u)*v
		      + dataPtr[GRIDIND3D(i+1,j,k,dataDimPtr)]*t*(one-u)*(one-v);
	    else if (nearestNeighborFlag == 1)
		// nearest neighbor interpolation
		{
			if (t >= 0.5)
				i = i + 1;
			if (u >= 0.5)
				j = j + 1;
			if (v >= 0.5)
				k = k + 1;

			f = dataPtr[GRIDIND3D(i,j,k,dataDimPtr)]; 
		}

		dataNewPtr[GRIDIND3D(iNew,jNew,kNew,dataNewDimPtr)] = f;
	}
  }

  // free the mallocs
  free(dataNewDimPtr);
}

int binSearch(float *a, float searchnum, int M)
/* Accepts a float array of data of length M ordered lowest to highest and a number 
called searchnum.  Returns the index of the first element of the array, a, that is 
less than or equal to the searchnum.  If the searchnum is less than a[0], then -1 
is returned, and if the searchnum is greater than a[M-1], then M is returned. */
{
    int bottom, mid, top;

    bottom = 0;
    top = M-1;

    // Ensure that the search parameter lies inside boundaries
	if(searchnum > a[top])
		return(M); 
	if(searchnum < a[bottom])
		return(-1);

	while(1)
	{
		mid = (top + bottom)/2;
		if      (searchnum == a[bottom]) 
			return(bottom);
		else if (searchnum == a[mid]) 
			return(mid);
		else if (searchnum == a[top])
			return(top);
		else if(searchnum < a[mid])
			top = mid - 1;
		else if(searchnum > a[mid + 1])
			bottom = mid + 1;
		else
			return(mid); 
	}
}