/*
  This file is part of the Astrometry.net suite.
  Copyright 2006-2008 Michael Blanton, Keir Mierle, David W. Hogg, Sam Roweis
  and 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
*/

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

#include "keywords.h"
#include "dimage.h"
#include "permutedsort.h"
#include "simplexy-common.h"

/*
 * dpeaks.c
 *
 * Find peaks in an image, for the purposes of deblending children.
 *
 * Mike Blanton
 * 1/2006 */

int dpeaks(float *image,
           int nx,
           int ny,
           int *npeaks,
           int *xcen,
           int *ycen,
           float sigma,    /* sky sigma */
           float dlim,     /* limiting distance */
           float saddle,   /* number of sigma for allowed saddle */
           int maxnpeaks,
           int smoothimage,
           int checkpeaks,
           float minpeak)
{
	int i, j, ip, jp, ist, jst, ind, jnd, highest, tmpnpeaks;
	float dx, dy, level;
        
        float *smooth = NULL;
        int *peaks = NULL;
        int *indx = NULL;
        int *object = NULL;
        int *keep = NULL;
        int *mask = NULL;
        int *fullxcen = NULL;
        int *fullycen = NULL;

	/* 1. smooth image */
	smooth = (float *) malloc(sizeof(float) * nx * ny);
	if (smoothimage) {
		dsmooth2(image, nx, ny, 1, smooth);
	} else {
		for (j = 0;j < ny;j++)
			for (i = 0;i < nx;i++)
				smooth[i + j*nx] = image[i + j*nx];
	}

	/* 2. find peaks (highest in the 3x3 neighbourhood) */
	peaks = (int *) malloc(sizeof(int) * nx * ny);
	*npeaks = 0;
	for (j = 1; j < ny - 1; j++) {
		jst = j - 1;
		jnd = j + 1;
		for (i = 1; i < nx - 1; i++) {
			if (smooth[i + j*nx] < minpeak)
				continue;
			ist = i - 1;
			ind = i + 1;
			highest = 1;
			for (ip = ist; ip <= ind; ip++)
				for (jp = jst; jp <= jnd; jp++)
					if (smooth[ip + jp*nx] > smooth[i + j*nx])
						highest = 0;
			if (highest) {
				peaks[*npeaks] = i + j * nx;
				(*npeaks)++;
			}
		}
	}

	// DEBUG
	for (i=0; i<(*npeaks); i++) {
		Unused float pk = smooth[peaks[i]];
		assert((peaks[i] % nx) >= 1);
		assert((peaks[i] % nx) <= (nx-2));
		assert((peaks[i] / nx) >= 1);
		assert((peaks[i] / nx) <= (ny-2));
		assert(pk >= minpeak);
		assert(pk >= smooth[peaks[i]-1]);
		assert(pk >= smooth[peaks[i]+1]);
		assert(pk >= smooth[peaks[i]-nx]);
		assert(pk >= smooth[peaks[i]+nx]);
		assert(pk >= smooth[peaks[i]+nx+1]);
		assert(pk >= smooth[peaks[i]+nx-1]);
		assert(pk >= smooth[peaks[i]-nx+1]);
		assert(pk >= smooth[peaks[i]-nx-1]);
	}

	/* 2. sort peaks */
	indx = realloc(peaks, sizeof(int) * (*npeaks));
	peaks = NULL;
    permuted_sort(smooth, sizeof(float), compare_floats_desc, indx, *npeaks);

	// DEBUG
	for (i=0; i<(*npeaks); i++) {
		Unused float pk = smooth[indx[i]];
		assert((indx[i] % nx) >= 1);
		assert((indx[i] % nx) <= (nx-2));
		assert((indx[i] / nx) >= 1);
		assert((indx[i] / nx) <= (ny-2));
		assert(pk >= minpeak);
		assert(pk >= smooth[indx[i]-1]);
		assert(pk >= smooth[indx[i]+1]);
		assert(pk >= smooth[indx[i]-nx]);
		assert(pk >= smooth[indx[i]+nx]);
		assert(pk >= smooth[indx[i]+nx+1]);
		assert(pk >= smooth[indx[i]+nx-1]);
		assert(pk >= smooth[indx[i]-nx+1]);
		assert(pk >= smooth[indx[i]-nx-1]);
	}
	for (i=1; i<(*npeaks); i++) {
		assert(smooth[indx[i-1]] >= smooth[indx[i]]);
	}

	if ((*npeaks) > maxnpeaks)
		*npeaks = maxnpeaks;

	fullxcen = (int *) malloc((*npeaks) * sizeof(int));
	fullycen = (int *) malloc((*npeaks) * sizeof(int));
	for (i = 0;i < (*npeaks);i++) {
		fullxcen[i] = indx[i] % nx;
		fullycen[i] = indx[i] / nx;
	}
	FREEVEC(indx);

	// DEBUG
	for (i = 0;i < (*npeaks);i++) {
		assert(fullxcen[i] >= 1);
		assert(fullxcen[i] <= nx-2);
		assert(fullycen[i] >= 1);
		assert(fullycen[i] <= ny-2);
	}


	/* 3. trim close peaks and joined peaks */
	mask = (int *) malloc(sizeof(int) * nx * ny);
	object = (int *) malloc(sizeof(int) * nx * ny);
	keep = (int *) malloc(sizeof(int) * (*npeaks));
	for (i = (*npeaks) - 1;i >= 0;i--) {
		keep[i] = 1;

		if (checkpeaks) {
			/* look for peaks joined by a high saddle to brighter peaks */
			level = (smooth[ fullxcen[i] + fullycen[i] * nx] - saddle * sigma);
			if (level < sigma)
				level = sigma;
			if (level > 0.99*smooth[ fullxcen[i] + fullycen[i] * nx]) 
				level= 0.99*smooth[ fullxcen[i] + fullycen[i] * nx]; 
			for (jp = 0;jp < ny;jp++)
				for (ip = 0;ip < nx;ip++)
					mask[ip + jp*nx] = smooth[ip + jp * nx] > level;
			dfind2(mask, nx, ny, object, NULL);
			for (j = i - 1;j >= 0;j--)
				if (object[ fullxcen[j] + fullycen[j]*nx] ==
				        object[ fullxcen[i] + fullycen[i]*nx] ||
				        object[ fullxcen[i] + fullycen[i]*nx] == -1 ) {
					keep[i] = 0;
				}
		}

		/* look for close peaks */
		for (j = i - 1;j >= 0;j--) {
			dx = (float)(fullxcen[j] - fullxcen[i]);
			dy = (float)(fullycen[j] - fullycen[i]);
			if (dx*dx + dy*dy < dlim*dlim)
				keep[i] = 0;
		}
	}

	// Grab just the keepers.
	tmpnpeaks = 0;
	for (i=0; i < (*npeaks); i++) {
		if (!keep[i])
			continue;
		xcen[tmpnpeaks] = fullxcen[i];
		ycen[tmpnpeaks] = fullycen[i];
		tmpnpeaks++;
		if (tmpnpeaks >= maxnpeaks)
			break;
	}
	(*npeaks) = tmpnpeaks;

	FREEVEC(smooth);
	FREEVEC(keep);
	FREEVEC(object);
	FREEVEC(mask);
	FREEVEC(fullxcen);
	FREEVEC(fullycen);

	return (1);
} /* end dpeaks */
