PROGRAM pzfit

c  Determination of complex poles of the transfer function
c  from measured amplitudes and phase

c  Based on the general-purpose inversion program UNIVERS by E. Wielandt
c  Language: NAS Fortran Plus Version 2 (compiler is no longer
c  commercially available, but other compilers should only require minor
c  modifications)
c  For data sets with more than 1200 frequencies, ndat must be increased
c  Version September 2010

      use nas_system ! this command makes runstring processing available

      parameter (mpar=24,ndat=1200)
      implicit double precision (a-h,o-z)
      character endtxt*15,filnam*30,txt*15,name*3,text(2)*55,dfile*30
      character polfile*30
      dimension x(mpar),dat1(ndat)
      common /xmap/ xxx(mpar),rho(mpar),ind(mpar),inv(mpar),name(mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd
      common /newpar/ xx(mpar),zpi,nhp1,nlp1,nhp2,nlp2
      data endtxt /'final accuracy.'/

      zero=0.d0
      one=1.d0
      zpi=8.d0*atan2(one,one)

      call param(dfile,methode)
      if(iargc().eq.1) then
        call getarg(1,dfile)
      else
        write(6,*)
        write(6,*) 'you may enter the name of the data file as as runst
     &ring parameter'
       write(6,*)
      endif
      le=index(dfile,'.')-1
      if(le.lt.1) le=len(trim(dfile))
      polfile=trim(dfile)(1:le)//'.pol'
      open(9,file=polfile)


      text(1)=' *** CONGRAD ***   Method of Conjugate Gradients'
      text(2)=' *** MATINV ***   Stabilised Matrix Inversion'
      write(6,*)
      write(6,*) text(methode)
      write(6,*) 
      write(6,1) mp,m
    1 format(1x,' Parameter mapping. ',i2,' parameters, ',i2,' active')
      write(6,'(a,12i5/9x,12i5') '     k = ',(k,k=1,mp)
      write(6,'(a,12i5/9x,12i5)') ' ind(k)= ',(ind(k),k=1,mp)
      write(6,'(a,12i5/9x,12i5)') ' inv(k)= ',(inv(k),k=1,m)
      write(6,3) maxit,dsqrt(qac),finstep
    3 format(' maxit=',i3,'   qac=',f12.9,'   finstep=',f9.6)
      write(6,*)

      call indat(dfile)

      filnam='ampli.dat      '
      txt=   'amplitude data '
      do i=1,nd
        dat1(i)=dat(i,2)
      enddo
      write(6,*)
      call output(filnam,dat1,nd,dt,txt)
      filnam='phase.dat      '
      txt=   'phase data     '
      do i=1,nd
        dat1(i)=dat(i,4)
      enddo
      call output(filnam,dat1,nd,dt,txt)


      qn=zero
      do 6 i=1,nd
    6   qn=qn+dat(i,3)*dat(i,2)**2+dat(i,5)*dat(i,4)**2

C  prepare the iteration

      mq=0
      iter=0
      step=one
      axi=step
      noimp=0
      mconj=m
      wq=one
      do 111 k=1,mp
  111 x(k)=zero

c  Start model

      q=quad(x)

      write(6,*)
      filnam='ampli.sta      '
      txt=   'amplitude start'
      do i=1,nd
        dat1(i)=syn(i,1)
      enddo
      call output(filnam,dat1,nd,dt,txt)
      filnam='phase.sta      '
      txt=   'phase start    '
      do i=1,nd
        dat1(i)=syn(i,2)
      enddo
      call output(filnam,dat1,nd,dt,txt)


      write(6,103) (name(k),k=1,mp)
  103 format(/' iter',5x,'rms-err',5(9x,a3)/(17x,5(9x,a3)))
      write(6,104)
      write(6,104) iter,dsqrt(q),(xxx(k),k=1,mp)
  104 format(i5,1x,6(1x,f11.6)/(18x,5(1x,f11.6)))
      write(6,105) (rho(k),k=1,mp)
  105 format(16x,'+-',5(1x,f11.6)/(18x,5(1x,f11.6)))
      write(6,104)

C  Iterative Parameter Fitting, Method of Conjugate Gradients
c  Step width is (between 1 and finstep) * rho

      write(6,103) (name(inv(k)),k=1,m)

      do 2 iter=1,maxit

        qalt=q
        if(methode.eq.1) then
          call congrd(x,q,gnorm)
        else
          call matin(x,q)
        endif

        write(6,104) iter,dsqrt(q),(x(k),k=1,m)

c  The iteration ends if within m steps the fit did not improve by more
c  than qac times the rms amplitude of the original signal AND no
c  parameter changed more than finstep*range.


        noimp=noimp+1
        if(dabs(qalt-q).gt.qac.or.axi.gt.polstep) noimp=0
        if(noimp.ge.m.or.axi.lt.finstep/100.d0) goto 5
    2 continue

      endtxt='no convergence (but results may be OK)'
      iter=maxit

    5 write(6,104)
      write(6,109)
  109 format(' final values of the active parameters:')
      write(6,103) (name(inv(k)),k=1,m)
      write(6,104)
      write(6,104) iter,dsqrt(q),(xxx(inv(k))+rho(inv(k))*x(k),k=1,m)
      write(6,108) mq,endtxt
  108 format(/' Complex Response was calculated ',i5,' times. ',a15)
      write(6,104)


      write(9,140) trim(dfile)
      write(6,140) trim(dfile)
  140 format('POLES AND ZEROS (Laplace, angular fr.) for data file ',a)
      write(9,*)
      write(6,*)
      write(9,*) nhp1,'  first-order high-pass sections (HP1)'
      write(6,*) nhp1,'  first-order high-pass sections (HP1)'
      write(9,*) nlp1,'  first-order  low-pass sections (LP1)'
      write(6,*) nlp1,'  first-order  low-pass sections (LP1)'
      write(9,*) nhp2,' second-order high-pass sections (HP2)'
      write(6,*) nhp2,' second-order high-pass sections (HP2)'
      write(9,*) nlp2,' second-order  low-pass sections (LP2)'
      write(6,*) nlp2,' second-order  low-pass sections (LP2)'
      write(9,*) nhp1+2*nhp2,' zeros at zero frequency'
      write(6,*) nhp1+2*nhp2,' zeros at zero frequency'
      write(9,*)
      write(6,*)
      write(9,'("Param.         value        pole re        pole im
     &     type"/)')
      write(6,'("Param.         value        pole re        pole im
     &     type"/)')

      n1=nhp1    
      n2=n1+nlp1 
      n3=n2+nhp2   
      n4=n3+nlp2  
      a0=one
      qre=one
      qim=zero
      fr=one
      frq=fr**2

      gain=xx(1)
      write(9,'(2x,a3,f15.6,35x,a)') name(1),xx(1),'gain'
      write(6,'(2x,a3,f15.6,35x,a)') name(1),xx(1),'gain'

      do k=1,n1
        k1=k+1
        f0=xx(k1)
        pre=-ZPI*f0
        write(9,*)
        write(6,*)
          call cmul(qre,qim,0.d0,fr,qre,qim)
          call cdiv(qre,qim,f0,fr,qre,qim)
        write(9,130) name(k1),xx(k1),pre,zero,'HP1'
        write(6,130) name(k1),xx(k1),pre,zero,'HP1'
  130   format(2x,a3,3f15.6,5x,a)
      enddo
      do k=n1+1,n2
        k1=k+1
        f0=xx(k1)
        pre=-ZPI*f0
        a0=a0*ZPI*f0
          call cmul(qre,qim,f0,0.d0,qre,qim)
          call cdiv(qre,qim,f0,fr,qre,qim)
        write(9,*)
        write(6,*)
        write(9,130) name(k1),xx(k1),pre,zero,'LP1'
        write(6,130) name(k1),xx(k1),pre,zero,'LP1'
      enddo

      do k=n2+1,n3
      k2=2*k-n2
      k3=k2+1
        f0=xx(k2)
        d=xx(k3)
          qre = -frq * qre
          qim = -frq * qim
          call cdiv(qre,qim,f0**2-frq,2.*fr*f0*d,qre,qim)
        if(d.lt.one)then
          pre=-ZPI*f0*d
          pim= ZPI*f0*dsqrt(one-d**2)
          write(9,*)
          write(6,*)
          write(9,130) name(k2),xx(k2),pre,pim,'HP2'
          write(6,130) name(k2),xx(k2),pre,pim,'HP2'
          write(9,130) name(k3),xx(k3),pre,-pim
          write(6,130) name(k3),xx(k3),pre,-pim
        else
          pre=-ZPI*f0*(d+dsqrt(d**2-one))
          pim=-ZPI*f0*(d-dsqrt(d**2-one))
        write(9,*)
        write(6,*)
        write(9,130) name(k2),xx(k2),pre,zero,'HP2'
        write(6,130) name(k2),xx(k2),pre,zero,'HP2'
        write(9,130) name(k3),xx(k3),pim,zero
        write(6,130) name(k3),xx(k3),pim,zero
        endif
      enddo
      do k=n3+1,n4
      k2=2*k-n2
      k3=k2+1
        f0=xx(k2)
        a0=a0*(ZPI*f0)**2
        d=xx(k3)
          qre = f0**2 * qre
          qim = f0**2 * qim
          call cdiv(qre,qim,f0**2-frq,2*fr*f0*d,qre,qim)
        if(d.lt.one)then
          pre=-ZPI*f0*d
          pim= ZPI*f0*dsqrt(one-d**2)
          write(9,*)
          write(6,*)
          write(9,130) name(k2),xx(k2),pre,pim,'LP2'
          write(6,130) name(k2),xx(k2),pre,pim,'LP2'
          write(9,130) name(k3),xx(k3),pre,-pim
          write(6,130) name(k3),xx(k3),pre,-pim
        else
          pre=-ZPI*f0*(d+dsqrt(d**2-one))
          pim=-ZPI*f0*(d-dsqrt(d**2-one))
        write(9,*)
        write(6,*)
        write(9,130) name(k2),xx(k2),pre,zero,'LP2'
        write(6,130) name(k2),xx(k2),pre,zero,'LP2'
        write(9,130) name(k3),xx(k3),pim,zero
        write(6,130) name(k3),xx(k3),pim,zero
        endif
      enddo

      write(6,*)
      write(9,*)
      write(6,120) dsqrt(q)
      write(9,120) dsqrt(q)
  120 format('  rms misfit with these parameters is ',f10.6)
      write(6,*)
      write(9,*)
      write(6,150) a0/sqrt(qre**2+qim**2) 
      write(9,150) a0/sqrt(qre**2+qim**2)
  150 format('  SEED normalization factor A0 at 1 Hz is ',e13.6)
      write(6,*)
      write(9,*)
      close(9)
      filnam='ampli.end      '
      txt=   'amplitude end  '
      do i=1,nd
        dat1(i)=syn(i,1)
      enddo
      call output(filnam,dat1,nd,dt,txt)
      filnam='phase.end      '
      txt=   'phase end      '
      do i=1,nd
        dat1(i)=syn(i,2)
      enddo
      call output(filnam,dat1,nd,dt,txt)
      filnam='ampli.mis      '
      txt=   'amplit. misfit '
      do 7 j=1,nd
    7 dat1(j)=dat(j,2)-syn(j,1)
      call output(filnam,dat1,nd,dt,txt)
      filnam='phase.mis      '
      txt=   'phase misfit   '                  
      do 8 j=1,nd
    8 dat1(j)=dat(j,4)-syn(j,2)
      call output(filnam,dat1,nd,dt,txt)
      
      write(6,*) 'Final parameters have been saved to ',polfile

      stop
      end


      function quad(x)
      parameter (mpar=24,ndat=1200)
      implicit double precision (a-h,o-z)
      dimension x(mpar) 
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd

c compute synthetic response

      call kurve(x)

c compute sum-squared-error

      qua=zero
      do 5 i=1,nd
    5 qua=qua+dat(i,3)*(syn(i,1)-dat(i,2))**2
     &       +dat(i,5)*(syn(i,2)-dat(i,4))**2

      quad=qua/qn
      return
      end

      subroutine matin(x,q)
c  Matrix inversion (alternative to Conjugate Gradients)
      parameter (mpar=24,ndat=1200)
      implicit double precision (a-h,o-z)
      dimension x(mpar),a(ndat,2,mpar),ata(mpar,mpar),rs(mpar),rsw(mpar) 
      dimension syn0(ndat,2),dx(mpar),x1(mpar),x2(mpar),atw(mpar,mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd
      diag=qn*qac*nd/mp
      
c  partial derivatives

      do 1 i=1,nd
      do 1 ii=1,2
    1   syn0(i,ii)=syn(i,ii)
      xinc=finstep
      do 2 k=1,mp
        x(k)=x(k)+xinc
        call kurve(x)
        do 3 i=1,nd
        do 3 ii=1,2
    3     a(i,ii,k)=(syn(i,ii)-syn0(i,ii))/xinc
    2     x(k)=x(k)-xinc

c build system of linear equations

      do 5 k=1,mp
      do 5 j=1,mp
        at=zero
        do 6 i=1,nd
        do 6 ii=1,2
    6     at=at+a(i,ii,k)*a(i,ii,j)
    5 ata(j,k)=at
      do 7 j=1,mp
        rs(j)=zero
        do 7 i=1,nd
        do 7 ii=1,2
    7     rs(j)=rs(j)+a(i,ii,j)*(dat(i,2*ii)-syn0(i,ii))

c  search for optimum stabilizing weight wq

      wq1=wq
      q0=q
      do 4 k=1,mp
      do 9 j=1,mp
    9 atw(j,k)=ata(j,k)
      rsw(k)=rs(k)
    4 atw(k,k)=atw(k,k)+diag*wq1
      call gauss(atw,mp,rsw,dx)
      ax1=zero
      do 8 j=1,mp
        ax1=ax1+dx(j)**2
    8   x1(j)=x(j)+dx(j)
      ax1=dsqrt(ax1)
      q1= quad(x1)
      if(q1.ge.q) then
        wq2=wq1*8
      else
        wq2=wq1/8
      endif
      do 14 k=1,mp
        do 19 j=1,mp
   19     atw(j,k)=ata(j,k)
        rsw(k)=rs(k)
   14   atw(k,k)=atw(k,k)+diag*wq2
      call gauss(atw,mp,rsw,dx)
      ax2=zero
      do 18 j=1,mp
        ax2=ax2+dx(j)**2
   18   x2(j)=x(j)+dx(j)
      ax2=dsqrt(ax2)
      q2= quad(x2)

      if(q1.lt.q0.and.q1.le.q2) then
        wq=wq1
        axi=ax1
        q=q1
        do 21 k=1,mp
   21   x(k)=x1(k)
      else if(q2.lt.q0.and.q2.le.q1) then
        wq=wq2
        axi=ax2
        q=q2
        do 22 k=1,mp
   22   x(k)=x2(k)
      else if(q1.gt.q0+qac.and.q2.gt.q0+qac) then
        axi=one
        wq=wq*8
      else
        axi=zero   
      endif
c     write(6,25) '  q0=',q,'  q1=',q1,'  q2=',q2,'  wq=',wq
   25 format(4(a5,f10.6))
      return
      end

    

      subroutine gauss(aik,m,rs,f)
c  solve linear equations
      parameter(mpar=24)
      implicit double precision (a-h,o-z)
      dimension aik(mpar,mpar),rs(mpar),f(mpar),h(mpar),imax(mpar)
      do 1401 j=1,m
      aikmax=0.d0
      do 1402 k=1,m
      h(k)=aik(j,k)
      if(dabs(h(k)).le.aikmax) go to 1402
      aikmax=dabs(h(k))
      index=k
 1402 continue
      hh=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*hh
      do 1405 k=1,m
 1405 aik(j,k)=h(k)
      rs(j)=hh
 1401 imax(j)=index
      do 1406 j=1,m
      index=imax(j)
 1406 f(index)=rs(j)/aik(j,index)
      return
      end


      subroutine congrd(x,q,gnorm)
c  Method of Conjugate Gradients nach Fletcher und Reeves (1964)
      parameter (mpar=24)
      implicit double precision (a-h,o-z)
      dimension x(mpar),g(mpar),d(mpar),dd(mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two

c  partial derivatives

      do 1 k=1,m
        x(k)=x(k)+finstep
        qq=quad(x)
        x(k)=x(k)-finstep-finstep
        qqq=quad(x)
        x(k)=x(k)+finstep
    1   g(k)=(qq-qqq)/two/finstep

c  determine new direction of descent

      if(mconj.ge.m.or.step.ge.one) then
        write(6,6)
    6   format(1x)
        gnorm=0.
        do 2 k=1,m
          gnorm=gnorm+g(k)**2
    2     d(k)=-g(k)
        mconj=1

      else
        ga=gnorm
        if(gnorm.eq.zero) then
          axi=gnorm
          return
        endif
        gnorm=zero
        do 3 k=1,m
    3     gnorm=gnorm+g(k)**2
        beta=gnorm/ga
        do 4 k=1,m
    4     d(k)=-g(k)+beta*d(k)
        mconj=mconj+1
      endif

      dlen=0.
      do 5 k=1,m
    5   dlen=dlen+d(k)**2
      dlen=dsqrt(dlen)
      if(dlen.eq.zero) then
        axi=dlen
        return
      endif

      do 8 k=1,m
    8 dd(k)=d(k)/dlen

c  direction has been determined, now find minimum in this direction

      call mini(x,q,dd)
      return
      end


      subroutine mini(x,q,d)

c  x and q are first start values, later optimal values
c  step width between 1 and finstep (times range)

      parameter (mpar=24)
      implicit double precision (a-h,o-z)
      logical tr
      dimension x(mpar),xl(mpar),xm(mpar),xr(mpar),xx(mpar),d(mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      tr=.false.

      call copy(x,q,xm,qm)

c  try one step left and right
  100 do 1 k=1,m
        xl(k)=xm(k)-step*d(k)
    1   xr(k)=xm(k)+step*d(k)

      ql=quad(xl)
      qr=quad(xr)

      if(ql.lt.qm.and.qr.lt.qm) then
c  This should not happe. If it does, halve step
        step=half*step
        write(6,'(a,f6.3)') ' qm maximal, half*step=',step
        if(step.lt.finstep) then
          write(6,'(a)') ' Fehler: q hat Maximum! STOP'
        else
          goto 100
        endif
      endif

c  check where q is minimum

 110  if(ql.lt.qm.and.ql.lt.qr) then
c  umdrehen, damit qr minimal wird, dann weiter mit xr
        call copy(xl,ql,xx,q0)
        call copy(xr,qr,xl,ql)
        call copy(xx,q0,xr,qr)
        do 9 k=1,m
    9     d(k)=-d(k)
      if(tr) write(6,'(a,f6.3)') ' ql minimal,    -step=',step
        goto 110

      else if(qr.lt.ql.and.qr.lt.qm) then
c  check if minimum is bracketed. If yes: interpolate.
c  If no: . Wenn nein: double step

        xi=(ql-qr)/(ql-two*qm+qr)/two*step
        axi=dabs(xi)
        if(axi.le.step) then
          step=max(axi,finstep)
          do 6 k=1,m
    6       x(k)=xm(k)+xi*d(k)
          q=quad(x)
      if(tr) write(6,'(a,f6.3)') ' qr minimal, qI, step=',step
        else
          step=two*step
          call copy(xr,qr,xm,qm)
          do 111 k=1,m
  111       xr(k)=xm(k)+step*d(k)
          qr=quad(xr)
      if(tr) write(6,'(a,f6.3)') ' qr minimal, two*step=',step
          goto 110
        endif

      else
c  interpolate and determine new step width
  112   xi=(ql-qr)/(ql-two*qm+qr)/two*step
        axi=dabs(xi)
        step=min(one,max(xi,finstep))
        do 7 k=1,m
    7     x(k)=xm(k)+xi*d(k)
        q=quad(x)
      if(tr) write(6,'(a,f6.3)') ' qm minimal, qI, step=',step
      endif

      return
      end


      subroutine copy(x1,q1,x2,q2)
c   x und q von 1 auf 2 kopieren
      parameter (mpar=24)
      implicit double precision (a-h,o-z)
      dimension x1(mpar),x2(mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      do 1 k=1,m
    1   x2(k)=x1(k)
      q2=q1
      return
      end

      subroutine indat(filnam)
      parameter(ndat=1200)
      implicit double precision(a-h,o-z)
      character filnam*30,form*20,text*50,zeile*80
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd
      write(6,*) 'reading data file ',trim(filnam),'       Header line:'
      open(7,file=filnam,status='old')
      read(7,'(a50)') text
      write(6,'(a50)') text
      i=0
      if(trim(filnam).eq.'response.lst') then
        do
          read(7,'(a)',end=11) zeile
          if (len_trim(zeile).lt.5) exit
          i=i+1
          read(zeile,*,err=100) dat(i,1),dat(i,2),dat(i,4)
          dat(i,3)=1.d0
          dat(i,5)=1.d0
        enddo
   11   nd=i
        dt=1.d0
      else
        do
          read(7,'(a)',end=12) zeile
          if (len_trim(zeile).lt.9) exit
          i=i+1
          read(zeile,*,err=100) (dat(i,k),k=1,5)
        enddo
   12   nd=i
        dt=1.d0
      endif
      close(7)
      do i=1,nd
        if(dat(i,2).gt.0.d0) then
          dat(i,2)=log10(dat(i,2))
        endif
      enddo
      write(6,*) nd,' lines of data read from file ',trim(filnam)
      return
  100 write(6,*) 'Error in file ',trim(filnam),', line i=',i+1
      stop
      end


      subroutine output(filnam,x,n,dt,text)
      implicit double precision (a-h,o-z)
      dimension x(n)
      character filnam*30,text*15,form*10
      form='(f12.6)   '
      open(8,file=filnam)
      write(8,'(a)') text
      write(8,3) n,form,dt,0.,0.
    3 format(i10,a10,10x,3f10.6)
      write(8,form) (x(i),i=1,n)
      close(8)
      write(6,*) n,' values ',text,' written into file ',filnam
      return
      end


      subroutine param(filnam,methode)
c  read parameter file
      parameter (mpar=24,ndat=1200)
      implicit double precision (a-h,o-z)
      character filnam*30,name*3
      common /xmap/ xxx(mpar),rho(mpar),ind(mpar),inv(mpar),name(mpar)
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd
      common /newpar/ xx(mpar),zpi,nhp1,nlp1,nhp2,nlp2
      zero=0.d0
      half=0.5d0
      one=1.d0
      two=2.d0
      open(11,file='pzfit.par',status='old')
      read(11,*)
      read(11,'(a15)') filnam
      read(11,*) methode
      read(11,*) mp
      read(11,*) maxit
      read(11,*) qac
      qac=qac**2
      read(11,*) finstep
      read(11,*)
      read(11,*) nhp1,nlp1,nhp2,nlp2

      if(mp.ne.1+nhp1+nlp1+2*nhp2+2*nlp2) then
        write(6,*) 'inconsistent number of parameters'
        stop
      endif

      do 1 k=1,mp
    1 read(11,*) xxx(k),rho(k),name(k)
      m=0
      do k=1,mp
        if(rho(k).gt.zero) then
          m=m+1
          inv(m)=k
          ind(k)=m
        else
          ind(k)=0
        endif
      enddo
      close(11)
      return
      end


      subroutine kurve(x)
c  calculate synthetic complex response
      parameter (mpar=24,ndat=1200)
      implicit double precision (a-h,o-z)
      character name*3
      common /aux/ step,finstep,axi,qac,qn,wq,mq,m,mp,maxit,mconj
      common /const/ zero,half,one,two
      common /data/ syn(ndat,2),dat(ndat,5),dt,nd
      dimension x(mpar),f0(mpar),d(mpar)
      common /xmap/ xxx(mpar),rho(mpar),ind(mpar),inv(mpar),name(mpar)
      common /newpar/ xx(mpar),zpi,nhp1,nlp1,nhp2,nlp2
      logical test

c  x are normalized parameter corrections, xx are the actual parameters,
c  xxx are the start values. Their number is mp, limited to mpar.
c  m parameters are active
c  Which parameters are active and passive is coded by ind and inv.
c  We first reconstruct the current parameter values.

       do i=1,mp
         if(ind(i).gt.0) then
           xx(i)=xxx(i)+rho(i)*x(ind(i))
         else
           xx(i)=xxx(i)
         endif
       enddo

c  the following code is specific to the problem. As a result, syn(i,1)
c  and syn(i,2) contain the synthetics calculated from the current
c  parameters, here: amplitude und phase of teh transfer function.
c
c The data are organized as follows:
c  dat(i,1) frequency
c  dat(i,2) amplitude
c  dat(i,3) weight of the error of the logarithmic amplitude
c  dat(i,4) phase
c  dat(i,5) weight of the phase error
c
c  We fit:: syn(i,1) to dat(i,2) und syn(i,2) to dat(i,4)
c

      n1=nhp1    
      n2=n1+nlp1 
      n3=n2+nhp2   
      n4=n3+nlp2  

      test=.false.

      if(test) then
c  testing the interpretation of parameters
        gain=xx(1)
        write(6,10) 0,1,gain,name(1)
   10   format(' system #',i2,'   parameter #',i2,f12.3,3x,a3)
        do k=1,n1
          f0(k)=xx(k+1)
          write(6,10) k,k+1,f0(k),name(k+1)
        enddo
        do k=n1+1,n2
          f0(k)=xx(k+1)
          write(6,10) k,k+1,f0(k),name(k+1)
        enddo
        do k=n2+1,n3
        k2=2*k-n2
        k3=k2+1
          f0(k)=xx(k2)
          d(k)=xx(k3)
          write(6,10) k,k2,f0(k),name(k2)
          write(6,10) k,k3,d(k),name(k3)
        enddo
        do k=n3+1,n4
          k2=2*k-n2
          k3=k2+1
          f0(k)=xx(k2)
          d(k)=xx(k3)
          write(6,10) k,k2,f0(k),name(k2)
          write(6,10) k,k3,d(k),name(k3)
        enddo
        stop
      endif

c  calculate complex response


      gain=xx(1)
      fab=1.d6
      do i=1,nd
        fr = dat(i,1)
        if(abs(fr-1.d0).lt.fab) then
          fab=abs(fr-1.d0)
          i1=i
        endif
        frq = fr**2
        qre = gain
        qim = 0.d0

        do k=1,n1
          f0(k)=xx(k+1)
          call cmul(qre,qim,0.d0,fr,qre,qim)
          call cdiv(qre,qim,f0(k),fr,qre,qim)
        enddo

        do k=n1+1,n2
          f0(k)=xx(k+1)
          call cmul(qre,qim,f0(k),0.d0,qre,qim)
          call cdiv(qre,qim,f0(k),fr,qre,qim)
        enddo

        do k=n2+1,n3
          k2=2*k-n2
          k3=k2+1
          f0(k)=xx(k2)
          d(k)=xx(k3)
          qre = -frq * qre
          qim = -frq * qim
          call cdiv(qre,qim,f0(k)**2-frq,2.*fr*f0(k)*d(k),qre,qim)
        enddo

        do k=n3+1,n4
          k2=2*k-n2
          k3=k2+1
          f0(k)=xx(k2)
          d(k)=xx(k3)
          qre = f0(k)**2 * qre
          qim = f0(k)**2 * qim
          call cdiv(qre,qim,f0(k)**2-frq,2*fr*f0(k)*d(k),qre,qim)
        enddo

        syn(i,1) = 0.5 * Log10(qre**2+qim**2)
        syn(i,2) = atan2(qim,qre)
      enddo

c  make the phase monotonous
      picor = ZPI
      syn(1,2) = syn(1,2) + picor
      do i = 2, nd
        syn(i,2) = syn(i,2) + picor
        If(syn(i,2).gt.syn(i - 1,2)) Then
          syn(i,2) = syn(i,2) - ZPI
          picor = picor - ZPI
        endif
      enddo
c  bring the phase at midband (1 Hz) close to zero
      icor=nint(syn(i1,2)/ZPI)
      do i=1,nd
        syn(i,2)=syn(i,2)-icor*ZPI
      enddo

      mq=mq+1
      return
      end

c     z = x * y complex
      subroutine cmul(xre, xim, yre, yim, zre, zim)
      implicit double precision (a-h,o-z)
      zr  = xre * yre - xim * yim
      zim = xre * yim + xim * yre
      zre = zr
      return
      end

c     z = x / y complex
      subroutine cdiv(xre, xim, yre, yim, zre, zim)
      implicit double precision (a-h,o-z)
      yq = yre**2 + yim**2
      zr  = (xre * yre + xim * yim) / yq
      zim = (xim * yre - xre * yim) / yq
      zre = zr
      return
      end