typedef unsigned int stack_t;
#define sign(x) (x>0?1:-1)
void hough_circ_grad(pixel_t *orig, pixel_t *edge, BITMAPINFOHEADER *ih)
{
	int T;
	unsigned int nx = ih->width, ny = ih->height,
	    X, Y, R,	// coordinates in Hough space
	    i, j, k, max_votes = 0;
	unsigned int	mx = nx / 4,	// Hough space discretization
	    		my = ny / 4,
			mr = 40,
			rmax = 20;	// maximum radius of circles to be found, px.

	int Gx, Gy;	// Gradients
	float slope;
	float	hx = mx / (float)nx,
	    	hy = my / (float)ny,
	    	hr = mr / (float)rmax,
		dx, dy, r;
	unsigned int Ntotal = mx * my * mr, Nxy = mx * my;
	unsigned short *out = calloc(Ntotal, sizeof(unsigned short));
	printf("circular Hough space size %ux%ux%u = %u\n", mx, my, mr, Ntotal);
	if(!out){
		perror("circular_hough_transform()");
		exit(1);
	}
	puts("Hough: init.");
	for(j=1; j<ny-1; j++)
		for(i=1; i<nx-1; i++) {
			if(edge[i+nx*j] < 100) continue;
			Gx = orig[i+1 + nx*j] - orig[i-1+nx*j];
			Gy = orig[i + nx*(j+1)] - orig[i+nx*(j-1)];
			slope = tan( atan2(Gy, Gx) - M_PI/2. );
			for(R=0; R<mr; R++) {
				r = R/(float)hr;
				if(fabs(slope) < 1e-12)
					dx = 0;
				else
					dx =  -sign(Gx) * r / sqrt(1 + 1/slope/slope);
				dy = -sign(Gy) * r / sqrt(1 + slope*slope);
				X = (int)(i + dx) * hx;
				Y = (int)(j + dy) * hy;
				k = X + mx * Y + Nxy * R;
				if(out[k] < USHRT_MAX && k<Ntotal)
				{
					out[k]++;
					if(out[k]>max_votes) max_votes = out[k];
				}
			}
		}
	puts("Hough: accumulation done.");

	T = max_votes*0.5;	// search threshold
	printf("T=%d\n", T);

	unsigned int counter = 0;

	for(i=0; i<Ntotal; i++) if(out[i] < T) counter++;
	printf("zeros=%d of %d: %g\%\n", counter, Ntotal, counter/(float)Ntotal*100);

	int u,v,w, test,
	    sr = 1;	// recursive search radius

	counter = 0;

// #if 1 -- recursive maxima search; should reduce multiple matches for simgle circle.
// #if 0 -- thresholding.
#if 1
	unsigned int smaxsize = 1000,  ssize, k2, maxv, maxk, len;
	stack_t *stack = malloc(sizeof(stack_t)*smaxsize);
	stack_t se;

	for(Y=1; Y<my-1; Y++)
		for(X=1; X<mx-1; X++)
			for(R=3; R<mr-1; R++) {
				k = X + mx * Y + Nxy * R;
				if( out[k] < T ) continue;

				len = 0;
				maxv  = 0;
				ssize = 1;
				stack[0] = k;

				do {
					len++;
					ssize--;
					se = stack[ssize];
					if( out[se] > maxv ) {
						maxv = out[se];
						maxk = se;
					}
					out[se] = 0;	// we were here

					// scan all neighbours
					for(u=-sr;u<=sr;u++)
					for(v=-sr;v<=sr;v++)
					for(w=-sr;w<=sr;w++)
					{
						if(u==0 && v==0 && w==0) continue;
						k2 = se + u+v*mx+w*Nxy;
						if( k2 < Ntotal && out[k2] >= T ) {
							stack[ssize] = k2;
							ssize++;
							if(ssize >= smaxsize){
							  puts("NOTE: Incrementing stack size");
							  smaxsize += 1000;
							  stack = realloc(stack, sizeof(stack_t)*smaxsize);
							}
						}
					}
				} while(ssize > 0);

				//if( len < 3 ) continue;
				R = maxk / (float)Nxy;
				Y = (maxk % Nxy) / (float) mx;
				X = (maxk % Nxy) % mx;// maxk - mx*Y - Nxy*R;
				i = (int)(X/(float)hx);
				j = (int)(Y/(float)hy);
				r = (int)(R/(float)hr);
				printf("%d:\tx=%d\ty=%d\tr=%d (R=%d)\tvotes=%u, k=%d, m=%d\n",
						counter++, X, Y, (int)r, R, maxv, maxk, len);
				cross(edge, ih, i, j, r);

			}
	printf("max stack size = %d\n", smaxsize);
	free(stack);

#else
	for(Y=1; Y<my-1; Y++)
		for(X=1; X<mx-1; X++)
			for(R=5; R<mr-1; R++) {
				k = X + mx * Y + Nxy * R;
				if( out[k] < T /*T*(R+1)*/) continue;
				test = 0;
				
				for(u=-1;u<2;u++)
				for(v=-1;v<2;v++)
				for(w=-1;w<2;w++)
					if( !(u==0 && v==0 && w==0) )
						test += (out[k + u+v*mx+w*Nxy] > out[k]);
						
				if( test == 0 );
				{
					i = (int)(X/(float)hx);
					j = (int)(Y/(float)hy);
					r = (int)(R/(float)hr);
					printf("%d:\tx=%d\ty=%d\tr=%d (R=%d)\tvotes=%u\n",
							counter++, i, j, (int)r, R, out[X + mx * Y + Nxy * R]);
					cross(edge, ih, i, j, r);
				}
			}
#endif
	puts("Hough: search done.");
	printf("Hough: max number of votes %d\n", max_votes);
	free(out);
}

hough_circ_grad(bitmap_data, temp_image, &ih);