/*
# This file is part of the Astrometry.net suite.
# Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>

#include "os-features.h"
#include "dimage.h"
#include "permutedsort.h"
#include "simplexy-common.h"
#include "log.h"
#include "mathutil.h"

/*
 * dallpeaks.c
 *
 * Take image and list of objects, and produce list of all peaks (and
 * which object they are in).
 *
 * BUGS:
 *   - Returns no error analysis if the centroid sux.
 *   - Uses dead-reckon pixel center if dcen3x3 sux.
 *   - No out-of-memory checks
 *
 * Mike Blanton
 * 1/2006 */

/* Finds all peaks in the image by cutting a bounding box out around
 each one */

static int max_gaussian(float* image, int W, int H, float sigma,
						int x0, int y0, float *p_x, float *p_y) {
	float x, y;
	int step;
	float stepsize = 0.1;
	// inverse-variance
	float iv = 1.0 / (sigma*sigma);
	// half inverse-variance
	float hiv = 1.0 / (2.0 * sigma*sigma);
	// clip at six sigma:
	int Nsigma = 6;
	// Nsigma=6: G=1.523e-08

	x = x0;
	y = y0;

	for (;;) {
		float xdir=0, ydir=0;
		anbool xflipped = FALSE, yflipped = FALSE;

		for (step=0; step<100; step++) {
			float dx, dy;
			float Gx, Gy;
			float V;
			int i,j;
			int ilo,ihi,jlo,jhi;

			debug("Stepsize %g, step %i\n", stepsize, step);

			V = Gx = Gy = 0;
			// compute gradients.
			ilo = MAX(0, floor(x-Nsigma*sigma));
			ihi = MIN(W-1, ceil(x+Nsigma*sigma));
			jlo = MAX(0, floor(y-Nsigma*sigma));
			jhi = MIN(H-1, ceil(y+Nsigma*sigma));
			//for (j=0; j<H; j++) {
			//for (i=0; i<W; i++) {
			for (j=jlo; j<=jhi; j++) {
				for (i=ilo; i<=ihi; i++) {
					float G;
					dx = i - x;
					dy = j - y;
					// ~ Gaussian
					G = exp(-(dx*dx + dy*dy) * hiv);
					// other common factors
					G *= (image[j*W + i] * iv);
					// Gradients
					Gx += (G * -dx);
					Gy += (G * -dy);
					V += G;
				}
			}
			debug("x,y = (%g,%g), V=%g, Gx=%g, Gy=%g\n", x, y, V, Gx, Gy);

			dx = SIGN(-Gx);
			dy = SIGN(-Gy);

			if (step == 0) {
				if (xdir == 0 && ydir == 0)
					break;
				xdir = dx;
				ydir = dy;
			}
			if (!xflipped && dx == xdir)
				x += dx * stepsize;
			else
				xflipped = TRUE;
			if (!yflipped && dy == ydir)
				y += dy * stepsize;
			else
				yflipped = TRUE;
			if (xflipped && yflipped)
				break;
		}
		if (stepsize <= 0.002)
			break;
		stepsize /= 10.0;
	}
	if (p_x)
		*p_x = x;
	if (p_y)
		*p_y = y;
	return 0;
}


#define IMGTYPE float
#define SUFFIX
#include "dallpeaks.inc"
#undef SUFFIX
#undef IMGTYPE

#define IMGTYPE uint8_t
#define SUFFIX _u8
#include "dallpeaks.inc"
#undef IMGTYPE
#undef SUFFIX

#define IMGTYPE int16_t
#define SUFFIX _i16
#include "dallpeaks.inc"
#undef IMGTYPE
#undef SUFFIX