/*
 This file is part of the Astrometry.net suite.
 Copyright 2006, 2007 Michael Blanton, Keir Mierle, David W. Hogg,
 Sam Roweis and Dustin Lang.
 Copyright 2009 Dustin Lang.

 The Astrometry.net suite is free software; you can redistribute
 it and/or modify it under the terms of the GNU General Public License
 as published by the Free Software Foundation, version 2.

 The Astrometry.net suite is distributed in the hope that it will be
 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with the Astrometry.net suite ; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
 */

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

#define GLUE2(a,b) a ## b
#define GLUE(a,b) GLUE2(a, b)

int GLUE(dallpeaks, SUFFIX)(IMGTYPE *image,
							int nx,
    			            int ny,
							int *object,
							float *xcen,
							float *ycen,
							int *npeaks,
							float dpsf,
							float sigma,
							float dlim,
							float saddle,
							int maxper,
							int maxnpeaks,
							float minpeak,
							int maxsize) {

#undef GLUE
#undef GLUE2

	int i, j, k, nobj;
	int xcurr, ycurr, imore ;
	float tmpxc, tmpyc, three[9];

	int *indx = NULL;
	float *oimage = NULL;
	float *simage = NULL;
	int npix = 0;
	int *xc = NULL;
	int *yc = NULL;

	/* Group the connected pixels together.  We do this by computing a
	 permutation index array that would sort the "object" array.
	 (Recall that the "object" array labels the connected components.)
	 All the unlabelled pixels (object == -1) are listed first, then
	 the indices of all the pixels labeled with (object == 0), (object
	 == 1), etc.  The pixel coordinate is (x,y) = (indx[i] % nx,
	 indx[i] / nx).
	 */
    indx = permuted_sort(object, sizeof(int), compare_ints_asc, NULL, nx*ny);

	// skip over the unlabelled pixels (object == -1)
	for (k=0; k<(nx*ny) && object[indx[k]] == -1; k++);

	nobj = 0;
	*npeaks = 0;
	xc = malloc(sizeof(int) * maxper);
	yc = malloc(sizeof(int) * maxper);
	while (k < (nx*ny)) {
		int current;
		int m;
		int di, dj;
		int oi, oj;
		int xmax, ymax, xmin, ymin, onx, ony, nc;

		// the object number we're looking at.
		current = object[indx[k]];

		// find the object limits in pixel space.
		xmax = -1;
		xmin = nx + 1;
		ymax = -1;
		ymin = ny + 1;
		for (m=k; m<(nx*ny) && object[indx[m]] == current; m++) {
			xcurr = indx[m] % nx;
			ycurr = indx[m] / nx;
			xmin = MIN(xmin, xcurr);
			xmax = MAX(xmax, xcurr);
			ymin = MIN(ymin, ycurr);
			ymax = MAX(ymax, ycurr);
		}
		// we've also computed where this object ends...
		// "k" is not used in the rest of this loop, so set it to its next value now.
		k = m;

		// skip if it is smaller than 3x3 or bigger than maxsize.
		onx = xmax - xmin + 1;
		ony = ymax - ymin + 1;
		if (onx < 3 || ony < 3) {
			logverb("Skipping object %i: too small, %ix%i (x %i:%i, y %i:%i)\n",
					current, onx, ony, xmin,xmax, ymin,ymax);
			continue;
		}
		if (ony > maxsize || onx > maxsize) {
			logverb("Skipping object %i: too big, %ix%i (x %i:%i, y %i:%i)\n",
					current, onx, ony, xmin,xmax, ymin,ymax);
			continue;
		}
		if (*npeaks > maxnpeaks) {
			logverb("Skipping all further objects: already found the maximum number (%i)\n", maxnpeaks);
			break;
		}

		// enlarge cutout arrays, if necessary.
		if (onx*ony > npix) {
			free(oimage);
			free(simage);
			npix = onx * ony;
			oimage = malloc(npix * sizeof(float));
			simage = malloc(npix * sizeof(float));
		}

		// make object cutout
		for (oj=0; oj<ony; oj++)
			for (oi=0; oi<onx; oi++) {
				oimage[oi + oj*onx] = 0.;
				i = oi + xmin;
				j = oj + ymin;
				// copy only pixels that are part of the current object
				if (object[i + j*nx] == current)
					oimage[oi + oj*onx] = image[i + j*nx];
			}

		// find peaks in cutout
		dsmooth2(oimage, onx, ony, dpsf, simage);
		dpeaks(simage, onx, ony, &nc, xc, yc,
			   sigma, dlim, saddle, maxper, 0, 1, minpeak);
		imore = 0;
		for (i=0; i<nc; i++) {
			if (xc[i] <= 0 || xc[i] >= onx-1 ||
				yc[i] <= 0 || yc[i] >= ony-1) {
				logverb("Skipping subpeak %i: position %i,%i out of bounds 1:%i, 1:%i\n",
						i, xc[i], yc[i], onx-1, ony-1);
				continue;
			}
			if (imore + *npeaks >= maxnpeaks) {
				logverb("Skipping all further subpeaks: exceeded max number (%i)\n", maxnpeaks);
				break;
			}

			/* install default centroid to begin */
			xcen[imore + (*npeaks)] = xc[i] + xmin;
			ycen[imore + (*npeaks)] = yc[i] + ymin;
			assert(isfinite(xcen[imore + *npeaks]));
			assert(isfinite(ycen[imore + *npeaks]));

			// cut out 3x3 box
			for (di=-1; di<=1; di++)
				for (dj=-1; dj<=1; dj++)
					three[(di+1) + (dj+1)*3] = simage[xc[i]+di + (yc[i]+dj)*onx];
			// try to find centroid in the 3x3 cutout
			if (dcen3x3(three, &tmpxc, &tmpyc)) {
				assert(isfinite(tmpxc));
				assert(isfinite(tmpyc));
				xcen[imore + (*npeaks)] = (tmpxc-1.0) + xc[i] + xmin;
				ycen[imore + (*npeaks)] = (tmpyc-1.0) + yc[i] + ymin;
				assert(isfinite(xcen[imore + *npeaks]));
				assert(isfinite(ycen[imore + *npeaks]));

			} else if (xc[i] > 1 && xc[i] < onx - 2 &&
					   yc[i] > 1 && yc[i] < ony - 2 &&
					   imore < (maxnpeaks - (*npeaks))) {
				debug("Peak %i subpeak %i at (%i,%i): searching for centroid in 3x3 box failed; trying 5x5 box...\n", current, i, xmin+xc[i], ymin+yc[i]);
				debug("3x3 box:\n  %g,%g,%g,%g,%g,%g,%g,%g,%g\n", three[0],three[1],three[2],three[3],three[4],three[5],three[6],three[7],three[8]);
				/* try to get centroid in the 5 x 5 box */
				for (di=-1; di<=1; di++)
					for (dj=-1; dj<=1; dj++)
						three[(di+1) + (dj+1)*3] = simage[xc[i]+(2*di) + (yc[i] + (2*dj)) * onx];
				if (dcen3x3(three, &tmpxc, &tmpyc)) {
					xcen[imore + (*npeaks)] = 2.0*(tmpxc-1.0) + xc[i] + xmin;
					ycen[imore + (*npeaks)] = 2.0*(tmpyc-1.0) + yc[i] + ymin;
					assert(isfinite(xcen[imore + *npeaks]));
					assert(isfinite(ycen[imore + *npeaks]));
				} else {
					// don't add this peak.
					logverb("Failed to find (5x5) centroid of peak %i, subpeak %i at (%i,%i)\n", current, i, xmin+xc[i], ymin+yc[i]);
					debug("5x5 box:\n  %g,%g,%g,%g,%g,%g,%g,%g,%g\n", three[0],three[1],three[2],three[3],three[4],three[5],three[6],three[7],three[8]);

					max_gaussian(oimage, onx, ony, dpsf, xc[i], yc[i], &tmpxc, &tmpyc);
					debug("max_gaussian: %g,%g\n", tmpxc, tmpyc);
					xcen[imore + (*npeaks)] = tmpxc + xmin;
					ycen[imore + (*npeaks)] = tmpyc + ymin;
					//continue;
				}
			} else {
				logverb("Failed to find (3x3) centroid of peak %i, subpeak %i at (%i,%i), and too close to edge for 5x5\n",
						current, i, xmin+xc[i], ymin+yc[i]);
			}
			imore++;
		}
		(*npeaks) += imore;
		nobj++;
	}
	
	FREEVEC(indx);
	FREEVEC(oimage);
	FREEVEC(simage);
	FREEVEC(xc);
	FREEVEC(yc);

	return 1;

} /* end dallpeaks */
