#include <stdio.h>
#include <math.h>
#include "mex.h"
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
	if ((nrhs != 3) || (nlhs != 1)) {
		mexPrintf("function res=houghtrans2(im,RHO_MAX,THETA_MAX)\n");
		mexPrintf("Translated from m file Copyright 1999 Gerox(c)%s\n",__DATE__);
		mexErrMsgTxt("\n");
	}
	int rhomax,thetamax,X,Y;
	rhomax = (int)mxGetScalar(prhs[1]);
	thetamax = (int)mxGetScalar(prhs[2]);
	//[X,Y]=size(im);
	Y = (int)mxGetN(prhs[0]);
	X = (int)mxGetM(prhs[0]);
	if (X != Y) {
		mexPrintf("Input image is not square. Exiting!\n");
		return;
	} else {
		if ((X % 2) == 1) {
			mexPrintf("Input image size has to be even in pixels. Exiting!\n");
			return;
		}
	}
	double *im;
	im = mxGetPr(prhs[0]);
	double pi = 3.14159265358979;
	double d_theta=pi / (double)thetamax;
//	theta=0:d_theta:pi-d_theta;
	double *smat = (double*)mxCalloc(thetamax,sizeof(double));
	double *cmat = (double*)mxCalloc(thetamax,sizeof(double));
	double dd_rho = (double)rhomax / (double)X;
	double tmp;
	int i;
	for (i = 0,tmp=0.0;i < thetamax;tmp += d_theta,i++) {
		smat[i] = sin(tmp) * dd_rho;
		cmat[i] = cos(tmp) * dd_rho;
	}
//	[x,y]=find(im);
	int rhomax_2=rhomax >> 1;
	int Y_2 = Y >> 1;
	int X_2 = X >> 1;
	plhs[0] = mxCreateDoubleMatrix(rhomax,thetamax,mxREAL);
	double *yy = mxGetPr(plhs[0]);
	for (register int ii = 0;ii < Y;ii++) {
		for(register int jj = 0;jj < X;jj++) {
			if(im[ii* X + jj] > 0) {
				for(register int k = 0;k < thetamax;k++) {
					register int rho = (int)(((ii-Y_2) * smat[k] + (jj-X_2) * cmat[k])+ rhomax_2 + 0.5);
//					if((rho > -1) && (rho < X)) yy[(int)(rho + k * rhomax)] += 1.0;
						if(rho < 0) continue;
						else {
							if(rho > X) continue;
							else yy[(int)(rho + k * rhomax)] += 1.0;
						}
				}
			}
		}
	}
	mxFree(smat);
	mxFree(cmat);
}