/**************************************************************************
 *                                                                        *
 * Copyright (c) 1996 Michael Richmond and Richard Treffers               *
 *                                                                        *
 *                    This software may be copied and distributed for     *
 *                    educational, research and not for profit services   *
 *                    provided that this copyright and statement are      *
 *                    included in all such copies.                        *
 *                                                                        *
 **************************************************************************/



/*
 * This used to be part of "axes.c", but I've moved it into its
 * own source code file so that it can be linked into the 
 * XVista library.   MWR 8/13/96
 *
 */

#include "pcvista.h"
#include "fits.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>


#undef DEBUG


/*	this is the smallest total number of pixels above sky to accept
	I really don't know what a reasonable value should be 
	previously no testing at all was done
*/

#define SMALL_SUM 10.0	


static int circular_gaussian(int16 **p, int sr, int sc, int nr, int nc, 
                             double cr, double cc, double sky, double fitrad, 
                             double *peak, double *fwhm);



/*
 *  do the actual computations on the data - find centroid, FWHM,
 *  and so forth... some of the algorithms herein are based upon 
 *  those found in the VISTA code of Lauer and Stover.
 *
 *  modified 10/3/92 by MWR to use a gaussian fitted to the marginal
 *  light distributions in each direction for the light-center,
 *  rather than the first moment of the light, AND to fit a 1-d
 *  gaussian to the radial profile of the star as well. 
 *
 *  We use only pixels within "fitrad" of the center of the box
 *  to measure the centroid.  However, we use all pixels in the
 *  box to measure second moments and orientation ....
 *
 *  This routine assumes that sky has NOT been subtracted from the
 *  data yet, and subtracts the given "sky" value as it goes.
 *
 *  The output arguments are:
 *
 *     row_cen           centroid in row direction, from gaussian fitted
 *                            to marginal sums
 *     col_cen           centroid in col direction, from gaussian fitted
 *                            to marginal sums
 *     row_width         FWHM of a gaussian fitted to marginal sums along
 *                            the row direction
 *     col_width         FWHM of a gaussian fitted to marginal sums along
 *                            the col direction
 *     fwhm              FWHM of a circular gaussian fitted to radial profile
 *     peak              peak of the fitted gaussian
 *     eccen             eccentricity of object, based on second moments
 *     major_axis        position angle of major axis, in degrees
 *     minor_axis        position angle of minor axis, in degrees
 * 
 * The function returns 0 if all goes well, or
 *              returns 1 if there's a problem (and prints error messages)
 */


int 
do_axes(p, sr, sc, nr, nc, sky, fitrad, row_cen, col_cen, row_width, col_width,
          fwhm, peak, eccen, major_axis, minor_axis)
int16 **p;
int sr, sc, nr, nc;
double sky, fitrad;
double *row_cen, *col_cen, *row_width, *col_width;
double *fwhm, *peak, *eccen, *major_axis, *minor_axis;
{
	static int i, row, col;
	int srow, scol, erow, ecol;
	static int px, py, bigger;
	static float *x, *y, *sig, chisq, amp, center, xwidth, ywidth;
	static double pix, sum, sumx, sumy, max, xc_ax, yc_ax;
	static double sumxx, sumxy, sumyy, a, b, c, temp;
	static double eigen1, eigen2, xmaj, xminor, ymaj, yminor;
	static double angmaj_x, angmin_x, ecc_ax;
	double circ_fwhm, peak_value;

	/* 
	 * compute the centroid, using only pixels within 'fitrad'
	 * of the center of the box 
	 */
	sum = 0.0;
	sumx = 0.0;
	sumy = 0.0;
	px = 0;
	py = 0;
	max = 0.0;

	if ((srow = (int)(sr + nr/2 + 0.5 - fitrad)) < sr) {
		srow = sr;
	}
	if ((scol = (int)(sc + nc/2 + 0.5 - fitrad)) < sc) {
		scol = sc;
	}
	if ((erow = (int)(sr + nr/2 + 0.5 + fitrad)) >= sr + nr) {
		erow = sr + (nr - 1);
	}
	if ((ecol = (int)(sc + nc/2 + 0.5 + fitrad)) >= sc + nc) {
		ecol = sc + (nc - 1);
	}
	for (row = srow; row <= erow; row++) {
		for (col = scol; col < ecol; col++) {
			pix = p[row][col] - sky;
			sum += pix;
			sumx += pix*row;
			sumy += pix*col;
			if (pix > max) {
				px = row;
				py = col;
				max = pix;
			}
		}
	} 

	/* 
	 * this test will fail if the user has supplied a "sky" value 
	 * much higher than appropriate.  Too bad for him. 
	 */
	if (sum < SMALL_SUM) {
		error(0, "not enough signal to compute axes");
		return(1);
	}

	xc_ax = sumx/sum;		/* centroid in x */
	yc_ax = sumy/sum;		/* centroid in y */

#ifdef DEBUG
	printf("done with first moments, center is (%8.2f, %8.2f)\n",
			xc_ax, yc_ax);
#endif

	/* allocate the vectors we'll need */
	if (nr > nc)
		bigger = nr;
	else
		bigger = nc;	
	if ((x = (float *) malloc(bigger*sizeof(float))) == NULL)
		error(1, "do_axes: can't allocate for x vector");
	if ((y = (float *) malloc(bigger*sizeof(float))) == NULL)
		error(1, "do_axes: can't allocate for y vector");
	if ((sig = (float *) malloc(bigger*sizeof(float))) == NULL)
		error(1, "do_axes: can't allocate for sig vector");
	for (i = 0; i < bigger; i++)
		sig[i] = 1.0;

	/* first do along the row (X) direction */
	for (row = srow; row <= erow; row++) {
		x[row - srow] = row;
		y[row - srow] = 0;
		for (col = scol; col <= ecol; col++) {
			y[row - srow] += p[row][col] - sky;
		}
	}
	if (gauss_fit(x, y, sig, (1 + erow - srow), 
			&amp, &center, &xwidth, &chisq) != 0) {
#ifdef DEBUG
		error(0, "do_axes: gauss_fit has problem in X-direction");
#endif
		xwidth = -1;	/* signal to use 2nd moment later on */
	}
	else {
		*row_width = xwidth;
		xc_ax = center;
	}

#ifdef DEBUG
	printf("done with fitting X center, is %8.2f\n", xc_ax);
#endif
			
	/* now do along the col (Y) direction */
	for (col = scol; col <= ecol; col++) {
		x[col - scol] = col;
		y[col - scol] = 0;
		for (row = srow; row <= erow; row++) {
			y[col - scol] += p[row][col] - sky;
		}
	}
	if (gauss_fit(x, y, sig, (1 + ecol - scol), 
			&amp, &center, &ywidth, &chisq) != 0) {
#ifdef DEBUG
		error(0, "do_axes: gauss_fit has problem in Y-direction");
#endif
		ywidth = -1; 	/* signal to use 2nd moment later on */
	}
	else {
		*col_width = ywidth;
		yc_ax = center;
	}


#ifdef DEBUG
	printf("done with fitting Y center, is %8.2f\n", yc_ax);
#endif

	free(x);
	free(y);
	free(sig);
	
	
	if ( (xc_ax < srow) || (xc_ax > erow) ) {
		error(0, "computed row centroid is out of bounds"); 
		return(1);
	}
	if ( (yc_ax < scol) || (yc_ax > ecol) ) {
		error(0, "computed col centroid is out of bounds"); 
		return(1);
	}

	/* find some value for the FWHM of the object */
	if (circular_gaussian(p, sr, sc, nr, nc, xc_ax, yc_ax, sky, fitrad, 
	                      &peak_value, &circ_fwhm) != 0) {
		error(0, "circular_gaussian returns with error");
	}
#ifdef DEBUG
	printf("done with fitting FWHM: peak %8.2f, fwhm=%6.2f \n",
			peak_value, circ_fwhm);
#endif


	/* now calculate the quadrupole moment - we need it to find the
	   object's principal axes AND the object's "width", if either
	   the X or Y gaussian-fitting failed. */
	sumxx = 0.0;
	sumyy = 0.0;
	sumxy = 0.0;
	for (row = srow; row < erow; row++) {
		for (col = scol; col < ecol; col++) {
			pix = p[row][col] - sky;
			sumxx += (row - xc_ax)*(row - xc_ax)*pix;
			sumxy += (row - xc_ax)*(col - yc_ax)*pix;
			sumyy += (col - yc_ax)*(col - yc_ax)*pix;
		}
	}				
	a = sumxx/sum;
	b = sumxy/sum;
	c = sumyy/sum;
	if (xwidth == -1.0)
		xwidth = a;
	if (ywidth == -1.0)
		ywidth = c;

	/*    find the eigenvalues */
	temp = sqrt((a - c)*(a - c) + 4*b*b);
	eigen1 = (a + c + temp)/2.0;
	eigen2 = (a + c - temp)/2.0;
	if (eigen1 == 0.0)
		eigen1 = 1.0;
	/* if the eigenvalues imply imaginary eccentricity, set it to 0.0 */
	ecc_ax = 1.0 - (eigen2/eigen1);
	if (ecc_ax <= 0.0) {
		ecc_ax = 0.0;
	} 
	else {
		ecc_ax = sqrt(ecc_ax);
	}

	/*    major axes */
	if (b == 0.0) {
		if (a >= c) {
			xmaj = 1.0;
			ymaj = 0.0;
		}
		else {
			xmaj = 0.0;
			ymaj = 1.0;
		}
	}
	else {
		xmaj = 1.0;
		ymaj = (eigen1 - a)/b;
		temp = sqrt(xmaj*xmaj + ymaj*ymaj);
		xmaj = xmaj/temp;
		ymaj = ymaj/temp;
	}

	/*    minor axes */
	if (b == 0.0) {
		if (a < c) {
			xminor = 1.0;
			yminor = 0.0;
		}
		else {
			xminor = 0.0;
			yminor = 1.0;
		}
	}
	else {
		xminor = 1.0;
		yminor = (eigen2 - a)/b;
		temp = sqrt(xminor*xminor + yminor*yminor);
		xminor = xminor/temp;
		yminor = yminor/temp;
	}

	angmaj_x = 90.0 + atan2(-ymaj,xmaj)*57.3;
	angmin_x = 90.0 + atan2(-yminor,xminor)*57.3;

#ifdef DEBUG
	printf("done with second-moment calculations \n");
#endif


	/*
	 * if we got here, all is well, and we can place results into
	 * the output arguments.
	 */
	*row_cen = xc_ax;
	*col_cen = yc_ax;
	*fwhm = circ_fwhm;
	*peak = peak_value;
	*eccen = ecc_ax;
	*major_axis = angmaj_x;
	*minor_axis = angmin_x;

	return(0);
}


	/*
	 * Calculate the FWHM of the object at the given center of the
	 * given small area of data.  We assume the object is
	 * axisymmetric, and fit a single 1-D gaussian to the radial profile.
	 *
	 * Subtract the "sky" value from each pixel as we go.
	 *
	 * Ignore all points more than 'fitrad' pixels from the center.
         *
	 * We double the actual number of points, placing a copy of each
	 * point at its negative radius.  Thus, we create a perfectly
	 * symmetric profile, in which each point appears once in the
	 * positive and once in the negative direction.  8/2/96 MWR
	 *
	 * Return 
	 *     0    if we could calculate amplitude and FWHM
	 *    -1    to indicate an error.
	 */

static int
circular_gaussian(p, sr, sc, nr, nc, cr, cc, sky, fitrad, peak, fwhm)
int16 **p;
int sr, sc, nr, nc;
double cr, cc, sky, fitrad;
double *peak, *fwhm;
{
	int i, ret, ndata, row, col, sign;
	float *x, *y, *sig, amp, center, width, chisq;
	double dx, dy, dr, pix;

	ndata = 2*nr*nc;
	if ((x = (float *) malloc(ndata*sizeof(float))) == NULL)
		error(1, "circular_gaussian: can't allocate for x vector");
	if ((y = (float *) malloc(ndata*sizeof(float))) == NULL)
		error(1, "circular_gaussian: can't allocate for y vector");
	if ((sig = (float *) malloc(ndata*sizeof(float))) == NULL)
		error(1, "circular_gaussian: can't allocate for sig vector");

	i = 0;
	sign = 1;
	for (row = sr; row < sr + nr; row++) {
		dx = row - cr;
		for (col = sc; col < sc + nc; col++) {
			pix = p[row][col] - sky;
			dy = col - cc;
			dr = sqrt(dx*dx + dy*dy);
			if (dr < fitrad) {
				x[i] = dr;
				y[i] = pix;
				sig[i++] = 1.0;
				x[i] = -dr;
				y[i] = pix;
				sig[i++] = 1.0;
			}
		}
	}
#ifdef DEBUG
	printf("circular_gaussian: done with filling arrays for gauss_fit \n");
#endif
	ret = gauss_fit(x, y, sig, i, &amp, &center, &width, &chisq);
#ifdef DEBUG
	printf("circular_gaussian: done with call to gauss_fit \n");
#endif

	free(x);
	free(y);
	free(sig);

	if (ret == 0) {
		*fwhm = 2.35*width;
		*peak = amp;
	}

	return(ret);
}