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

  Thomas Szeifert  --  LSW Heidelberg  --  12.8.1997

  lfit.c
  
  this is lfit.c of the numerical recipes but all functions have been changed
  to double precision. Therefore the names of the routines are the same as
  in the NR but with a prefix d   --   e.g.   lfit ==> dlfit    

*/

/* system includes */

#include <math.h>

/* FEROS specific includes */

#include <mutil.h> 
#include <proto_nrutil.h>
#include <dlfit.h>

void 
dlfit
#ifdef __STDC__
(
 double x[], double y[], double sig[], int ndat, double a[], int ia[],
 int ma, double **covar, double *chisq, 
 void (*funcs)(double, double [], int)
 )
#else
     ( 
      x, y, sig, ndat, a, ia, ma, covar, chisq, funcs
      )
     int ndat,ia[],ma;
     double x[],y[],sig[],a[],**covar,*chisq;
     void (*funcs) ();
#endif
{

  int i,j,k,l,m,mfit=0;
  double ym,wt,sum,sig2i,**beta,*afunc;
  
  beta=dmatrix(1,ma,1,1);
  afunc=dvector(1,ma);
  for (j=1;j<=ma;j++)
    if (ia[j]) mfit++;
  if (mfit == 0) nrerror("lfit: no parameters to be fitted");
  for (j=1;j<=mfit;j++) {
    for (k=1;k<=mfit;k++) covar[j][k]=0.0;
    beta[j][1]=0.0;
  }
  for (i=1;i<=ndat;i++) {
    (*funcs)(x[i],afunc,ma);
    ym=y[i];
    if (mfit < ma) {
      for (j=1;j<=ma;j++)
	if (!ia[j]) ym -= a[j]*afunc[j];
    }
    sig2i=1.0/DSQR(sig[i]);
    for (j=0,l=1;l<=ma;l++) {
      if (ia[l]) {
	wt=afunc[l]*sig2i;
	for (j++,k=0,m=1;m<=l;m++)
	  if (ia[m]) covar[j][++k] += wt*afunc[m];
	beta[j][1] += ym*wt;
      }
    }
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++)
      covar[k][j]=covar[j][k];
  dgaussj(covar,mfit,beta,1);
  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) a[l]=beta[++j][1];
  *chisq=0.0;
  for (i=1;i<=ndat;i++) {
    (*funcs)(x[i],afunc,ma);
    for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
    *chisq += DSQR((y[i]-sum)/sig[i]);
  }
  dcovsrt(covar,ma,ia,mfit);
  free_dvector(afunc,1,ma);
  free_dmatrix(beta,1,ma,1,1);

  return;
}


/*
  this is lfit.c of the numerical recipes but all functions have been changed
  to double precision and we can work in two dimensions:
  
  y --> y(x1,x2) +- sig_y

  Thomas Szeifert  --  LSW Heidelberg  --  12.8.1997

*/

void dlfit_2D
#ifdef __STDC__
(
 double x1[], double x2[], double y[], double sig[], int ndat, 
 double a[], int ia[], int ma, double **covar, double *chisq, 
 void (*funcs)(double, double, double [], int)
 )
#else
     ( 
      x1,x2, y, sig, ndat, a, ia, ma, covar, chisq, funcs
      )
     int ndat,ia[],ma;
     double x1[],x2[],y[],sig[],a[],**covar,*chisq;
     void (*funcs) ();
#endif
{

  int i,j,k,l,m,mfit=0;
  double ym,wt,sum,sig2i,**beta,*afunc;
  
  beta=dmatrix(1,ma,1,1);
  afunc=dvector(1,ma);
  for (j=1;j<=ma;j++)
    if (ia[j]) mfit++;
  if (mfit == 0) nrerror("lfit: no parameters to be fitted");
  for (j=1;j<=mfit;j++) {
    for (k=1;k<=mfit;k++) covar[j][k]=0.0;
    beta[j][1]=0.0;
  }
  for (i=1;i<=ndat;i++) {
    (*funcs)(x1[i],x2[i],afunc,ma);
    ym=y[i];
    if (mfit < ma) {
      for (j=1;j<=ma;j++)
	if (!ia[j]) ym -= a[j]*afunc[j];
    }
    sig2i=1.0/DSQR(sig[i]);
    for (j=0,l=1;l<=ma;l++) {
      if (ia[l]) {
	wt=afunc[l]*sig2i;
	for (j++,k=0,m=1;m<=l;m++)
	  if (ia[m]) covar[j][++k] += wt*afunc[m];
	beta[j][1] += ym*wt;
      }
    }
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++)
      covar[k][j]=covar[j][k];
  dgaussj(covar,mfit,beta,1);
  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) a[l]=beta[++j][1];
  *chisq=0.0;
  for (i=1;i<=ndat;i++) {
    (*funcs)(x1[i],x2[i],afunc,ma);
    for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
    *chisq += DSQR((y[i]-sum)/sig[i]);
  }
  dcovsrt(covar,ma,ia,mfit);
  free_dvector(afunc,1,ma);
  free_dmatrix(beta,1,ma,1,1);

  return;
}


static double swap;
#define LSWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

void dcovsrt
#ifdef __STDC__
(
 double **covar, int ma, int ia[], int mfit
 )
#else
     ( 
      covar,  ma,  ia, mfit
      )
     double **covar;
     int  ma, ia[], mfit;
#endif
{
  int i,j,k;

  for (i=mfit+1;i<=ma;i++)
    for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
  k=mfit;
  for (j=ma;j>=1;j--) {
    if (ia[j]) {
      for (i=1;i<=ma;i++) LSWAP(covar[i][k],covar[i][j])
			    for (i=1;i<=ma;i++) LSWAP(covar[k][i],covar[j][i])
						  k--;
    }
  }
  return;
}

void dgaussj
#ifdef __STDC__
(
 double **a, int n, double **b, int m
 )
#else
     (
      a, n, b, m
      )
     double **a, **b;
     int m, n;
#endif
{
  int *indxc,*indxr,*ipiv;
  int i,icol,irow,j,k,l,ll;
  double big,dum,pivinv;

  indxc=ivector(1,n);
  indxr=ivector(1,n);
  ipiv=ivector(1,n);
  for (j=1;j<=n;j++) ipiv[j]=0;
  for (i=1;i<=n;i++) {
    big=0.0;
    for (j=1;j<=n;j++)
      if (ipiv[j] != 1)
	for (k=1;k<=n;k++) {
	  if (ipiv[k] == 0) {
	    if (fabs(a[j][k]) >= big) {
	      big=fabs(a[j][k]);
	      irow=j;
	      icol=k;
	    }
	  } else if (ipiv[k] > 1) nrerror("dgaussj: Singular Matrix-1");
	}
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l=1;l<=n;l++) LSWAP(a[irow][l],a[icol][l])
			   for (l=1;l<=m;l++) LSWAP(b[irow][l],b[icol][l])
			   }
    indxr[i]=irow;
    indxc[i]=icol;
    if (a[icol][icol] == 0.0) nrerror("dgaussj: Singular Matrix-2");
    pivinv=1.0/a[icol][icol];
    a[icol][icol]=1.0;
    for (l=1;l<=n;l++) a[icol][l] *= pivinv;
    for (l=1;l<=m;l++) b[icol][l] *= pivinv;
    for (ll=1;ll<=n;ll++)
      if (ll != icol) {
	dum=a[ll][icol];
	a[ll][icol]=0.0;
	for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
	for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
      }
  }
  for (l=n;l>=1;l--) {
    if (indxr[l] != indxc[l])
      for (k=1;k<=n;k++)
	LSWAP(a[k][indxr[l]],a[k][indxc[l]]);
  }
  free_ivector(ipiv,1,n);
  free_ivector(indxr,1,n);
  free_ivector(indxc,1,n);

  return;
}

#undef LSWAP