      SUBROUTINE LSTSQ(Q,TQ,NI,NF,NFREQ,FREQ,AMP,PHI,SIGMAA,SIGMAP,A0,  
     *                 MD,YMEAN,YSTDEV,RMEAN,RSTDEV)  
C     
C     ROUTINE TO SIMULTANEOUSLY FIT THE NFREQ FREQUENCIES IN FREQ 
C     BY LEAST SQUARES SINSUSOIDS TO THE DATA IN Q AT TIMES TQ.   
C     THE TECHNIQUE IS DESCRIBED IN DEEMING,T.J.,19XX,VISTAS IN   
C     ASTRONOMY,      . NFREQ MUST BE LESS THAN OR EQUAL TO 40.   
C     
      double precision work1(80,80)
      dimension iwork2(80)
      DIMENSION Q(NF),FREQ(1),AMP(1),PHI(1),SIGMAA(1),SIGMAP(1)    
      DOUBLE PRECISION TQ(NF)
      DOUBLE PRECISION COV(80,80),SUMX(80),SUMXY(80),COVIN(80,80) 
      DOUBLE PRECISION V(2),A(80),TESTF(40),TESTP(40) 
      DOUBLE PRECISION JUNK(80),SXJ,SXI,SXIXJ,SXJY,SY,SYY,SAXI    
      DOUBLE PRECISION OMEGA(80),TWOPI,TIME,omeg
      IF(NFREQ.GT.40)THEN
        PRINT 99,NFREQ
   99   FORMAT(//12X,'NUMBER OF FREQUENCIES REQUESTED IS ',I5/
     *           12X,'THIS IS GREATER THAN THE 40 ALLOWED FOR ',
     *           'THIS ROUTINE'/12X,'TALK TO DOD')
        STOP
      END IF
      TWOPI=2.0D0*3.141592653 
      NQQ=NF-NI+1 
      IDEBUG=0    
      IF(MD.LT.0)IDEBUG=1     
C     
C     FIRST COMPUTE OMEGA = 2 PI FREQ. REMEMBER FOR EACH FREQUENCY
C     THERE ARE 2 CO-EFFS TO BE COMPUTED.  THE ODD WILL REFER TO THE    
C     COSINE AND THE EVEN WILL REFER TO THE SINE CO-EFFICIENT.    
C     
      DO 10 J=1,NFREQ   
        OMEGA(J*2)=TWOPI*FREQ(J)    
        OMEGA(J*2-1)=OMEGA(2*J)     
   10 CONTINUE    
C     
C     THE NUMBER OF TERMS IN THE COVARIANCE MATRIX IS NTERM X NTERM WHERE     
C     NTERM = 2 * NFREQ.  COMPUTE THIS MATRIX TOGETHER WITH THE VECTOR  
C     RESPRESENTING THE SUM OF XI AND Y AS IN DEEMING'S PAPER : SI.     
C     
      NTERM=2*NFREQ     
      DO 100 I=1,NTERM  
        DO 90 J=1,NTERM 
          SXI=0.0 
          SXJ=0.0 
          SXIXJ=0.0     
          SXJY=0.0
          SY=0.0  
          SYY=0.0 
          A(I)=0.0
          DO 50 K=NI,NF 
            TIME=TQ(K)  
            XI=DMOD(OMEGA(I)*TIME,TWOPI)  
            XJ=DMOD(OMEGA(J)*TIME,TWOPI)  
            IF(MOD(I,2).NE.0)THEN   
              XI=COS(XI)                               ! DS001
            ELSE  
              XI=SIN(XI)                               ! DS001
            END IF
            IF(MOD(J,2).NE.0)THEN   
              XJ=COS(XJ)                               ! DS001
            ELSE  
              XJ=SIN(XJ)                               ! DS001
            END IF
            SXI=SXI+XI  
            SXJ=SXJ+XJ  
            SXIXJ=SXIXJ+XI*XJ 
            SXJY=SXJY+Q(K)*XJ 
            SY=SY+Q(K)  
            SYY=SYY+Q(K)*Q(K) 
   50     CONTINUE
          COV(I,J)=(SXIXJ/NQQ)-(SXI/NQQ)*(SXJ/NQQ)    
          IF(I.EQ.1)THEN
            SUMXY(J)=(SXJY/NQQ)-(SXJ/NQQ)*(SY/NQQ)    
            SUMX(J)=SXJ 
          END IF  
   90   CONTINUE  
  100 CONTINUE    
      YMEAN=SY/NQQ
      YSTDEV=DSQRT((SYY-SY**2/NQQ)/(NQQ-1))     
      IF(IDEBUG.EQ.1)THEN     
        PRINT 105 
  105   FORMAT(1X,'COVARIANCE MATRIX :'/) 
        DO 120 I=1,NTERM
          PRINT 110,(COV(I,J),J=1,NTERM)  
  110     FORMAT(1X,1P10E13.4)
          PRINT 110     
  120   CONTINUE  
        PRINT 125,(SUMXY(J),J=1,NTERM)    
  125   FORMAT(' VECTOR SI : '/(1X,1P10E13.4))  
      END IF
C     
C     CALCULATE THE INVERSE OF COV AND STORE IT IN COVIN    
C     
      DO 140 I=1,NTERM  
        DO 140 J=1,NTERM
          COVIN(I,J)=COV(I,J) 
        CONTINUE  
  140 CONTINUE    
C     
C     INVERT THE NORMAL MATRIX
C     
      V(1)=1.0    

c     Replace upper triangle of WORK1 with the lower triangle
c     ie. make it a complete inverse matrix

c     Invert covinv; inverse appears in work1

      do 800 i=1,nterm
        do 801 jj=1,nterm
 801    work1(i,jj)=0.
 800  work1(i,i)=1.
      call ludcmp(covin,nterm,80,iwork2,work2)
      do 802 i=1,nterm
        call lubksb(covin,nterm,80,iwork2,work1(1,i))
 802  continue


c     Put the inverse matrix into COVINV
      do i = 1,nterm
         do j = 1,nterm
            covin(i,j) = work1(i,j)
         enddo
      enddo

      IF(IDEBUG.EQ.1)THEN     
        PRINT 145 
  145   FORMAT(' INVERSE OF COV MAT IS : '/)    
        DO 150 I=1,NTERM
          PRINT 110,(COVIN(I,J),J=1,NTERM)
          PRINT 110     
  150   CONTINUE  
      END IF
C     
C     CALCULATE THE CO-EFFICIENTS OF THE COS AND SIN COMPONENTS OF
C     EACH OF THE FREQUENCIES 
C     
      DO 170 I=1,NTERM  
        DO 160 J=1,NTERM
          A(J)=A(J)+SUMXY(I)*COVIN(I,J)   
  160   CONTINUE  
  170 CONTINUE    
C     
C     NOW CALCULATE THE CONSTANT TERM A0 (DEEMING'S VARIABLE B)   
C     
      SAXI=0.0    
      DO 180 I=1,NTERM  
        SAXI=SAXI+SUMX(I)*A(I)
  180 CONTINUE    
      A0=(SY/NQQ)-(SAXI/NQQ)  
C     
C     NOW CALCULATE THE AMPLITUDES AND PHASES   
C     
      DO 200 J=1,NFREQ  
        AMP(J)=DSQRT(A(J*2)**2+A(J*2-1)**2)     
        PHI(J)=DATAN2(-A(2*J),A(2*J-1))   
  200 CONTINUE    
      IF(IDEBUG.EQ.1)PRINT 190,(J,A(2*J),A(2*J-1),J=1,NFREQ)
  190 FORMAT(' SOLUTION VECTOR : '/(1X,'FREQUENCY ',I2,5X,  
     *       'A(2*J)= ',1PE15.5,5X,'A(2*J-1)= ',E15.5))     
C     
C     CALCULATE SIGMAE, THE VARIANCE OF THE RESIDUALS AND REPLACE 
C     Q BY THE RESIDUALS IF MD = 1  
C     
      SUMRES=0.0  
      SSQRES=0.0  
      DO 250 J=NI,NF    
        RESID=Q(J)
        RESID=RESID-A0  
        DO 225 I=1,NFREQ
          ARG=DMOD(OMEGA(2*I)*TQ(J)+PHI(I),TWOPI)     
          RESID=RESID-AMP(I)*COS(ARG)     
  225   CONTINUE  
        SUMRES=SUMRES+RESID   
        SSQRES=SSQRES+RESID**2
        IF(MD.EQ.1)Q(J)=RESID 
  250 CONTINUE    
      SIGMAE=(SSQRES-(SUMRES**2/NQQ))/(NQQ-1)   
      RMEAN=SUMRES/NQQ  
      RSTDEV=SQRT(SIGMAE)     
      DO 300 I=1,NFREQ  
        SIGMA1=DSQRT(SIGMAE*COVIN(2*I-1,2*I-1)/NQQ)   
        SIGMA2=DSQRT(SIGMAE*COVIN(2*I,2*I)/NQQ) 
        SIGMAA(I)=DSQRT((A(2*I-1)*SIGMA1)**2+(A(2*I)*SIGMA2)**2)/AMP(I) 
      SIGMAP(I)=DSQRT((A(2*I-1)*SIGMA2)**2+(A(2*I)*SIGMA1)**2)/AMP(I)**2
  300 CONTINUE    
      IF(MD.GE.0)RETURN 
C     
C     OTHERWISE NOW TEST THE GOODNESS OF FIT OF THE 1ST FREQUENCY 
C     FREQ(1), AMP(1), PHI(1) BY VARYING FREQ(1) AND PHI(1) 
C     AROUND THE BEST VALUES  
C     
      T=TQ(NF)-TQ(NI)   
      DF=1./(T*40)
      DPHI=SIGMAP(1)/2. 
      PRINT 320   
  320 FORMAT(//)  
      DO 400 I=1,20     
        F=FREQ(1)-(11-I)*DF   
        TESTF(I)=F
        DO 375 J=1,20   
          P=PHI(1)-(11-J)*DPHI
          TESTP(J)=P    
          OMEG=TWOPI*F  
          SSQRES=0.0    
          DO 325 K=NI,NF
            ARG=DMOD(OMEG*TQ(K)+P , TWOPI)
            RESID=Q(K)-A0-AMP(1)*COS(ARG) 
            SSQRES=SSQRES+RESID*RESID     
  325     CONTINUE
C         USE ARRAY COV TO STORE THE ANSWER IN  
          COV(I,J)=SSQRES     
  375   CONTINUE  
  400 CONTINUE    
C     
C     NOW FIND THE MINIMUM SUM OF SQUARES OF RESIDUALS
C     
      SSQMIN=1.0E30     
      DO 420 I=1,20     
        DO 410 J=1,20   
          IF(COV(I,J).LT.SSQMIN)THEN
            IMIN=I
            JMIN=J
            SSQMIN=COV(I,J)   
          END IF  
  410   CONTINUE  
  420 CONTINUE    
C     
C     NOW NORMALIZE THE SSQRES IN COV BY THE MINIMUM  
C     
      DO 440 I=1,40     
        DO 430 J=1,40   
          COV(I,J)=COV(I,J)/SSQMIN  
  430   CONTINUE  
  440 CONTINUE    
      PRINT 450,SSQMIN  
  450 FORMAT(12X,'GOODNESS OF FIT TEST : '//    
     *       12X,'MINIMUM SUM OF SQUARES ',1PE15.5//) 
      PRINT 475,((COV(I,J),J=1,40),I=1,40)
  475 FORMAT(12X,20F6.2)
      PRINT 320   
      PRINT 500,(TESTF(J),J=1,20)   
      PRINT 500,(TESTP(J),J=1,20)   
  500 FORMAT(2(12X,1P10E12.4/))     
      RETURN
      END   
