/* @(#)2Dstuff.c	17.1.1.1 (ESO-IPG) 01/25/02 17:51:57 */
/* 

   Anton Malina

   2Dstuff.c 

   2D-fit routines based on 1D-routines from Numerical Recipes 
   
*/

/* system includes */

#include <math.h>

/* FEROS specific includes */

#include <proto_nrutil.h>
#include <2Dstuff.h>
#include <misc.h>

#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

void mrqmin2D 
#ifdef __STDC__
(
 double x1[], double x2[], double y[], double sig[], int ndata, double a[],
 int ia[], int ma, double **covar, double **alpha, double *chisq,
 void (*funcs)(double, double, double [], double *, double [], int),
 double *alamda
 )
#else
     (
      x1, x2, y, sig, ndata, a, ia, ma, covar, alpha, chisq, funcs, alamda
      )
     double x1[], x2[], y[], sig[], a[], **covar, **alpha, *chisq, *alamda;
     int ndata, ia[], ma;
     void (*funcs)();
     
#endif
{
  int j,k,l,m;
  static int mfit;
  static double ochisq,*atry,*beta,*da,**oneda;
  
  if (*alamda < 0.0) {
    atry=dvector(1,ma);
    beta=dvector(1,ma);
    da=dvector(1,ma);
    for (mfit=0,j=1;j<=ma;j++)
      if (ia[j]) mfit++;
    oneda=dmatrix(1,mfit,1,1);
    *alamda=0.001;
    mrqcof2D(x1,x2,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
    ochisq=(*chisq);
    for (j=1;j<=ma;j++) atry[j]=a[j];
  }
  for (j=0,l=1;l<=ma;l++) {
    if (ia[l]) {
      for (j++,k=0,m=1;m<=ma;m++) {
	if (ia[m]) {
	  k++;
	  covar[j][k]=alpha[j][k];
	}
      }
      covar[j][j]=alpha[j][j]*(1.0+(*alamda));
      oneda[j][1]=beta[j];
    }
  }
  gaussjd(covar,mfit,oneda,1);
  for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
  if (*alamda == 0.0) {
    covsrtd(covar,ma,ia,mfit);
    free_dmatrix(oneda,1,mfit,1,1);
    free_dvector(da,1,ma);
    free_dvector(beta,1,ma);
    free_dvector(atry,1,ma);
    return;
  }
  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) atry[l]=a[l]+da[++j];
  mrqcof2D(x1,x2,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
  if (*chisq < ochisq) {
    *alamda *= 0.1;
    ochisq=(*chisq);
    for (j=0,l=1;l<=ma;l++) {
      if (ia[l]) {
	for (j++,k=0,m=1;m<=ma;m++) {
	  if (ia[m]) {
	    k++;
	    alpha[j][k]=covar[j][k];
	  }
	}
	beta[j]=da[j];
	a[l]=atry[l];
      }
    }
  } else {
    *alamda *= 10.0;
    *chisq=ochisq;
  }
}

void mrqcof2D 
#ifdef __STDC__
(
 double x1[], double x2[], double y[], double sig[], int ndata, double a[],
 int ia[], int ma, double **alpha, double beta[], double *chisq,
 void (*funcs)(double, double, double [], double *, double [], int)
 )
#else
     (
      x1, x2, y, sig, ndata, a, ia, ma, alpha, beta, chisq, funcs
      )
     double x1[], x2[], y[], sig[], a[], **alpha, beta[], *chisq;
     int ndata, ia[], ma;
     void (*funcs)();
#endif
{
  int i,j,k,l,m,mfit=0;
  double ymod,wt,sig2i,dy,*dyda;

  dyda=dvector(1,ma);
  for (j=1;j<=ma;j++)
    if (ia[j]) mfit++;
  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)(x1[i],x2[i],a,&ymod,dyda,ma);
    sig2i=1.0/(sig[i]*sig[i]);
    dy=y[i]-ymod;
    for (j=0,l=1;l<=ma;l++) {
      if (ia[l]) {
	wt=dyda[l]*sig2i;
	for (j++,k=0,m=1;m<=l;m++)
	  if (ia[m]) alpha[j][++k] += wt*dyda[m];
	beta[j] += dy*wt;
      }
    }
    *chisq += dy*dy*sig2i;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_dvector(dyda,1,ma);
}

void covsrtd
#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;
	double swap;

	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++) SWAP(covar[i][k],covar[i][j])
			for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
			k--;
		}
	}
}

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

  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("gaussjd: Singular Matrix-1");
	}
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
			   for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
			   }
    indxr[i]=irow;
    indxc[i]=icol;
    if (a[icol][icol] == 0.0) nrerror("gaussjd: 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++)
	SWAP(a[k][indxr[l]],a[k][indxc[l]]);
  }
  free_ivector(ipiv,1,n);
  free_ivector(indxr,1,n);
  free_ivector(indxc,1,n);
}
#undef SWAP