/**************************************************************

find.c (C-Munipack project)
Copyright (C) 2003 David Motl, dmotl@volny.cz

This program 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; either version 2
of the License, or (at your option) any later version.

This program 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 this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

**************************************************************/

#include <stdlib.h>

#include "comfun.h"
#include "cmpack_common.h"
#include "phot.h"
#include "phfun.h"
#include "center.h"

static CmpackPhotList *plist_prepend(CmpackPhotList *list, CmpackPhotStar *item)
{
	CmpackPhotList *it = cmpack_malloc(sizeof(CmpackPhotList));
	it->next = list;
	it->data = item;
	return it;
}

static int plist_count(CmpackPhotList *list)
{
	int count = 0;
	CmpackPhotList *ptr = list;
	while (ptr) {
		count++;
		ptr = ptr->next;
	}
	return count;
}

static void plist_free(CmpackPhotList *list)
{
	CmpackPhotList *ptr = list;
	while (ptr) {
		CmpackPhotList *next = ptr->next;
		cmpack_free(ptr);
		ptr = next;
	}
}

/* Find stars */
int CenterPos(CmpackPhot *kc, double *x, double *y)
{
	int left, top, ncol, nrow, nhalf, rowwidth, nbox, i, j, ix, iy, jx, jy, n;
	int firstro, firstcl, lastro, lastcl, success;
	CmpackPhotFrame *frame = &kc->frame;
	const double *d;				/* image */
	const double *g = frame->g;		/* subarray with Gaussian curve */
	const double *h = frame->h;		/* central heights of the best fitting Gaussian function for each pixel */
	const char *skip = frame->skip;		/* which pixels will be skipped */
	double wt, p, sumg, sumgsq, sd, sg, datum, dat, hmax;
	double sumgd, sumd, sdgdx, sdgdxs, sddgdx, sgdgdx, dgdx, hx, skylvl, dx, xcen, hy, dy, ycen;

	ClearStarList(frame);

	d = (const double*)cmpack_image_const_data(frame->image);
	rowwidth = cmpack_image_width(frame->image);

	nhalf = frame->nhalf;
	nbox = 2 * nhalf + 1;			/* length of the side of the subarray */

	left = frame->left;
	top = frame->top;
	ncol = frame->ncol;
	nrow = frame->nrow;

	ix = (int)(*x) - nhalf;
	iy = (int)(*y) - nhalf;

	firstcl = ix - frame->left;
	if (firstcl < frame->left + nhalf)
		firstcl = frame->left + nhalf;

	lastcl = ix + nbox - frame->left;
	if (lastcl >= frame->left + frame->ncol - nhalf)
		lastcl = frame->left + frame->ncol - nhalf - 1;

	firstro = iy - frame->top;
	if (firstro < frame->top + nhalf)
		firstro = frame->top + nhalf;

	lastro = iy + nbox - frame->top;
	if (lastro >= frame->top + frame->nrow - nhalf)
		lastro = frame->top + frame->nrow - nhalf - 1;

	hmax = -1;
	jx = jy = success = 0;
	for (j = firstro; j <= lastro; j++) {
		for (i = firstcl; i <= lastcl; i++) {
			if (!SKIP(i - ix, j - iy)) {
				double height = H(i, j);
				if (height > hmax) {
					hmax = height;
					jx = i;
					jy = j;
					success = 1;
				}
			}
		}
	}

	if (!success) 
		return CMPACK_ERR_STAR_NOT_FOUND;

	/* Derive the shape indices */
	datum = D(left + jx, top + jy);

	sumgd = 0.0;                  /* sum of the Gaussian times the data */
	sumgsq = 0.0;                 /* sum of the Gaussian squared */
	sumg = 0.0;                   /* sum of the Gaussian */
	sumd = 0.0;                   /* sum of the data */
	sdgdx = 0.0;                  /* sum of d(Gaussian)/dx */
	sdgdxs = 0.0;                 /* sum of {d(Gaussian)/dx}^2 */
	sddgdx = 0.0;                 /* sum of data times d(Gaussian)/dx */
	sgdgdx = 0.0;                 /* sum of Gaussian times d(Gaussian)/dx */
	p = 0.0;
	n = 0;
	for (i = 0; i < nbox; i++) {
		sg = 0.0;                  /* temporary sum of the Gaussian */
		sd = 0.0;                  /* temporary sum of the data */
		for (j = 0; j < nbox; j++) {
			wt = nhalf - fabs(j - nhalf) + 1;
			dat = D(left + jx - nhalf + i, top + jy - nhalf + j);
			if (frame->datalo < dat && dat < frame->datahi) {
				sd += (dat - frame->skymod)*wt;
				sg += G(i, j)*wt;
			}
		}
		if (sg > 0.0) {
			wt = nhalf - fabs(i - nhalf) + 1;
			sumgd += wt * sg*sd;
			sumgsq += wt * sg*sg;
			sumg += wt * sg;
			sumd += wt * sd;
			dgdx = sg * (nhalf - i);
			sdgdxs += wt * dgdx*dgdx;
			sdgdx += wt * dgdx;
			sddgdx += wt * sd*dgdx;
			sgdgdx += wt * sg*dgdx;
			p += wt;
			n++;
		}
	}

	if (n <= 2) 
		return CMPACK_ERR_STAR_NOT_FOUND;   /* we need at least three points */
	hx = (sumgd - sumg * sumd / p) / (sumgsq - (sumg*sumg) / p);
	if (hx <= 0.0) 
		return CMPACK_ERR_STAR_NOT_FOUND;
	skylvl = (sumd - hx * sumg) / p;
	dx = (sgdgdx - (sddgdx - sdgdx * (hx*sumg + skylvl * p))) / (hx*sdgdxs / frame->sigsq);
	/* compute the first-order correction to the x-centroid of the star */
	xcen = jx + dx / (1.0 + fabs(dx));

	if (xcen < 0.5 || xcen > ncol - 0.5) {
		/* if the best estimate of the star's center falls outside the lc->image, reject it */
		return CMPACK_ERR_STAR_NOT_FOUND;
	}

	/* Compute the height of the y-marginal Gaussian distribution */
	sumgd = 0.0;
	sumgsq = 0.0;
	sumg = 0.0;
	sumd = 0.0;
	sdgdx = 0.0;
	sdgdxs = 0.0;
	sddgdx = 0.0;
	sgdgdx = 0.0;
	p = 0.0;
	n = 0;
	for (j = 0; j < nbox; j++) {
		sg = sd = 0.0;
		for (i = 0; i < nbox; i++) {
			wt = nhalf - fabs(i - nhalf) + 1;
			dat = D(left + jx - nhalf + i, top + jy - nhalf + j);
			if (frame->datalo < dat && dat < frame->datahi) {
				sd += (dat - frame->skymod)*wt;
				sg += G(i, j)*wt;
			}
		}
		if (sg > 0.0) {
			wt = nhalf - fabs(j - nhalf) + 1;
			sumgd += wt * sg*sd;
			sumgsq += wt * sg*sg;
			sumg += wt * sg;
			sumd += wt * sd;
			dgdx = sg * (nhalf - j);
			sdgdx += wt * dgdx;
			sdgdxs += wt * dgdx*dgdx;
			sddgdx += wt * sd*dgdx;
			sgdgdx += wt * sg*dgdx;
			p += wt;
			n++;
		}
	}

	if (n <= 2) 
		return CMPACK_ERR_STAR_NOT_FOUND;   /* we need at least three points */
	hy = (sumgd - sumg * sumd / p) / (sumgsq - (sumg*sumg) / p);
	if (hy <= 0.0) 
		return CMPACK_ERR_STAR_NOT_FOUND;
	skylvl = (sumd - hy * sumg) / p;
	dy = (sgdgdx - (sddgdx - sdgdx * (hy*sumg + skylvl * p))) / (hy*sdgdxs / frame->sigsq);
	ycen = jy + dy / (1.0 + fabs(dy));

	if (ycen < 0.5 || ycen > nrow - 0.5)
		return CMPACK_ERR_STAR_NOT_FOUND;

	/* Save its parameters */
	*x = left + xcen + 0.5;
	*y = top + ycen + 0.5;

	return CMPACK_ERR_OK;
}

/* Find stars */
int FindMax(CmpackPhot *kc)
{
	int left, top, ncol, nrow, nhalf, rowwidth, nbox, obj, pos, i, j, ix, iy, jx, jy, count;
	int firstro, firstcl, lastro, lastcl;
	CmpackPhotFrame *frame = &kc->frame;
	const double *d;				/* image */
	const double *h = frame->h;		/* central heights of the best fitting Gaussian function for each pixel */
	const char *skip = frame->skip;		/* which pixels will be skipped */
	double hmax;
	CmpackPhotStar *rec;
	CmpackPhotList *list = NULL, *ptr;

	ClearStarList(frame);

	d = (const double*)cmpack_image_const_data(frame->image);
	rowwidth = cmpack_image_width(frame->image);

	nhalf = frame->nhalf;
	nbox = 2 * nhalf + 1;			/* length of the side of the subarray */
	left = frame->left;
	ncol = frame->ncol;
	top = frame->top;
	nrow = frame->nrow;

	/* SECTION 3 - Search for local maxima in the convolved brightness data */
	for (obj = 0, pos = 0; obj < kc->usr_length; obj++, pos += 2) {

		ix = (int)kc->usr_coords[pos] - nhalf;
		iy = (int)kc->usr_coords[pos + 1] - nhalf;

		firstcl = ix - frame->left;
		if (firstcl < frame->left + nhalf)
			firstcl = frame->left + nhalf;

		lastcl = ix + nbox - frame->left;
		if (lastcl >= frame->left + frame->ncol - nhalf)
			lastcl = frame->left + frame->ncol - nhalf - 1;

		firstro = iy - frame->top;
		if (firstro < frame->top + nhalf)
			firstro = frame->top + nhalf;

		lastro = iy + nbox - frame->top;
		if (lastro >= frame->top + frame->nrow - nhalf)
			lastro = frame->top + frame->nrow - nhalf - 1;

		hmax = -1;
		jx = ix + nhalf;
		jy = iy + nhalf;
		for (j = firstro; j <= lastro; j++) {
			for (i = firstcl; i <= lastcl; i++) {
				if (!SKIP(i - ix, j - iy)) {
					double height = H(i, j);
					if (height > hmax) {
						hmax = height;
						jx = i;
						jy = j;
					}
				}
			}
		}

		rec = cmpack_calloc(1, sizeof(CmpackPhotStar));
		rec->xmax = left + jx;
		rec->ymax = top + jy;
		rec->xcen = kc->usr_coords[pos];
		rec->ycen = kc->usr_coords[pos + 1];
		rec->height = D(left + jx, top + jy);
		list = plist_prepend(list, rec);
	}

	/* Section 4 - frees alocated memory */
	count = plist_count(list);
	frame->list = cmpack_malloc(count * sizeof(CmpackPhotStar*));
	ptr = list;
	i = 0;
	while (ptr) {
		frame->list[i++] = ptr->data;
		ptr = ptr->next;
	}
	frame->length = frame->nstar = count;
	frame->find_processed = 1;

	plist_free(list);

	return CMPACK_ERR_OK;
}
