#include <math.h>

void
greycomat3D(int* input, int xi, int yi, int zi,
	    int* mask, int xm, int ym, int zm,
	    int* distances, int dsize,
	    int* thetas, int tsize,
	    int* phis, int psize, 
	    int levels, int* output) {
  int x,y,z, i, j, d_idx, d_val, theta_idx, phi_idx, xval, yval, zval;
  double theta,phi,st,stsp,stcp,ct,dstsp,dstcp,dct;

  /* 5-dimensional histogram
     P = f(i, j, d, theta,phi) where i and j are grey levels
     See Pattern Recognition Engineering (Morton Nadler & Eric
     P. Smith) - modified S Van Der Walt's 2D version to make
     3D version - KY
  */

  for (theta_idx = 0; theta_idx < tsize; theta_idx++) {
    theta = thetas[theta_idx];
    st = sin(theta);
    ct = cos(theta);
    for (phi_idx = 0; phi_idx < psize; phi_idx++) {
      /* VERY SIMPLE check against overcounting */
      if (thetas[theta_idx] > 0 || phi_idx == 0)
	{
	  phi = phis[phi_idx];
	  stcp = st*cos(phi);
	  stsp = st* sin(phi);

	  for (d_idx = 0; d_idx < dsize; d_idx++) {
	    d_val = distances[d_idx];
	    dstcp = (int)floor(stcp * d_val + 0.5);
	    dstsp = (int)floor(stsp * d_val + 0.5);
	    dct = (int)floor(ct * d_val + 0.5);
  
	    for (x = 0; x < xi; x++) {
	      for (y = 0; y < yi; y++) {
		for (z = 0; z < zi; z++) {
		  if(mask[x*yi*zi + y*zi + z] == 1)
		    { 
		      i = input[x*yi*zi + y*zi + z];
	    
		      xval = x + dstcp;
		      yval = y + dstsp;
		      zval = z + dct;

		      if ((xval >= 0) && (xval < xi) &&
			  (yval >= 0) && (yval < yi) &&
			  (zval >= 0) && (zval < zi))
			{
			  if (mask[xval*yi*zi + yval*zi + zval] == 1)
			    {
			      j = input[xval*yi*zi + yval*zi + zval];

			      if (i >= 0 && i < levels && j >= 0 && j < levels) {
				output[(((i*levels + j)*dsize
					 + d_idx)*tsize + theta_idx)*psize + phi_idx]++;
			      } // else raise a warning
			    }
			}
		    }
		}
	      }
	    }
	  }
	}
    }
  }
}