C+
C NAME:
C	lm_fit
C PURPOSE:
C	To make a Levenberg-Marquardt fit of a simple function.
C
C CATEGORY:
C	Data processing
C CALLING SEQUENCE:
C	run lm_fit
C INPUTS:
C	Main program
C OUTPUTS:
C	Fits parameters and errors of the function fit
C FUNCTIONS/SUBROUTINES:
C	mrqmin.f
C INCLUDE:
C COMMON BLOCKS:
C SIDE EFFECTS:
C RESTRICTIONS:
C PROCEDURE:
C	Latest 10/11/13 work
C	logical $CAM.
C MODIFICATION HISTORY:
C	October-2013, Bernard Jackson (UCSD)
C-

	program lm_fit
C
	parameter      (NdataM	=  50,		! Max # data points
     &			MAM	=  2,		! Max # of coefficients
     &			MfitM	=  2)		! Max # of coefficients fit
C
	real		X	(NdataM),	! Values of numbers input (spectral values)
     &			Xin	(NdataM),	! Input set of numbers (same as above but passed to mrqmin)
     &			Ydat	(NdataM),	! Data values at each value of Xin
     &			Y	(NdataM),	! The function values
     &			Sig	(NdataM),	! Individual standard deviations of the values fit
     &			A	(MAM),		! Coefficients to be fit
     &			Covar	(MfitM,MfitM),	! scratch array	
     &			Alpha	(MfitM,MfitM)	! scratch array
C
	integer		ListA	(MAM)
	external	Funcs			!  Put in to declare an external function by B. Jackson - needed here

	Ndata 	= 10				! # of actual data points
	MA 	= MAM				! # of coefficients
	Mfit	= MfitM				! # number of coeficients fit
	NCA	= Mfit				! a # greater or equal to Mfit 
C	Mfit = 1
C
C Set up the data points to be fit by Levenberg-Marquardt.
C
	a1 = 1.0				! This is the actual value of A(1)
	b1 = 2.0				! This is the actual value of A(2)

	do i=1,Ndata
	  X(i)    = float(i)
	  Xin(i) = X(i)
	  Ydat(i) = a1*X(i)**2 + b1
C	  Sig(i) = 1.0
	  Sig(i) = 1.0*X(i)**2
C	  Sig(i) = 0.5*X(i)**2
	end do
C	Ydat(2) = 4.0
	Ydat(4) = 16.0
	Ydat(8) = 68.0
C	Ydat(9) = 79.0
	Lista(1) = 1
	Lista(2) = 2
C
C End of set up of points to be fit.
C
C Guess the values of the coefficients.
C
	A(1)	= 1.3
	A(2)	= 1.8
	Alamda	= -1.0
C
C Call the Minimization
C
	write(*,'(A,12F6.2)') 'Xin     =',(Xin(I),I=1,12) 
	Chisqold = 15.0
	Chisq	 = 10.0
	do i=1,15
	  write(*,'(A,I5,2F15.4,F15.10)') 'Enter Chisqold, Chisq  =',i, Chisqold, Chisq, Chisq-Chisqold	
	  if((Chisq-Chisqold).lt.-0.001.or.(Chisq-Chisqold).gt.0.0) then
 	    print *, 'Outside mngmin'
	    Chisqold = Chisq	    
	    call mrqmin(Xin,Ydat,Sig,Ndata,A,MA,ListA,Mfit,Covar,Alpha,NCA,Chisq,Funcs,Alamda)
	    print *, '  '
	    print *, 'Chisqold, Chisq  =',Chisqold, Chisq
	    print *, 'Alamda =',Alamda
C	    write(*,'(A,12F6.2)') 'Ydat  =',(Ydat(J),J=1,12) 
C	    write(*,'(A,12F6.2)') 'Xin   =',(Xin (J),J=1,12) 
	  end if
	end do
	Alamda = 0.0
	print *, '  '
	print *, 'Outside last mrqmin'
	call mrqmin(Xin,Ydat,Sig,Ndata,A,MA,ListA,Mfit,Covar,Alpha,NCA,Chisq,Funcs,Alamda) 
	print *, 'Program sucessfully completed'
	write(*,'(A,2F10.5)') ' Final values of A     =', A(1),A(2)
	write(*,'(A,4F10.5)') ' Final values of Covar =', Covar(1,1),Covar(1,2),Covar(2,1),Covar(2,2)
	stop
	end
C************************************************************************************************
	subroutine Funcs(X,A,Xmod,Dxda,NA)
C
	real	A	(NA),
     &		Dxda	(NA)
	real    er	(NA),
     &		AA	(20)
C
	call formula(X,A,Xmod,NA)
C	Xmod = A(1)*X**2 + A(2)
C
C Determine A derivatives
C
	er(1) = 0.001
	er(2) = 0.001
C
	call derivatives(X,A,Dxda,er,NA)
C
	write(*,'(A,5F12.4)') 'Inside Funcs 1', Xmod, A(1), A(2), Dxda(1), Dxda(2)
C
C	Dxda(1) = X**2
C	Dxda(2) = 1.0
C end A derivative determination
C
C	write(*,'(A,5F12.4)') 'Inside Funcs 2', Xmod, A(1), A(2), Dxda(1), Dxda(2)
C
	return
	end
C
C**************************************************************************************************
C
	subroutine formula(X,A,Xmod,NA)
C
	real	A	(NA)
C
C The formula that fits the data
C
	  Xmod = A(1)*X**2 + A(2)
C
	return
	end
C
C**************************************************************************************************
C
	subroutine derivatives(X,A,Dxda,er,NA)
C
	real	A	(NA),
     &		Dxda	(NA)
	real    er	(NA),
     &		AA	(20)
C
C The derivatives
C
C	Dxda(1) = X**2
C	Dxda(2) = 1.0
C

	do i = 1, NA
	  do j = 1, 20
	    AA(j) = A(j)
          end do
	  AA(i) = A(i) + er(i)
	  call formula(X,AA,XmodAP,NA)
	  AA(i) = A(i) - er(i)
	  call formula(X,AA,XmodAM,NA)
	  Dxda(i) = (XmodAP-XmodAM)/(2.0*er(i))
	end do
C
	return
	end
C
C  ***********************************************************************************************************
C
      SUBROUTINE MRQMIN(X,Y,SIG,NDATA,A,MA,LISTA,MFIT,
     *    COVAR,ALPHA,NCA,CHISQ,FUNCS,ALAMDA)
      PARAMETER (MMAX=20)
      DIMENSION X(NDATA),Y(NDATA),SIG(NDATA),A(MA),LISTA(MA),
     *  COVAR(NCA,NCA),ALPHA(NCA,NCA),ATRY(MMAX),BETA(MMAX),DA(MMAX)
      external FUNCS								! put in to declare external by B. Jackson - not needed
      save OCHISQ								! absolutely needed, otherwise chisquare not remembered
C
C	print *, 'Inside MRQMIN', Alamda,A
      IF(ALAMDA.LT.0.0)THEN
        KK=MFIT+1
        DO 12 J=1,MA
          IHIT=0
          DO 11 K=1,MFIT
            IF(LISTA(K).EQ.J)IHIT=IHIT+1
11        CONTINUE
          IF (IHIT.EQ.0) THEN
            LISTA(KK)=J
            KK=KK+1
          ELSE IF (IHIT.GT.1) THEN
            PAUSE 'Improper permutation in LISTA'
          ENDIF
12      CONTINUE
        IF (KK.NE.(MA+1)) PAUSE 'Improper permutation in LISTA'
        ALAMDA=0.001
        CALL MRQCOF(X,Y,SIG,NDATA,A,MA,LISTA,MFIT,ALPHA,BETA,NCA,CHISQ,FUNCS)
        OCHISQ=CHISQ
        DO 13 J=1,MA
          ATRY(J)=A(J)
13      CONTINUE
      ENDIF
	write(*,'(A,I3,4E12.4)'),'1st MFIT,ALPHA',MFIT,  ALPHA(1,1),ALPHA(1,2),ALPHA(2,1),ALPHA(2,2)
	write(*,'(A,F10.7,2E12.5,F15.7)'),' Alamda, BETA, CHISQ',Alamda,BETA(1),BETA(2),CHISQ
      DO 15 J=1,MFIT
        DO 14 K=1,MFIT
          COVAR(J,K)=ALPHA(J,K)
14      CONTINUE
        COVAR(J,J)=ALPHA(J,J)*(1.+ALAMDA)
        DA(J)=BETA(J)
15    CONTINUE
      CALL GAUSSJ(COVAR,MFIT,NCA,DA,1,1)
      IF(ALAMDA.EQ.0.)THEN
        CALL COVSRT(COVAR,NCA,MA,LISTA,MFIT)
        RETURN
      ENDIF
      DO 16 J=1,MFIT
        ATRY(LISTA(J))=A(LISTA(J))+DA(J)
16    CONTINUE
      print *, ' '
      write(*,'(A,4E15.7)') 'Initial A     ',A(1),A(2),A(3),A(4)
      print *, ' '
      write(*,'(A,4E15.7)') 'New try with A',ATRY(1),ATRY(2),ATRY(3),ATRY(4)
      print *, ' '
      CALL MRQCOF(X,Y,SIG,NDATA,ATRY,MA,LISTA,MFIT,COVAR,DA,NCA,CHISQ,FUNCS)
      IF(CHISQ.LT.OCHISQ)THEN
        ALAMDA=0.1*ALAMDA
        OCHISQ=CHISQ
        DO 18 J=1,MFIT
          DO 17 K=1,MFIT
            ALPHA(J,K)=COVAR(J,K)
17        CONTINUE
          BETA(J)=DA(J)
          A(LISTA(J))=ATRY(LISTA(J))
18      CONTINUE
	write(*,'(A,I11,4F12.4)'),'2nd MFIT,ALPHA',MFIT,  ALPHA(1,1),ALPHA(1,2),ALPHA(2,1),ALPHA(2,2)
	write(*,'(A,4F12.5)'),' Alamda, BETA, CHISQ',Alamda,BETA(1),BETA(2),CHISQ
      ELSE
        ALAMDA=10.*ALAMDA
        CHISQ=OCHISQ
      ENDIF
      RETURN
      END
C
C  ***********************************************************************************************
C
      SUBROUTINE MRQCOF(X,Y,SIG,NDATA,A,MA,LISTA,MFIT,ALPHA,BETA,NALP,
     *CHISQ,FUNCS)
      PARAMETER (MMAX=20)
      DIMENSION X(NDATA),Y(NDATA),SIG(NDATA),ALPHA(NALP,NALP),BETA(MA),
     *    DYDA(MMAX),LISTA(MFIT),A(MA)
      DO 12 J=1,MFIT
        DO 11 K=1,J
          ALPHA(J,K)=0.0
11      CONTINUE
        BETA(J)=0.0
12    CONTINUE
      CHISQ=0.
      DO 15 I=1,NDATA

C	write(*,'(A,4E15.7)') 'before funcs',A(1), A(2), A(3), A(4)

        CALL FUNCS(X(I),A,YMOD,DYDA,MA)
C	print *, I, X(I), A(1), A(2), Y(I), YMOD
        SIG2I=1./(SIG(I)*SIG(I))
        DY=Y(I)-YMOD
        DO 14 J=1,MFIT
          WT=DYDA(LISTA(J))*SIG2I
          DO 13 K=1,J
            ALPHA(J,K)=ALPHA(J,K)+WT*DYDA(LISTA(K))
13        CONTINUE
          BETA(J)=BETA(J)+DY*WT
14      CONTINUE
        CHISQ=CHISQ+DY*DY*SIG2I
	write(*,'(A,I4,E12.2,F10.4,E15.4,F10.4,E15.4)'), 'In mrqcof', I, A(1),A(2),DY, SIG2I, CHISQ
15    CONTINUE
      DO 17 J=2,MFIT
        DO 16 K=1,J-1
          ALPHA(K,J)=ALPHA(J,K)
16      CONTINUE
17    CONTINUE
      RETURN
      END
C
C  ***********************************************************************************************
C
      SUBROUTINE COVSRT(COVAR,NCVM,MA,LISTA,MFIT)
      DIMENSION COVAR(NCVM,NCVM),LISTA(MFIT)
      DO 12 J=1,MA-1
        DO 11 I=J+1,MA
          COVAR(I,J)=0.0
11      CONTINUE
12    CONTINUE
      DO 14 I=1,MFIT-1
        DO 13 J=I+1,MFIT
          IF(LISTA(J).GT.LISTA(I)) THEN
            COVAR(LISTA(J),LISTA(I))=COVAR(I,J)
          ELSE
            COVAR(LISTA(I),LISTA(J))=COVAR(I,J)
          ENDIF
13      CONTINUE
14    CONTINUE
      SWAP=COVAR(1,1)
      DO 15 J=1,MA
        COVAR(1,J)=COVAR(J,J)
        COVAR(J,J)=0.0
15    CONTINUE
      COVAR(LISTA(1),LISTA(1))=SWAP
      DO 16 J=2,MFIT
        COVAR(LISTA(J),LISTA(J))=COVAR(1,J)
16    CONTINUE
      DO 18 J=2,MA
        DO 17 I=1,J-1
          COVAR(I,J)=COVAR(J,I)
17      CONTINUE
18    CONTINUE
      RETURN
      END
C
C  ***********************************************************************************************
C
      SUBROUTINE GAUSSJ(A,N,NP,B,M,MP)
      PARAMETER (NMAX=50)
      DIMENSION A(NP,NP),B(NP,MP),IPIV(NMAX),INDXR(NMAX),INDXC(NMAX)
      DO 11 J=1,N
        IPIV(J)=0
11    CONTINUE
      DO 22 I=1,N
        BIG=0.0
        DO 13 J=1,N
          IF(IPIV(J).NE.1)THEN
            DO 12 K=1,N
              IF (IPIV(K).EQ.0) THEN
                IF (ABS(A(J,K)).GE.BIG)THEN
                  BIG=ABS(A(J,K))
                  IROW=J
                  ICOL=K
                ENDIF
              ELSE IF (IPIV(K).GT.1) THEN
                PAUSE 'Singular matrix'
              ENDIF
12          CONTINUE
          ENDIF
13      CONTINUE
        IPIV(ICOL)=IPIV(ICOL)+1
        IF (IROW.NE.ICOL) THEN
          DO 14 L=1,N
            DUM=A(IROW,L)
            A(IROW,L)=A(ICOL,L)
            A(ICOL,L)=DUM
14        CONTINUE
          DO 15 L=1,M
            DUM=B(IROW,L)
            B(IROW,L)=B(ICOL,L)
            B(ICOL,L)=DUM
15        CONTINUE
        ENDIF
        INDXR(I)=IROW
        INDXC(I)=ICOL
        IF (A(ICOL,ICOL).EQ.0.) PAUSE 'Singular matrix.'
        PIVINV=1.0/A(ICOL,ICOL)
        A(ICOL,ICOL)=1.
        DO 16 L=1,N
          A(ICOL,L)=A(ICOL,L)*PIVINV
16      CONTINUE
        DO 17 L=1,M
          B(ICOL,L)=B(ICOL,L)*PIVINV
17      CONTINUE
        DO 21 LL=1,N
          IF(LL.NE.ICOL)THEN
            DUM=A(LL,ICOL)
            A(LL,ICOL)=0.0
            DO 18 L=1,N
              A(LL,L)=A(LL,L)-A(ICOL,L)*DUM
18          CONTINUE
            DO 19 L=1,M
              B(LL,L)=B(LL,L)-B(ICOL,L)*DUM
19          CONTINUE
          ENDIF
21      CONTINUE
22    CONTINUE
      DO 24 L=N,1,-1
        IF(INDXR(L).NE.INDXC(L))THEN
          DO 23 K=1,N
            DUM=A(K,INDXR(L))
            A(K,INDXR(L))=A(K,INDXC(L))
            A(K,INDXC(L))=DUM
23        CONTINUE
        ENDIF
24    CONTINUE
      RETURN
      END