      SUBROUTINE FIT(LINE,ERROR)
C-----------------------------------------------------------------------------
C ORBIT:
C   Driver routine for the Levenberg-Marquardt non linear least square fit 
C   of the orbital elements. 
C-----------------------------------------------------------------------------
      CHARACTER LINE*(*)
      LOGICAL ERROR
*      
      INCLUDE 'constant.inc'
      INCLUDE 'elements.inc'
*
      INTEGER MITER
      REAL*8 DUMMY(MA,1)
      LOGICAL QUIET,UPDATE,WARNING
*
      MITER=100
      ERROR = .FALSE.
*
      CALL SIC_I4(LINE,0,1,MITER,.FALSE.,ERROR)
      IF (ERROR) RETURN
*
      QUIET      = .FALSE.
      UPDATE     = .TRUE.
*
      CALL DO_FIT(MITER,EL,UPDATE,QUIET,WARNING,ERROR)
      IF (ERROR) RETURN
* Print the resulting orbital elements
      CALL PRINT_ELEMENTS(6,.FALSE.,1,0.5,DUMMY,.FALSE.,ERROR)
      RETURN
      END

      SUBROUTINE DO_FIT(MITER,ELEM,UPDATE,QUIET,WARNING,ERROR)
C-----------------------------------------------------------------------------
C ORBIT:
C   Driver routine for the Levenberg-Marquardt non linear least square fit 
C   of the orbital elements. 
C-----------------------------------------------------------------------------
      LOGICAL QUIET,ERROR
      REAL*8 ELEM(*)
      LOGICAL UPDATE,WARNING
      INTEGER MITER
*      
      INCLUDE 'constant.inc'
      include 'elements.inc'
      include 'control.inc'
      INCLUDE 'pi.inc'
*
      INTEGER NITER
      INTEGER I,J
      REAL BETA(MA)        
      INTEGER MFIT,NDATA,LISTA(MA),ALIST(MA)
      REAL*8 EL_TRY(MA)
      REAL ALPHA_TRY(MA,MA),BETA_TRY(MA)
      REAL LAMBDA          ! Levenberg-Marquardt diagonal enhancement factor
      REAL LOCAL_SIG,OCHISQ
      LOGICAL LAST_ITER, BAD_STEP, COMPUTE_COV, WAS_LAST
      CHARACTER STEP_STATUS*16
* External functions
      LOGICAL SIC_CTRLC
C -----------------------------------------
*
* Correct correlations profile for deviations from a gaussian
      IF (PROFILE_METHOD.EQ.'CORRECTED') THEN
         CALL UPDATE_PROFILE_CORRECTIONS(QUIET,ERROR)
         IF (ERROR) RETURN
      ENDIF
* Build up a list (and an inverse list) of the adjustable elements, and 
* initialise the trial element set
      DO I=1,MA
         ALIST(I) = 0
      ENDDO
      MFIT=0
      DO I=1,MA
         IF (EL_STATUS(I)(1:6).EQ.'Adjust') THEN
            MFIT = MFIT+1
            LISTA(MFIT) = I
            ALIST(I) = MFIT
         ELSE
            ALIST(I) = 0
         ENDIF
         EL_TRY(I) = ELEM(I)
      ENDDO
*
      LAMBDA=0.01
      NITER =0
      LAST_ITER = .FALSE.
      WAS_LAST  = .FALSE.
      BAD_STEP  = .FALSE.
      WARNING   = .FALSE.
      OCHISQ = 1E37
*
      IF (.NOT.QUIET) WRITE(6,'(A)') 
     +     '  IT  CHISQ      SD(VA) SD(VB) SD(TETA) SD(RO)'
*
* Main loop: solve the linearised least square problem till chi**2 
* converges to a stable value
      DO WHILE (.TRUE.)
* Build the normal equation system for the linearised least square problem,
* for current trial parameters. Residuals for this same parameter set are
* also simultaneously evaluated here (and only here), so convergence can 
* only be tested further inside the loop.
         CALL BUILD_NORMAL(MFIT,LISTA,EL_TRY,ALPHA_TRY,BETA_TRY,
     +           CHISQ,NMEAS,SD,CHI2,LOCAL_SIG,NDATA,QUIET,ERROR)
!         do j=1,mfit
!!!            beta_try(j) = -betA_TRY(J)
!            write(6,*) (alpha_try(i,j),i=1,mfit), '        ',
!     +           beta_try(j)
!         enddo
!         write(6,*) (el_try(lista(i)),i=1,mfit)
         IF (ERROR) RETURN
* Evaluate the previous iteration for convergence, and print a brief 
* summary of its residuals
         CALL EVALUATE_STEP(CHISQ,OCHISQ,WAS_LAST,BAD_STEP,
     +        STEP_STATUS,LAMBDA,LAST_ITER)
         IF (.NOT.QUIET) WRITE (6,1310) NITER,CHISQ, 
     +        (SD(J), J=1,NDTYPE)
 1310    FORMAT (1X,I3,1X,F10.3,2(1X,F6.3),(1X,F4.1),20(1X,F5.3))
* 
* Step was successful: adopt the trial parameters, and update the normal
* equation system
         IF (STEP_STATUS.EQ.'Successful') THEN
            OCHISQ=CHISQ
            DO J=1,MFIT
               ELEM(LISTA(J)) = EL_TRY(LISTA(J))
               BETA(J) = BETA_TRY(J)
               DO I=1,MFIT
                  ALPHA(I,J) = ALPHA_TRY(I,J)
               ENDDO
            ENDDO
         ENDIF
         IF (LAST_ITER) GOTO 100
*
         NITER=NITER+1
         IF (NITER.GT.MITER) THEN
            IF (.NOT.QUIET) WRITE(6,*) 
     +           'W-FIT,  Maximum iteration count reached',
     +           ' without convergence'
            GOTO 99
         ENDIF
* Test for interruption by CtrlC, except in quiet mode where this is done
* at a higher level
         IF (.NOT.QUIET.AND.SIC_CTRLC()) THEN
            ERROR = .TRUE.
            RETURN
         ENDIF
* Construct the (diagonally enhanced) Levenberg-Marquardt system from the
* linearised least square system
         BAD_STEP = .FALSE. ! Hopefully
         DO J=1,MFIT
            DO I=1,MFIT
               ALPHA_TRY(I,J) = ALPHA(I,J) 
            ENDDO
            ALPHA_TRY(J,J) = ALPHA_TRY(J,J)*(1+LAMBDA)
            IF (ALPHA_TRY(J,J).GT.MAXR4) THEN
               IF (.NOT.QUIET) WRITE(6,*) 
     +              'W-FIT,  Marquardt enhancement factor',
     +        ' has increased to infinity without convergence'
               ERROR = .TRUE.
               RETURN
            ENDIF
            BETA_TRY(J) = BETA(J)
         ENDDO
*     
* Solve the Levenberg-Marquardt system 
         CALL SOLVE(METHOD,ALPHA_TRY,MFIT,MA,BETA_TRY,LISTA,
     +           NAMEL,QUIET,WARNING,BAD_STEP,ERROR)
* Update the trial orbital elements
         IF (.NOT.BAD_STEP) THEN
            CALL UPDATE_ELEMENTS(ELEM,LISTA,MFIT,
     +           SCLEL,EL_TRY,BETA_TRY)
         ENDIF
      ENDDO  !     Go on with iterations
* Converged: some final processing
 100  CONTINUE
* Build the normal matrix for the final solution, and invert it to obtain
* the true covariance matrix
      COMPUTE_COV = .TRUE.
      DO WHILE(COMPUTE_COV)
         COMPUTE_COV = .FALSE.
         CALL BUILD_NORMAL(MFIT,LISTA,EL_TRY,ALPHA_TRY,BETA_TRY,
     +        CHISQ,NMEAS,SD,CHI2,LOCAL_SIG,NDATA,QUIET,ERROR)
         IF (ERROR) RETURN
         CALL SOLVE(METHOD,ALPHA_TRY,MFIT,MA,BETA_TRY,LISTA,
     +        NAMEL,QUIET,WARNING,BAD_STEP,ERROR)
         IF (ERROR) RETURN
* Some cosmetic massaging of the elements to make them more eye-pleasing,
* though equivalent. Do not do that in non-interactive mode, to avoid 
* meaningless confidence intervals broadened by one period for epochs,
* and similar accidents.
         IF (.NOT.QUIET) THEN
            DO I=1,MORBIT
* Try forcing positive velocity amplitudes (through a PI phase shift) 
* when needed, for each orbit in turn
               CALL POSITIVE_K1_K2(I,EL_TRY,COMPUTE_COV)
* Modify the periastron epoch, within reason, to minimize its correlation 
* with the period. Also modify the epoch of the coordinate zero point to
* minimize the correlation with proper motion.
               CALL OPTIMIZE_EPOCH(I,EL_TRY,SCLEL,
     +              ALPHA_TRY,ALIST,COMPUTE_COV)
            ENDDO
         ENDIF
      ENDDO
*
      CALL PRINT_CHI(QUIET,CHISQ,NDATA-MFIT,LOCAL_SIG,ERROR)
      IF (ERROR) RETURN
* Store final elements
      DO J=1,MFIT
         ELEM(LISTA(J)) = EL_TRY(LISTA(J))
      ENDDO
      ELEM(I_EPOCH) = EL_TRY(I_EPOCH)  ! Special case for this "pseudo-element"
* For true adjustments (not those done during montecarlo simulations):
* update reduced Chi and element standard errors
      IF (UPDATE) THEN
         SIG = LOCAL_SIG
         DO I=1,MFIT
            J=LISTA(I)
            ELERR(J)=ABS(SCLEL(J))*SIG*SQRT(ABS(ALPHA_TRY(I,I)))
         ENDDO 
      ENDIF
* Don't accept a solution that remains poorly conditioned at last step.
      IF (BAD_STEP) THEN
         ERROR = .TRUE.
      ENDIF
*
* Rearrange a covariance matrix into the order of all MA elements. Insert 
* apropriate diagonal terms for the fixed elements if standard errors
* were provided for them.
! This should in principle only be done for true adjustments (not montecarlo
! simulations) but for the moment the initial matrix isn't saved. Not too bad
! a problem anyway, covariance matrix should be fairly stable for a well
! behaved solution
      DO J=1,MA
         DO I=1,MA
            ALPHA(I,J) = 0
         ENDDO
! Not quite correct for the moment, reduced CHI indiscriminately rescales
! all errors, even the totally independent ones that may be provided for the
! fixed parameters... Should be 1 any way, so don't fix that for the moment.
         ALPHA(J,J) = SNGL((ELERR(J)/SCLEL(J)/SIG)**2)
      ENDDO
      DO J=1,MFIT
         DO I=1,MFIT
            ALPHA(LISTA(I),LISTA(J)) = ALPHA_TRY(I,J)
         ENDDO
      ENDDO
      RETURN
* Error exit
 99   ERROR = .TRUE.
      RETURN
      END
c<FF>
      SUBROUTINE POSITIVE_K1_K2(IORB,ELEM,UNCHANGED)
C---------------------------------------------------------------------------
C Element massaging routine:
C Try forcing positive velocity amplitudes (through a PI phase shift) 
C when needed
C---------------------------------------------------------------------------
      INTEGER IORB
      REAL*8  ELEM(*)
      LOGICAL UNCHANGED
*
      INCLUDE 'constant.inc'
* External functions
      REAL*8 DMODULO
* Local
      INTEGER IK1,IK2,IOMEGA_L
*
      IK1 = ELINDX(IORB)+I_K1
      IK2 = ELINDX(IORB)+I_K2
      IF (ELEM(IK1).LT.0.AND.ELEM(IK2).LE.0) THEN 
         IOMEGA_L = (ELINDX(IORB)+I_OMEGA_L)
         ELEM(IK1) = -ELEM(IK1)
         ELEM(IK2) = -ELEM(IK2)
         ELEM(IOMEGA_L) = DMODULO(ELEM(IOMEGA_L)+180.D0,360.D0)
         UNCHANGED = .FALSE.    ! To update the covariance matrix
      ENDIF
      END
c<FF>
      SUBROUTINE OPTIMIZE_EPOCH(IORB,ELEM,SCALE,ALPHA,
     +     ALIST,UNCHANGED)
C---------------------------------------------------------------------------
C Element massaging routine:
C - modify the periastron epoch to minimize its correlation with the period.
C   The best epoch is T0-Cov(P,T0)/Cov(P,P)*P, but the formula is slightly 
C   modified because we solve for 2*pi/P rather than P. The result is then 
C   rounded to the closest periastron epoch, and forced to remain within
C   at most one period of the measurement interval.
C - choose the coordinate peoch that minimizes the correlation with the
C   proper motion.
C---------------------------------------------------------------------------
      INCLUDE 'constant.inc'
      INCLUDE 'data.inc'
*
      INTEGER IORB,ALIST(*)
      REAL*8  ELEM(*)
      REAL    SCALE(*),ALPHA(MA,MA)
      LOGICAL UNCHANGED
*
* Local
      INTEGER IT0, IPERIOD, J_T0, J_PERIOD, J_X0, J_Y0, J_MUX, J_MUY
      INTEGER NDT
      REAL NP, NP_MIN, NP_MAX, DT
      REAL MODULO
      EXTERNAL MODULO
*
      IT0     = ELINDX(IORB)+I_T0
      IPERIOD = ELINDX(IORB)+I_PERIOD
      J_T0     = ALIST(IT0)
      J_PERIOD = ALIST(IPERIOD)
      IF (J_T0.NE.0.AND.J_PERIOD.NE.0) THEN! Check both are adjustable
         IF (ALPHA(J_PERIOD,J_PERIOD).NE.0) THEN
            NP = -ALPHA(J_PERIOD,J_T0)
     +           /(ALPHA(J_PERIOD,J_PERIOD)*SCALE(IPERIOD))
            NP = NINT(NP)
* Ensure the new perioastron epoch is at most one period earlier or later
* than the respectively first and last available measurements.
            NP_MIN = SNGL((FIRST_JDB-ELEM(IT0))/ELEM(IPERIOD))
            NP_MAX = SNGL((LAST_JDB -ELEM(IT0))/ELEM(IPERIOD))
            NP_MIN = NINT(NP_MIN-MODULO(NP_MIN,1.))
            NP_MAX = NINT(NP_MAX-MODULO(NP_MAX,1.)+1)
            NP = MAX(NP_MIN,NP)
            NP = MIN(NP_MAX,NP)
* Warn that something is being done
            IF (NP.NE.0) THEN
               WRITE(6,'(1X,A,A,2(A,F10.3),A)') 
     +              'I-FIT,  Shifting T0',ORBNAME(IORB),'from ',
     +              ELEM(IT0),' to ',
     +              ELEM(IT0) + NP*ELEM(IPERIOD),
     +              ' to minimize (T0,P) covariance'
               ELEM(IT0) = ELEM(IT0) + NP*ELEM(IPERIOD)
               UNCHANGED = .FALSE. ! Request an update of the covariance matrix
            ENDIF
         ENDIF
      ENDIF
* Same thing for the positions, on first orbit by convention
      IF (IORB.EQ.IORB0) THEN
         NDT = 0
         DT  = 0.0
         J_X0     = ALIST(I_X0)
         J_MUX    = ALIST(I_MuX)
         J_Y0     = ALIST(I_Y0)
         J_MUY    = ALIST(I_MuY)
         IF ((J_X0.NE.0.AND.J_MUX.NE.0).AND.
     +       (J_Y0.NE.0.AND.J_MUY.NE.0)) THEN ! Both are adjustable
            IF (ALPHA(J_MUX,J_MUX).NE.0) THEN
               NDT = NDT+1
               DT = DT-ALPHA(J_MUX,J_X0)
     +              /ALPHA(J_MUX,J_MUX)
            ENDIF
            IF (ALPHA(J_MUY,J_MUY).NE.0) THEN
               NDT = NDT+1
               DT = DT-ALPHA(J_MUY,J_Y0)
     +              /ALPHA(J_MUY,J_MUY)
            ENDIF
*     Apply the correction
            IF (NDT.GT.0) THEN
               DT = DT/NDT
               ELEM(I_X0) = ELEM(I_X0)+DT*ELEM(I_MuX)
               ELEM(I_Y0) = ELEM(I_Y0)+DT*ELEM(I_MuY)
               ELEM(I_EPOCH) = ELEM(I_EPOCH) + DT*365.25
               UNCHANGED = .FALSE. ! Request an update of the covariance matrix
            ENDIF
         ENDIF
      ENDIF
      RETURN
      END
c<FF>
      SUBROUTINE EVALUATE_STEP(CHISQ,OCHISQ,forced,BAD,
     +     STATUS,LAMBDA,LAST)
      REAL CHISQ           ! Input:    Chi^2 for current iteration
      REAL OCHISQ          ! Modified: Best Chi^2 to date
      LOGICAL FORCED       ! Input:    Force current iteration to be final
      LOGICAL BAD          ! Input:    Current iteration poorly condtioned
      CHARACTER*(*) STATUS ! Output:   Improved?
      REAL LAMBDA          ! Modified: Levenberg-Marquardt parameter
      LOGICAL LAST         ! Output:   Convergence flag
*
      INCLUDE 'pi.inc'
*
* Just a final covariance matrix update for a converged solution. Do not 
* iterate any further.
      IF (FORCED) THEN
         LAST = .TRUE.
         STATUS = 'Successful'
* Unsuccessful step, increase steepest descent character of the algorithm,
* and retry from same point.
      ELSE
     + IF (CHISQ.GT.OCHISQ*(1+10*EPSR4)
     +        .OR.CHISQ.NE.CHISQ.OR.BAD) THEN
         STATUS = 'Unsuccessful'
         LAMBDA=LAMBDA*10
* Successful step
      ELSE                      ! 
         STATUS = 'Successful'
         LAST = (CHISQ.GE.OCHISQ*(1-1E-4)) ! Converged, will stop
* Converged, request computation of the actual covariance matrix before 
* exiting
         IF (LAST) THEN
            LAMBDA    = 0
* Not yet converged. Increase the linearised least square character of 
* the algorithm, and iterate
         ELSE 
            LAMBDA=LAMBDA/5
         ENDIF
      ENDIF
      RETURN
      END
c<ff>

      SUBROUTINE PRINT_CHI(QUIET,CHISQ,NDEG,CHI,ERROR)
      LOGICAL QUIET
      REAL CHISQ,CHI
      INTEGER NDEG
      LOGICAL ERROR
* External
      REAL*4 GAMMQ,ERFCC  ! Erf and Gamma functions
*
      REAL GOODNESS
*      
      IF (.NOT.QUIET) THEN
         IF (CHISQ.LE.1E-3.AND.NDEG.EQ.0) THEN 
            GOODNESS=1.
            CHI = 1
         ELSEIF ((NDEG).LE.500) THEN
            GOODNESS=gammq(0.5*(NDEG),0.5*chisq,ERROR)
            IF (ERROR) THEN
               WRITE(6,*) 'E-FIT,  Error evaluating Goodness of Fit',
     +              ' for ',chisq,' and ',ndeg
               RETURN
            ENDIF
* Computation of GAMMQ is expensive for large arguments, and the chi**2 
* statistics has then already converged to a normal law with mean Nu and 
* standard deviation sqrt(2*Nu)
         ELSE
            GOODNESS=.5*erfcc(chisq-(NDEG))/
     &           SQRT(4.0*(NDEG))
         ENDIF
         WRITE(6,1300)
     +        'Chi**2 = ',chisq,
     +        'for expected ', NDEG,
     +        '+-',sqrt(2.0*(NDEG))
         WRITE(6,1305)
     +        'Reduced CHI = ', CHI,
     +        'P(Chi**2) = ',Goodness
      ENDIF
 1300 FORMAT((1X,A,F9.2),(1X,A,I6),(1X,A,F6.2))
 1305 FORMAT((1X,A,F6.2),(1X,A,F8.6))
! Shout if goodness of fit falls below 1E-2 or 1E-3? Also maybe if Q 
! is too close to 1?
      RETURN
      END
*<ff> 

      SUBROUTINE UPDATE_ELEMENTS(ELEM,LISTA,MFIT,SCLEL,EL_TRY,BETA_TRY)
C---------------------------------------------------------------------------
C Update the trial orbital elements after one successful step
C---------------------------------------------------------------------------
      INCLUDE 'constant.inc'
      INCLUDE 'pi.inc'
      REAL*8 ELEM(MA),EL_TRY(MA)
      INTEGER MFIT,LISTA(MA)
      REAL SCLEL(MA),BETA_TRY(MA)
*
      INTEGER I,J
*
      DO I=1,MORBIT
* One actually solves for the frequency, but stores and displays the period.
* Correction and error estimate must both be multiplied by d(2*pi/x)/dx
         IF (ELEM(ELINDX(I)+I_PERIOD).NE.0) THEN
            SCLEL(ELINDX(I)+I_PERIOD)=
     +           SNGL(-ELEM(ELINDX(I)+I_PERIOD)**2/PI2)
         ELSE
            SCLEL(ELINDX(I)+I_PERIOD)=1
         ENDIF
      ENDDO
      DO I=1,MFIT
         J=LISTA(I)
         EL_TRY(J)=ELEM(J)+SCLEL(J)*BETA_TRY(I)
!               write(6,*) i,j,elem(j),sclel(j),beta_try(i),el_try(j)
      ENDDO 
      RETURN
      END
c<FF>
      SUBROUTINE SOLVE(METHOD,ALPHA_TRY,MFIT,MA,
     +     BETA_TRY,LISTA,
     +     NAMEL,QUIET,WARNING,BAD_STEP,ERROR)
C-----------------------------------------------------------------------------
c Wrapper routine that calls the currently selected linear system solver and
c performs some low level sanity checks.
C-----------------------------------------------------------------------------
*
      INTEGER MA,MFIT
      CHARACTER*(*) METHOD,NAMEL(MA)
      INTEGER LISTA(MA)
      REAL ALPHA_TRY(MA,MA),BETA_TRY(MA)
      LOGICAL QUIET,WARNING,BAD_STEP,ERROR
*
      INTEGER I
*

      IF (METHOD.EQ.'GAUSS-JORDAN') THEN 
* Gauss-Jordan elimination            
         CALL GAUSSJ(ALPHA_TRY,MFIT,MA,BETA_TRY,1,1,
     +        QUIET,WARNING,BAD_STEP)
      ELSEIF (METHOD.EQ.'DIAGONALISE') THEN
* Diagonalise the matrix, and then solve the system
         CALL DIAGSOLVE(ALPHA_TRY,MFIT,MA,BETA_TRY,LISTA,
     +        NAMEL,QUIET,WARNING,BAD_STEP)
      ELSE
         WRITE(6,*) 'F-FIT,  Unknown solution method'
         ERROR = .TRUE.
         RETURN
      ENDIF
* Protect against NaNs in corrections
      DO I=1,MFIT
         IF (BETA_TRY(I).NE.BETA_TRY(I)) BAD_STEP = .TRUE.  
      ENDDO
      RETURN
      END














