/*  mrqmin.c

    This is a roughly self-contained code module for
    Levenberg-Marquardt gradient descent iterative minimization
    of a nonlinear function.
    This started with the Numerical Recipes in C code, but uses the
    m_elem data type defined in matmath.h throught in place of floats.

    Parts of this code Copr. 1986-92 Numerical Recipes Software 2.1.9-153.

    J. Watlington, 12/8/95

    Modified:
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "my_math.h"
#include "my_struct.h"

/*  Prototypes of internal functions   */

//void 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));


void mrqcof_xy(double x[], double x1[], 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, struct MODPAR modpar[], struct PSF *psf), 
	    struct MODPAR modpar[], struct PSF *psf);


void mrqcof_psf(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, double *, int, int), 
	    double *psf, int nx_psf, int ny_psf);

void covsrt(double **covar, int ma, int ia[], int mfit);

int par_limits(struct MODPAR *modpar, int n_mod);
int write_par(double *a, struct MODPAR *modpar, int n_mod);
int read_par(double *a, int *ia, struct MODPAR *modpar, int n_mod);
int n_par(struct MODPAR *modpar, int n_mod);
int print_par(struct MODPAR *modpar, int n_mod);

void my_mrqcof_xy(double x[], double x1[], double y[], double sig[], int ndata, 
		  double **alpha, double beta[], double *chisq,
	    void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma), 
		  struct MODPAR modpar[], int n_mod);

void my_mrqmin_xy( double x[], double x1[], double y[], double sig[], int ndata, 
		   double **covar, double **alpha, double *chisq,
		   void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma),
		   double *alamda, struct MODPAR modpar[], int n_mod);


void my_mrqcof_back(double x[], double x1[], double y[], double sig[], int ndata, 
		  double **alpha, double beta[], double *chisq,
	    void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma, struct BACKGROUND back[], int n_back), 
		  struct MODPAR modpar[], int n_mod, struct BACKGROUND back[], int n_back);

void my_mrqmin_back( double x[], double x1[], double y[], double sig[], int ndata, 
		   double **covar, double **alpha, double *chisq,
		   void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma, struct BACKGROUND back[], int n_back),
		   double *alamda, struct MODPAR modpar[], int n_mod, struct BACKGROUND back[], int n_back);



//void function(double x, double y, double *flux, double dp[], struct MODPAR modpar[],int n_mod, int ma);

int n_par(struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    i=1;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    i++;
		}
	    }
	}
    }
    return(i-1);
}


int par_limits(struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    if (modpar[k].a[j]<modpar[k].p_a[j]) {
			modpar[k].a[j]=modpar[k].p_a[j];
		    }
		    if (modpar[k].a[j]>modpar[k].p_a2[j]) {
			modpar[k].a[j]=modpar[k].p_a2[j];
		    }
		} else {			    
		    if (modpar[k].p_a2[j]==0) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]+modpar[k].p_a[j]; 		    
		    } 
		    if (modpar[k].p_a2[j]==1) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]*modpar[k].p_a[j]; 		    
		    } 
		}
	    }
	}
    }
    return(0);
}


int print_par(struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    
    i=1;
    for (k=0;k<n_mod;k++) {
	printf("\n%s: ",modpar[k].model);
//	for(j=1;j<=modpar[k].na;j++) {
	for(j=1;j<=4;j++) {
//	    printf(" %4.2f+-%4.2f",modpar[k].a[j],modpar[k].p_a[j]);
//	    printf(" %4.4f+-%4.4f",modpar[k].a[j],modpar[k].p_a[j]);
	    printf(" %4.4f+-%4.4f",modpar[k].a[j],modpar[k].e_a[j]);
	}
    }
    printf("\n");
    return(0);
}

int read_par(double a[], int ia[], struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    i=1;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    a[i]=modpar[k].a[j];
		    ia[i]=modpar[k].ia[j];
		    i++;
		}
	    }
	}
    }
    return(0);
}



int write_par(double a[], struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    i=1;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    modpar[k].a[j]=a[i];
		    i++;
		} else {			    

		    if (modpar[k].p_a2[j]==0) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]+modpar[k].p_a[j]; 		    
		    } 
		    if (modpar[k].p_a2[j]==1) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]*modpar[k].p_a[j]; 		    
		    } 
//		    modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]+modpar[k].p_a[j]; 		    
		}
	    }
	}
    }
    return(0);
}


int write_error_par(double **covar, struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    i=1;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    modpar[k].e_a[j]=covar[i][i];
		    i++;
		} else {			    
		    if (modpar[k].p_a2[j]==0) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]+modpar[k].p_a[j]; 		    
		    } 
		    if (modpar[k].p_a2[j]==1) {
			modpar[k].a[j]=modpar[modpar[k].fixa[j]-1].a[j]*modpar[k].p_a[j]; 		    
		    } 

		}
	    }
	}
    }
    return(0);
}




int write_only_error_par(double **covar, struct MODPAR *modpar, int n_mod) {
    int k,i,j;
    i=1;
    for (k=0;k<n_mod;k++) {
	for(j=1;j<=modpar[k].na;j++) {
	    if (modpar[k].ia[j]==1) {
		if (modpar[k].fixa[j]==-1) {
		    modpar[k].e_a[j]=covar[i][i];//*0.33;
		    i++;
		} else {			    
		    if (modpar[k].p_a2[j]==0) {
			modpar[k].e_a[j]=modpar[modpar[k].fixa[j]-1].e_a[j];
		    } 
		    if (modpar[k].p_a2[j]==1) {
			modpar[k].e_a[j]=modpar[modpar[k].fixa[j]-1].e_a[j];
		    } 
		}
	    } else {
		modpar[k].e_a[j]=0.0;
	    }
	}
    }
    return(0);
}




/*
 * Modified MRQMIN, to include 2D functions F=F(X,Y,
*/

void my_mrqmin_xy( double x[], double x1[], double y[], double sig[], int ndata, 
		   double **covar, double **alpha, double *chisq,
		   void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma),
		   double *alamda, struct MODPAR modpar[], int n_mod)
{
  int j,k,l,m;
  static int mfit;
  static double ochisq,*atry,*beta,*da,**oneda;
  double *a;
  int *ia;
  int ma,ndim;
  int singular=0;


  ma=n_par(modpar,n_mod);
  a=dvector(1,ma);
  ia=ivector(1,ma);
  read_par(a,ia,modpar,n_mod);


  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;    
    write_par(a,modpar,n_mod);
    my_mrqcof_xy(x,x1,y,sig,ndata,alpha,beta,chisq,funcs,modpar,n_mod);    
    par_limits(modpar,n_mod);
    read_par(a,ia,modpar,n_mod);
    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];
    }
  }


  singular=gaussj(covar,mfit,oneda,1);

  if (singular<0) {

      free_matrix(oneda,1,mfit,1,1);
      free_vector(da,1,ma);
      free_vector(beta,1,ma);
      free_vector(atry,1,ma);
      free_vector(a,1,ma);
      free_ivector(ia,1,ma);

      

      *alamda=-666;
      return;
  }

  for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
  if (*alamda < 0.00001) {
    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);
    write_par(a,modpar,n_mod);
    free_vector(a,1,ma);
    free_ivector(ia,1,ma);

    return;
  }

  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) atry[l]=a[l]+da[++j];
    write_par(atry,modpar,n_mod);
    par_limits(modpar,n_mod);
    my_mrqcof_xy(x,x1,y,sig,ndata,alpha,beta,chisq,funcs,modpar,n_mod);    

    read_par(atry,ia,modpar,n_mod);
    if (*chisq < ochisq) {
	*alamda *= 0.1;
//	*alamda *= 0.05;
	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;
    }
    write_par(a,modpar,n_mod);
    free_matrix(oneda,1,mfit,1,1);
    free_vector(da,1,ma);
    free_vector(beta,1,ma);
    free_vector(atry,1,ma);
    free_dvector(a,1,ma);
    free_ivector(ia,1,ma);

}

/*
 * Modified MRQCOF to take in account F=F(X,Y) functions
 *
 */

void my_mrqcof_xy(double x[], double x1[], double y[], double sig[], int ndata, 
		  double **alpha, double beta[], double *chisq,
	    void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma), 
	    struct MODPAR modpar[], int n_mod)
{
  int i,j,k,l,m,mfit=0;
  double ymod,wt,sig2i,dy,*dyda;
  double *a;
  int *ia;
  int ma,ndim;

  ma=n_par(modpar,n_mod);
  a=dvector(1,ma);
  ia=ivector(1,ma);
  read_par(a,ia,modpar,n_mod);



  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],x1[i],&ymod,dyda,modpar,n_mod,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;
    exit;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_vector(dyda,1,ma);
  free_dvector(a,1,ma);
  free_ivector(ia,1,ma);
}



/*
 * Modified MRQMIN, to include 2D functions F=F(X,Y,
*/

void mrqmin_xy( double x[], double x1[], 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, struct MODPAR modpar[], struct PSF *psf),
	    double *alamda, struct MODPAR modpar[], struct PSF *psf)
{
  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_xy(x,x1,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs,modpar,psf);    
    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];
    }
  }
  gaussj(covar,mfit,oneda,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;
  }
  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) atry[l]=a[l]+da[++j];
  mrqcof_xy(x,x1,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs,modpar,psf);
  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;
  }
}

/*
 * Modified MRQCOF to take in account F=F(X,Y) functions
 *
 */

void mrqcof_xy(double x[], double x1[], 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, struct MODPAR modpar[], struct PSF *psf), 
	    struct MODPAR modpar[], struct PSF *psf)
{
  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++) {
    psf->flag=i;
    (*funcs)(x[i],x1[i],a,&ymod,dyda,ma,modpar,psf);
    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;
    exit;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_vector(dyda,1,ma);
}


/*  mrqmin
    This is THE function.  If called with a negative alamda, it initializes.
    It should be called repeatedly without modifying alamda until it converges.
    Then it may be called one final time with alamda set to zero to obtain the
    final covariances.

    See NR in C, pp. 683 - 688 for real documentation, and don't forget to
    read the note about multi-dimensional use on p. 680.
*/

void 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)
{
  int j,k,l,m;
  static int mfit;
  static double ochisq,*atry,*beta,*da,**oneda;

//  printf("alamda=%f\n",*alamda);
  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;
    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=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];
  }





  gaussj(covar,mfit,oneda,1);




  for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
  if (*alamda == 0.0) {
      covsrt(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];
      }
  }


  mrqcof(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
  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];
      }
      for (l=1;l<=ma;l++) a[l]=atry[l];
  } else {
      *alamda *= 10.0;
      *chisq=ochisq;
  }

}

/*   mrqcof
     Used by mrqmin to evaluate the linearized fitting matrix (alpha) and
     vector( beta) and calculate chi squared.
*/
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);
    //    printf("%g, %g, %g\n",ymod,sig[i],dyda[1]);
    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;
    exit;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_vector(dyda,1,ma);
}

/*  covsrt
    This function expands and contracts the covariance matrix
    to account for the fact that there may be components of the
    state which we may hold constant and not optimize.
    */
#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

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--;
    }
  }
}

#undef SWAP


/****************************************************************
 * PSF convolution
 ****************************************************************/

void mrqmin_psf( 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 *, int , int),
	    double *alamda, double *psf, int nx_psf, int ny_psf)
{
  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_psf(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs,psf,nx_psf,ny_psf);
    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];
    }
  }
  gaussj(covar,mfit,oneda,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;
  }
  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) atry[l]=a[l]+da[++j];
  mrqcof_psf(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs,psf,nx_psf,ny_psf);
  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;
  }
}

/*   mrqcof_psf
     Used by mrqmin to evaluate the linearized fitting matrix (alpha) and
     vector( beta) and calculate chi squared.
*/
void mrqcof_psf(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, double *, int , int), 
	    double *psf, int nx_psf, int ny_psf)
{
  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,psf,nx_psf,ny_psf);
    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;
    exit;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_vector(dyda,1,ma);
}


/*
 * New minimization routines to include the background fitting
 *
 */


void my_mrqmin_back( double x[], double x1[], double y[], double sig[], int ndata, 
		   double **covar, double **alpha, double *chisq,
		   void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma, struct BACKGROUND back[], int n_back),
		   double *alamda, struct MODPAR modpar[], int n_mod , struct BACKGROUND back[], int n_back)
{
  int j,k,l,m;
  static int mfit;
  static double ochisq,*atry,*beta,*da,**oneda;
  double *a;
  int *ia;
  int ma,ndim;
  int singular=0;



  ma=n_par(modpar,n_mod);
  a=dvector(1,ma);
  ia=ivector(1,ma);
  read_par(a,ia,modpar,n_mod);



  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;    
    write_par(a,modpar,n_mod);
//    printf("PASO1 AA %d %d\n",n_mod,n_back);
//    fflush(stdout);
    my_mrqcof_back(x,x1,y,sig,ndata,alpha,beta,chisq,funcs,modpar,n_mod,back,n_back);
    fflush(stdout);
    par_limits(modpar,n_mod);
    read_par(a,ia,modpar,n_mod);
    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];
    }
  }



  singular=gaussj(covar,mfit,oneda,1);

//  printf("PASO NEW 4\n");
//  fflush(stdout);

  if (singular<0) {

      free_matrix(oneda,1,mfit,1,1);
      free_vector(da,1,ma);
      free_vector(beta,1,ma);
      free_vector(atry,1,ma);
      free_vector(a,1,ma);
      free_ivector(ia,1,ma);

      

      *alamda=-666;
      return;
  }

  for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
  if (*alamda < 0.00001) {
    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);
    write_par(a,modpar,n_mod);
    free_vector(a,1,ma);
    free_ivector(ia,1,ma);

    return;
  }

  for (j=0,l=1;l<=ma;l++)
    if (ia[l]) atry[l]=a[l]+da[++j];
    write_par(atry,modpar,n_mod);
    par_limits(modpar,n_mod);
    my_mrqcof_back(x,x1,y,sig,ndata,alpha,beta,chisq,funcs,modpar,n_mod,back,n_back);    

    read_par(atry,ia,modpar,n_mod);
    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;
    }
    write_par(a,modpar,n_mod);
    free_matrix(oneda,1,mfit,1,1);
    free_vector(da,1,ma);
    free_vector(beta,1,ma);
    free_vector(atry,1,ma);
    free_dvector(a,1,ma);
    free_ivector(ia,1,ma);

}

/*
 * Modified MRQCOF to take in account F=F(X,Y) functions
 *
 */

void my_mrqcof_back(double x[], double x1[], double y[], double sig[], int ndata, 
		  double **alpha, double beta[], double *chisq,
	    void (*funcs)(double, double, double *, double [], struct MODPAR modpar[], int n_mod, int ma, struct BACKGROUND back[], int n_back), 
	    struct MODPAR modpar[], int n_mod, struct BACKGROUND back[], int n_back)
{
  int i,j,k,l,m,mfit=0;
  double ymod,wt,sig2i,dy,*dyda;
  double *a;
  int *ia;
  int ma,ndim;

  ma=n_par(modpar,n_mod);
  a=dvector(1,ma);
  ia=ivector(1,ma);
  read_par(a,ia,modpar,n_mod);


//  printf(".");
//  fflush(stdout);
  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++) {
//      printf("PASO\n");
  fflush(stdout);
//      printf("PASO\n");
    (*funcs)(x[i],x1[i],&ymod,dyda,modpar,n_mod,ma,back,n_back);
//    printf("PASO %d %f %f\n",i,x[i],x1[i]);
    fflush(stdout);
    if (sig[i]!=0) {
	sig2i=1.0/(sig[i]*sig[i]);
    } else {
	sig2i=1.0;
    }

    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;
    exit;
  }
  for (j=2;j<=mfit;j++)
    for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
  free_vector(dyda,1,ma);
  free_dvector(a,1,ma);
  free_ivector(ia,1,ma);


}

