/* @(#)fitnol.c	17.1.1.1 (ESO-IPG) 01/25/02 17:51:58 */
/*

  Based on code by Christan Levin 

  fitnol.c
  
  various fitting functions

*/

/* system includes */

#include <math.h>

/* FEROS specific includes */

#include <proto_nrutil.h>
#include <proto_fitnol.h>

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

void mrqmin
#ifdef __STDC__
( 
 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 *, int), 
 double *alamda
 )
#else
     ( 
      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;
  
  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;
}

void mrqcof
#ifdef __STDC__
( 
 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 *, int)
 ) 
#else
     ( 
      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, ma);
    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);
}

void covsrt
#ifdef __STDC__
( 
 double **covar, int ma, int lista[], int mfit
 )
#else
     ( 
      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).
*/

void fgauss
#ifdef __STDC__
(
 double x, double a[], double *y, double dyda[], int na
 )
#else
     (
      x, a, y, dyda,na
      )
     double x,a[],*y,dyda[];
     int na;
#endif
{
  /* na/3 gauss-function with A, x0, sigma in a[1], a[2], a[3]... */
  int i;
  double fac, ex, arg;
      
  *y = 0.0;
  for(i = 1; i < na; i += 3)
    {
      
      arg = (x - a[i+1]) / a[i+2];
      ex = exp(-0.5 * arg * arg);
      *y += a[i] * ex;
      dyda[i] = ex;
      fac = a[i] * ex * arg / a[i+2];
      dyda[i+1] = fac;
      dyda[i+2] = fac * arg;
      
    }
}

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

#define EPS	0.001

int   fit_gauss
#ifdef __STDC__
( 
 double *x, double *y, int n, double *a, int nfp
 )
#else
     (
      x, y, n, a, nfp
      )
     double *x, *y, *a;
     int n, nfp;
#endif

{

  int *lista;
  int nfit, ncoefs;
  int i, iter = 1;
  double **covar, **alpha;
  double *sig, chisq, ochisq, alamda = -1.;

  nfit = nfp;
  ncoefs = nfp;
  
  sig = dvector(1, n);
  lista = ivector(1, ncoefs);
  covar = dmatrix(1, nfit, 1, nfit);
  alpha = dmatrix(1, ncoefs, 1, ncoefs);
  
  for(i = 1; i <= n; i++)
    sig[i] = 1.0;
  for(i = 1; i <= ncoefs; 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 );
    
  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, ncoefs);
  free_dmatrix(covar, 1, nfit, 1, nfit);
  free_dmatrix(alpha, 1, ncoefs, 1, ncoefs);
  return 0;
}