      program sndrv6
c
c     Simple program to illustrate the idea of reverse communication
c     in shift-invert mode for a generalized nonsymmetric eigenvalue problem.
c
c     We implement example six of ex-nonsym.doc in DOCUMENTS directory
c
c\Example-6
c
c     ... Suppose we want to solve A*x = lambda*B*x in shift-invert mode
c         The matrix A is the tridiagonal matrix with 2 on the diagonal,
c         -2 on the subdiagonal and 3 on the superdiagonal.  The matrix M
c         is the tridiagonal matrix with 4 on the diagonal and 1 on the
c         off-diagonals.
c     ... The shift sigma is a complex number (sigmar, sigmai).
c     ... OP = Imaginary_Part{inv[A-(SIGMAR,SIGMAI)*M]*M  and  B = M.
c     ... Use mode 4 of SNAUPD.
c
c\BeginLib
c
c\Routines called:
c     snaupd  ARPACK reverse communication interface routine.
c     sneupd  ARPACK routine that returns Ritz values and (optionally)
c             Ritz vectors.
c     cgttrf  LAPACK complex matrix factorization routine.
c     cgttrs  LAPACK complex linear system solve routine.
c     slapy2  LAPACK routine to compute sqrt(x**2+y**2) carefully.
c     saxpy   Level 1 BLAS that computes y <- alpha*x+y.
c     sdot    Level 1 BLAS that computes the dot product of two vectors.
c     snrm2   Level 1 BLAS that computes the norm of a vector.
c     av      Matrix vector subroutine that computes A*x.
c     mv      Matrix vector subroutine that computes M*x.
c
c\Author
c     Richard Lehoucq
c     Danny Sorensen
c     Chao Yang
c     Dept. of Computational &
c     Applied Mathematics
c     Rice University
c     Houston, Texas
c
c\SCCS Information: @(#)
c FILE: ndrv6.F   SID: 2.5   DATE OF SID: 10/17/00   RELEASE: 2
c
c\Remarks
c     1. None
c
c\EndLib
c-------------------------------------------------------------------------
c
c     %-----------------------------%
c     | Define leading dimensions   |
c     | for all arrays.             |
c     | MAXN:   Maximum dimension   |
c     |         of the A allowed.   |
c     | MAXNEV: Maximum NEV allowed |
c     | MAXNCV: Maximum NCV allowed |
c     %-----------------------------%
c
      integer           maxn, maxnev, maxncv, ldv
      parameter         (maxn=256, maxnev=10, maxncv=25,
     &                   ldv=maxn )
c
c     %--------------%
c     | Local Arrays |
c     %--------------%
c
      integer           iparam(11), ipntr(14), ipiv(maxn)
      logical           select(maxncv)
      Real
     &                  ax(maxn), mx(maxn), d(maxncv,3), resid(maxn),
     &                  v(ldv,maxncv), workd(3*maxn),
     &                  workev(3*maxncv),
     &                  workl(3*maxncv*maxncv+6*maxncv)
      Complex
     &                  cdd(maxn), cdl(maxn), cdu(maxn),
     &                  cdu2(maxn), ctemp(maxn)
c
c     %---------------%
c     | Local Scalars |
c     %---------------%
c
      character         bmat*1, which*2
      integer           ido, n, nev, ncv, lworkl, info, ierr, j,
     &                  nconv, maxitr, ishfts, mode
      Real
     &                  tol, numr, numi, denr, deni, sigmar, sigmai
      Complex
     &                  c1, c2, c3
      logical           first, rvec
c
c     %-----------------------------%
c     | BLAS & LAPACK routines used |
c     %-----------------------------%
c
      external          cgttrf, cgttrs
      Real
     &                  sdot, snrm2, slapy2
      external          sdot, snrm2, slapy2
c
c     %------------%
c     | Parameters |
c     %------------%
c
      Real
     &                   zero
      parameter          (zero = 0.0E+0)
c
c     %--------------------%
c     | Intrinsic Function |
c     %--------------------%
c
      intrinsic          aimag, cmplx, abs
c
c     %-----------------------%
c     | Executable statements |
c     %-----------------------%
c
c     %----------------------------------------------------%
c     | The number N is the dimension of the matrix.  A    |
c     | generalized eigenvalue problem is solved (BMAT =   |
c     | 'G').  NEV is the number of eigenvalues (closest   |
c     | to the shift (SIGMAR,SIGMAI)) to be approximated.  |
c     | Since the shift-invert mode is used, WHICH is set  |
c     | to 'LM'.  The user can modify NEV, NCV, SIGMA to   |
c     | solve problems of different sizes, and to get      |
c     | different parts of the spectrum. However, The      |
c     | following conditions must be satisfied:            |
c     |                     N <= MAXN,                     |
c     |                   NEV <= MAXNEV,                   |
c     |               NEV + 2 <= NCV <= MAXNCV             |
c     %----------------------------------------------------%
c
      n     = 100
      nev   = 4
      ncv   = 20
      if ( n .gt. maxn ) then
         print *, ' ERROR with _NDRV6: N is greater than MAXN '
         go to 9000
      else if ( nev .gt. maxnev ) then
         print *, ' ERROR with _NDRV6: NEV is greater than MAXNEV '
         go to 9000
      else if ( ncv .gt. maxncv ) then
         print *, ' ERROR with _NDRV6: NCV is greater than MAXNCV '
         go to 9000
      end if
      bmat  = 'G'
      which = 'LM'
      sigmar = 4.0E-1
      sigmai = 6.0E-1
c
c     %----------------------------------------------------%
c     | Construct C = A - (SIGMAR,SIGMAI)*M in complex     |
c     | arithmetic, and factor C in complex arithmetic     |
c     | (using LAPACK subroutine cgttrf). The matrix A is  |
c     | chosen to be the tridiagonal matrix with -2 on the |
c     | subdiagonal, 2 on the diagonal and 3 on the        |
c     | superdiagonal. The matrix M is chosen to be the    |
c     | symmetric tridiagonal matrix with 4 on the         |
c     | diagonal and 1 on the off-diagonals.               |
c     %----------------------------------------------------%
c
      c1 = cmplx(-2.0E+0-sigmar, -sigmai)
      c2 = cmplx( 2.0E+0-4.0E+0*sigmar, -4.0E+0*sigmai)
      c3 = cmplx( 3.0E+0-sigmar, -sigmai)
c
      do 10 j = 1, n-1
         cdl(j) = c1
         cdd(j) = c2
         cdu(j) = c3
  10  continue
      cdd(n) = c2
c
      call cgttrf(n, cdl, cdd, cdu, cdu2, ipiv, ierr)
      if ( ierr .ne. 0 ) then
         print*, ' '
         print*, ' ERROR with _gttrf in _NDRV6.'
         print*, ' '
         go to 9000
      end if
c
c     %-----------------------------------------------------%
c     | The work array WORKL is used in SNAUPD as           |
c     | workspace.  Its dimension LWORKL is set as          |
c     | illustrated below.  The parameter TOL determines    |
c     | the stopping criterion. If TOL<=0, machine          |
c     | precision is used.  The variable IDO is used for    |
c     | reverse communication, and is initially set to 0.   |
c     | Setting INFO=0 indicates that a random vector is    |
c     | generated in SNAUPD to start the Arnoldi iteration. |
c     %-----------------------------------------------------%
c
      lworkl = 3*ncv**2+6*ncv
      tol    = zero
      ido    = 0
      info   = 0
c
c     %---------------------------------------------------%
c     | This program uses exact shift with respect to     |
c     | the current Hessenberg matrix (IPARAM(1) = 1).    |
c     | IPARAM(3) specifies the maximum number of Arnoldi |
c     | iterations allowed.  Mode 3 of SNAUPD is used     |
c     | (IPARAM(7) = 3).  All these options can be        |
c     | changed by the user. For details, see the         |
c     | documentation in SNAUPD.                          |
c     %---------------------------------------------------%
c
      ishfts = 1
      maxitr = 300
      mode   = 3
c
      iparam(1) = ishfts
      iparam(3) = maxitr
      iparam(7) = mode
c
c     %------------------------------------------%
c     | M A I N   L O O P(Reverse communication) |
c     %------------------------------------------%
c
 20   continue
c
c        %---------------------------------------------%
c        | Repeatedly call the routine SNAUPD and take |
c        | actions indicated by parameter IDO until    |
c        | either convergence is indicated or maxitr   |
c        | has been exceeded.                          |
c        %---------------------------------------------%
c
         call snaupd ( ido, bmat, n, which, nev, tol, resid,
     &                 ncv, v, ldv, iparam, ipntr, workd,
     &                 workl, lworkl, info )
c

         if (ido .eq. -1) then
c
c           %------------------------------------------------------------%
c           |                           Perform                          |
c           | y <--- OP*x = Imaginary_Part{inv[A-(SIGMAR,SIGMAI)*M]*M*x} |
c           | to force starting vector into the range of OP. The user    |
c           | should supply his/her own matrix vector multiplication     |
c           | routine and a complex linear system solver.  The matrix    |
c           | vector multiplication routine should take workd(ipntr(1))  |
c           | as the input. The final result (a real vector) should be   |
c           | returned to workd(ipntr(2)).                               |
c           %------------------------------------------------------------%
c
            call mv (n, workd(ipntr(1)), workd(ipntr(2)))
            do 30 j = 1, n
               ctemp(j) = cmplx(workd(ipntr(2)+j-1))
  30        continue
c
            call cgttrs('N', n, 1, cdl, cdd, cdu, cdu2, ipiv,
     &                  ctemp, maxn, ierr)
            if ( ierr .ne. 0 ) then
               print*, ' '
               print*, ' ERROR with _gttrs in _NDRV6.'
               print*, ' '
               go to 9000
            end if
            do  40 j = 1, n
               workd(ipntr(2)+j-1) = aimag(ctemp(j))
  40        continue
c
c           %-----------------------------------------%
c           | L O O P   B A C K to call SNAUPD again. |
c           %-----------------------------------------%
c
            go to 20
c
         else if ( ido .eq. 1) then
c
c           %------------------------------------------------------------%
c           |                          Perform                           |
c           | y <--- OP*x = Imaginary_Part{inv[A-(SIGMAR,SIGMAI)*M]*M*x} |
c           | M*x has been saved in workd(ipntr(3)). The user only need  |
c           | the complex linear system solver here that takes           |
c           | complex[workd(ipntr(3))] as input, and returns the result  |
c           | to workd(ipntr(2)).                                        |
c           %------------------------------------------------------------%
c
            do 50 j = 1,n
               ctemp(j) = cmplx(workd(ipntr(3)+j-1))
  50        continue
            call cgttrs ('N', n, 1, cdl, cdd, cdu, cdu2, ipiv,
     &                   ctemp, maxn, ierr)
            if ( ierr .ne. 0 ) then
               print*, ' '
               print*, ' ERROR with _gttrs in _NDRV6.'
               print*, ' '
               go to 9000
            end if
            do  60 j = 1, n
               workd(ipntr(2)+j-1) = aimag(ctemp(j))
  60        continue
c
c           %-----------------------------------------%
c           | L O O P   B A C K to call SNAUPD again. |
c           %-----------------------------------------%
c
            go to 20
c
         else if ( ido .eq. 2) then
c
c           %---------------------------------------------%
c           |          Perform  y <--- M*x                |
c           | Need matrix vector multiplication routine   |
c           | here that takes workd(ipntr(1)) as input    |
c           | and returns the result to workd(ipntr(2)).  |
c           %---------------------------------------------%
c
            call mv (n, workd(ipntr(1)), workd(ipntr(2)))
c
c           %-----------------------------------------%
c           | L O O P   B A C K to call SNAUPD again. |
c           %-----------------------------------------%
c
            go to 20
c
         end if
c
c     %-----------------------------------------%
c     | Either we have convergence, or there is |
c     | an error.                               |
c     %-----------------------------------------%
c
c
      if ( info .lt. 0 ) then
c
c        %--------------------------%
c        | Error message, check the |
c        | documentation in SNAUPD. |
c        %--------------------------%
c
         print *, ' '
         print *, ' Error with _naupd, info = ',info
         print *, ' Check the documentation of _naupd.'
         print *, ' '
c
      else
c
c        %-------------------------------------------%
c        | No fatal errors occurred.                 |
c        | Post-Process using SNEUPD.                |
c        |                                           |
c        | Computed eigenvalues may be extracted.    |
c        |                                           |
c        | Eigenvectors may also be computed now if  |
c        | desired.  (indicated by rvec = .true.)    |
c        %-------------------------------------------%
c
         rvec = .true.
         call sneupd ( rvec, 'A', select, d, d(1,2), v, ldv,
     &        sigmar, sigmai, workev, bmat, n, which, nev, tol,
     &        resid, ncv, v, ldv, iparam, ipntr, workd,
     &        workl, lworkl, ierr )
c
c        %-----------------------------------------------%
c        | The real part of the eigenvalue is returned   |
c        | in the first column of the two dimensional    |
c        | array D, and the IMAGINARY part is returned   |
c        | in the second column of D.  The corresponding |
c        | eigenvectors are returned in the first NEV    |
c        | columns of the two dimensional array V if     |
c        | requested.  Otherwise, an orthogonal basis    |
c        | for the invariant subspace corresponding to   |
c        | the eigenvalues in D is returned in V.        |
c        %-----------------------------------------------%
c
         if ( ierr .ne. 0) then
c
c           %------------------------------------%
c           | Error condition:                   |
c           | Check the documentation of SNEUPD. |
c           %------------------------------------%
c
            print *, ' '
            print *, ' Error with _neupd, info = ', ierr
            print *, ' Check the documentation of _neupd. '
            print *, ' '
c
         else
c
            first = .true.
            nconv =  iparam(5)
            do 70 j=1, nconv
c
c               %-------------------------------------%
c               | Use Rayleigh Quotient to recover    |
c               | eigenvalues of the original problem.|
c               %-------------------------------------%
c
                if ( d(j,2) .eq. zero) then
c
c                  %----------------------------%
c                  | Eigenvalue is real.        |
c                  | Compute d = x'(Ax)/x'(Mx). |
c                  %----------------------------%
c
                   call av(n, v(1,j), ax )
                   numr = sdot(n, v(1,j), 1, ax, 1)
                   call mv(n, v(1,j), ax )
                   denr = sdot(n, v(1,j), 1, ax, 1)
                   d(j,1) =  numr / denr
c
                else if (first) then
c
c                  %------------------------%
c                  | Eigenvalue is complex. |
c                  | Compute the first one  |
c                  | of the conjugate pair. |
c                  %------------------------%
c
c                  %----------------%
c                  | Compute x'(Ax) |
c                  %----------------%
c
                   call av(n, v(1,j), ax )
                   numr = sdot(n, v(1,j), 1, ax, 1)
                   numi = sdot(n, v(1,j+1), 1, ax, 1)
                   call av(n, v(1,j+1), ax)
                   numr = numr + sdot(n,v(1,j+1),1,ax,1)
                   numi = -numi + sdot(n,v(1,j),1,ax,1)
c
c                  %----------------%
c                  | Compute x'(Mx) |
c                  %----------------%
c
                   call mv(n, v(1,j), ax )
                   denr = sdot(n, v(1,j), 1, ax, 1)
                   deni = sdot(n, v(1,j+1), 1, ax, 1)
                   call mv(n, v(1,j+1), ax)
                   denr = denr + sdot(n,v(1,j+1),1,ax,1)
                   deni = -deni + sdot(n,v(1,j),1, ax,1)
c
c                  %----------------%
c                  | d=x'(Ax)/x'(Mx)|
c                  %----------------%
c
                   d(j,1) = (numr*denr+numi*deni) /
     &                      slapy2(denr, deni)
                   d(j,2) = (numi*denr-numr*deni) /
     &                      slapy2(denr, deni)
                   first = .false.
c
                else
c
c                  %------------------------------%
c                  | Get the second eigenvalue of |
c                  | the conjugate pair by taking |
c                  | the conjugate of the last    |
c                  | eigenvalue computed.         |
c                  %------------------------------%
c
                   d(j,1) = d(j-1,1)
                   d(j,2) = -d(j-1,2)
                   first = .true.
c
                end if
c
  70        continue
c
c           %---------------------------%
c           | Compute the residual norm |
c           |                           |
c           |   ||  A*x - lambda*x ||   |
c           |                           |
c           | for the NCONV accurately  |
c           | computed eigenvalues and  |
c           | eigenvectors.  (iparam(5) |
c           | indicates how many are    |
c           | accurate to the requested |
c           | tolerance)                |
c           %---------------------------%
c
            first = .true.
            nconv = iparam(5)
            do 80 j=1, nconv
c
               if (d(j,2) .eq. zero)  then
c
c                 %--------------------%
c                 | Ritz value is real |
c                 %--------------------%
c
                  call av(n, v(1,j), ax)
                  call mv(n, v(1,j), mx)
                  call saxpy(n, -d(j,1), mx, 1, ax, 1)
                  d(j,3) = snrm2(n, ax, 1)
                  d(j,3) = d(j,3) / abs(d(j,1))
c
               else if (first) then
c
c                 %------------------------%
c                 | Ritz value is complex  |
c                 | Residual of one Ritz   |
c                 | value of the conjugate |
c                 | pair is computed.      |
c                 %------------------------%
c
                  call av(n, v(1,j), ax)
                  call mv(n, v(1,j), mx)
                  call saxpy(n, -d(j,1), mx, 1, ax, 1)
                  call mv(n, v(1,j+1), mx)
                  call saxpy(n, d(j,2), mx, 1, ax, 1)
                  d(j,3) = snrm2(n, ax, 1)
                  call av(n, v(1,j+1), ax)
                  call mv(n, v(1,j+1), mx)
                  call saxpy(n, -d(j,1), mx, 1, ax, 1)
                  call mv(n, v(1,j), mx)
                  call saxpy(n, -d(j,2), mx, 1, ax, 1)
                  d(j,3) = slapy2( d(j,3), snrm2(n, ax, 1) )
                  d(j,3) = d(j,3) / slapy2(d(j,1),d(j,2))
                  d(j+1,3) = d(j,3)
                  first = .false.
               else
                  first = .true.
               end if
c
  80        continue
c
c           %-----------------------------%
c           | Display computed residuals. |
c           %-----------------------------%
c
            call smout(6, nconv, 3, d, maxncv, -6,
     &           'Ritz values (Real,Imag) and relative residuals')
c
        end if
c
c       %-------------------------------------------%
c       | Print additional convergence information. |
c       %-------------------------------------------%
c
         if ( info .eq. 1) then
             print *, ' '
             print *, ' Maximum number of iterations reached.'
             print *, ' '
         else if ( info .eq. 3) then
             print *, ' '
             print *, ' No shifts could be applied during implicit',
     &                ' Arnoldi update, try increasing NCV.'
             print *, ' '
         end if
c
         print *, ' '
         print *, ' _NDRV6 '
         print *, ' ====== '
         print *, ' '
         print *, ' Size of the matrix is ', n
         print *, ' The number of Ritz values requested is ', nev
         print *, ' The number of Arnoldi vectors generated',
     &            ' (NCV) is ', ncv
         print *, ' What portion of the spectrum: ', which
         print *, ' The number of converged Ritz values is ',
     &              nconv
         print *, ' The number of Implicit Arnoldi update',
     &            ' iterations taken is ', iparam(3)
         print *, ' The number of OP*x is ', iparam(9)
         print *, ' The convergence criterion is ', tol
         print *, ' '
c
      end if
c
c     %---------------------------%
c     | Done with program sndrv6. |
c     %---------------------------%
c
 9000 continue
c
      end
c
c==========================================================================
c
c     matrix vector multiplication subroutine
c
      subroutine mv (n, v, w)
      integer           n, j
      Real
     &                  v(n), w(n), one, four
      parameter         (one = 1.0E+0, four = 4.0E+0)
c
c     Compute the matrix vector multiplication y<---M*x
c     where M is a n by n symmetric tridiagonal matrix with 4 on the
c     diagonal, 1 on the subdiagonal and superdiagonal.
c
      w(1) =  four*v(1) + one*v(2)
      do 10 j = 2,n-1
         w(j) = one*v(j-1) + four*v(j) + one*v(j+1)
 10   continue
      w(n) =  one*v(n-1) + four*v(n)
      return
      end
c------------------------------------------------------------------
      subroutine av (n, v, w)
      integer           n, j
      Real
     &                  v(n), w(n), three, two
      parameter         (three = 3.0E+0, two = 2.0E+0)
c
c     Compute the matrix vector multiplication y<---A*x
c     where M is a n by n symmetric tridiagonal matrix with 2 on the
c     diagonal, -2 on the subdiagonal and 3 on the superdiagonal.
c
      w(1) =  two*v(1) + three*v(2)
      do 10 j = 2,n-1
         w(j) = -two*v(j-1) + two*v(j) + three*v(j+1)
 10   continue
      w(n) =  -two*v(n-1) + two*v(n)
      return
      end


      SUBROUTINE STUMPF(IPAIR,Z)
*
*       Modified Stumpff functions.
*       ---------------------------
*
*       Developed by Seppo Mikkola 10/1997.
*       ns is the max order included in z^n.
*
      INCLUDE 'common6.h'
      parameter (ns=12)
      real*8 c(12)
*
*
*       Generate Stumpff functions for argument Z (SCOEFF set by START).
      c(ns)=1-z*SCOEFF(12)
      c(ns-1)=1-z*SCOEFF(11)
      do i=ns-2,3,-1
          c(i)=1-z*c(i+2)*SCOEFF(i)
      end do
      c(1) = 1.0
      c(2) = 1.0
*
*       Copy the five first coefficients (for predictor & corrector).
      DO 2 I = 1,5
          SF(I,IPAIR) = C(I)
*         SF(I,IPAIR) = 1.0
    2 CONTINUE
*
*       Generate Stumpff functions for argument 4*Z (only down to c_5).
      Z4 = 4.0D0*Z
      c(ns)=1-z4*SCOEFF(12)
      c(ns-1)=1-z4*SCOEFF(11)
      do i=ns-2,5,-1
          c(i)=1-z4*c(i+2)*SCOEFF(i)
      end do
*
*       Copy c_5 & c_6 to sixth & seventh location (for time integration).
      DO 3 I = 6,7
          SF(I,IPAIR) = C(I-1)
*         SF(I,IPAIR) = 1.0
    3 CONTINUE
*
      RETURN
*
      END


! =====================================================
subroutine rp1(maxm,meqn,mwaves,maux,mbc,mx,ql,qr,auxl,auxr,fwave,s,amdq,apdq)
! =====================================================
!     # Adjoint Riemann solver for the acoustics equations in 1d.

! waves:     2
! equations: 2

! Conserved quantities:
!       1 pressure
!       2 velocity

!     # On input, ql contains the state vector at the left edge of each cell
!     #           qr contains the state vector at the right edge of each cell

!     # On output, wave contains the waves,
!     #            s the speeds,
!     #            amdq the  left-going flux difference  A^- \Delta q
!     #            apdq the right-going flux difference  A^+ \Delta q

!     # Note that the i'th Riemann problem has left state qr(i-1,:)
!     #                                    and right state ql(i,:)
!     # From the basic clawpack routines, this routine is called with ql = qr

!     # aux arrays not used in this solver.

    implicit double precision (a-h,o-z)

    dimension fwave(meqn, mwaves, 1-mbc:maxm+mbc)
    dimension    s(mwaves, 1-mbc:maxm+mbc)
    dimension   ql(meqn, 1-mbc:maxm+mbc)
    dimension   qr(meqn, 1-mbc:maxm+mbc)
    dimension apdq(meqn, 1-mbc:maxm+mbc)
    dimension amdq(meqn, 1-mbc:maxm+mbc)
    dimension auxl(maux, 1-mbc:maxm+mbc)
    dimension auxr(maux, 1-mbc:maxm+mbc)

!     local arrays
!     ------------
    dimension delta(2)

!     # density, bulk modulus, and sound speed, and impedence of medium:
!     # (should be set in setprob.f)
    common /cparam/ rho,bulk,cc,zz

!     # split the jump in f(q) at each interface into waves

!     # find b1 and b2, the coefficients of the 2 eigenvectors:
    do 20 i = 2-mbc, mx+mbc
!       # material properties
        rhoi = zz/cc
        bulki = rhoi*cc**2

!       # f-wave splitting
        delta(1) = -(ql(2,i)/rhoi - qr(2,i-1)/rhoi)
        delta(2) = -(bulki*ql(1,i) - bulki*qr(1,i-1))

        beta1 = (zz*delta(1) + delta(2)) / (2.d0*zz)
        beta2 = (zz*delta(1) - delta(2)) / (2.d0*zz)

!       # Compute the waves. Eigenvectors switched for adjoint:
!       # (speeds negative of eigenvalues because we are going backward in time)

        fwave(1,1,i) = beta1
        fwave(2,1,i) = beta1*zz
        s(1,i) = -cc

        fwave(1,2,i) = beta2
        fwave(2,2,i) = -beta2*zz
        s(2,i) = cc

    20 END DO

!     # compute the leftgoing and rightgoing flux differences:
!     # Note s(i,1) < 0   and   s(i,2) > 0.

!   # f-wave splitting, so do not multiply by wave speeds:
    forall (m=1:meqn,  i=2-mbc: mx+mbc)
    amdq(m,i) = fwave(m,1,i)
    apdq(m,i) = fwave(m,2,i)
    end forall

    return
    end subroutine rp1


      MODULE m_gkptwgt
      CONTAINS
        SUBROUTINE gkptwgt(&
             &                   kpts,cell,latnam)
          !
          !     this subroutine generates the weight factor of a k-point
          !     in the irreducible wedge of a 2d-brillouin zone
          !     ver are vertex coordinates in counter clockwise order and
          !     in units of pi/a(1) and pi/a(2)
          !     it is assumed that each k-point has the same 'area'. D.S.Wang
          !
          !     changed by                        Stefan Bl"ugel, IFF, Jan.96
          !
          USE m_constants
          USE m_types_kpts
          USE m_types_cell
          USE m_juDFT
          IMPLICIT NONE
          TYPE(t_cell),INTENT(IN)   :: cell
          TYPE(t_kpts),INTENT(INOUT):: kpts
          CHARACTER(len=*),INTENT(IN) :: latnam

          !
          !     .. was an Argument
          REAL    :: wt
          !     ..
          !     .. Array Arguments ..
          !     ..
          !     .. Local Scalars ..
          REAL cross,dot,eps,x1,x2,y1,y2
          REAL s1,s2
          INTEGER i,j,ikpt,nver
          !     ..
          !     .. Local Arrays ..
          REAL ver(2,4),dummy(2,kpts%nkpt)
          !     ..
          !     .. Intrinsic Functions ..
          INTRINSIC abs,atan2
          !     ..
          !     .. Data Statements ..
          DATA ver/0.e0,0.e0,1.e0,0.e0,1.e0,1.e0,0.e0,1.e0/
          DATA eps/1.e-6/
          !     ..

          !      nver = 3
          IF ( latnam.EQ.'squ' ) THEN
             nver = 3
          ELSEIF ( latnam.EQ.'p-r' .OR. latnam.EQ.'c-r' ) THEN
             nver = 4
          ELSEIF ( latnam.EQ.'hex' ) THEN
             nver = 3
             ver(2,3) = 1./3.
          ELSEIF ( latnam.EQ.'hx3' .OR. latnam.EQ. 'obl' ) THEN
             CALL juDFT_error("weights for hx3 or obl not defined" ,calledby&
                  &        ="gkptwgt")
          ENDIF
          !
          !     transform from internal coordinates to xy-coordinates
          !
          !                            changed by shz Feb.96
          DO 10 ikpt = 1 , kpts%nkpt
             kpts%wtkpt(ikpt) = 0
             dummy(1,ikpt)=kpts%bk(1,ikpt)
             dummy(2,ikpt)=kpts%bk(2,ikpt)
             s1 = 0.0
             s2 = 0.0
             DO i = 1,2
                s1 = s1+cell%bmat(i,1)*kpts%bk(i,ikpt)
                s2 = s2+cell%bmat(i,2)*kpts%bk(i,ikpt)
             ENDDO
             IF (latnam.EQ.'hex') THEN
                kpts%bk(1,ikpt) = s1*cell%amat(2,2)/tpi_const
                kpts%bk(2,ikpt) = s2*cell%amat(1,1)/pi_const
             ELSE
                kpts%bk(1,ikpt) = s1*cell%amat(1,1)/pi_const
                kpts%bk(2,ikpt) = s2*cell%amat(2,2)/pi_const
             ENDIF
10        ENDDO

          DO 20 ikpt = 1 , kpts%nkpt
             DO 30 i = 1,nver
                x1 = ( ver(1,i)-kpts%bk(1,ikpt) ) / cell%amat(1,1)
                y1 = ( ver(2,i)-kpts%bk(2,ikpt) ) / cell%amat(2,2)
                j  = i + 1
                IF ( j.GT.nver ) j = 1
                x2 = ( ver(1,j)-kpts%bk(1,ikpt) ) / cell%amat(1,1)
                y2 = ( ver(2,j)-kpts%bk(2,ikpt) ) / cell%amat(2,2)
                dot = x1*x2 + y1*y2
                cross = x1*y2 - y1*x2
                IF ( ABS(cross).GE.eps ) THEN
                   kpts%wtkpt(ikpt) = kpts%wtkpt(ikpt) + ATAN2(cross,dot)
                ENDIF
30           ENDDO
20        ENDDO
          !
          DO ikpt = 1 , kpts%nkpt
             kpts%wtkpt(ikpt) = kpts%wtkpt(ikpt) /tpi_const
          ENDDO
          !
          wt = 0.0
          DO ikpt = 1,kpts%nkpt
             wt = wt + kpts%wtkpt(ikpt)
             kpts%bk(1,ikpt)=dummy(1,ikpt)
             kpts%bk(2,ikpt)=dummy(2,ikpt)
          ENDDO

          RETURN
        END SUBROUTINE gkptwgt
      END


      FUNCTION GASDEV(IDUM)
      DATA ISET/0/
      IF (ISET.EQ.0) THEN
1       V1=2.*RAN1(IDUM)-1.
        V2=2.*RAN1(IDUM)-1.
        R=V1**2+V2**2
        IF(R.GE.1.)GO TO 1
        FAC=SQRT(-2.*LOG(R)/R)
        GSET=V1*FAC
        GASDEV=V2*FAC
        ISET=1
      ELSE
        GASDEV=GSET
        ISET=0
      ENDIF
      RETURN
      END


***********************************************************************
c                    ORBEL_FHYBRID.F
***********************************************************************
*     PURPOSE:  Solves Kepler's eqn. for hyperbola using hybrid approach.
*
*             Input:
*                           e ==> eccentricity anomaly. (real scalar)
*                           n ==> hyperbola mean anomaly. (real scalar)
*             Returns:
*               orbel_fhybrid ==>  eccentric anomaly. (real scalar)
*
*     ALGORITHM: For abs(N) < 0.636*ecc -0.6 , use FLON
*	         For larger N, uses FGET
*     REMARKS:
*     AUTHOR: M. Duncan
*     DATE WRITTEN: May 26,1992.
*     REVISIONS:
*     REVISIONS: 2/26/93 hfl
***********************************************************************

	real*8 function orbel_fhybrid(e,n)

      include '../swift.inc'

c...  Inputs Only:
	real*8 e,n

c...  Internals:
	real*8 abn
        real*8 orbel_flon,orbel_fget

c----
c...  Executable code

	abn = n
	if(n.lt.0.d0) abn = -abn

	if(abn .lt. 0.636d0*e -0.6d0) then
	  orbel_fhybrid = orbel_flon(e,n)
	else
	  orbel_fhybrid = orbel_fget(e,n)
	endif

	return
	end  ! orbel_fhybrid
c-------------------------------------------------------------------


!*****************************************************************************************
!>
!  Gravity model
!
!### See also
!  * [[geopotential_module]]

    module gravity_module

    use kind_module
    use numbers_module
    use iso_fortran_env, only: output_unit

    implicit none

    private

    public :: third_body_gravity
    public :: gravity_j2_j3_j4

    contains
!*****************************************************************************************

!*****************************************************************************************
!>
!  Third-body (pointmass) gravitational acceleration.

    subroutine third_body_gravity(r,rb,mu,acc)

    implicit none

    real(wp),dimension(3),intent(in)  :: r   !! satellite position vector [km]
    real(wp),dimension(3),intent(in)  :: rb  !! third-body position vector [km]
    real(wp),intent(in)               :: mu  !! third-body gravitational parameter [km^3/s^2]
    real(wp),dimension(3),intent(out) :: acc !! gravity acceleration vector [km/s^2]

    real(wp),dimension(3) :: r_sc_b  !! vector from third-body to spacecraft [km]
    real(wp) :: rb_mag               !! distance between origin and third-body [km]
    real(wp) :: r_sc_b_mag           !! distance between spacecraft and third-body [km]

    r_sc_b     = rb - r
    r_sc_b_mag = norm2(r_sc_b)
    rb_mag     = norm2(rb)
    acc        = (mu/r_sc_b_mag**3)*r_sc_b - (mu/rb_mag**3)*rb

    end subroutine third_body_gravity
!*****************************************************************************************

!*****************************************************************************************
!>
!  Gravitational acceleration due to simplified spherical harmonic
!  expansion (only the J2-J4 terms are used).
!
!### Reference
!  * http://www.ni.com/pdf/manuals/370762a.pdf

    subroutine gravity_j2_j3_j4(r,mu,req,j2,j3,j4,acc)

    implicit none

    real(wp),dimension(3),intent(in)  :: r   !! satellite position vector [km]
    real(wp),intent(in)               :: mu  !! central body gravitational parameter [km^3/s^2]
    real(wp),intent(in)               :: req !! body equatorial radius [km]
    real(wp),intent(in)               :: j2  !! j2 coefficient
    real(wp),intent(in)               :: j3  !! j3 coefficient
    real(wp),intent(in)               :: j4  !! j4 coefficient
    real(wp),dimension(3),intent(out) :: acc !! gravity acceleration vector [km/s^2]

    real(wp) :: mmor3,reqor,reqor2,reqor3,reqor4,&
                rmag,rmag2,rmag3,rzor,rzor2,rzor3,rzor4,c,d

    rmag2 = dot_product(r,r)
    rmag  = sqrt(rmag2)

    if (rmag==zero) then

        write(output_unit,'(A)') 'Error in gravity_j2_j3_j4: spacecraft at center of body.'
        acc = zero

    else

        rmag3  = rmag*rmag2
        mmor3  = -mu/rmag3
        reqor  = req/rmag
        reqor2 = reqor*reqor
        reqor3 = reqor2*reqor
        reqor4 = reqor3*reqor
        rzor   = r(3)/rmag
        rzor2  = rzor*rzor
        rzor3  = rzor2*rzor
        rzor4  = rzor3*rzor

        c = mmor3 * (1.0_wp - 1.5_wp*J2*reqor2*(5.0_wp*rzor2-1.0_wp) + &
                     2.5_wp*J3*reqor3*(3.0_wp*rzor-7.0_wp*rzor3) - &
                     0.625_wp*J4*reqor4*(3.0_wp-42.0_wp*rzor2+63.0_wp*rzor4))
        d = mmor3 * (r(3) + 1.5_wp*J2*reqor2*(3.0_wp-5.0_wp*rzor2)*r(3) + &
                   0.5_wp*J3*reqor3*(30.0_wp*rzor*r(3)-35.0_wp*rzor3*r(3)-3.0_wp*rmag) - &
                   0.625_wp*J4*reqor4*(15.0_wp-70.0_wp*rzor2+63.0_wp*rzor4)*r(3))

        acc(1) = c * r(1)
        acc(2) = c * r(2)
        acc(3) = d

    end if

    end subroutine gravity_J2_J3_J4
!*****************************************************************************************

!*****************************************************************************************
    end module gravity_module
!*****************************************************************************************


      function offsetlocal(xhrap,yhrap)

c--------------------------------------------------------------------
c  Estimation of offset of time comparing to Z-time
c  IDAY - day of year. Not much difference for different days. Assume
c         constant value =361 when offset close to annual average
c  IHR  - hour, offset practically constant for all hours.
c         Assume constant = 12
c  ITZONE - zone time in hours; uses Greenvich meridian zone time
c  xhrap, yhrap - HRAP coordinates
c--------------------------------------------------------------------

      parameter (ITZONE = 0, IHR = 12, IDAY = 361)

c  Convert HRAP to LatLon
      call sbllgd(rlon,rlat,1,xhrap,yhrap,0,istat)
      if(istat .eq. -1) then
       write(*,*) 'ERROR:',wron hrap to LatLon transform,xhrap,yhrap
       stop
      endif

c  Estimate offset in hours
      gama=2*3.14*(IDAY-1+(IHR-12)/24.0)/365.0
      egtime=229.18*(0.000075+0.001868*cos(gama)-0.032077*sin(gama)-
     +               0.014615*cos(2*gama)-0.040849*sin(2*gama))
      dtime=(egtime-4*rlon+60*ITZONE)
      offsetlocal=-dtime/60.0

      return
      end


Function gauss1(f, a, b, eps)
  External f
  Dimension w(12), x(12)
  Save
  Data const/1.0E-12/
  Data w/0.1012285, .2223810, .3137067, .3623838, .0271525, .0622535, 0.0951585, .1246290, .1495960, .1691565, .1826034, .1894506/
  Data x/0.9602899, .7966665, .5255324, .1834346, .9894009, .9445750, 0.8656312, .7554044, .6178762, .4580168, .2816036, .0950125/
  delta = const*abs(a-b)
  gauss1 = 0.0
  aa = a
  5 y = b - aa
  If (abs(y)<=delta) Return
  2 bb = aa + y
  c1 = 0.5*(aa+bb)
  c2 = c1 - aa
  s8 = 0.0
  s16 = 0.0
  Do i = 1, 4
    u = x(i)*c2
    s8 = s8 + w(i)*(f(c1+u)+f(c1-u))
  End Do
  Do i = 5, 12
    u = x(i)*c2
    s16 = s16 + w(i)*(f(c1+u)+f(c1-u))
  End Do
  s8 = s8*c2
  s16 = s16*c2
  If (abs(s16-s8)>eps*(1.+abs(s16))) Goto 4
  gauss1 = gauss1 + s16
  aa = bb
  Goto 5
  4 y = 0.5*y
  If (abs(y)>delta) Goto 2
  Write (6, 7)
  gauss1 = 0.0
  Return
  7 Format (1X, 'GAUSS1....TOO HIGH ACURACY REQUIRED')
End Function gauss1


      subroutine ero_eiusle

!!    ~ ~ ~ PURPOSE ~ ~ ~
!!    This subroutine computes the USLE erosion index (EI)

!!    ~ ~ ~ INCOMING VARIABLES ~ ~ ~
!!    name        |units       |definition
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~
!!    idg(:)      |none        |array location of random number seed
!!                             |used for a given process
!!    ihru        |none        |HRU number
!!    ovrlnd(:)   |mm H2O      |overland flow onto HRU from upstream
!!                             |routing unit
!!    rndseed(:,:)|none        |random number generator seed
!!    snomlt      |mm H2O      |amount of snow melt in HRU on current day
!!    tconc(:)    |hr          |time of concentration
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~

!!    ~ ~ ~ OUTGOING VARIABLES ~ ~ ~
!!    name        |units                  |definition
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~
!!    usle_ei     |100(ft-tn in)/(acre-hr)|USLE rainfall erosion index
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~

!!    ~ ~ ~ LOCAL DEFINITIONS ~ ~ ~
!!    name        |units       |definition
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~
!!    xa          |none        |fraction of daily rainfall occuring during
!!                             |half-hour of max intensity rain
!!    ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~

!!    ~ ~ ~ SUBROUTINES/FUNCTIONS CALLED ~ ~ ~
!!    Intrinsic: Log, Log10
!!    SWAT: Expo, Atri

!!    ~ ~ ~ ~ ~ ~ END SPECIFICATIONS ~ ~ ~ ~ ~ ~

      use climate_module
      use hydrograph_module
      use hru_module, only : hru, usle_ei, usle_eifac, ovrlnd, ihru, iwgen, peakr, precipday, snomlt,  &
        usle_ei, usle_eifac

      implicit none

      integer :: j         !none        |HRU number
      real :: xb           !none        |intermediate calculation
      real ::pkrf          !none        |intermediate calculation
      real ::pkrf30        !mm/hr       |maximum 30-min. storm intensity
      integer :: iob       !            |

      j = ihru
      iob = hru(j)%obj_no
      iwst = ob(iob)%wst

      if (precipday > 1.e-4) then
        xb = -2. * Log(1. - wst(iwst)%weat%precip_half_hr)
        pkrf30 = 2. * precipday * wst(iwst)%weat%precip_half_hr
        pkrf = xb * precipday
        usle_ei = precipday * (12.1 + 8.9 * (Log10(pkrf) - .4343)) * pkrf30 / 1000.
        if (usle_ei < 1.e-4) usle_ei = 0.
        usle_eifac(j) = usle_ei
      endif

      return
      end subroutine ero_eiusle


use booklib, only: spline_fit,spline_int

implicit none

integer, parameter	:: n=4,no=11
real			  	:: x(n),y(n),xx(no)
real 				:: yy(n)
integer				:: i,error
real	            :: result
real				:: yp1,ypn,ypp(n)

x(1:4)= (/1.,2.,3.,4./)
y(1:4)= (/1.,2.,3.,4./)

xx(1:no)= (/1.0,1.1,1.2,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0/)
!xx(1:no)= (/1.0,2.0,3.0/)

call spline_fit(x,y,n,ypp,error)
do i=1,no
	!call interp(x,y,n,xx(i),result,error)
	call spline_int(x,y,n,ypp,xx(i),result,error)
	write(*,*) xx(i),result,error
enddo

end


program main
    implicit none
    real :: l
    real, external :: period
    open(unit=1, file='out.txt')
    write (*, "(A)") 'Please input the length of pendulum:'
    read  (*, *) l
    write (1, "(A, 2(F0.6, A))") 'The period of the pendulum with length ', l, 'm is ', period(l), 's.'
    close(1)
    stop
end program main

function period(l)
    implicit none
    real :: l
    real :: period
    real, parameter :: pi = 3.1415926535
    real, parameter :: g = 9.80665
    period = 2 * pi * (l / g) ** 0.5
    return
end function


c-------------------------------------------------
c FTAN filter: y =x*exp(-alpha*((om-om0)/om0)**2
c-------------------------------------------------
      subroutine ftfilt(alpha,om0,dom,n,a,fs, b)
      implicit none
      integer*4 k, n
      real*8 alpha,om0,dom,ome,om2,b(32768)
      double complex a(32768), fs(32768)
      do k=1,n
         fs(k) = dcmplx(0.0d0, 0.0d0)
         b(k)=0.0d0
         ome = (k-1)*dom
         om2 = -(ome-om0)*(ome-om0)*alpha/om0/om0
         if( dabs(om2) .le. 40.0d0 ) then
             b(k) = dexp(om2)
             fs(k) = a(k)*b(k)
         endif
      enddo
      return
      end


      SUBROUTINE AVEVFC(LEVEL,NEXTAD,ITRACE,NSAMPS,INDX1,NPTS1,
     *   INDX2,NPTS2,INDX3,NPTS3,INDX4,NPTS4)
C     AVEVFC PERFORMS AVERAGE AMPLITUDE NORMALIZATION IN THE AP USING THE VECTOR
C  FUNCTION CHAINER.  AVERAGE AMPLITUDE NORMALIZE ADJUSTS THE AVERAGE ABSOLUTE
C  AMPLITUDE WITHIN EACH USER DEFINED WINDOW TO A USER SPECIFIED LEVEL.  IT DOES
C  THIS BY DIVIDING THE SPECIFIED LEVEL BY THE AVERAGE ABSOLUTE VALUE OF THE
C  WINDOW AND THEN APPLYING THIS AS A MULTIPLIER TO THE CENTER OF THE WINDOW.
C  MULTIPLIERS BETWEEN WINDOW CENTERS ARE DETERMINED BY LINEAR INTERPOLATION.
C  MULTIPLIERS BEFORE THE FIRST CENTER ARE THE SAME AS THE FIRST AND
C  ALL THE MULTIPLIERS AFTER THE CENTER OF THE LAST WINDOW ARE THE SAME AS THE
C  MULTIPLIER OF THE CENTER OF THE LAST WINDOW.
C      THE PROGRAM CREATES A SCRATCH TRACE WITH MULTIPLIERS IN IT AND
C  THEN MULTIPLIES IT AGAINST THE DATA TRACE.
C    A MAXIMUM OF 4 WINDOWS MAY BE USED.
C    NOTE THAT THE DISTANCE BETWEEN WINDOW CENTERS MUST BE IN THE AP IMMEDIATELY
C  FOLLOWING THE 4 LEVELS (AT AP ADDRESS LEVELS).  THESE DISTANCES ARE THE
C  FLOATING POINT (REAL) NUMBER OF DATA POINTS.  THE FIRST DISTANCE IS THE
C  DISTANCE BETWEEN THE CENTER OF THE FIRST WINDOW AND THE CENTER OF THE
C  SECOND WINDOW.
C
C  ARGUMENTS:
C  LEVEL  - THE AP ADDRESS OF THE 4 WINDOW LEVELS. THE DISTANCE (IN DATA POINTS)
C           BETWEEN THE WINDOW CENTERS MUST BE AT LEVELS+4,LEVELS+5,.....
C           PUT THE LEVELS IN THE AP PRIOR TO CALLING AVEVFC.
C  NEXTAD- THE AP ADDRESS OF A SCRATCH ARRAY.  MUST BE NSAMPS+10 LONG.
C  ITRACE - THE AP ADDRESS OF THE FIRST DATA WORD OF THE TRACE TO BE NORMALIZED.
C  NSAMPS - THE NUMBER OF SAMPLES IN THE ENTIRE TRACE.  (THE TRACE LENGTH).
C  INDX1  - THE AP ADDRESS OF THE BEGINNING OF THE FIRST NORMALIZE WINDOW.
C  NPTS1  - THE NUMBER OF DATA POINTS IN THE FIRST WINDOW.
C  INDX2  - THE AP ADDRESS OF THE BEGINNING OF THE SECOND NORMALIZE WINDOW.
C  NPTS2  - THE NUMBER OF DATA POINTS IN THE SECOND WINDOW.
C           THE LIST OF WINDOWS MUST TERMINATE WITH THE NUMBER OF POINTS=0
C           (UNLESS ALL 4 WINDOWS ARE GIVEN.)
C  ETC.
C
C  COPYRIGHTED BY:
C  PAUL HENKART, SCRIPPS INSTITUTION OF OCEANOGRAPHY, MAY 1980
C
      LEVELS=LEVEL
      NEXT=NEXTAD
      NEXT1=NEXTAD+1
      INCRE=NEXT1+1
      MULTS=INCRE+1
      NPTS=LEVELS+3
      ICOUNT=0
100   ICOUNT=ICOUNT+1                                                   ! /* THE WINDOW COUNTER
      IF(ICOUNT.EQ.1) GO TO 1
      IF(ICOUNT.EQ.2) GO TO 2
      IF(ICOUNT.EQ.3) GO TO 3
      IF(ICOUNT.EQ.4) GO TO 4
      GO TO 500
1     I=INDX1
      N=NPTS1
      GO TO 200
2     I=INDX2
      N=NPTS2
      GO TO 200
3     I=INDX3
      N=NPTS3
      GO TO 200
4     I=INDX4
      N=NPTS4
      GO TO 200
C
C
200   IF(N.EQ.0) GO TO 500                                              ! /* ANY MORE WINDOWS
      CALL MEAMGV(I,1,NEXT1,N)                                          ! /* FIND THE MEAN ABSOLUTE VALE OF THE WINDOW
      N=N/2                                                             ! /* FIND THE CENTER OF THE WINDOW
      CALL VDIV(NEXT1,0,LEVELS,0,NEXT1,0,1)                             ! /* CONVERT TO A MULTIPLIER
      I=I+N                                                             ! /* THE AP ADDRESS OF THE CENTER OF THE WINDOW
      IF(ICOUNT.EQ.1) GO TO 400                                         ! /*  IS IT THE FIRST WINDOW
      CALL VSUB(NEXT,0,NEXT1,0,INCRE,0,1)                               ! /* SUBTRACT THE LAST MULTIPLIER
      CALL VDIV(NPTS,0,INCRE,0,INCRE,0,1)                               ! /* THE MULTIPLIER INCREMENT BETWEEN WINDOWS
      N=I-LASTI
      NULTS=MULTS-1
      M=N+1
      CALL VRAMP(NULTS,INCRE,NULTS,1,M)
      MULTS=MULTS+N
      LASTI=I
300   ITEMP=NEXT                                                        ! /* CHANGE NEXT AND NEXT1
      NEXT=NEXT1
      NEXT1=ITEMP
      LEVELS=LEVELS+1                                                   ! /*  POINT TO THE NEXT WINDOW LEVEL
      NPTS=NPTS+1
      GO TO 100                                                         ! /* GO DO THE NEXT WINDOW
C
C    DO THE FIRST WINDOW - THE BEGINNING TOF THE CENTER OF THE FIRST WINDOW
C     GET THE SAME MULTIPLIER
C
400   N=I-ITRACE
      LASTI=ITRACE+N
      N=N+1
      CALL VFILL(NEXT1,MULTS,1,N)                                       ! /* FILL FROM THE BEGINNING OF THE TRACE
      MULTS=MULTS+N
      GO TO 300
C
C     DO THE LAST WINDOW - FROM THE CENTER OF THE LAST WINDOW TO THE END
C      OF THE TRACE GET THE SAME MULTIPLIER
C
500   N=NSAMPS-LASTI                                                    ! /* DO THE LAST WINDOW
      N=N-ITRACE                                                        ! /* USE THE LAST MULTIPLIER UNTIL THE END OF THE TRACE
      N=N+1
      NULTS=MULTS-1
      CALL VFILL(NULTS,MULTS,1,N)
C
C       MULTIPLY THE MULTIPLIER TRACE AGAINST THE DATA TRACE
C
      MULTS=INCRE+1
      CALL VMUL(ITRACE,1,MULTS,1,ITRACE,1,NSAMPS)
      RETURN
      END


SUBROUTINE SFDEC(FI,TET,h,X,Y,Z,fi0,tet0)
REAL PI/3.1415926/,rz/6371./
PER=PI/180.
TET1=TET* PER
r=rz-h
Y1=R*COS((FI-fi0)*PER)*COS(TET1)
X=R*SIN((FI-fi0)*PER)*COS(TET1)
Z1=R*SIN(TET1)
T=tet0*PER
Y=-Y1*SIN(T)+Z1*COS(T)
Z=Rz-(Y1*COS(T)+Z1*SIN(T))
RETURN
END


      SUBROUTINE MB02MD( JOB, M, N, L, RANK, C, LDC, S, X, LDX, TOL,
     $                   IWORK, DWORK, LDWORK, IWARN, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve the Total Least Squares (TLS) problem using a Singular
C     Value Decomposition (SVD) approach.
C     The TLS problem assumes an overdetermined set of linear equations
C     AX = B, where both the data matrix A as well as the observation
C     matrix B are inaccurate. The routine also solves determined and
C     underdetermined sets of equations by computing the minimum norm
C     solution.
C     It is assumed that all preprocessing measures (scaling, coordinate
C     transformations, whitening, ... ) of the data have been performed
C     in advance.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             Determines whether the values of the parameters RANK and
C             TOL are to be specified by the user or computed by the
C             routine as follows:
C             = 'R':  Compute RANK only;
C             = 'T':  Compute TOL only;
C             = 'B':  Compute both RANK and TOL;
C             = 'N':  Compute neither RANK nor TOL.
C
C     Input/Output Parameters
C
C     M       (input) INTEGER
C             The number of rows in the data matrix A and the
C             observation matrix B.  M >= 0.
C
C     N       (input) INTEGER
C             The number of columns in the data matrix A.  N >= 0.
C
C     L       (input) INTEGER
C             The number of columns in the observation matrix B.
C             L >= 0.
C
C     RANK    (input/output) INTEGER
C             On entry, if JOB = 'T' or JOB = 'N', then RANK must
C             specify r, the rank of the TLS approximation [A+DA|B+DB].
C             RANK <= min(M,N).
C             Otherwise, r is computed by the routine.
C             On exit, if JOB = 'R' or JOB = 'B', and INFO = 0, then
C             RANK contains the computed (effective) rank of the TLS
C             approximation [A+DA|B+DB].
C             Otherwise, the user-supplied value of RANK may be
C             changed by the routine on exit if the RANK-th and the
C             (RANK+1)-th singular values of C = [A|B] are considered
C             to be equal, or if the upper triangular matrix F (as
C             defined in METHOD) is (numerically) singular.
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N+L)
C             On entry, the leading M-by-(N+L) part of this array must
C             contain the matrices A and B. Specifically, the first N
C             columns must contain the data matrix A and the last L
C             columns the observation matrix B (right-hand sides).
C             On exit, the leading (N+L)-by-(N+L) part of this array
C             contains the (transformed) right singular vectors,
C             including null space vectors, if any, of C = [A|B].
C             Specifically, the leading (N+L)-by-RANK part of this array
C             always contains the first RANK right singular vectors,
C             corresponding to the largest singular values of C. If
C             L = 0, or if RANK = 0 and IWARN <> 2, the remaining
C             (N+L)-by-(N+L-RANK) top-right part of this array contains
C             the remaining N+L-RANK right singular vectors. Otherwise,
C             this part contains the matrix V2 transformed as described
C             in Step 3 of the TLS algorithm (see METHOD).
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= max(1,M,N+L).
C
C     S       (output) DOUBLE PRECISION array, dimension (min(M,N+L))
C             If INFO = 0, the singular values of matrix C, ordered
C             such that S(1) >= S(2) >= ... >= S(p-1) >= S(p) >= 0,
C             where p = min(M,N+L).
C
C     X       (output) DOUBLE PRECISION array, dimension (LDX,L)
C             If INFO = 0, the leading N-by-L part of this array
C             contains the solution X to the TLS problem specified
C             by A and B.
C
C     LDX     INTEGER
C             The leading dimension of array X.  LDX >= max(1,N).
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             A tolerance used to determine the rank of the TLS
C             approximation [A+DA|B+DB] and to check the multiplicity
C             of the singular values of matrix C. Specifically, S(i)
C             and S(j) (i < j) are considered to be equal if
C             SQRT(S(i)**2 - S(j)**2) <= TOL, and the TLS approximation
C             [A+DA|B+DB] has rank r if S(i) > TOL*S(1) (or S(i) > TOL,
C             if TOL specifies sdev (see below)), for i = 1,2,...,r.
C             TOL is also used to check the singularity of the upper
C             triangular matrix F (as defined in METHOD).
C             If JOB = 'R' or JOB = 'N', then TOL must specify the
C             desired tolerance. If the user sets TOL to be less than or
C             equal to 0, the tolerance is taken as EPS, where EPS is
C             the machine precision (see LAPACK Library routine DLAMCH).
C             Otherwise, the tolerance is computed by the routine and
C             the user must supply the non-negative value sdev, i.e. the
C             estimated standard deviation of the error on each element
C             of the matrix C, as input value of TOL.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (L)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK, and DWORK(2) returns the reciprocal of the
C             condition number of the matrix F.
C             If INFO > 0, DWORK(1:min(M,N+L)-1) contain the unconverged
C             non-diagonal elements of the bidiagonal matrix whose
C             diagonal is in S (see LAPACK Library routine DGESVD).
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK = max(2, 3*(N+L) + M, 5*(N+L)),       if M >= N+L;
C             LDWORK = max(2, M*(N+L) + max( 3M+N+L, 5*M), 3*L),
C                                                          if M <  N+L.
C             For optimum performance LDWORK should be larger.
C
C             If LDWORK = -1, then a workspace query is assumed;
C             the routine only calculates the optimal size of the
C             DWORK array, returns this value as the first entry of
C             the DWORK array, and no error message related to LDWORK
C             is issued by XERBLA.
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0:  no warnings;
C             = 1:  if the rank of matrix C has been lowered because a
C                   singular value of multiplicity greater than 1 was
C                   found;
C             = 2:  if the rank of matrix C has been lowered because the
C                   upper triangular matrix F is (numerically) singular.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             > 0:  if the SVD algorithm (in LAPACK Library routine
C                   DBDSQR) has failed to converge. In this case, S(1),
C                   S(2), ..., S(INFO) may not have been found
C                   correctly and the remaining singular values may
C                   not be the smallest. This failure is not likely
C                   to occur.
C
C     METHOD
C
C     The method used is an extension (see [3,4,5]) of the classical
C     TLS algorithm proposed by Golub and Van Loan [1].
C
C     Let [A|B] denote the matrix formed by adjoining the columns of B
C     to the columns of A on the right.
C
C     Total Least Squares (TLS) definition:
C     -------------------------------------
C
C       Given matrices A and B, find a matrix X satisfying
C
C            (A + DA) X = B + DB,
C
C       where A and DA are M-by-N matrices, B and DB are M-by-L matrices
C       and X is an N-by-L matrix.
C       The solution X must be such that the Frobenius norm of [DA|DB]
C       is a minimum and each column of B + DB is in the range of
C       A + DA. Whenever the solution is not unique, the routine singles
C       out the minimum norm solution X.
C
C     Define matrix C = [A|B] and s(i) as its i-th singular value for
C     i = 1,2,...,min(M,NL), where NL = N + L. If M < NL, then s(j) = 0
C     for j = M+1,...,NL.
C
C     The Classical TLS algorithm proceeds as follows (see [3,4,5]):
C
C     Step 1: Compute part of the singular value decomposition (SVD)
C             USV' of C = [A|B], namely compute S and V'. (An initial
C             QR factorization of C is used when M is larger enough
C             than NL.)
C
C     Step 2: If not fixed by the user, compute the rank r0 of the data
C             [A|B] based on TOL as follows: if JOB = 'R' or JOB = 'N',
C
C                s(1) >= ... >= s(r0) > TOL*s(1) >= ... >= s(NL).
C
C             Otherwise, using [2], TOL can be computed from the
C             standard deviation sdev of the errors on [A|B]:
C
C                TOL = SQRT(2 * max(M,NL)) * sdev,
C
C             and the rank r0 is determined (if JOB = 'R' or 'B') using
C
C                s(1) >= ... >= s(r0) > TOL >= ... >= s(NL).
C
C             The rank r of the approximation [A+DA|B+DB] is then equal
C             to the minimum of N and r0.
C
C     Step 3: Let V2 be the matrix of the columns of V corresponding to
C             the (NL - r) smallest singular values of C, i.e. the last
C             (NL - r) columns of V.
C             Compute with Householder transformations the orthogonal
C             matrix Q such that:
C
C                       |VH   Y|
C              V2 x Q = |      |
C                       |0    F|
C
C             where VH is an N-by-(N - r) matrix, Y is an N-by-L matrix
C             and F is an L-by-L upper triangular matrix.
C             If F is singular, then lower the rank r with the
C             multiplicity of s(r) and repeat this step.
C
C     Step 4: If F is nonsingular then the solution X is obtained by
C             solving the following equations by forward elimination:
C
C                X F = -Y.
C
C     Notes :
C     The TLS solution is unique if r = N, F is nonsingular and
C     s(N) > s(N+1).
C     If F is singular, however, then the computed solution is infinite
C     and hence does not satisfy the second TLS criterion (see TLS
C     definition). For these cases, Golub and Van Loan [1] claim that
C     the TLS problem has no solution. The properties of these so-called
C     nongeneric problems are described in [4] and the TLS computations
C     are generalized in order to solve them. As proven in [4], the
C     proposed generalization satisfies the TLS criteria for any
C     number L of observation vectors in B provided that, in addition,
C     the solution | X| is constrained to be orthogonal to all vectors
C                  |-I|
C     of the form |w| which belong to the space generated by the columns
C                 |0|
C     of the submatrix |Y|.
C                      |F|
C
C     REFERENCES
C
C     [1] Golub, G.H. and Van Loan, C.F.
C         An Analysis of the Total Least-Squares Problem.
C         SIAM J. Numer. Anal., 17, pp. 883-893, 1980.
C
C     [2] Staar, J., Vandewalle, J. and Wemans, M.
C         Realization of Truncated Impulse Response Sequences with
C         Prescribed Uncertainty.
C         Proc. 8th IFAC World Congress, Kyoto, I, pp. 7-12, 1981.
C
C     [3] Van Huffel, S.
C         Analysis of the Total Least Squares Problem and its Use in
C         Parameter Estimation.
C         Doctoral dissertation, Dept. of Electr. Eng., Katholieke
C         Universiteit Leuven, Belgium, June 1987.
C
C     [4] Van Huffel, S. and Vandewalle, J.
C         Analysis and Solution of the Nongeneric Total Least Squares
C         Problem.
C         SIAM J. Matr. Anal. and Appl., 9, pp. 360-372, 1988.
C
C     [5] Van Huffel, S. and Vandewalle, J.
C         The Total Least Squares Problem: Computational Aspects and
C         Analysis.
C         Series "Frontiers in Applied Mathematics", Vol. 9,
C         SIAM, Philadelphia, 1991.
C
C     NUMERICAL ASPECTS
C
C     The algorithm consists in (backward) stable steps.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Apr. 1997.
C     Supersedes Release 2.0 routine MB02AD by S. Van Huffel, Katholieke
C     University, Leuven, Belgium.
C
C     REVISIONS
C
C     June 24, 1997, Feb. 27, 2000, Oct. 19, 2003, Feb. 21, 2004,
C     June 13, 2012.
C
C     KEYWORDS
C
C     Least-squares approximation, singular subspace, singular value
C     decomposition, singular values, total least-squares.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOB
      INTEGER           INFO, IWARN, L, LDC, LDWORK, LDX, M, N, RANK
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      INTEGER           IWORK(*)
      DOUBLE PRECISION  C(LDC,*), DWORK(*), S(*), X(LDX,*)
C     .. Local Scalars ..
      LOGICAL           CRANK, CTOL, LJOBN, LJOBR, LJOBT, LQUERY
      INTEGER           ITAU, J, JWORK, LDW, K, MINMNL, MINWRK, N1, NL,
     $                  P, R1, WRKOPT
      DOUBLE PRECISION  FNORM, RCOND, SMAX, TOLTMP
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANGE, DLANTR
      EXTERNAL          DLAMCH, DLANGE, DLANTR, LSAME
C     .. External Subroutines ..
      EXTERNAL          DGERQF, DGESVD, DLACPY, DLASET, DORMRQ, DSWAP,
     $                  DTRCON, DTRSM, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, INT, MAX, MIN, SQRT
C     .. Executable Statements ..
C
      IWARN = 0
      INFO = 0
      NL = N + L
      K = MAX( M, NL )
      P = MIN( M, N )
      MINMNL = MIN( M, NL )
      LDW = MAX( 3*MINMNL + K, 5*MINMNL )
      LJOBR = LSAME( JOB, 'R' )
      LJOBT = LSAME( JOB, 'T' )
      LJOBN = LSAME( JOB, 'N' )
C
C     Determine whether RANK or/and TOL is/are to be computed.
C
      CRANK = .NOT.LJOBT .AND. .NOT.LJOBN
      CTOL  = .NOT.LJOBR .AND. .NOT.LJOBN
C
C     Test the input scalar arguments.
C
      IF( CTOL .AND. CRANK .AND. .NOT.LSAME( JOB, 'B' ) ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( L.LT.0 ) THEN
         INFO = -4
      ELSE IF( .NOT.CRANK .AND. RANK.GT.P ) THEN
         INFO = -5
      ELSE IF( LDC.LT.MAX( 1, K ) ) THEN
         INFO = -7
      ELSE IF( LDX.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( CTOL .AND. TOL.LT.ZERO ) THEN
         INFO = -11
      ELSE
         IF( M.GE.NL ) THEN
            MINWRK = MAX( 2, LDW )
         ELSE
            MINWRK = MAX( 2, M*NL + LDW, 3*L )
         END IF
         WRKOPT = MINWRK
         LQUERY = LDWORK.EQ.-1
         IF( LQUERY ) THEN
            IF ( M.GE.NL ) THEN
               CALL DGESVD( 'N', 'O', M, NL, C, LDC, S, DWORK, 1, DWORK,
     $                      1, DWORK, -1, INFO )
               WRKOPT = MAX( MINWRK, INT( DWORK(1) ) )
            ELSE
               CALL DGESVD( 'N', 'A', M, NL, DWORK, M, S, DWORK, 1, C,
     $                      LDC, DWORK, -1, INFO )
               WRKOPT = MAX( MINWRK, INT( DWORK(1) ) + M*NL )
            END IF
C
            IF ( L.GT.0 ) THEN
               CALL DGERQF( L, NL-1, C, LDC, DWORK, DWORK, -1, INFO )
               WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + L )
               CALL DORMRQ( 'R', 'T', N, NL-1, L, C, LDC, DWORK, C, LDC,
     $                      DWORK, -1, INFO )
               WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + L )
            END IF
         END IF
         IF( LDWORK.LT.MINWRK .AND. .NOT.LQUERY )
     $       INFO = -14
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB02MD', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( CRANK )
     $   RANK = P
      IF ( MIN( M, NL ).EQ.0 ) THEN
         IF ( M.EQ.0 ) THEN
            CALL DLASET( 'Full', NL, NL, ZERO, ONE, C, LDC )
            CALL DLASET( 'Full', N, L, ZERO, ZERO, X, LDX )
         END IF
         DWORK(1) = TWO
         DWORK(2) = ONE
         RETURN
      END IF
C
C     Subroutine MB02MD solves a set of linear equations by a Total
C     Least Squares Approximation.
C
C     Step 1: Compute part of the singular value decomposition (SVD)
C             USV' of C = [A   |B   ], namely compute S and V'.
C                           M,N  M,L
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      IF ( M.GE.NL ) THEN
C
C        M >= N + L:  Overwrite V' on C.
C        Workspace: need max(3*min(M,N+L) + max(M,N+L), 5*min(M,N+L)).
C
         JWORK = 1
         CALL DGESVD( 'No left vectors', 'Overwritten on C', M, NL, C,
     $                LDC, S, DWORK, 1, DWORK, 1, DWORK(JWORK),
     $                LDWORK-JWORK+1, INFO )
      ELSE
C
C        M < N + L:  Save C in the workspace and compute V' in C.
C        Note that the previous DGESVD call cannot be used in this case.
C        Workspace: need M*(N+L) + max(3*min(M,N+L) + max(M,N+L),
C                                      5*min(M,N+L)).
C
         CALL DLACPY( 'Full', M, NL, C, LDC, DWORK, M )
         JWORK = M*NL + 1
         CALL DGESVD( 'No left vectors', 'All right vectors', M, NL,
     $                DWORK, M, S, DWORK, 1, C, LDC, DWORK(JWORK),
     $                LDWORK-JWORK+1, INFO )
      END IF
C
      IF ( INFO.GT.0 ) THEN
C
C        Save the unconverged non-diagonal elements of the bidiagonal
C        matrix and exit.
C
         DO 10 J = 1, MINMNL - 1
            DWORK(J) = DWORK(JWORK+J)
   10    CONTINUE
C
         RETURN
      END IF
      WRKOPT = MAX( 2, INT( DWORK(JWORK) ) + JWORK - 1 )
C
C     Transpose V' in-situ (in C).
C
      DO 20 J = 2, NL
         CALL DSWAP( J-1, C(J,1), LDC, C(1,J), 1 )
   20 CONTINUE
C
C     Step 2: Compute the rank of the approximation [A+DA|B+DB].
C
      IF ( CTOL ) THEN
         TOLTMP = SQRT( TWO*DBLE( K ) )*TOL
         SMAX = TOLTMP
      ELSE
         TOLTMP = TOL
         IF ( TOLTMP.LE.ZERO ) TOLTMP = DLAMCH( 'Precision' )
         SMAX = MAX( TOLTMP*S(1), DLAMCH( 'Safe minimum' ) )
      END IF
C
      IF ( CRANK ) THEN
C        WHILE ( RANK .GT. 0 ) .AND. ( S(RANK) .LE. SMAX ) DO
   40    IF ( RANK.GT.0 ) THEN
            IF ( S(RANK).LE.SMAX ) THEN
               RANK = RANK - 1
               GO TO 40
            END IF
         END IF
C        END WHILE 40
      END IF
C
      IF ( L.EQ.0 ) THEN
         DWORK(1) = WRKOPT
         DWORK(2) = ONE
         RETURN
      END IF
C
      N1 = N + 1
      ITAU  = 1
      JWORK = ITAU + L
C
C     Step 3: Compute the orthogonal matrix Q and matrices F and Y
C     such that F is nonsingular.
C
C     REPEAT
C
C        Adjust the rank if S(RANK) has multiplicity greater than 1.
C
   60    CONTINUE
         R1 = RANK + 1
         IF ( RANK.LT.MINMNL ) THEN
C           WHILE RANK.GT.0 .AND. S(RANK)**2 - S(R1)**2.LE.TOL**2 DO
   80       IF ( RANK.GT.0 ) THEN
               IF ( ONE - ( S(R1)/S(RANK) )**2.LE.( TOLTMP/S(RANK) )**2
     $            ) THEN
                  RANK = RANK - 1
                  IWARN = 1
                  GO TO 80
               END IF
            END IF
C           END WHILE 80
         END IF
C
         IF ( RANK.EQ.0 ) THEN
C
C           Return zero solution.
C
            CALL DLASET( 'Full', N, L, ZERO, ZERO, X, LDX )
            DWORK(1) = WRKOPT
            DWORK(2) = ONE
            RETURN
         END IF
C
C        Compute the orthogonal matrix Q (in factorized form) and the
C        matrices F and Y using RQ factorization. It is assumed that,
C        generically, the last L rows of V2 matrix have full rank.
C        The code could not be the most efficient one when RANK has been
C        lowered, because the already created zero pattern of the last
C        L rows of V2 matrix is not exploited.
C        Workspace: need 2*L;  prefer L + L*NB.
C
         R1 = RANK + 1
         CALL DGERQF( L, NL-RANK, C(N1,R1), LDC, DWORK(ITAU),
     $                DWORK(JWORK), LDWORK-JWORK+1, INFO )
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
C        Workspace: need N+L;  prefer L + N*NB.
C
         CALL DORMRQ( 'Right', 'Transpose', N, NL-RANK, L, C(N1,R1),
     $                LDC, DWORK(ITAU), C(1,R1), LDC, DWORK(JWORK),
     $                LDWORK-JWORK+1, INFO )
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
         CALL DLASET( 'Full', L, N-RANK, ZERO, ZERO, C(N1,R1), LDC )
         IF ( L.GT.1 )
     $      CALL DLASET( 'Lower', L-1, L-1, ZERO, ZERO, C(N1+1,N1),
     $                   LDC )
C
C        Estimate the reciprocal condition number of the matrix F,
C        and lower the rank if F can be considered as singular.
C        Workspace: need 3*L.
C
         CALL DTRCON( '1-norm', 'Upper', 'Non-unit', L, C(N1,N1), LDC,
     $                RCOND, DWORK, IWORK, INFO )
         WRKOPT = MAX( WRKOPT, 3*L )
C
         FNORM = DLANTR( '1-norm', 'Upper', 'Non-unit', L, L, C(N1,N1),
     $                   LDC, DWORK )
         IF ( RCOND.LE.TOLTMP*FNORM ) THEN
            RANK = RANK - 1
            IWARN = 2
            GO TO 60
         ELSE IF ( FNORM.LE.TOLTMP*DLANGE( '1-norm', N, L, C(1,N1), LDC,
     $                                     DWORK ) ) THEN
            RANK = RANK - L
            IWARN = 2
            GO TO 60
         END IF
C     UNTIL ( F nonsingular, i.e., RCOND.GT.TOL*FNORM or
C                                  FNORM.GT.TOL*norm(Y) )
C
C     Step 4: Solve X F = -Y by forward elimination,
C             (F is upper triangular).
C
      CALL DLACPY( 'Full', N, L, C(1,N1), LDC, X, LDX )
      CALL DTRSM( 'Right', 'Upper', 'No transpose', 'Non-unit', N, L,
     $            -ONE, C(N1,N1), LDC, X, LDX )
C
C     Set the optimal workspace and reciprocal condition number of F.
C
      DWORK(1) = WRKOPT
      DWORK(2) = RCOND
C
      RETURN
C *** Last line of MB02MD ***
      END


!---------------------------------------------------!
! Copyright (c) 2017 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
subroutine preparation_GS
  use global_variables
  implicit none
  real(dp),parameter :: sigma0 = sqrt(2d0)
  integer :: ix,iy
  real(dp) :: tmp,r
  real(dp) :: tmp_wfn(0:Nx,0:Nx)

  write(*,'(A)')'===== preparatin_GS =============================================================='
  write(*,'(A)')
  write(*,'(A)')

  allocate(wfn(0:Nx,0:Nx), v_ext(0:Nx), v_int(0:Nx,0:Nx), v_all(0:Nx,0:Nx))
  allocate(rho(0:Nx), jx(0:Nx))

  write(*,'(A)')'=== preparing initial wave-function ===='
! wfn(0,:) == wfn(Nx,:) == 0
  wfn(:,:) = 0d0

  do iy = 1,Nx-1
  do ix = 1,Nx-1
     call random_number(tmp)
     tmp = tmp-0.5d0
     wfn(ix,iy) = tmp
  end do
  end do

  tmp_wfn(:,:) = wfn(:,:)

! Symmetrize
  do iy = 0,Nx
  do ix = 0,Nx
     wfn(ix,iy)=0.5d0*(tmp_wfn(ix,iy)+tmp_wfn(iy,ix))
  end do
  end do

! Normalize
  tmp = sum(wfn(:,:)**2)*dx**2
  wfn(:,:)=wfn(:,:)/sqrt(tmp)

  write(*,'(A)')'=== preparing external potential ===='
  do ix=0,Nx
!     v_ext(ix) = 0.5d0*xn(ix)**2
!     v_ext(ix) = -2d0/sqrt(1d0+xn(ix)**2)
!     v_ext(ix) = -1d0/sqrt(1d0+xn(ix)**2)
     v_ext(ix) = -1d0/sqrt(sigma0**2+xn(ix)**2)
  end do
!  v_KS_tdehf_old = v_ext
!  v_KS_tdehf = v_ext

  write(*,'(A)')'=== preparing interaction potential ===='
  do iy=0,Nx
  do ix=0,Nx
     r = xn(ix) - xn(iy)
!     v_int(ix,iy) = 0d0 !1d0/sqrt(1d0+r**2) ! debut
!     v_int(ix,iy) = 1d0/sqrt(1d0+r**2)
     v_int(ix,iy) = 1d0/sqrt(sigma0**2+r**2)
  end do
  end do

  write(*,'(A)')'=== preparing total potential ===='
  do iy=0,Nx
  do ix=0,Nx
     v_all(ix,iy) = v_ext(ix) + v_ext(iy) + v_int(ix,iy)
  end do
  end do

  write(*,'(A)')
  write(*,'(A)')
  write(*,'(A)')'===== Complete preparatin_GS ====================================================='

  return
end subroutine preparation_GS


!=============================================================================!
        subroutine blkdiag3D( mat, Dh, Dhi, Vh11, Vh22 )
!
!  For the block diagonal matrix
!
!=============================================================================!
        use global
        use stencil
        implicit none

        complex :: mat(ndof,ndof,nx,ny)
        real    :: Dh(ndof,ndof,nx,ny), Dhi(ndof,ndof,nx,ny),
        real    :: Vh11(6,nx,ny), Vh22(6,nx,ny)

        real    :: a3, b3

        integer :: i, j, idof, jdof
!=============================================================================!

!.... fourth-order stencil

        a3 = da3 / dxi**2
        b3 = da3 / deta**2

        !$omp parallel do private (i,idof,jdof)
        loop_j: do j = 1, ny
        loop_i: do i = 1, nx

!.... \hat{D} term

        do idof = 1, ndof
          do jdof = 1, ndof
            mat(idof,jdof,i,j) = Dh(idof,jdof,i,j) + im * Dhi(idof,jdof,i,j)
          end do
        end do

!.... \hat{V}_{\xi\xi} term

        mat(2,2,i,j) = mat(2,2,i,j) - a3 * Vh11(1,i,j)
        mat(2,3,i,j) = mat(2,3,i,j) - a3 * Vh11(5,i,j)
        mat(3,2,i,j) = mat(3,2,i,j) - a3 * Vh11(6,i,j)
        mat(3,3,i,j) = mat(3,3,i,j) - a3 * Vh11(2,i,j)
        mat(4,4,i,j) = mat(4,4,i,j) - a3 * Vh11(3,i,j)
        mat(5,5,i,j) = mat(5,5,i,j) - a3 * Vh11(4,i,j)

!.... \hat{v}_{\xi\eta} term

!.... \hat{V}_{\eta\eta} term

        mat(2,2,i,j) = mat(2,2,i,j) - b3 * Vh22(1,i,j)
        mat(2,3,i,j) = mat(2,3,i,j) - b3 * Vh22(5,i,j)
        mat(3,2,i,j) = mat(3,2,i,j) - b3 * Vh22(6,i,j)
        mat(3,3,i,j) = mat(3,3,i,j) - b3 * Vh22(2,i,j)
        mat(4,4,i,j) = mat(4,4,i,j) - b3 * Vh22(3,i,j)
        mat(5,5,i,j) = mat(5,5,i,j) - b3 * Vh22(4,i,j)

        end do loop_i
        end do loop_j

        end subroutine blkdiag3D


module electron_conductivity
    use conductivity_def
    implicit none

contains

    function ee_PCY(ne,T) result(nu)
        ! fmla. from Potekhin, Chabrier, and Yakovlev (1997)
        use constants_def
        real(dp), intent(in) :: ne, T
        real(dp) :: nu
        real(dp) :: yfac
        real(dp) :: mec2
        real(dp) :: eefac
        real(dp) :: befac
        real(dp) :: onesixth = 1.0/6.0
        real(dp) :: plasma_theta, kF
        real(dp) :: x, eF, beta, b2, be3, y, lJ, llJ, J0,J1,J2,J

        yfac = sqrt(4.0*finestructure/pi)
        mec2 = Melectron*clight**2
        eefac = 1.5*finestructure**2*mec2/pi**3/hbar
        befac = (finestructure/pi)**1.5

        kF = (3.0*pi**2*ne)**onethird
        x = kF*hbar/Melectron/clight
        eF = sqrt(1.0+x**2)
        beta = x/eF
        b2 = beta**2
        be3 = befac/beta**1.5
        plasma_theta = boltzmann*T/mec2
        y = yfac*x**1.5/plasma_theta/sqrt(ef)
        lJ = 1.0 + (2.810-0.810*b2)/y
        llj = log(lJ)
        J0 = onethird/(1.0+0.07414*y)**3
        J1 = J0*llJ + onesixth*pi**5*y/(13.91+y)**4
        J2 = 1.0+0.4*(3.0+1.0/x**2)/x**2
        J = y**3 * J1*J2

        nu = eefac*plasma_theta**2*J/eF/be3
    end function ee_PCY

    function ee_SY06(ne,T) result(nu)
        ! uses fmla of Shternin & Yakovlev (2006). Checked against routine courtesy of D. Page.
        !
        use constants_def
        real(dp), intent(in) :: ne, T
        real(dp) :: nu
        real(dp) :: nu_pre
        real(dp) :: Tp_pre
        real(dp) :: Il, It, Ilt, I
        real(dp) :: At, Ct, Ct1, Ct2, Alt, Blt, Clt, Clt1, Clt2
        real(dp) :: u2, u3, u4, tu
        real(dp) :: kF, Tp, theta, xF, eF, u, mstar

        nu_pre = 36.0*finestructure**2*hbar**2*clight/pi/boltzmann/Melectron
        Tp_pre = hbar*sqrt(4.0*pi*finestructure*hbar*clight/Melectron)/boltzmann

        kF = (threepisquare*ne)**onethird
        xF = hbar*kF/Melectron/clight
        eF = sqrt(1.0+xF**2)
        u = xF/eF
        Tp = Tp_pre*sqrt(ne/eF)
        theta = sqrt(3.0)*Tp/T

        u2 = u**2
        u3 = u**3
        u4 = u**4
        tu = theta*u
        At = 20.0+450.0*u3
        Ct1 = 0.05067+0.03216*u2
        Ct2 = 0.0254+0.04127*u4
        Ct = At*exp(Ct1/Ct2)

        Alt = 12.2 + 25.2*u3
        Blt = 1.0-0.75*u
        Clt1 = 0.123636+0.016234*u2
        Clt2 = 0.0762+0.05714*u4
        Clt = Alt*exp(Clt1/Clt2)

        Il = (1.0/u)*(0.1587 - 0.02538/(1.0+0.0435*theta)) &
        &       *log(1.0+128.56/theta/(37.1 + theta*(10.83 + theta)))
        It = u3*(2.404/Ct + (Ct2-2.404/Ct)/(1.0+0.1*tu)) &
        &       *log(1.0 + Ct/tu/(At + tu))
        Ilt = u*(18.52*u2/Clt + (Clt2-18.2*u2/Clt)/(1.0+0.1558*theta**Blt)) &
        &       *log(1.0 + Clt/(Alt*theta + 10.83*tu**2 + tu**(8.0/3.0)))

        I = Il + It + Ilt
        nu = nu_pre*ne*I/eF/T
    end function ee_SY06

    function eion(kF,Gamma_e,eta,Ye,Z,Z2,Z53,A) result(nu)
        ! implements fmla. of Baiko et al. (1999)
        use constants_def
        real(dp), intent(in) :: kF, Gamma_e, eta, Ye, Z, Z2, Z53, A
        real(dp) :: nu
        ! interface
        !   function eone(z)
        !       real(dp)(kind=8) :: z, eone
        !   end function eone
        ! end interface
        real(dp), parameter :: um1 = 2.8, um2 = 12.973
        real(dp), parameter :: onesixth = 1.0/6.0, fourthird = 4.0*onethird
        real(dp), parameter :: sw_max = log(huge(1.0_dp))
        real(dp) :: electroncompton
        real(dp) :: aB
        real(dp) :: eifac
        real(dp) :: kF2,x,eF,Gamma,v,v2,kTF2,eta02,aei,qD2,beta,qi2,qs2
        real(dp) :: Gs,Gk0,Gk1,GkZ,Gk,a0,D1,D,s,w1,w,sw,fac
        real(dp) :: L1,L2,Lei

        electroncompton = hbar/Melectron/clight
        aB = electroncompton/finestructure
        eifac = fourthird*clight*finestructure**2/electroncompton/pi

        kF2 = kF**2
        x = electroncompton*kF
        eF = sqrt(1.0+x**2)
        v = x/eF
        v2 = v**2
        Gamma = Gamma_e*Z53
        s = finestructure/pi/v
        kTF2 = 4.0*s*kF2
        eta02 = (0.19/Z**onesixth)**2
        aei = (4.0/9.0/pi)**onethird * kF
        qD2 = 3.0*Gamma_e*aei**2 * Z2/Z
        beta = pi*finestructure*Z*v

        qi2 = qD2*(1.0+0.06*Gamma)*exp(-sqrt(Gamma))
        qs2 = (qi2+kTF2)*exp(-beta)

        Gs = eta/sqrt(eta**2+eta02)*(1.0+0.122*beta**2)
        Gk0 = 1.0/(eta**2+0.0081)**1.5
        Gk1 = 1.0 + beta*v**3
        GkZ = 1.0-1.0/Z
        Gk = Gs + 0.0105*GkZ*Gk1*Gk0*eta

        a0 = 4.0*kF2/qD2/eta
        D1 = exp(-9.1*eta)
        D = exp(-a0*um1*D1*0.25)

        w1 = 1.0+onethird*beta
        w = 4.0*um2*kF2/qD2*w1

        sw = s*w
        if (sw < sw_max) then
            fac = exp(sw)*(eone(sw)-eone(sw+w))
            L1 = 0.5*(log(1.0+1.0/s) + (1.0-exp(-w))*s/(s+1.0) - fac*(1.0+sw))
            L2 = 0.5*(1.0-(1.0-exp(-w))/w + s*(s/(1.0+s)*(1.0-exp(-w))  &
            & - 2.0*log(1.0+1.0/s)  +fac*(2.0+sw)))
        else
            L1 = 0.5*(log((s+1.0)/s) - 1.0/(s+1.0))
            L2 = (2.0*s+1.0)/(2.0*s+2.0) - s*log((s+1.0)/s)
        end if
        Lei = (Z2/A)*(L1-v2*L2)*Gk*D
        nu = eifac*eF*Lei*A/Z

        contains
        function eone(z)
            real(dp), intent(in) :: z
            real(dp) :: eone

            real(dp), parameter :: a0 = 0.1397, a1 = 0.5772, a2 = 2.2757
            real(dp) :: z2, z3, z4
            z2 = z*z; z3 = z2*z; z4 = z2*z2
            eone = exp(-z4/(z3+a0))*(log(1.0+1.0/z)-a1/(1.0+a2*z2))
        end function eone
    end function eion

    function eQ(kF,T,Ye,Z,Z2,A,Q) result (nu)
        ! impurity scattering: Iton (1994), also Potekhin, priv. communication
        use constants_def
        real(dp), intent(in) :: kF,T,Ye,Z,Z2,A,Q
        real(dp) :: nu
        real(dp) :: dZ2,kF2,x,gamma,v2,v,qs2,s,L1,L
        real(dp) :: electron_compton, mec2
        real(dp) :: fac

        electron_compton = hbar/Melectron/clight
        mec2 = Melectron*clight**2
        fac = 4.0*onethird*clight*finestructure**2/electron_compton/pi

        dZ2 = Q/A
        kF2 = kF**2
        x = electron_compton*kF
        gamma = sqrt(1.0+x**2)
        v = x/gamma
        v2 = v**2
        s = finestructure/pi/v
        qs2 = 4.0*kF2*s

        L1 = 1.0+4.0*v2*s
        L = 0.5*(L1*log(1.0+1.0/s) - v2 -1.0/(s+1.0) - v2*s/(1.0+s))
        nu = fac*gamma*dZ2*L*A/Z
    end function eQ

    function eQ_page(kF,T,Ye,Z,Z2,A,Q) result(nu)
        use constants_def
        real(dp), intent(in) :: kF,T,Ye,Z,Z2,A,Q
        real(dp) :: nu
        real(dp) :: dZ2,kF2,x,v,qs,L,gamma
        real(dp) :: electron_compton, mec2, fac

        electron_compton = hbar/Melectron/clight
        mec2 = Melectron*clight**2
        fac = 4.0*onethird*clight*finestructure**2/electron_compton/pi

        dZ2 = Q/A
        kF2 = kF**2
        x = electron_compton*kF
        gamma = sqrt(1.0+x**2)
        v = x/gamma
        qs = 0.048196/sqrt(v)
        L = log(1.0/qs)-0.5*(1+v**2)
        nu = fac*gamma*dZ2*L*A/Z
    end function eQ_page

end module electron_conductivity