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

// unit vectors used to compute gradient orientation
double uu[9] = {1.0000,
        0.9397,
        0.7660,
        0.500,
        0.1736,
        -0.1736,
        -0.5000,
        -0.7660,
        -0.9397};
double vv[9] = {0.0000,
        0.3420,
        0.6428,
        0.8660,
        0.9848,
        0.9848,
        0.8660,
        0.6428,
        0.3420};

static inline double minP(double x, double y) { return (x <= y ? x : y); }
static inline double maxP(double x, double y) { return (x <= y ? y : x); }

static inline int minP(int x, int y) { return (x <= y ? x : y); }
static inline int maxP(int x, int y) { return (x <= y ? y : x); }

int roundP (double x) {
  int i = (int) x;
  if (x >= 0.0) {
    return ((x-i) >= 0.5) ? (i + 1) : (i);
  } else {
    return (-x+i >= 0.5) ? (i - 1) : (i);
  }

}

mxArray *process(const mxArray *mximage, const mxArray *mxsbin) {
  double *im = (double *)mxGetPr(mximage);
  double dd;
  const int *dims = mxGetDimensions(mximage);
  if (mxGetNumberOfDimensions(mximage) != 2 ||
      mxGetClassID(mximage) != mxDOUBLE_CLASS)
    mexErrMsgTxt("Invalid input");

  int sbin = (int)mxGetScalar(mxsbin);
  int blocks[2];
  blocks[0] = (int)roundP((double)dims[0]/(double)sbin);
  blocks[1] = (int)roundP((double)dims[1]/(double)sbin);

  int visible[2];
  visible[0] = blocks[0]*sbin;
  visible[1] = blocks[1]*sbin;

  int out[3] = {visible[0]-2, visible[1]-2, 2};

  mxArray *mxGradient = mxCreateNumericArray(3, out, mxDOUBLE_CLASS, mxREAL);
  double *gradient = (double *)mxGetPr(mxGradient);

  double *s;
  double dot, o;
  double dx, dy, v;

  double *tempGradientOBase   = gradient;
  double *tempGradientVBase   = gradient + ( out[0] * out[1]);

  double *tempGradientDot;
  double *tempGradientO;
  double *tempGradientV;

  int mul = dims[0]*dims[1];

  for (int x = 1; x < visible[1] - 1; x++) {
    for (int y = 1; y < visible[0] - 1; y++) {

    // Gray channel
    s = im + minP(x, dims[1]-2)*dims[0] + minP(y, dims[0]-2);
    dy = *(s+1) - *(s-1);
    dx = *(s+dims[0]) - *(s-dims[0]);
    v = sqrt(dx*dx + dy*dy);

    double best_dot = 0;
    int best_o = 0;
    for (int o = 0; o < 9; o++) {
      double dot = uu[o]*dx + vv[o]*dy;
      if (dot > best_dot) {
    best_dot = dot;
    best_o = o;
      } else if (-dot > best_dot) {
    best_dot = -dot;
    best_o = o+9;
      }
    }

    int val = ((x - 1) * out[0]) + (y - 1);
    tempGradientO   = tempGradientOBase + val;
    tempGradientV   = tempGradientVBase  + val;
    tempGradientO[0]   = (double) best_o;
    tempGradientV[0]   = v;
    }
  }
  return mxGradient;
}

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]);
}