program sinfit
c
c  least-squares-fit of sinewaves

      parameter(ndim=480000)
c
      dimension x(ndim),amp(2),phase(2),null(0:250)
      character text*72
      character*24 name(2)
      logical auto
      data name /2*'                        '/
      data auto/.true./
      write(6,120)
  120 format(/'*** Sinewave fit to input and output signal ***'/)
      write(6,*) 'File 1 (reference)?'
      read(5,'(a)') name(1)
      write(6,*) 'File 2 (signal)?'
      read(5,'(a)') name(2)
      write(6,*) 'Test frequency in mHz? (0=automatic choice)'
      read(5,*) freq
      if(freq.gt.0.) then
        auto=.false.
        per=1000./freq
      endif
c
      do 111 nfile=1,2
      n=ndim
      call input(name(nfile),text,n,dt,tmin,tsec,x)
      if(n.eq.0.or.dt.eq.0.) then
      write(6,*) 'n or dt unspecified - check data header'
      stop
      endif
      n1=1
      n2=n
      if(nfile.eq.2) goto 110
      if(.not.auto) goto 113
      nnull=0
      do 114 j=2,n
      if(x(j)*x(j-1)) 115,115,114
  115 if(nnull.lt.250) then
      if(j.le.null(nnull)+1) goto 114
      nnull=nnull+1
      null(nnull)=j
      else
      goto 116
      endif
  114 continue
  116 write(6,105)  (null(j)-null(j-1),j=2,nnull)
  105 format('half-periods found:'/(10i6))
      nper=(nnull-1)/2
      if(nper.gt.1) then
      n1=null(1)
      n2=null(2*nper+1)
      nn=n2-n1
      per=nn*dt/nper
      freq=1000./per
      write(6,101) nper,freq,per
  101 format('The record contains ',i3,' periods of the signal'/
     & 'estimated frequency',f10.3,' mHz, period',f10.3,' sec'/)
      else
      write(6,*) 'no periodicity recognized - stop'
      stop
      endif
  113 n2a=n2
      if(auto) then
        nloop=1
        fac=nper
      else
        nloop=3
        fac=3.
      endif
      do 200 loop=1,nloop
      fac=min(fac,real(int((n2a-n1+1)*dt/per)))
      len=nint(fac*per/dt)
      n2=min(n2a,n1+len-1)
      sigma=2.*freq/(n2-n1)
      write(6,*)
     &'      freq       cos       sin       amp     phase       errms'
      call sinf(x,n1,n2,dt,freq-sigma,amp(nfile),phase(nfile),e1)
      call sinf(x,n1,n2,dt,freq      ,amp(nfile),phase(nfile),e2)
      call sinf(x,n1,n2,dt,freq+sigma,amp(nfile),phase(nfile),e3)

  112 if(e1.lt.e2) then
        freq=freq-sigma
        e3=e2
        e2=e1
        call sinf(x,n1,n2,dt,freq-sigma,amp(nfile),phase(nfile),e1)
        goto 112 
      else if(e3.lt.e2) then
        freq=freq+sigma
        e1=e2
        e2=e3
        call sinf(x,n1,n2,dt,freq+sigma,amp(nfile),phase(nfile),e3)
        goto 112
      endif

      delta=(e1-e3)/(e1+e3-e2-e2)/2.
      freqalt=freq
      freq=freqalt+delta*sigma
      per=1000./freq
      write(6,103) nint((n2-n1+1)*dt/per),freq,per
  103 format(i4,' periods used: frequency=',f8.3,' mHz, period='
     & ,f8.3,' sec'/)
  200 fac=8.*fac
  110 write(6,*)
     &'      freq       cos       sin       amp     phase       errms'
  111 call sinf(x,n1,n2,dt,freq,amp(nfile),phase(nfile),errsq)
      am=amp(2)/amp(1)
      ph=phase(2)-phase(1)
      if(ph.gt.180.) ph=ph-360.
      if(ph.lt.-180.) ph=ph+360.
      write(6,104) freq,per,am,ph
  104 format(
     &/'RESULT:       frequency=',f11.4,' mHz, period=',f10.3,' sec'
     &/'              rel. gain=',f11.4,', rel. phase=',f10.3,' deg'/)
      stop
      end

c
      subroutine input(name,text,n,dt,tmin,tsec,x)
      dimension x(n)
      character iform*20,name*(*),text*(*),pro*60
      open(7,file=name,status='old')
      read(7,'(a72)') text
   21 read(7,'(a60)') pro
      if(pro(1:1).ne.'%') goto 20
      goto 21
   20 read(pro,1) nn,iform,dt,tmin,tsec
    1 format(i10,a20,3f10.3)
      write(6,5) name
    5 format(/'reading file ',a)
      if(n.lt.nn.or.nn.eq.0) then
      write(6,*) 'number of samples limited to ',n
      nn=n
      endif
      n=nn
      do 97 j=1,n
   97 x(j)=0.d0
      if(iform(1:1).eq.'*'.or.iform(2:2).eq.'*') then
      read(7,*) (x(j),j=1,n)
      else
      nin=0
      read(7,iform,end=2,err=7) (x(j),j=1,n)
      endif
      goto 3
    7 read(7,1,end=2,err=7) n,iform,dt,tmin,tsec
      goto 3
    2 n=j+nin-1
      write(6,*) 'end of data encountered'
    3 close(7)
      write(6,*) n,' samples read'
      return
      end

      subroutine output(name,par,text,n,dt,tmin,tsec,x)
      dimension z(16),x(n)
      character name*(*),text*(*),par*(*),iform*20,iform1*20
      data iform1/' ***unformatiert*** '/
      close(5)
      iform='(8f9.3)'
      if(par(1:3).eq.'nil') return
      if(par(1:3).eq.'unf') iform=iform1
      if(par(1:1).eq.'(') read(par,'(a20)') iform
      write(6,*) 'writing file ',name
      open(8,file=name)
      write(8,'(a72)') text
      write(8,1) n,iform,dt,tmin,tsec
    1 format(i10,a20,f10.6,2f10.3)
      if(iform.eq.iform1) then
      do 31 nout=0,n-1,16
      nwr=min(16,n-nout)
      do 32 j=1,nwr
   32 z(j)=x(j+nout)
   31 write(8) (z(j),j=1,nwr)
      else
      write(8,iform) (x(j),j=1,n)
      endif
      close(8)
      return
      end

      subroutine gauss(aik,m,n,rs,f)
c  solve linear equations
      dimension aik(n,n),rs(n),f(n),h(14),imax(13)
      do 1401 j=1,m
      aikmax=0.d0
      do 1402 k=1,m
      h(k)=aik(j,k)
      if(abs(h(k)).le.aikmax) go to 1402
      aikmax=abs(h(k))
      index=k
 1402 continue
      h(m+1)=rs(j)
      do 1403 k=1,m
      q=aik(k,index)/h(index)
      do 1404 l=1,m
 1404 aik(k,l)=aik(k,l)-q*h(l)
 1403 rs(k)=rs(k)-q*h(m+1)
      do 1405 k=1,m
 1405 aik(j,k)=h(k)
      rs(j)=h(m+1)
 1401 imax(j)=index
      do 1406 j=1,m
      index=imax(j)
 1406 f(index)=rs(j)/aik(j,index)
      return
      end

      subroutine sinf(x,n1,n2,dt,freq,amp,phase,errsq)
c  fist a single sinewave to the signal
      dimension x(n2),a(3,3),rs(3),c(3),f(3)
      zpi=8.*atan(1.)
      omeg=zpi*freq/1000.
c  determine partial amplitudes by least-squares fit
      do 1 i=1,3
      rs(i)=0.d0
      do 1 k=1,3
    1 a(i,k)=0.d0
c  set up system of linear equations
      c(1)=1.
      do 2 j=n1,n2
      t=(j-1)*dt
      c(2)  =cos(omeg*t)
      c(3)=sin(omeg*t)
      do 2 i=1,3
      rs(i)=rs(i)+x(j)*c(i)
      do 2 k=1,3
    2 a(i,k)=a(i,k)+c(i)*c(k)
c  solve for partial amplitudes
      call gauss(a,3,3,rs,f)
      errsq=0.
c  determine residual signal
      do 3 j=n1,n2
      t=(j-1)*dt
      c(2)=cos(omeg*t)
      c(3)=sin(omeg*t)
      xgez1=f(1)+f(2)*c(2)+f(3)*c(3)
    3 errsq=errsq+(x(j)-xgez1)**2
      amp=sqrt(f(2)**2+f(3)**2)
      phase=-360./zpi*atan2(f(3),f(2))
      write(6,9) freq,f(2),f(3),amp,phase,sqrt(errsq/(n2-n1+1))
    9 format(f10.3,3f10.3,f10.3,f12.3)
      return
      end