#include <stdio.h>
#include <math.h>
#include "mex.h"
/*
function res=hough(im,rhomax,thetamax)
% Name:			hough(im,RHO_MAX,THETA_MAX)
% Version:		v1.0
% Author:		Dimitrios Ioannou
%			dioan@robotsg.nuceng.ufl.edu
%			dioan@alder.circa.ufl.edu
% Date:			08/23/95
% Arguments:
%			im: is the input,binary, image. If the
%			image is not binary pixels having
%			non-zero values are considered.
%			RHO_MAX: is an integer number specifying
%			the rho quantization.
%			THETA_MAX: is an integer number
%			specifying the theta quantization
% Purpose:
%			perform the hough transform of a binary
%			image
% Dependencies:
%			None
% Example:		v=hough(im,256,256)
%			input is the image im, and the
%			quantization is d_rho=X/256 and d_theta=pi/256
%			if the size of the image is 256 by 256
%			d_rho=1.
%
%			v is the number of votes in the
%			paremeter space. v(i,j) is the number 
%			of votes for the strip having distance from 
%			the center of the image equal to 
%			(i-RHO_MAX/2)*d_rho (d_rho=X/RHO_MAX, the 
%			image is X by X	pixels),and its normal has 
%			angle j*d_theta,(d_theta=pi/THETA_MAX)
%
%			for a 256 by 256 image, the center of the
%			image is the center of the pixel (128,128)
%			i=1 => rho=(i-1-128)*d_rho=-128*d_rho
%			i=256 => rho=(i-1-128)*d_rho=127*d_rho
%			this essentially means that:
%			'the image is not symmetric around its center'.
%
Arranged by Gerox(c) for mex file 1999
Arranged by Gerox(c) for sparse matrix 1999.08.22
Arranged by Gerox(c) for uint8 matrix 1999.08.25
Optimaized for speed & bug fix by Gerox(c) 2002.10.05
Now can be apply non-square image by Gerox(c) 2004.02.07
------------------
close all;
data=zeros(50,50);
data(10,:)=1;
figure(1);set(1,'doublebuffer','on');
tdat1=[ ];tdat2=[ ];
for theta=1:5:180
	data1=imrotate(data,theta,'nearest','crop');
	tic;hout=houghf(data1,50,50);tdat1=[tdat1 toc];
	subplot(2,2,1);imagesc(data1);axis square
	subplot(2,2,2);imagesc(hout);axis square
	tic;hout=houghf(data1,50,50);tdat2=[tdat2 toc];
	subplot(2,2,3);imagesc(data1);axis square
	subplot(2,2,4);imagesc(hout);axis square
	drawnow;
end
mean(tdat1),mean(tdat2)
-------------------
*/
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
	int i,ii,jj;
	int theta,rho;
	int rhomax,thetamax,X,Y;
	double pi = 3.14159265358979;
	double *im;
	double d_theta;
	int *smat,*cmat;
	double dd_rho,tmp;
	double rhomax_2;
	int X_2,Y_2;
	double *yy;
	int jjY;
	double jjX_2;
	double iiY_2;
	if ((nrhs != 3) || (nlhs != 1)) {
		mexPrintf("function res=hough(im,RHO_MAX,THETA_MAX)\n");
		mexPrintf("Based on m-script file (Author:Dimitrios Ioannou: hough.m v1.0)\n");
		mexPrintf(" which is available from www.mathworks.com under user contributed library\n");
		mexPrintf("Arranged for MEX version by Gerox(c) Copyright 1999 %s\n",__DATE__);
		mexErrMsgTxt("\n");
	}
	rhomax = (int)mxGetScalar(prhs[1]);
	thetamax = (int)mxGetScalar(prhs[2]);
	//[M,N]=size(im);
	X = (int)mxGetN(prhs[0]);
	Y = (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;
		}
		if ((Y % 2) == 1) {
			mexPrintf("Input image size has to be even in pixels. Exiting!\n");
			return;
		}
//	}
	im = mxGetPr(prhs[0]);
//	Since we need theta from 0 to pi
//	check result of 
//	mesh(hough(ones(120,120),120,120));
//	mesh(houghf(ones(120,120),120,120));
	d_theta = pi / (double)thetamax;
//	theta=0:d_theta:pi-d_theta;
	smat = (int*)mxCalloc(thetamax,sizeof(int));
	cmat = (int*)mxCalloc(thetamax,sizeof(int));
	dd_rho = (double)rhomax/ (double)sqrt(X*X+Y*Y);
	for (i = 0,tmp=0.0;i < thetamax;tmp += d_theta,i++) {
		smat[i] = (int)(sin(tmp) * dd_rho * (1 << 16));
		cmat[i] = (int)(cos(tmp) * dd_rho * (1 << 16));
	}
//	[x,y]=find(im);
	rhomax_2 = (double)(rhomax >> 1);
	X_2 = (int)(X >> 1);
	Y_2 = (int)(Y >> 1);
	plhs[0] = mxCreateDoubleMatrix(rhomax,thetamax,mxREAL);
	yy = mxGetPr(plhs[0]);
	if(mxIsDouble(prhs[0])) {
		for(jj = 0;jj < X;jj++) {
			jjY = jj * Y;
			jjX_2 = (double)(jj - X_2);
			for (ii = 0;ii < Y;ii++) {
				if(im[jjY + ii] > 0) {
					iiY_2 = (double)(Y_2-ii-1);
					for(theta = 0;theta < thetamax;theta++) {
						rho = (int)(rhomax_2-1-((int)(jjX_2 * cmat[theta] +iiY_2 * smat[theta])>>16));
						if((rho < 0) || (rho > rhomax-1)) continue;
						yy[theta * rhomax + rho] += 1.0;
					}
				}
			}
		}
	} else {
		int *ir;
		int *jc;
		int cnt;
		int col;
		if(mxIsSparse(prhs[0])) {
//			[x,y]=find(im);
			ir = mxGetIr(prhs[0]);
			jc = mxGetJc(prhs[0]);
			for (col = 0;col < X;col++) { 
				int start = jc[col]; 
				int stop = jc[col+1]; 
				if (start == stop) continue;
				jjX_2 = (double)(col - X_2);
				for (cnt = start; cnt < stop; cnt++)  {
//					mexPrintf("\t(%d,%d) = %g\n", ir[cnt]+1, col+1, data[total++]);
					iiY_2 = (double)(-ir[cnt] + Y_2 - 1);
					for(theta = 0;theta < thetamax;theta++) {
						rho = (int)(rhomax_2 - 1 - ((int)(jjX_2 * cmat[theta] + iiY_2 * smat[theta])>>16));
						if((rho < 0) || (rho > rhomax-1)) continue;
						yy[theta * rhomax + rho] += 1.0;
					}
				}
			}
		} else {
			unsigned char *imi = (unsigned char*)mxGetPr(prhs[0]);
			if(mxIsUint8(prhs[0])) {
				for(jj = 0;jj < X;jj++) {
					jjY = jj * Y;
					jjX_2 = (double)(jj - X_2);
					for (ii = 0;ii < Y;ii++) {
						if(imi[jjY + ii] > 0) {
							iiY_2 = (double)(Y_2 - ii - 1);
							for(theta = 0;theta < thetamax;theta++) {
								rho = (int)(rhomax_2 - 1 - ((int)(jjX_2 * cmat[theta] + iiY_2 * smat[theta])>>16));
								if((rho < 0) || (rho > rhomax-1)) continue;
								yy[theta * rhomax + rho] += 1.0;
							}
						}
					}
				}	
			} else {
				mxFree(smat);
				mxFree(cmat);
				mexErrMsgTxt("Input must be double or sparse or uint8 Matrix");
			}
		}
	}
	mxFree(smat);
	mxFree(cmat);
}