#include <stdio.h>
#include <math.h>
#ifndef NOUNISTD_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);
}