#include <stdio.h>
#include <math.h>
#ifdef  HAVE_UNISTD_H
#include <unistd.h>
#endif
#include "dalloca.h"
#include "head.h"

#define ERRR (-1)

static int mrqcof(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **alpha, double *beta, double *chisq);
static int gaussj(double **a, int n, double **b, int m);
static int covsrt(double **covar, int ma, int *lista, int mfit);


int Ft_mrqmin(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **covar, double **alpha, double *chisq, double *alamda)
{
	int k,kk,j,ihit;
	static double *da = 0;
	static double *aold = 0;
	static double **oneda = 0;
	static double *beta = 0;
	static int sma = 0;
	static int smfit = 0;
	static double ochisq;
	double *Ft_dvector(int nl, int nh);
	double **Ft_dmatrix(int nrl, int nrh, int ncl, int nch);
	void Ft_free_dmatrix(double **m, int nrl, int nrh, int ncl, int nch);
	void Ft_free_dvector(double *v, int nl, int nh);

	if (*alamda < 0.0) {  /* clean and allocate on start */
		if (oneda)
			Ft_free_dmatrix(oneda, 1, smfit, 1, 1);  oneda = 0;
		if (aold)
			Ft_free_dvector(aold, 1, sma); aold = 0;
		if (da)
			Ft_free_dvector(da, 1, sma); da = 0;
		if (beta)
			Ft_free_dvector(beta, 1, sma); beta = 0;
		sma = ma;
		smfit = mfit;
		if ((oneda=Ft_dmatrix(1,mfit,1,1)) == NULL) {
			fprintf(stderr, "mrqmin: Allocation error.\n");
			Ft_catcher(ERRR);
		}
		if ((aold=Ft_dvector(1,ma)) == NULL) {
			fprintf(stderr, "mrqmin: Allocation error.\n");
			Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
			Ft_catcher(ERRR);
		}
		if ((da=Ft_dvector(1,ma)) == NULL) {
			Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
			Ft_free_dvector(aold, 1, ma); aold = 0;
			fprintf(stderr, "mrqmin: Allocation error.\n");
			Ft_catcher(ERRR);
		}
		if ((beta=Ft_dvector(1,ma)) == NULL) {
			Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
			Ft_free_dvector(aold, 1, ma); aold = 0;
			Ft_free_dvector(da, 1, ma); da = 0;
			fprintf(stderr, "mrqmin: Allocation error.\n");
			Ft_catcher(ERRR);
		}
		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) {
				fprintf(stderr, "mrqmin-1: Bad Lista permutation.\n");
				Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
				Ft_free_dvector(aold, 1, ma); aold = 0;
				Ft_free_dvector(da, 1, ma); da = 0;
				Ft_free_dvector(beta, 1, ma); beta = 0;
				Ft_catcher(ERRR);
			}
		}
		if (kk != ma+1) {
			fprintf(stderr, "mrqmin-2: Bad Lista permutation.\n");
			Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
			Ft_free_dvector(aold, 1, ma); aold = 0;
			Ft_free_dvector(da, 1, ma); da = 0;
			Ft_free_dvector(beta, 1, ma); beta = 0;
			Ft_catcher(ERRR);
		}
		*alamda=0.001;
		if (mrqcof(coef,f,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq)
		== ERRR) { /* first shut */
			Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
			Ft_free_dvector(aold, 1, ma); aold = 0;
			Ft_free_dvector(da, 1, ma); da = 0;
			Ft_free_dvector(beta, 1, ma); beta = 0;
			Ft_catcher(ERRR);
		}
		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];
	}
	if (gaussj(covar,mfit,oneda,1) == ERRR) {
		Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
		Ft_free_dvector(aold, 1, ma); aold = 0;
		Ft_free_dvector(da, 1, ma); da = 0;
		Ft_free_dvector(beta, 1, ma); beta = 0;
		Ft_catcher(ERRR);
	}
	for (j=1;j<=mfit;j++)
		da[j]=oneda[j][1];
	if (*alamda == 0.0) {  /* get error and clean up */
		covsrt(covar,ma,lista,mfit);
		Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
		Ft_free_dvector(aold, 1, ma); aold = 0;
		Ft_free_dvector(da, 1, ma); da = 0;
		Ft_free_dvector(beta, 1, ma); beta = 0;
		return(0);
	}
	for (j=1;j<=ma;j++) aold[j]=a[j];
	for (j=1;j<=mfit;j++)
		a[lista[j]] += da[j];
	if (mrqcof(coef,f,y,sig,ndata,a,ma,lista,mfit,covar,da,chisq)
	== ERRR) {
		Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
		Ft_free_dvector(aold, 1, ma); aold = 0;
		Ft_free_dvector(da, 1, ma); da = 0;
		Ft_free_dvector(beta, 1, ma); beta = 0;
		Ft_catcher(ERRR);
	}
	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];
		}
	} else {
		for (j=1;j<=ma;j++) a[j]=aold[j];
		*alamda *= 10.0;
		*chisq=ochisq;
	}
	return(0);
}

static int mrqcof(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **alpha, double *beta, double *chisq)
{
	int i, j, k;
	double wt, sig2i, dy;
	void Ft_fml_fit(int ndata);

	for (j=1;j<=mfit;j++) {
		for (k=1;k<=j;k++)
			alpha[j][k]=0.0;
		beta[j]=0.0;
	}
	*chisq=0.0;
	Ft_fml_fit(ndata);
	for (i=1;i<=ndata;i++) {
		sig2i=1.0/(sig[i]*sig[i]);
		dy=y[i]-f[i];
		for (j=1;j<=mfit;j++) {
			wt=coef[lista[j]][i]*sig2i;
			for (k=1;k<=j;k++)
				alpha[j][k] += wt*coef[lista[k]][i];
			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];
	return(0);
}

static int covsrt(double **covar, int ma, int *lista, int mfit)
{
	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];

	return(0);
}

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

static int gaussj(double **a, int n, double **b, int m)
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll;
	double big,dum,pivinv;

	AIVECTOR(indxc, 1, n, "gaussj", return);
	AIVECTOR(indxr, 1, n, "gaussj", return);
	AIVECTOR(ipiv, 1, n, "gaussj", return);

	icol = irow = 1;
	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) {
						fprintf(stderr, "gaussj: Singular Matrix 1.\n");
						return(ERRR);
					}
				}
		++(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) {
			fprintf(stderr, "gaussj: Singular Matrix 2.\n");
			return(ERRR);
		}
		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]]);
	}
	return(0);
}

#undef SWAP

int Ft_lfit(double **coef, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **covar, double *chisq)
{
	int k,kk,j,ihit,i;
	double ym,wt,sum,sig2i,**beta;
	double tmp;

	ADMATRIX(beta, 1, ma, 1, 1, "lfit", Ft_catcher);
	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) {
			fprintf(stderr, "lfit: Bad 'adjust' permutation-1.");
			Ft_catcher(ERRR);
		}
	}
	if (kk != (ma+1)) {
		fprintf(stderr, "lfit: Bad 'adjust' permutation-2.");
		Ft_catcher(ERRR);
	}
	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<=ndata;i++) {
		ym=y[i];
		if (mfit < ma)
			for (j=(mfit+1);j<=ma;j++)
				ym -= a[lista[j]]*coef[lista[j]][i];
		sig2i=1.0/(sig[i]*sig[i]);
		for (j=1;j<=mfit;j++) {
			wt=coef[lista[j]][i]*sig2i;
			for (k=1;k<=j;k++)
				covar[j][k] += wt*coef[lista[k]][i];
			beta[j][1] += ym*wt;
		}
	}
	if (mfit > 1)
		for (j=2;j<=mfit;j++)
			for (k=1;k<=j-1;k++)
				covar[k][j]=covar[j][k];
	if (gaussj(covar,mfit,beta,1) == ERRR) {
		Ft_catcher(ERRR);
	}
	for (j=1;j<=mfit;j++) a[lista[j]]=beta[j][1];
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*coef[j][i];
		tmp = (y[i]-sum)/sig[i];
		*chisq += tmp*tmp;
	}
	covsrt(covar,ma,lista,mfit);
	return(0);
}

