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

#define eps 0.0001
#define bin 5
// main function:
// takes a double depth image and a bin size 
// returns averaged depth values
static inline double max(double x, double y) { return (x <= y ? y : x); }
mxArray *process(const mxArray *mximage, const mxArray *mxsbin) {
  double *im = (double *)mxGetPr(mximage);
  const int *dims = mxGetDimensions(mximage);
  if (mxGetNumberOfDimensions(mximage) != 3 ||
    dims[2] != 3 ||
    mxGetClassID(mximage) != mxDOUBLE_CLASS)
    mexErrMsgTxt("Invalid input");

  int sbin = (int)mxGetScalar(mxsbin);

  // memory for caching orientation histograms & their norms
  int blocks[2];
  blocks[0] = (int)round((double)dims[0]/(double)sbin);
  blocks[1] = (int)round((double)dims[1]/(double)sbin);
  
  // memory for HOG features
  int out[3];
  out[0] = max(blocks[0]-2, 0);
  out[1] = max(blocks[1]-2, 0);
  out[2] = 3*bin;
  mxArray *mxfeat = mxCreateNumericArray(3, out, mxDOUBLE_CLASS, mxREAL);
  double *feat = (double *)mxGetPr(mxfeat);
  
  // compute features
  double interval = 1.0/bin;
  for (int x = 0; x < out[1]; x++	) {
    for (int y = 0; y < out[0]; y++) {
      double *dst = feat + x*out[0] + y;      
      double *s = im + (x+1)*sbin*dims[0] + (y+1)*sbin;
      int index;

      for (int xx = 0; xx < sbin; xx++ ) {
          for (int yy = 0; yy < sbin; yy++){
              for (int zz = 0; zz < 3; zz++){
                  index = floor(*(s + xx*dims[0] + yy + zz*dims[0]*dims[1])/(interval+eps));
                  *(dst + (zz*bin + index)*out[0]*out[1]) = *(dst+ (zz*bin + index)*out[0]*out[1]) + 1.0/sbin/sbin/3.0;
              }
          }
      }
    
    }
  }

  return mxfeat;
}

// matlab entry point
// F = dfeatures(image, bin)
// image should be color with double values
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 
  if (nrhs != 2)
    mexErrMsgTxt("Wrong number of inputs"); 
  if (nlhs != 1)
    mexErrMsgTxt("Wrong number of outputs");
  plhs[0] = process(prhs[0], prhs[1]);
}



