      SUBROUTINE EPNLLSQ(Q,TQ,NI,NF,CALC,
     *                  IPOLY,A,B,C,SIGA,SIGB,SIGC,NFREQ,    
     *                  FREQ,AMP,PHI,SIGMAF,SIGMAA,SIGMAP,IPRINT,MD)    
C     
C     THIS ROUTINE CALCULATES THE BEST VALUES OF AMP,FREQ,PHI     
C     FOR NFREQ SINUSOIDS AS WELL AS FOR A POLYNOMIAL OF ORDER    
C     IPOLY-1 (MAXIMUM 2) WHOSE COEFFICIENTS ARE STORED IN A,B,C  
C     THE TAYLOR EXPANSION NON-LINEAR LEAST SQUARES TECHNIQUE IS USED   
C     
      double precision work1(33,33)
      DOUBLE PRECISION COV(33,33),COVINV(33,33),Y(33),DEL(33),CM(33,33) ! DS001
      DOUBLE PRECISION DERIV(33),UNIT(33,33),PARMS(33),V(2) 
      DIMENSION iwork2(33)
      DOUBLE PRECISION CALC(NF),TQ(NF),Q(NF),A,B,C
      DIMENSION FREQ(1),AMP(1),PHI(1),SIGMAF(1),SIGMAA(1),SIGMAP(1)     
      DOUBLE PRECISION TWOPI,ARG,TI,QI,SI,CI,Q0
      REAL*8 FAC,FAC1,FAC2
      IF(NFREQ.GT.10)THEN
        PRINT 99,NFREQ
   99   FORMAT(//12X,'NUMBER OF FREQUENCIES REQUESTED IS ',I5/
     *           12X,'THIS IS GREATER THAN THE 10 ALLOWED FOR ',
     *           'THIS ROUTINE'/12X,'TALK TO DOD')
        STOP
      END IF
      TWOPI=2.0*3.141592653D0 
      N=NF-NI+1   
      IDEBUG=0    
      IF(MD.LT.0)IDEBUG=1     
      IPRINT=1    
      SIG=0.0     
      NUM=IPOLY+3*NFREQ 
      DO 1 J=1,NFREQ    
        IND=(J-1)*3     
        PARMS(IND+1)=AMP(J)   
        PARMS(IND+2)=FREQ(J)  
        PARMS(IND+3)=PHI(J)   
    1 CONTINUE    
      PARMS(3*NFREQ+1)=A          
      PARMS(3*NFREQ+2)=B
      PARMS(3*NFREQ+3)=C
      DO 2 J=NUM+1,33     
    2 PARMS(J)=0.0
      ICONV=0     
      ITER=0
      SIG=0.0     
      DO 5 I=NI,NF
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 3 J=1,NFREQ  
          IND=(J-1)*3   
          ARG=DMOD(TWOPI*PARMS(IND+2)*TI+PARMS(IND+3),TWOPI)
          CI=DCOS(ARG)  
          Q0=Q0+PARMS(IND+1)*CI     
    3   CONTINUE  
        DO 4 J=1,IPOLY  
          IF(TI.EQ.0.0.AND.J.EQ.1)THEN    
            Q0=Q0+PARMS(NFREQ*3+J)  
          ELSE    
            Q0=Q0+PARMS(NFREQ*3+J)*(TI**(J-1))  
          END IF  
    4   CONTINUE  
        SIG=SIG+(QI-Q0)**2    
    5 CONTINUE    
      SIG=SIG/(N-NUM)   
      IF(IPRINT.EQ.1)PRINT 8,NI,NF  
    8 FORMAT(12X,'NON-LINEAR LEAST SQUARES FIT OF POINTS ',I5,' TO ',I5)
      IF(IPRINT.EQ.1.AND.IPOLY.GT.0)PRINT 9,IPOLY-1   
    9 FORMAT(/12X,'POLYNOMIAL OF ORDER ',I2,' INCLUDED IN FIT')   
      IF(IPRINT.EQ.1.AND.NFREQ.GT.0)PRINT 10,NFREQ,   
     *  (FREQ(J),AMP(J),PHI(J),J=1,NFREQ),
     *  (PARMS(J),J=NFREQ*3+1,NFREQ*3+IPOLY)
   10 FORMAT(/12X,I2,' SINUSOIDS INCLUDED IN THE FIT'/ 
     *       12X,'STARTING VALUES GIVEN BY :'// 
     *       12X,6X,'FREQUENCY',5X,6X,'AMPLITUDE',15X,'PHASE'/    
     *       12X,55('-')/12X,3(1PE15.8,5X)/12X,55('-')/
     *       12X,6X,' EPOCH   ',5X,6X,' PERIOD  ',14X,'SECOND'/
     *       12X,55('-')/12X,0PF15.7,5X,F15.10,5X,1PE15.3)
      PRINT 12    
   12 FORMAT(12X,55('-')////12X,'ITER',5X,'AVE SIG**2'/12X,109('-'))    
      PRINT 225,ITER,SIG,(PARMS(J),J=1,NUM)     
   15 ITER=ITER+1 
      DO 20 I=1,NUM     
        DEL(I)=0.0
        Y(I)=0.0  
        DO 20 J=1,NUM   
          CM(I,J)=0.0   
          COV(I,J)=0.0  
   20 CONTINUE    
C     
C     CALCULATE THE NORMAL EQUATION MATRIX AND Q VECTOR     
C     
      DO 50 I=NI,NF     
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 30 J=1,NFREQ 
          IND=(J-1)*3   
          ARG=DMOD(TWOPI*PARMS(IND+2)*TI+PARMS(IND+3),TWOPI)
          SI=DSIN(ARG)  
          CI=DCOS(ARG)  
          DERIV(IND+1)=CI     
          DERIV(IND+2)=-PARMS(IND+1)*TWOPI*TI*SI
          DERIV(IND+3)=-PARMS(IND+1)*SI   
          Q0=Q0+PARMS(IND+1)*CI     
   30   CONTINUE  
        DO 32 J=1,IPOLY 
          IF(TI.EQ.0.0.AND.J.EQ.1)THEN    
            ARG=1.0     
          ELSE    
            ARG=TI**(J-1)     
          END IF  
          IND=NFREQ*3   
          DERIV(IND+J)=ARG    
          Q0=Q0+PARMS(IND+J)*ARG    
   32   CONTINUE  
        DO 40 J=1,NUM   
          DO 35 K=1,NUM 
            COV(J,K)=COV(J,K)+DERIV(J)*DERIV(K) 
   35     CONTINUE
          Y(J)=Y(J)+(QI-Q0)*DERIV(J)
   40   CONTINUE  
   50 CONTINUE    
C     
C     INVERT THE NORMAL MATRIX
C     
      DO 60 J=1,NUM     
      DO 60 K=1,NUM     
   60 COVINV(J,K)=COV(J,K)    
      V(1)=1.0    



c       Invert covinv; inverse appears in work1

        do 800 i=1,num
        do 801 jj=1,num
 801    work1(i,jj)=0.
 800    work1(i,i)=1.
        call ludcmp(covinv,num,33,iwork2,work2)
        do 802 i=1,num
        call lubksb(covinv,num,33,iwork2,work1(1,i))
 802    continue



c     Put the inverse matrix into COVINV
      do i = 1,num
         do j = 1,num
            covinv(i,j) = work1(i,j)
         enddo
      enddo
c     idebug=0
C     
C     FROM HERE DOWN TO ABOUT LABEL 135 ARE DEBUG CALCULATIONS & PRINTS 
C     
   65 IF(IDEBUG.EQ.1)THEN     
        PRINT 70  
   70   FORMAT(//' COVARIANCE MATRIX IS : '/)   
        DO 90 I=1,NUM   
          PRINT 80,(COV(I,J),J=1,NUM)     
   80     FORMAT(1X,1P10E13.4)
          PRINT 80
   90   CONTINUE  
        PRINT 95  
   95   FORMAT(///' INVERSE IS : '/)
        DO 100 I=1,NUM  
          PRINT 80,(COVINV(I,J),J=1,NUM)  
          PRINT 80
  100   CONTINUE  
        PRINT 105 
  105   FORMAT(///' THE Y VECTOR IS : '/) 
        PRINT 80,(Y(J),J=1,NUM)     
        PRINT 80  
C     
C       TEST FOR GOOD MATRIX INVERSION BY PRODUCING A UNIT MATRIX 
C       AND PRINTING IT AND THEN CALCULATE THE CONTRIBUTION TO THE
C       SOLUTIONS BY CALCULATING A MATRIX GIVEN BY COVINV(I,J)*Y(J)     
C     
        PRINT 120 
        DO 125 I=1,NUM  
          DO 115 K=1,NUM
            UNIT(I,K)=0.0     
            DO 110 J=1,NUM    
              UNIT(I,K)=UNIT(I,K)+COVINV(I,J)*COV(J,K)
  110       CONTINUE    
  115     CONTINUE
  120     FORMAT(///' PRODUCT OF COVARIANCE AND ITS INVERSE IS : '/)    
          PRINT 80,(UNIT(I,K),K=1,NUM)    
          PRINT 80
  125   CONTINUE  
        PRINT 128 
  128   FORMAT(///' PRODUCT OF COVARIANCE INVERSE AND Y VECTOR :'/)     
        DO 135 I=1,NUM  
          DO 130 J=1,NUM
            UNIT(I,J)=COVINV(I,J)*Y(J)    
  130     CONTINUE
          PRINT 80,(UNIT(I,J),J=1,NUM)    
          PRINT 80
  135   CONTINUE  
      END IF
C     
C     CALCULATE THE CORRECTIONS TO PARMS AND STORE IN DEL   
C     
      DO 140 I=1,NUM    
        DO 140 J=1,NUM  
          DEL(I)=DEL(I)+COVINV(I,J)*Y(J)  
  140 CONTINUE    
C     
C     APPLY CORRECTIONS 
C     
      DO 145 I=1,NUM    
  145 PARMS(I)=PARMS(I)+DEL(I)
C     
C     CALCULATE THE NEW SIGMA**2 OF THE RESIDUALS     
C     
      SIGNEW=0.0  
      DO 150 I=NI,NF    
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 146 J=1,NFREQ
          IND=(J-1)*3   
          ARG=DMOD(TWOPI*PARMS(IND+2)*TI+PARMS(IND+3),TWOPI)
          CI=DCOS(ARG)  
          Q0=Q0+PARMS(IND+1)*CI     
  146   CONTINUE  
        DO 148 J=1,IPOLY
          IF(TI.EQ.0.0.AND.J.EQ.1)THEN    
            Q0=Q0+PARMS(NFREQ*3+J)  
          ELSE    
            Q0=Q0+PARMS(NFREQ*3+J)*(TI**(J-1))  
          END IF  
  148   CONTINUE  
        CALC(I)=Q0
        SIGNEW=SIGNEW+(QI-Q0)**2    
  150 CONTINUE    
      SIGNEW=SIGNEW/(N-NUM)   
C     
C     CALCULATE THE CORRELATION MATRIX FOR THE ERRORS 
C     
      DO 180 I=1,NUM    
        CM(I,I)=COVINV(I,I)*SIGNEW  
  180 CONTINUE    
      DO 200 I=1,NUM    
      DO 200 J=1,I-1    
        FAC1=COVINV(I,I)
        FAC2=COVINV(J,J)
        FAC=SQRT(FAC1*FAC2)
        IF(FAC.EQ.0.0)PRINT 190,FAC1,FAC2,FAC,I,J
  190   FORMAT(' FAC1= ',1PE15.6,' FAC2= ',1PE15.6,
     *         ' FAC= ',1PE15.6,' I= ',I5,' J= ',I5)
        CM(I,J)=COVINV(I,J)/FAC
  200 CONTINUE
C     
C     CHECK FOR CONVERGENCE   
C     
      ERROR=1.0E-7
      DELSIG=ABS(SIGNEW-SIG)/SIG    
      IF(DELSIG.LE.ERROR)ICONV=1    
      SIG=SIGNEW  
C     
C     PRINT RESULTS SECTION   
C     
      IF(IPRINT.EQ.1)THEN     
        IF(ICONV.EQ.1)PRINT 210,ITER
  210   FORMAT(/12X,'*** CONVERGENCE AFTER ',I3,' ITERATIONS ***'/)     
        IF(ITER.GT.49)PRINT 220
  220   FORMAT(/12X,'*** NO CONVERGENCE AFTER 50 ITERATIONS ***'/)
        PRINT 225,ITER,SIG,(PARMS(J),J=1,NUM)   
  225   FORMAT(12X,I4,1P7E15.6/(31X,6E15.6))    
        IF(ICONV.EQ.1.OR.ITER.GT.49)PRINT 230    
  230   FORMAT(12X,109('-')//)
      END IF
      IF(ICONV.EQ.1.OR.ITER.GT.49)THEN     
        IF(MD.EQ.1)THEN 
C     
C         PREWHITEN THE DATA VIA THE FINAL SOLUTION   
C     
          DO 235 I=NI,NF
            TI=TQ(I)    
            QI=Q(I)     
            Q0=0.0
            DO 232 J=1,NFREQ  
              IND=(J-1)*3     
              ARG=DMOD(TWOPI*PARMS(IND+2)*TI+PARMS(IND+3),TWOPI)  
              CI=DCOS(ARG)    
              Q0=Q0+PARMS(IND+1)*CI 
  232       CONTINUE    
            DO 234 J=1,IPOLY  
              IF(TI.EQ.0.0.AND.J.EQ.1)THEN
                Q0=Q0+PARMS(NFREQ*3+J)    
              ELSE
                Q0=Q0+PARMS(NFREQ*3+J)*(TI**(J-1))    
              END IF    
  234       CONTINUE    
            Q(I)=Q(I)-Q0
  235     CONTINUE
        END IF    
C     
C       CALCULATE ERRORS ON PARMS   
C     
        DO 240 J=1,NFREQ
          IND=(J-1)*3   
          AMP(J)=PARMS(IND+1) 
          FREQ(J)=PARMS(IND+2)
          PHI(J)=PARMS(IND+3) 
          SIGMAA(J)=SQRT(CM(IND+1,IND+1)) 
          SIGMAF(J)=SQRT(CM(IND+2,IND+2)) 
          SIGMAP(J)=SQRT(CM(IND+3,IND+3)) 
  240   CONTINUE  
        IND=NFREQ*3     
        SIGA=SQRT(CM(IND+1,IND+1))  
        SIGB=SQRT(CM(IND+2,IND+2))  
        SIGC=SQRT(CM(IND+3,IND+3))  
        A=PARMS(IND+1)  
        B=PARMS(IND+2)  
        C=PARMS(IND+3)  
        IF(IPRINT.EQ.1)PRINT 245    
  245   FORMAT(12X,'FINAL VALUES : '//12X,33X,'SIGMA'/12X,38('-'))
        IF(IPRINT.EQ.1.AND.NFREQ.GT.0)PRINT 250,(AMP(J),SIGMAA(J),
     *                    FREQ(J),SIGMAF(J),PHI(J),SIGMAP(J),J=1,NFREQ) 
        IF(IPRINT.EQ.1.AND.IPOLY.GT.0)    
     *    PRINT 252,(PARMS(IND+J),SQRT(CM(IND+J,IND+J)),J=1,IPOLY)
  250   FORMAT(12X,' AMP',5X,1PE15.8,5X,E9.1/   
     *         12X,'FREQ',5X,E15.8,5X,E9.1/     
     *         12X,' PHI',5X,E15.8,5X,E9.1)     
  252   FORMAT(12X,'   A',5X,1PE15.8,5X,E9.1/   
     *         12X,'   B',5X,E15.8,5X,E9.1/     
     *         12X,'   C',5X,E15.8,5X,E9.1)     
        PRINT 254,SQRT(SIG)   
  254   FORMAT(12X,38('-')//12X,'STANDARD DEVIATION OF RESIDUALS IS ',  
     *         1PE14.6/)
        IF(IPRINT.EQ.1)THEN   
          PRINT 255     
  255     FORMAT(12X,'CORRELATION COEFFICIENTS ARE :'/)     
          DO 270 I=2,NUM
            PRINT 260,(CM(I,J),J=1,I-1)   
  260       FORMAT(12X,15F8.3)
  270     CONTINUE
        END IF    
        IF(IDEBUG.EQ.1)THEN   
C     
C         DO A LITTLE SEARCH IN PHASE AND FREQUENCY AROUND THE SOLUTION 
C     
          DO 300 J=1,5  
            DO 290 K=1,5
              F=FREQ(1)+(J-3)*SIGMAF(1)   
              PHASE=PHI(1)+(K-3)*SIGMAP(1)
              SIGNEW=0.0
              DO 280 I=NI,NF  
                TI=TQ(I)
                QI=Q(I) 
                ARG=DMOD(TWOPI*F*TI+PHASE,TWOPI)
                Q0=PARMS(1)*DCOS(ARG)     
                IF(IPOLY.GT.0)Q0=Q0+PARMS(NFREQ*3+1)  
     *                           +PARMS(NFREQ*3+2)*TI 
     *                           +PARMS(NFREQ*3+3)*TI*TI    
                SIGNEW=SIGNEW+((QI-Q0)**2)
  280         CONTINUE  
              SIGNEW=SIGNEW/(N-NUM) 
              Y(K)=SIGNEW/SIG 
              DEL(K)=PHASE    
  290       CONTINUE    
            IF(J.EQ.1)PRINT 292,(DEL(K),K=1,5)  
  292       FORMAT(//16X,1P5E12.4/16X,60('-'))  
            PRINT 294,F,(Y(K),K=1,5)
  294       FORMAT(1X,1PE15.8,0P5F12.4)   
  300     CONTINUE
        END IF    
        RETURN    
      END IF
C     
C     RETURN FOR THE NEXT ITERATION 
C     
      GO TO 15    
      END   
