[u/mrichter/AliRoot.git] / HLT / comp / AliL3FitUtilities.c

/* $Id$ */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <setjmp.h>
#include "AliL3FitUtilities.h"

jmp_buf env;

DPOINT *plane;

void f2gauss5(double x,double a[],double *y,double dyda[],int na)
{
  /*Function describing a sum of 2D gaussians with 5 parameters.
    There number of gaussians is na/5. */
  
  int i,index;
  double fac,fac1,fac2,ex,ex1,ex2,arg1,arg2,u,v;
  
  /*printf("fitting na %d, with pad %f time %f amplitude %f padwidth %f timewidth %f\n",na,a[2],a[4],a[1],a[3],a[5]);*/
        index = nint(x);
	if( index < 0 || index >=FIT_PTS )
	{
		fprintf( stderr, "ff2gauss: wrong index %d\n", index );
		return;
	}
	u     = plane[index].u;
	v     = plane[index].v;
	/*printf("u %f v %f\n",u,v);*/
	*y=0.0;
	for (i=1;i<=na-1;i+=5)
	{
		arg1      = (u-a[i+1])/a[i+2];
		arg2      = (v-a[i+3])/a[i+4];
		ex1       = exp(-0.5*arg1*arg1);
		ex2       = exp(-0.5*arg2*arg2);
		ex        = ex1*ex2;
		fac       = a[i]*ex*2.0;
		fac1      = fac * arg1;
		fac2      = fac * arg2;
		*y       += a[i] * ex;
		dyda[i]   = ex;
		dyda[i+1] = fac1/a[i+2];
		dyda[i+2] = fac1*arg1/a[i+2];
		dyda[i+3] = fac2/a[i+4];
		dyda[i+4] = fac2*arg2/a[i+4];
	}
}

void nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
  /*printf("%s\n",error_text);
    exit(1);*/
}

void free_vector(double *v, long nl, long nh)
/* free a double vector allocated with vector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_ivector(int *v, long nl, long nh)
/* free an int vector allocated with ivector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
/* free a double matrix allocated by matrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

int *ivector(long nl, long nh)
/* allocate an int vector with subscript range v[nl..nh] */
{
	int *v;

	v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
	if (!v) nrerror("allocation failure in ivector()");
	return v-nl+NR_END;
}

double *vector(long nl,long nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;

	v=(double *)malloc((unsigned int) ((nh-nl+1+NR_END)*sizeof(double)));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl+NR_END;
}

double **matrix(long nrl,long nrh,long ncl,long nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;

	/* allocate pointers to rows */
	m=(double **) malloc((unsigned int)((nrow+NR_END)*sizeof(double*)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(double *) malloc((unsigned int)((nrow*ncol+NR_END)*sizeof(double)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

int gaussj(double **a, int n, double **b, int m)
{
	int *indxc,*indxr,*ipiv;
	int i,icol=0,irow=0,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) {
						free_ivector(ipiv,1,n);
						free_ivector(indxr,1,n);
						free_ivector(indxc,1,n);
						nrerror("gaussj: Singular Matrix-1");
						return -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){
			free_ivector(ipiv,1,n);
			free_ivector(indxr,1,n);
			free_ivector(indxc,1,n);
			nrerror("gaussj: Singular Matrix-2");
			return -1;
		}
		if (mabs(a[icol][icol]) < EPSILON ) 
		  {
		    nrerror("gaussj: a[icol][icol] == 0");
		    return -1;
		  }
		pivinv=1.0/a[icol][icol];

		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);
	return 0;
}

void covsrt(double **covar, int ma, int ia[], int mfit)
{
	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 mrqcof(double x[], double y[], double sig[], int ndata, double a[], int ia[],
	int ma, double **alpha, double beta[], double *chisq,
	void (*funcs)(double, double [], double *, double [], int))
{
	int i,j,k,l,m,mfit=0;
	double ymod,wt,sig2i,dy,*dyda;

	dyda=vector(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)(x[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_vector(dyda,1,ma);
}

int mrqmin(double x[], double y[], double sig[], int ndata, double a[], int ia[],
	int ma, double **covar, double **alpha, double *chisq,
	void (*funcs)(double, double [], double *, double [], int), double *alamda)
{
	void covsrt(double **covar, int ma, int ia[], int mfit);
	int gaussj(double **a, int n, double **b, int m);
	void mrqcof(double x[], double y[], double sig[], int ndata, double a[],
		int ia[], int ma, double **alpha, double beta[], double *chisq,
		void (*funcs)(double, double [], double *, double [], int));
	int j,k,l,m;
	static int mfit;
	static double ochisq,*atry,*beta,*da,**oneda;

	if (*alamda < 0.0) {
		atry=vector(1,ma);
		beta=vector(1,ma);
		da=vector(1,ma);
		for (mfit=0,j=1;j<=ma;j++)
			if (ia[j]) mfit++;
		oneda=matrix(1,mfit,1,1);
		*alamda=0.001;
		mrqcof(x,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];
		}
	}
	if(gaussj(covar,mfit,oneda,1)<0)
	  {
	    free_matrix(oneda,1,mfit,1,1);
	    free_vector(da,1,ma);
	    free_vector(beta,1,ma);
	    free_vector(atry,1,ma);
	    return -1;
	  }
	for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
	if (*alamda == 0.0) {
	        covsrt(covar,ma,ia,mfit);
		free_matrix(oneda,1,mfit,1,1);
		free_vector(da,1,ma);
		free_vector(beta,1,ma);
		free_vector(atry,1,ma);
		return 0;
	}
	for (j=0,l=1;l<=ma;l++)
		if (ia[l]) atry[l]=a[l]+da[++j];
	mrqcof(x,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;
	}
	return 0;
}


int lev_marq_fit( double x[], double y[], double sig[], int NPT, double a[], int ia[], double dev[], int MA,
                  double *chisq_p, void (*funcs)(double, double [], double *, double [], int) )
{
	int     i,itst,k;
	double   alamda,chisq,ochisq,**covar,**alpha;
	
	covar=matrix(1,MA,1,MA);
	alpha=matrix(1,MA,1,MA);

	if( setjmp(env) == 0 ) {	
		alamda = -1;
		if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
		  {
		    free_matrix(alpha,1,MA,1,MA);
		    free_matrix(covar,1,MA,1,MA);
		    return -1;
		  }
		k=1;
		itst=0;
		for (;;) {
			k++;
			ochisq=chisq;
			if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
			  {
			    free_matrix(alpha,1,MA,1,MA);
			    free_matrix(covar,1,MA,1,MA);
			    return -1;
			  }
			if (chisq > ochisq)
				itst=0;
			else if (fabs(ochisq-chisq) < 0.1)
				itst++;
			if (itst < 4) continue;
			alamda=0.0;
			if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
			  {
			    free_matrix(alpha,1,MA,1,MA);
			    free_matrix(covar,1,MA,1,MA);
			    return -1;
			  }
			*chisq_p = chisq;
			for (i=1;i<=MA;i++) 
				dev[i] = sqrt(covar[i][i]);
			break;
		}
		free_matrix(alpha,1,MA,1,MA);
		free_matrix(covar,1,MA,1,MA);
		return 0;
	}
	else {
	  /*if( control_g.print_fit_errors==2 )*/
	  fprintf( stderr, " runtime error\n" );

	        free_matrix(alpha,1,MA,1,MA);
		free_matrix(covar,1,MA,1,MA);
		return -1;
	}
}
Commit	Line	Data
de3c3890	1	/* $Id$ */
6b8f6f6e	2
6b8f6f6e	3	#include <stdio.h>
	4	#include <stdlib.h>
	5	#include <math.h>
	6	#include <setjmp.h>
eb86303b	7	#include "AliL3FitUtilities.h"
6b8f6f6e	8
6b8f6f6e	9	jmp_buf env;
	10
	11	DPOINT *plane;
	12
6b8f6f6e	13	void f2gauss5(double x,double a[],double *y,double dyda[],int na)
6b8f6f6e	14	{
a3282264	15	/*Function describing a sum of 2D gaussians with 5 parameters.
a3282264	16	There number of gaussians is na/5. */
6f388e0d	17
6b8f6f6e	18	int i,index;
	19	double fac,fac1,fac2,ex,ex1,ex2,arg1,arg2,u,v;
	20
a3282264	21	/printf("fitting na %d, with pad %f time %f amplitude %f padwidth %f timewidth %f\n",na,a[2],a[4],a[1],a[3],a[5]);/
6f388e0d	22	index = nint(x);
6b8f6f6e	23	if( index < 0 \|\| index >=FIT_PTS )
6b8f6f6e	24	{
43d89950	25	fprintf( stderr, "ff2gauss: wrong index %d\n", index );
6b8f6f6e	26	return;
	27	}
	28	u = plane[index].u;
	29	v = plane[index].v;
a3282264	30	/printf("u %f v %f\n",u,v);/
6b8f6f6e	31	*y=0.0;
	32	for (i=1;i<=na-1;i+=5)
	33	{
	34	arg1 = (u-a[i+1])/a[i+2];
	35	arg2 = (v-a[i+3])/a[i+4];
6f388e0d	36	ex1 = exp(-0.5arg1arg1);
6f388e0d	37	ex2 = exp(-0.5arg2arg2);
6b8f6f6e	38	ex = ex1*ex2;
	39	fac = a[i]ex2.0;
	40	fac1 = fac * arg1;
	41	fac2 = fac * arg2;
	42	y += a[i] ex;
	43	dyda[i] = ex;
	44	dyda[i+1] = fac1/a[i+2];
	45	dyda[i+2] = fac1*arg1/a[i+2];
	46	dyda[i+3] = fac2/a[i+4];
	47	dyda[i+4] = fac2*arg2/a[i+4];
	48	}
	49	}
	50
	51	void nrerror(char error_text[])
	52	/* Numerical Recipes standard error handler */
de3c3890	53	{
a3282264	54	/*printf("%s\n",error_text);
a3282264	55	exit(1);*/
6b8f6f6e	56	}
	57
	58	void free_vector(double *v, long nl, long nh)
	59	/* free a double vector allocated with vector() */
	60	{
	61	free((FREE_ARG) (v+nl-NR_END));
	62	}
	63
	64	void free_ivector(int *v, long nl, long nh)
	65	/* free an int vector allocated with ivector() */
	66	{
	67	free((FREE_ARG) (v+nl-NR_END));
	68	}
	69
	70	void free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
	71	/* free a double matrix allocated by matrix() */
	72	{
	73	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	74	free((FREE_ARG) (m+nrl-NR_END));
	75	}
	76
	77	int *ivector(long nl, long nh)
	78	/* allocate an int vector with subscript range v[nl..nh] */
	79	{
	80	int *v;
	81
	82	v=(int )malloc((size_t) ((nh-nl+1+NR_END)sizeof(int)));
	83	if (!v) nrerror("allocation failure in ivector()");
	84	return v-nl+NR_END;
	85	}
	86
	87	double *vector(long nl,long nh)
	88	/* allocate a double vector with subscript range v[nl..nh] */
	89	{
	90	double *v;
	91
	92	v=(double )malloc((unsigned int) ((nh-nl+1+NR_END)sizeof(double)));
	93	if (!v) nrerror("allocation failure in vector()");
	94	return v-nl+NR_END;
	95	}
	96
	97	double **matrix(long nrl,long nrh,long ncl,long nch)
	98	/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
	99	{
	100	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	101	double **m;
	102
	103	/* allocate pointers to rows */
	104	m=(double *) malloc((unsigned int)((nrow+NR_END)sizeof(double*)));
	105	if (!m) nrerror("allocation failure 1 in matrix()");
	106	m += NR_END;
	107	m -= nrl;
	108
	109	/* allocate rows and set pointers to them */
	110	m[nrl]=(double ) malloc((unsigned int)((nrowncol+NR_END)*sizeof(double)));
	111	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	112	m[nrl] += NR_END;
	113	m[nrl] -= ncl;
	114
	115	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
	116
	117	/* return pointer to array of pointers to rows */
	118	return m;
	119	}
120
02f030e3	121	int gaussj(double a, int n, double b, int m)
6b8f6f6e	122	{
6b8f6f6e	123	int indxc,indxr,*ipiv;
43d89950	124	int i,icol=0,irow=0,j,k,l,ll;
6b8f6f6e	125	double big,dum,pivinv,swap;
	126
	127	indxc=ivector(1,n);
	128	indxr=ivector(1,n);
	129	ipiv=ivector(1,n);
	130	for (j=1;j<=n;j++) ipiv[j]=0;
	131	for (i=1;i<=n;i++) {
	132	big=0.0;
	133	for (j=1;j<=n;j++)
	134	if (ipiv[j] != 1)
	135	for (k=1;k<=n;k++) {
	136	if (ipiv[k] == 0) {
	137	if (fabs(a[j][k]) >= big) {
	138	big=fabs(a[j][k]);
	139	irow=j;
	140	icol=k;
	141	}
	142	} else if (ipiv[k] > 1) {
	143	free_ivector(ipiv,1,n);
	144	free_ivector(indxr,1,n);
	145	free_ivector(indxc,1,n);
	146	nrerror("gaussj: Singular Matrix-1");
02f030e3	147	return -1;
6b8f6f6e	148	}
	149	}
	150	++(ipiv[icol]);
	151	if (irow != icol) {
	152	for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
	153	for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
	154	}
	155	indxr[i]=irow;
	156	indxc[i]=icol;
	157	if (a[icol][icol] == 0.0){
	158	free_ivector(ipiv,1,n);
	159	free_ivector(indxr,1,n);
	160	free_ivector(indxc,1,n);
	161	nrerror("gaussj: Singular Matrix-2");
02f030e3	162	return -1;
6b8f6f6e	163	}
6b8f6f6e	164	if (mabs(a[icol][icol]) < EPSILON )
02f030e3	165	{
	166	nrerror("gaussj: a[icol][icol] == 0");
	167	return -1;
	168	}
6b8f6f6e	169	pivinv=1.0/a[icol][icol];
	170
	171	pivinv=1.0/a[icol][icol];
	172	a[icol][icol]=1.0;
	173	for (l=1;l<=n;l++) a[icol][l] *= pivinv;
	174	for (l=1;l<=m;l++) b[icol][l] *= pivinv;
	175	for (ll=1;ll<=n;ll++)
	176	if (ll != icol) {
	177	dum=a[ll][icol];
	178	a[ll][icol]=0.0;
	179	for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
	180	for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
	181	}
	182	}
	183	for (l=n;l>=1;l--) {
	184	if (indxr[l] != indxc[l])
	185	for (k=1;k<=n;k++)
	186	SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	187	}
	188	free_ivector(ipiv,1,n);
	189	free_ivector(indxr,1,n);
	190	free_ivector(indxc,1,n);
02f030e3	191	return 0;
6b8f6f6e	192	}
	193
	194	void covsrt(double **covar, int ma, int ia[], int mfit)
	195	{
	196	int i,j,k;
	197	double swap;
	198
	199	for (i=mfit+1;i<=ma;i++)
	200	for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
	201	k=mfit;
	202	for (j=ma;j>=1;j--) {
	203	if (ia[j]) {
	204	for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
	205	for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
	206	k--;
	207	}
	208	}
	209	}
	210
	211	void mrqcof(double x[], double y[], double sig[], int ndata, double a[], int ia[],
	212	int ma, double *alpha, double beta[], double chisq,
	213	void (funcs)(double, double [], double , double [], int))
	214	{
	215	int i,j,k,l,m,mfit=0;
	216	double ymod,wt,sig2i,dy,*dyda;
	217
	218	dyda=vector(1,ma);
	219
	220	for (j=1;j<=ma;j++)
	221	if (ia[j]) mfit++;
	222	for (j=1;j<=mfit;j++) {
	223	for (k=1;k<=j;k++) alpha[j][k]=0.0;
	224	beta[j]=0.0;
	225	}
	226	*chisq=0.0;
	227	for (i=1;i<=ndata;i++) {
	228	(*funcs)(x[i],a,&ymod,dyda,ma);
	229	sig2i=1.0/(sig[i]*sig[i]);
	230	dy=y[i]-ymod;
	231	for (j=0,l=1;l<=ma;l++) {
	232	if (ia[l]) {
	233	wt=dyda[l]*sig2i;
	234	for (j++,k=0,m=1;m<=l;m++)
	235	if (ia[m]) alpha[j][++k] += wt*dyda[m];
	236	beta[j] += dy*wt;
	237	}
	238	}
	239	chisq += dydy*sig2i;
	240	}
	241	for (j=2;j<=mfit;j++)
	242	for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
	243	free_vector(dyda,1,ma);
	244	}
	245
02f030e3	246	int mrqmin(double x[], double y[], double sig[], int ndata, double a[], int ia[],
6b8f6f6e	247	int ma, double covar, double alpha, double *chisq,
	248	void (funcs)(double, double [], double , double [], int), double *alamda)
	249	{
	250	void covsrt(double **covar, int ma, int ia[], int mfit);
02f030e3	251	int gaussj(double a, int n, double b, int m);
6b8f6f6e	252	void mrqcof(double x[], double y[], double sig[], int ndata, double a[],
	253	int ia[], int ma, double *alpha, double beta[], double chisq,
	254	void (funcs)(double, double [], double , double [], int));
	255	int j,k,l,m;
	256	static int mfit;
	257	static double ochisq,atry,beta,da,*oneda;
	258
	259	if (*alamda < 0.0) {
	260	atry=vector(1,ma);
	261	beta=vector(1,ma);
	262	da=vector(1,ma);
	263	for (mfit=0,j=1;j<=ma;j++)
	264	if (ia[j]) mfit++;
	265	oneda=matrix(1,mfit,1,1);
	266	*alamda=0.001;
	267	mrqcof(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
	268	ochisq=(*chisq);
	269	for (j=1;j<=ma;j++) atry[j]=a[j];
	270	}
	271	for (j=0,l=1;l<=ma;l++) {
	272	if (ia[l]) {
	273	for (j++,k=0,m=1;m<=ma;m++) {
	274	if (ia[m]) {
	275	k++;
	276	covar[j][k]=alpha[j][k];
	277	}
	278	}
	279	covar[j][j]=alpha[j][j](1.0+(alamda));
	280	oneda[j][1]=beta[j];
	281	}
	282	}
02f030e3	283	if(gaussj(covar,mfit,oneda,1)<0)
	284	{
	285	free_matrix(oneda,1,mfit,1,1);
	286	free_vector(da,1,ma);
	287	free_vector(beta,1,ma);
	288	free_vector(atry,1,ma);
	289	return -1;
	290	}
6b8f6f6e	291	for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
6b8f6f6e	292	if (*alamda == 0.0) {
02f030e3	293	covsrt(covar,ma,ia,mfit);
6b8f6f6e	294	free_matrix(oneda,1,mfit,1,1);
	295	free_vector(da,1,ma);
	296	free_vector(beta,1,ma);
	297	free_vector(atry,1,ma);
02f030e3	298	return 0;
6b8f6f6e	299	}
	300	for (j=0,l=1;l<=ma;l++)
	301	if (ia[l]) atry[l]=a[l]+da[++j];
	302	mrqcof(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
	303	if (*chisq < ochisq) {
	304	alamda = 0.1;
	305	ochisq=(*chisq);
	306	for (j=0,l=1;l<=ma;l++) {
	307	if (ia[l]) {
	308	for (j++,k=0,m=1;m<=ma;m++) {
	309	if (ia[m]) {
	310	k++;
	311	alpha[j][k]=covar[j][k];
	312	}
	313	}
	314	beta[j]=da[j];
	315	a[l]=atry[l];
	316	}
	317	}
	318	} else {
	319	alamda = 10.0;
	320	*chisq=ochisq;
	321	}
02f030e3	322	return 0;
6b8f6f6e	323	}
	324
	325
	326	int lev_marq_fit( double x[], double y[], double sig[], int NPT, double a[], int ia[], double dev[], int MA,
	327	double chisq_p, void (funcs)(double, double [], double *, double [], int) )
	328	{
	329	int i,itst,k;
	330	double alamda,chisq,ochisq,covar,alpha;
	331
	332	covar=matrix(1,MA,1,MA);
	333	alpha=matrix(1,MA,1,MA);
	334
	335	if( setjmp(env) == 0 ) {
	336	alamda = -1;
02f030e3	337	if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
	338	{
	339	free_matrix(alpha,1,MA,1,MA);
	340	free_matrix(covar,1,MA,1,MA);
	341	return -1;
	342	}
6b8f6f6e	343	k=1;
	344	itst=0;
	345	for (;;) {
	346	k++;
	347	ochisq=chisq;
02f030e3	348	if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
	349	{
	350	free_matrix(alpha,1,MA,1,MA);
	351	free_matrix(covar,1,MA,1,MA);
	352	return -1;
	353	}
6b8f6f6e	354	if (chisq > ochisq)
	355	itst=0;
	356	else if (fabs(ochisq-chisq) < 0.1)
	357	itst++;
	358	if (itst < 4) continue;
	359	alamda=0.0;
02f030e3	360	if(mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda)<0)
	361	{
	362	free_matrix(alpha,1,MA,1,MA);
	363	free_matrix(covar,1,MA,1,MA);
	364	return -1;
	365	}
6b8f6f6e	366	*chisq_p = chisq;
	367	for (i=1;i<=MA;i++)
	368	dev[i] = sqrt(covar[i][i]);
	369	break;
	370	}
	371	free_matrix(alpha,1,MA,1,MA);
	372	free_matrix(covar,1,MA,1,MA);
	373	return 0;
	374	}
	375	else {
a3282264	376	/if( control_g.print_fit_errors==2 )/
6b8f6f6e	377	fprintf( stderr, " runtime error\n" );
6b8f6f6e	378
02f030e3	379	free_matrix(alpha,1,MA,1,MA);
6b8f6f6e	380	free_matrix(covar,1,MA,1,MA);
	381	return -1;
	382	}
	383	}
	384