/* @(#)fitnol.c	8.3 (ESO-IPG) 11/18/94 09:23:19 */
/* @(#)fitnol.c	8.3 (ESO-IPG) 11/18/94 09:23:19 */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
.IDENT        fitnol.c
.MODULE       subroutines 
.LANGUAGE     C
.AUTHOR       Cristian Levin - ESO La Silla
.PURPOSE      gaussian fitting.
.KEYWORDS     gaussian fitting.
.COMMENTS     Most of this routines were taken as they are from
              the book "Numerical Recipes in C" -- 1st edition.
.VERSION 1.0  1-Apr-1991   Implementation
.ENVIRONMENT  UNIX
------------------------------------------------------------*/

#include <math.h>
#include <proto_nrutil.h>


#ifndef FALSE
#define FALSE           0
#define TRUE            (!FALSE)
#endif

#ifdef __STDC__
void mrqmin( double x[], double y[], double sig[], int ndata, 
             double a[], int ma, int lista[], int mfit, double **covar,
             double **alpha, double *chisq, 
             void (*funcs) ( double, double *, double *, double * ), 
             double *alamda)
#else
   void mrqmin( x, y, sig, ndata, a, ma, lista, mfit, covar, alpha, chisq, 
             funcs, alamda)
   double x[],y[],sig[],a[],**covar,**alpha,*chisq,*alamda;
   int    ndata,ma,lista[],mfit;
   void   (*funcs) ();
#endif
{
	int k,kk,j,ihit;
	static double *da,*atry,**oneda,*beta,ochisq;
#ifdef __STDC__
	void mrqcof( double [], double [], double [], int, double [],
             int, int [], int, double **, double [],
             double *, void (*funcs)(double, double *, double *, double *)); 
        void covsrt( double **, int, int [], int );
        void gaussj(double** , int , double** , int ); 
#else
	void mrqcof(), covsrt();
        void gaussj(); 
#endif

	if (*alamda < 0.0) {
		oneda=dmatrix(1,mfit,1,1);
		atry=dvector(1,ma);
		da=dvector(1,ma);
		beta=dvector(1,ma);
		kk=mfit+1;
		for (j=1;j<=ma;j++) {
			ihit=0;
			for (k=1;k<=mfit;k++)
				if (lista[k] == j) ihit++;
			if (ihit == 0)
				lista[kk++]=j;
			else if (ihit > 1) nrerror("Error in non linear fitting");
		}
		if (kk != ma+1) nrerror("Error in non linear fitting");
		*alamda=0.001;
		mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
		ochisq=(*chisq);
	}
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
		covar[j][j]=alpha[j][j]*(1.0+(*alamda));
		oneda[j][1]=beta[j];
	}
	gaussj(covar,mfit,oneda,1);
	for (j=1;j<=mfit;j++)
		da[j]=oneda[j][1];
	if (*alamda == 0.0) {
		covsrt(covar,ma,lista,mfit);
		free_dvector(beta,1,ma);
		free_dvector(da,1,ma);
		free_dvector(atry,1,ma);
		free_dmatrix(oneda,1,mfit,1,1);
		return;
	}
	for (j=1;j<=ma;j++) atry[j]=a[j];
	for (j=1;j<=mfit;j++)
		atry[lista[j]] = a[lista[j]]+da[j];
	mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
	if (*chisq < ochisq) {
		*alamda *= 0.1;
		ochisq=(*chisq);
		for (j=1;j<=mfit;j++) {
			for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
			beta[j]=da[j];
			a[lista[j]]=atry[lista[j]];
		}
	} else {
		*alamda *= 10.0;
		*chisq=ochisq;
	}
	return;
}

#ifdef __STDC__
   void mrqcof( double x[], double y[], double sig[], int ndata, double a[],
                int ma, int lista[], int mfit, double **alpha, double beta[],
                double *chisq, void (*funcs)(double,double *,double *,double *
                )) 
#else
   void mrqcof( x, y, sig, ndata, a, ma, lista, mfit,alpha, beta,
                chisq, funcs) 
   double  x[],y[],sig[],a[],**alpha,beta[],*chisq;
   int ndata,ma,lista[],mfit;
   void (*funcs)();
#endif

{
	int k,j,i;
	double ymod,wt,sig2i,dy,*dyda;

	dyda=dvector(1,ma);
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=j;k++) alpha[j][k]=0.0;
		beta[j]=0.0;
	}
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		(*funcs)(x[i],a,&ymod,dyda);
		sig2i=1.0/(sig[i]*sig[i]);
		dy=y[i]-ymod;
		for (j=1;j<=mfit;j++) {
			wt=dyda[lista[j]]*sig2i;
			for (k=1;k<=j;k++)
				alpha[j][k] += wt*dyda[lista[k]];
			beta[j] += dy*wt;
		}
		(*chisq) += dy*dy*sig2i;
	}
	for (j=2;j<=mfit;j++)
		for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
	free_dvector(dyda,1,ma);
}

#ifdef __STDC__
   void covsrt( double **covar, int ma, int lista[], int mfit)
#else
   void covsrt( covar, ma, lista, mfit)
   double **covar;
   int ma, lista[],mfit;
#endif
{
	int i,j;
	double swap;

	for (j=1;j<ma;j++)
		for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
	for (i=1;i<mfit;i++)
		for (j=i+1;j<=mfit;j++) {
			if (lista[j] > lista[i])
				covar[lista[j]][lista[i]]=covar[i][j];
			else
				covar[lista[i]][lista[j]]=covar[i][j];
		}
	swap=covar[1][1];
	for (j=1;j<=ma;j++) {
		covar[1][j]=covar[j][j];
		covar[j][j]=0.0;
	}
	covar[lista[1]][lista[1]]=swap;
	for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
	for (j=2;j<=ma;j++)
		for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
}

/************************************************************
  fgauss(): optimized adding fac1, fac2. (C.Levin)
  	    optimized using only 3 coefs. (1 gaussian) (C.Levin).
*/
/* #ifdef __STDC__
   void fgauss(double x, double a[], double *y, double dyda[], int na)
#else
   void fgauss( x, a, y, dyda, na)
   double x,a[],*y,dyda[];
   int na;
#endif */
#ifdef __STDC__
   void fgauss(double x, double a[], double *y, double dyda[])
#else
   void fgauss( x, a, y, dyda)
   double x,a[],*y,dyda[];
#endif
{
/*	int i; */
	double fac1, fac2, ex, arg;

	*y = 0.0;
	
	arg = (x - a[2]) / a[3];
	dyda[1] = ex = exp(-0.5 * arg * arg);
 	*y = fac1 = a[1] * ex;
	dyda[2] = fac2 = fac1 * 2.0 * arg / a[3];
	dyda[3] = fac2 * arg;
}

/************************************************************
 *
 * fit_gauss(): Gaussian fitting. 
 * 
 * calls   : fitnol.c{mrqmin} 
 * modified: Criterium of stopping is more relaxed (C.Levin).
 *
 ************************************************************/


#define EPS	0.001       /* delta chi**2 / chi**2    TS-0896*/
#define NITER   20          /* max number of iteration*/

#ifdef __STDC__
   fit_gauss( double *x, double *y, int n, double *a )
#else
   fit_gauss( x, y, n, a )
   double *x,*y,*a;
   int n;
#endif
{
    int *lista;
    int nfit = 3, ncoefs = 3;
    int i, iter = 1, itst = 0;
    double **covar, **alpha;
    double *sig, chisq, ochisq, alamda = -1.;

#ifdef __STDC__
    void fgauss(double , double [], double *, double [] );
#else
    void fgauss();
#endif

    sig = dvector( 1, n );
    lista = ivector( 1, 3 );
    covar = dmatrix( 1, 3, 1, 3 );
    alpha = dmatrix( 1, 3, 1, 3 );

    for ( i = 1; i <= n; i++ )
	sig[i] = 1.0;

    for ( i = 1; i <= 3; i++ )
	lista[i] = i;

    mrqmin( x, y, sig, n, a, ncoefs, lista, nfit, covar, alpha,
	    &chisq, fgauss, &alamda );

    do {
	iter++;
	ochisq = chisq;
        mrqmin( x, y, sig, n, a, ncoefs, lista, nfit, covar, alpha,
	        &chisq, fgauss, &alamda );
    } while ( ((ochisq - chisq) / chisq > EPS) && (iter <= NITER) ); 
    /*    if ( ((ochisq - chisq) / chisq > EPS)) printf("niter = %6d %8.5f\n",iter,((ochisq - chisq) / chisq));*/
    /*limit niter by TS0896*/

    alamda = 0.;  /* To de-allocate memory */
    mrqmin( x, y, sig, n, a, ncoefs, lista, nfit, covar, alpha,
      &chisq, fgauss, &alamda ); 

    free_dvector( sig, 1, n );
    free_ivector( lista, 1, 3 );
    free_dmatrix( covar, 1, 3, 1, 3 );
    free_dmatrix( alpha, 1, 3, 1, 3 );
}