#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=houghtrans(im,RHO_MAX,THETA_MAX)\n");
		mexPrintf("Translated from m file Copyright 1999 Gerox(c)%s\n",__DATE__);
		mexErrMsgTxt("\n");
	}
	double rhomax,thetamax;
	rhomax = mxGetScalar(prhs[1]);
	thetamax = mxGetScalar(prhs[2]);
	//[X,Y]=size(im);
	int Y = (int)mxGetN(prhs[0]);
	int 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_rho=double(X)/ rhomax;
	double d_theta=pi / thetamax;
//	theta=0:d_theta:pi-d_theta;
	double tmp;
	int i,j;
//smat=sin(theta);
//cmat=cos(theta);
	double *smat = (double*)mxCalloc(thetamax,sizeof(double));
	double *cmat = (double*)mxCalloc(thetamax,sizeof(double));
	double dd_rho = rhomax / double(X);
	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 f_length = 0;
	for (i = 0;i < X * Y;i++) if(im[i] > 0) f_length++;
	int *x = (int*)mxCalloc(f_length,sizeof(int));
	int *y = (int*)mxCalloc(f_length,sizeof(int));
	int count = 0;
	for (i = 0;i < Y;i++) {
		for(j = 0;j < X;j++) {
			if(im[i * X + j] > 0) {
				x[count] = j - X/2;
				y[count] = i - Y/2;
				count++;
			}
		}
	}
	int **h2;
	h2 = (int**)mxCalloc(f_length,sizeof(int*));
	for(i = 0;i < f_length;i++) h2[i] = (int*)mxCalloc(thetamax,sizeof(int));
	int rhomax_2=int(rhomax / 2.0);
	for(i = 0;i < f_length;i++) {
		for(j = 0;j < thetamax;j++) {
			h2[i][j] = int((y[i] * smat[j] + x[i] * cmat[j]) + rhomax_2 + 0.5);
		}
	}
	plhs[0] = mxCreateDoubleMatrix((int)rhomax,(int)thetamax,mxREAL);
	double *yy = mxGetPr(plhs[0]);
	for (register int jjj = 0;jjj < rhomax;jjj++) {
		for (register int iii = 0;iii < thetamax;iii++) {
			for (register int k = 0; k < f_length;k++) {
				if(h2[k][iii] == jjj) yy[(int)(jjj+iii * rhomax)] += 1.0;
			}
		}
	}
	mxFree(smat);
	mxFree(cmat);
	mxFree(x);
	mxFree(y);
	for(i = 0;i < f_length;i++) mxFree(h2[i]);
	mxFree(h2);
}