/* file gfitc.c, gaussian fit routine */
/* 1/19/2005 - add a cosine fit routine also */
#include <stdio.h>
#include "ana_structures.h"
#include <math.h>
 int	gfit_fix_a, gfit_fix_b, gfit_fix_x0, gfit_fix_del, gfit_show;
 int	gfit_guess_mode, gfit_converged, gfit_warnings={1};
 /*------------------------------------------------------------------------- */
int gfit(x,y,f,n,amp,bac,x0,del,tol,niter)
 /* this program is modified from an old pdp code used for batch gaussian fits
 it could use some re-writing */
 /* converted to c 3/19/93 */
 float	*x, *y, *f, *amp, *bac, *x0, *del, tol;
 int	niter, n;
 {
  double	a[16], sx[4], *pd;
  float		sp[4], xq, yq, *xp, *yp, sum1, sum2;
  float		da, db, dc, dd, dx, tem, fq;
  float		*p;
  int		np={4}, iq, jq, i, j, upflag, iter, status, nc;

 /* check the input guesses; if 0, assume that user wants us to make a guess */
 if (*bac == 0) *bac=.5*( *y + *(y + n - 1));      /* mean of start and end */
 /* if either amp or x0 need work, do the loop */
 if (*amp == 0.0 || *x0 == 0.0) { xq = yq =  *y;   iq = jq = 0;   p = y + 1;
 nc = n - 1;	i = 1;
 while (nc--) {
 	if ( *p > xq ) { xq = *p;	iq = i; }
 	if ( *p < yq ) { yq = *p;	jq = i; }
	p++;	i++; }
 /* determine if amp is + or -, upflag = 1 for + */
 if ( ABS(xq-*bac) >= ABS(*bac-yq) )  upflag = 1;  else  upflag = 0;
 /* set *x0 to x value at max (or min if upflag is 0) */
 if (*x0 == 0.0) {
 	if (upflag) *x0 = *(x + iq); else  *x0 = *(x + jq); }
 /* set amp to highest value minus bac */
 if (*amp == 0.0) {
 	if (upflag) *amp = xq - *bac; else  *amp = yq - *bac; }
 }
 /* now for the del */
 if (*del == 0.0) {
	sum1 = sum2 = 0.0;  nc = n;  xp = x;   yp = y;
	if (upflag) {	while (nc--) { 	xq = MAX(0.0, *yp - *bac);  yp++;
		yq = *xp++ - *x0;	sum1 += xq;	sum2 += xq*yq*yq; }
	} else {	while (nc--) { xq = MAX(0.0, *bac - *yp );  yp++;
		yq = *xp++ - *x0;	sum1 += xq;	sum2 += xq*yq*yq; }
	}
 *del = sqrt( 2.0 * sum2 / sum1 );
 }
  /* pack the input guesses into sp */
 sp[0] = *amp;  
 sp[1] = *bac;  
 sp[2] = *x0;  
 sp[3] = *del;  
 iter = 0;
 if (gfit_show)
 	fprintf(stderr, "iter = %d, parameters = %f %f %f %f\n",iter,sp[0],sp[1],sp[2],sp[3]);
 while (1) {		/* iteration loop */
 nc = 4;	pd = sx;	while (nc--) *pd++ = (double) 0.0;
 nc = 16;	pd = a;		while (nc--) *pd++ = (double) 0.0;
 xq = 2.* *sp / *(sp+3);
 nc = n;	yp = y;		xp = x;
 while (nc--) {
 /* skip point if value is zero */
 if (*yp) {
 dx = (*xp - *(sp+2) )/ (*(sp+3));
 da = exp(-dx*dx);
 /* note that db = 1.0, so db terms don't show */
 dc = dx*da*xq;
 dd = dx*dc;
 fq = *sp * da + *(sp+1);
 *a += da*da;
 *(a+1) += da;
 *(a+2) += dc*da;
 *(a+3) += dd*da;
 *(a+5) += 1.0;
 *(a+6) += dc;
 *(a+7) += dd;
 *(a+10) += dc*dc;
 *(a+11) += dd*dc;
 *(a+15) += dd*dd;
 tem = *yp - fq;
 *(sx) += da*tem;
 *(sx+1) += tem;
 *(sx+2) += dc*tem;
 *(sx+3) += dd*tem;
 }
 xp++;	yp++;
 }
 /* we handle fixed parameters by adjusting the correction matrix now */
 if (gfit_fix_a) { a[0] = 1.0;  a[1] = a[2] = a[3] = 0.0; sx[0] = 0.0; }
 if (gfit_fix_b) { a[5] = 1.0;  a[6] = a[7] = a[1] = 0.0; sx[1] = 0.0; }
 if (gfit_fix_x0) { a[10] = 1.0;  a[11] = a[2] = a[6] = 0.0; sx[2] = 0.0; }
 if (gfit_fix_del) { a[15] = 1.0;  a[11] = a[7] = a[3] = 0.0; sx[3] = 0.0; }
 /* make symmetric */
 *(a+4) = *(a+1); *(a+8) = *(a+2); *(a+9) = *(a+6); *(a+12) = *(a+3);
 *(a+13) = *(a+7); *(a+14) = *(a+11);

 d_decomp(a,4,4);
 d_solve(a,sx,4,4);

 /* we just computed the correction, add it to results with .75 damping */
 
 nc = 4;  p = sp;  pd = sx;  while (nc--) *p++ += 0.75 * *pd++;

 if (gfit_show) fprintf(stderr, "iter = %d, parameters = %f %f %f %f\n",iter,*sp,
  *(sp+1), *(sp+2),*(sp+3) );
 /* negative widths are bad */
 if ( *(sp+3) < 0) { if (gfit_warnings) fprintf(stderr, "width became negative\n");
  status = 0; break; }
 /* we only check the amplitude and the width for tolerance
    because of possible singularities in the background and center position */
 if ( (*sp != 0.0 && *(sp+3) != 0.0 ) && ( ABS(*sx / *sp) <= tol)
 	&& ( ABS(*(sx+3) / *(sp+3) ) <= tol) )	{ status = 1; break; }
 if (iter++ > niter) {
 if (gfit_warnings) fprintf(stderr, "gfit did not converge in %d iterations\n", niter);
 status = 0;		break; }
 }
 /* if things were OK, load the output fit in f */
 nc = n;	p = f;		xp = x;
 if (status) {
 			/* load results into output arguments */
	*amp = sp[0]; *bac = sp[1]; *x0 = sp[2]; *del = sp[3];
 	while (nc--) { dx = (*xp++ - *(sp+2) )/ (*(sp+3));
		*f++ = *bac + *amp * exp(-dx*dx); }
 } else { while (nc--) { *f++ = 0.0; }
 	if (gfit_warnings) fprintf(stderr, "GFIT: couldn't fit\n");
 			/* load zeroes into output arguments */
	 *amp = 0.0; *bac = 0.0; *x0 = 0.0; *del = 0.0;
 }
 return status;
 }
 /*------------------------------------------------------------------------- */
 int	cosinefit_fix_a, cosinefit_fix_b, cosinefit_fix_x0, cosinefit_fix_del, cosinefit_show;
 int	cosinefit_guess_mode, cosinefit_converged, cosinefit_warnings={1};
 /*------------------------------------------------------------------------- */
int cosinefit(x,y,f,n,amp,bac,x0,del,tol,niter)
 /* the cosine fit routine, fits to a single sine curve, the guess should be
 reasonable, there are 4 parameters like the gfit code but they are:
   bac:		a fixed background or bias level
   amp:		amplitude of the fitted function
   x0:		a phase
   del:		the period
   
   the fit is f = bac + amp * sin( 2 pi (x - x0)/p)
   
   tol and niter are a tolerance and maximum # of iterations as in the gfit
   code  */
 float	*x, *y, *f, *amp, *bac, *x0, *del, tol;
 int	niter, n;
 {
  double	a[16], sx[4], *pd;
  float		sp[4], xq, yq, *xp, *yp, sum1, sum2;
  float		da, db, dc, dd, tem, fq;
  float		*p;
  int		np={4}, iq, jq, i, j, iter, status, nc;

 /* check the input guesses; if 0, assume that user wants us to make a guess */
 if (*bac == 0) *bac=.5*( *y + *(y + n - 1));      /* mean of start and end */
 /* if either amp or x0 need work, do the loop */
 if (*amp == 0.0 || *x0 == 0.0) {
   xq = yq = *y;	iq = jq = 0;   p = y + 1; 
   nc = n - 1;	i = 1;
   while (nc--) {
 	  if ( *p > xq ) { xq = *p;	iq = i; }
 	  if ( *p < yq ) { yq = *p;	jq = i; }
	  p++;	i++; }
   /* set amp to (max - min)/2 */
   if (*amp == 0.0) *amp = 0.5 * (xq - yq);
   if (*x0  == 0.0) *x0 = *(x + iq);
   /* now for the period (in del) */
   if (*del == 0.0) { *del = 2.0; }
 }
  /* pack the input guesses into sp */
 sp[0] = *amp;  
 sp[1] = *bac;  
 sp[2] = *x0;  
 sp[3] = *del;  
 iter = 0;
 if (cosinefit_show)
   fprintf(stderr, "iter = %d, parameters = %f %f %f %f\n",iter,sp[0], sp[1], sp[2],sp[3] );
 while (1) {		/* iteration loop */
 nc = 4;	pd = sx;	while (nc--) *pd++ = (double) 0.0;
 nc = 16;	pd = a;		while (nc--) *pd++ = (double) 0.0;
 xq = 2.* *sp / *(sp+3);
 nc = n;	yp = y;		xp = x;
 while (nc--) {
   xq = 6.283185308 * ( *xp - sp[2] )/ sp[3];
   da = cos(xq);
   /* note that db = 1.0, so db terms don't show */
   dc = sp[0] * sin(xq) * 6.283185308/ sp[3];
   dd = sp[0] * sin(xq) * xq/ sp[3];
   fq = sp[0] * da + sp[1];
   *a += da*da;
   *(a+1) += da;
   *(a+2) += dc*da;
   *(a+3) += dd*da;
   *(a+5) += 1.0;
   *(a+6) += dc;
   *(a+7) += dd;
   *(a+10) += dc*dc;
   *(a+11) += dd*dc;
   *(a+15) += dd*dd;
   tem = *yp - fq;
   *(sx) += da*tem;
   *(sx+1) += tem;
   *(sx+2) += dc*tem;
   *(sx+3) += dd*tem;
   xp++;	yp++;
 }
 /* we handle fixed parameters by adjusting the correction matrix now */
 if (cosinefit_fix_a) { a[0] = 1.0;  a[1] = a[2] = a[3] = 0.0; sx[0] = 0.0; }
 if (cosinefit_fix_b) { a[5] = 1.0;  a[6] = a[7] = a[1] = 0.0; sx[1] = 0.0; }
 if (cosinefit_fix_x0) { a[10] = 1.0;  a[11] = a[2] = a[6] = 0.0; sx[2] = 0.0; }
 if (cosinefit_fix_del) { a[15] = 1.0;  a[11] = a[7] = a[3] = 0.0; sx[3] = 0.0; }
 /* make symmetric */
 *(a+4) = *(a+1); *(a+8) = *(a+2); *(a+9) = *(a+6); *(a+12) = *(a+3);
 *(a+13) = *(a+7); *(a+14) = *(a+11);

 d_decomp(a,4,4);
 d_solve(a,sx,4,4);

 /* we just computed the correction, add it to results with .75 damping */
 
 nc = 4;  p = sp;  pd = sx;  while (nc--) *p++ += 0.75 * *pd++;

 if (cosinefit_show) fprintf(stderr, "iter = %d, parameters = %f %f %f %f\n",iter,*sp,
  *(sp+1), *(sp+2),*(sp+3) );
 /* negative periods are bad */
 if ( *(sp+3) < 0) { if (cosinefit_warnings) fprintf(stderr, "period became negative\n");
  status = 0; break; }
 /* we only check the amplitude and the width for tolerance
    because of possible singularities in the background and center position */
 if ( (*sp != 0.0 && *(sp+3) != 0.0 ) && ( ABS(*sx / *sp) <= tol)
 	&& ( ABS(*(sx+3) / *(sp+3) ) <= tol) )	{ status = 1; break; }
 if (iter++ > niter) {
 if (cosinefit_warnings) fprintf(stderr, "cosinefit did not converge in %d iterations\n", niter);
 status = 0;		break; }
 }
 /* if things were OK, load the output fit in f */
 nc = n;	p = f;		xp = x;
 if (status) {
   /* load results into output arguments */
   *amp = sp[0]; *bac = sp[1]; *x0 = sp[2]; *del = sp[3];
   while (nc--) {
     xq = 6.283185308 * ( *xp++ - sp[2] )/ sp[3];
     *f++ = *bac + *amp * cos(xq); }
 } else { while (nc--) { *f++ = 0.0; }
   if (cosinefit_warnings) fprintf(stderr, "cosinefit: couldn't fit\n");
   /* load zeroes into output arguments */
    *amp = 0.0; *bac = 0.0; *x0 = 0.0; *del = 0.0;
 }
 return status;
 }
 /*------------------------------------------------------------------------- */