      SUBROUTINE ssmied
c
c                          PROCESS SSMIGR
c                          ------- ------
c
c  Document date:
c
c                        Split-Step Migration
c
c    Reference: Stoffa et al., Split-step Fourier Migration,
c                              Geophysics,55,p.410-421,1990.
c
c  This process performs a "split-step" Fourier migration on
c  stacked seismic data.  The method is designed to provide a
c  fast f-k approach to migration in laterally varying velocity
c  media.  The approach is straightforward.  The data are migrated
c  in small depth increments of dz.  For each dz a loop over a
c  frequency range fmin to fmax is performed in which the data are:
c
c  1) trasformed from f-x to f-k;
c  2) phase shifted by exp(i*dz*Kz) [Kz=csqrt(w^2*u0^2-Kx^2)]
c     using a reference slowness u0;
c  3) transformed from f-k to f-x;
c  4) phase shifted by exp(i*w*(u(x,z)-u0))
c
c  Imaging is then done at depth Zj by a sum over frequencies
c  from fmin to fmax.
c
c  Notes:
c
c  The complex square root calculation of Kz results in an exponentially
c  damped response to inhomogeneous interface waves, a difficulty in
c  other implementations of this type of phase-shift-plus-correction
c  method.
c
c  Extreme lateral velocity variations, such as seen at some
c  continental margins, may be handled by breaking the migration
c  into several overlapping panels which are later spliced together.
c
c  Big step, ZBIG:
c    You may migrate in one big step through an upper constant
c    slowness region such as the water column.  No imaging is done
c    for this region.  If there is a deep water delay in your data,
c    you may want to remove delay to negate temporal wrap-around.
c    This is most easily done through start-end-time pair in process
c    DISKIN (i.e. set 0.0 8.0)
c
c  Migration bandwidth, FMIN, FMAX:
c    Runtime increases in direct proportion to migration bandwidth.
c    It is worthwhile doing a couple of small tests to see what
c    frequencies are actually useful and adjusting the parameters
c    FMIN and FMAX accordingly.
c
c  Depth step, DELTAZ:
c    The migration depth step dz need not be tiny, but in general small
c    enough to provide unaliased sampling of the smallest vertical
c    wavenumber of interest.
c
c  Data tapers, BPAD, EPAD:
c    The user is given the option of padding either side of the data
c    panel being migrated. The padded region is filled with copies of
c    the end traces which are tapered down within the pad region. This
c    is highly recommended as it reduces Gibbs phenomena and Nyquist
c    noise.  It should be kept in mind, however, that a power of 2 FFT
c    is used and that the length of the FFT is determined as the next
c    power of 2 abover (data panel length)+BPAD+EPAD.  So if you are
c    migrating 1900 traces and you pad with 100 on each side, you will
c    be using a 4096 FFT for each depth step. If you had padded with 50
c    on each you would be using a 2048 FFT, much faster.
c
c  Migration taper, MTAP:
c    In addition to the tapered padding of the first and last trace, an
c    exponential taper can be applied to the ends of the spatial window
c    at each migration step.  This inhibits "wraparound" of migration
c    smiles from the data panel ends sides.  An exponental taper on the
c    order of MTAP=25 traces is recommended.  The example below should
c    clarify the issue.  It is important to note that the FFT length is
c    not based on the length of the exponential taper, only on the
c    (data panel length)+BPAD+EPAD, so that if care is not taken data
c    may be affected by the exponental taper.
c
c    MTAP=5
c    |<5>|                                      |<5>|
c    eeeee11111111111111111111111111111111111111eeeee  <-*1
c    tttttttdddddddddddddddddddddddddddttttttt0000000
c    |< 7 >||                         ||< 7 >|      |
c    |BPAD=7|                         |EPAD=7       |
c    |      |                         |             |
c    |      |<- data panel = 27 ----->|             |
c    |                                              |
c    |<-------- 2^n based on 27+(2*7) = 48 -------->|
c
c
c    *1) this vector is applied to each frequency at each depth
c        step.  In f-k jargon this type of taper is called a sponge.
c
c  EXAMPLE MIGRATION:
c  Shallow Sediments near ODP HOLE 504B
c  diskin
c      ipath 504B.dmo.stack.224.624 set 0.0 8.0 end
c  end
c  ssmigr
c      deltax 12.5 deltaz 10 vskip 1500 zskip 3000 ez 10000
c      bpad 50 epad 50 mtap 25 twinlen 0.25
c      ref 0 nvsmth 3 vpath v.scratch
c      fmin 5 fmax 40 path ps.scratch sgypath vmodel.segy
c      fno  224 lno  224 vdp 1500.0 3480.0 1850.0 3490.0 1850.0 3721.3 end
c      fno  424 lno  424 vdp 1500.0 3442.5 1850.0 3452.5 1850.0 3785.5 end
c      fno  524 lno  524 vdp 1500.0 3397.5 1850.0 3407.5 1850.0 3694.3 end
c      fno  624 lno  624 vdp 1500.0 3390.0 1850.0 3400.0 1850.0 3686.8 end
c  end
c
c  PARAMETER DICTIONARY
c  --------- ----------
c  EZ     - End Depth, in meters.  The number of samples output will be
c           EDEPTH  / DELTAZ + 1.  The first output sample is ALWAYS 0.
c         - Required.  e.g.  ez 6000
c  DELTAX - The distance between traces, in meters.
c           Required.
c  DELTAZ - The output sample interval in meters per sample.  DELTAZ may
c           not exceed 32 since it is carried in the SEGY header in
c           millimeters and as a 16 bit integer (thus 32767 millimeters
c           is the maximum).  This is analogous to the sample interval
c           in time which the SEG-Y format carries as nanoseconds
c           (milliseconds / 1000).
c           Required.
C TWINLEN - Time WINdow LENgth of temporal taper at end of trace. This
C           taper is exponential in nature and is given in seconds. The
C           time taper should reduce energy migrating from truncation
C           of time trace.
C           Preset = 0.25
c  MTAP   - Migration TAPer.  The exponential spatial taper discussed above.
C           preset = 25
c  FMIN   - Minimum frequency of interest.
c           Preset = 0
c  FMAX   - Maximum frequency of interest
c         - preset = nyquist / 2  or   (4/si)
c  VDP    - The rms velocity to use in migration, given as a list of
c           Velocity Depth Pairs.  The velocity should be in units of
c           meters per second and the depth should be in units of
c           meters.
c           Required.    velocity range 350 to 32000
c  NVSMTH - This parameter specifies the number of Velocity SMooTHing
c           operations desired before constructing velocity file.
c           The VSMTH parameter is useful for smoothing across velocity
c           contrasts which otherwise can cause distortion in depth
c           migration. This is a 3 point convolutional operator.
c           Preset = 3
c  FNO    - The first shot/rp number the parameter list applies to.
c           Preset = the first shot/rp received.    e.g.   fno 101
c  LNO    - The last shot/rp number the parameter list applies to.
c           Preset = the last shot/rp received.    e.g.   lno 101
c  BPAD   - The number of zero amplitude traces to insert prior to the
c           first trace.
c           Preset = 1   range 1 to 500      e.g. bpad 10
c  EPAD   - The number of zero amplitude traces to append after the last
c           trace.
c           Preset = 1   range 1 to 500      e.g. epad 10
c  ZSKIP  - The first depth to compute.  The first output sample is
c           ALWAYS 0.  This saves computer time!  The first velocity of
c           VDP of the first FNO/LNO list is used as the velocity for
c           time to depth conversion unless parameter VSKIP is given.
c           Preset = 0.
c  VSKIP  - Velocity used in extrapolation for parameter ZSKIP, usually
c           the water velocity in marine work. If using ZSKIP, then VSKIP
c           is required.
c  REF    - Reference slowness, u0, from a given depth.  The minimum
c           slowness may provide a better reference than the average
c           slowness for imaging features such as a rough basement
c           surface beneath low velocity sediments.
c         =0, minimum slowness.
c         =1, average slowness.
c         =2, maximum slowness.
c           Preset = 1
c  PATH   - The pathname (filename) of a scratch file SSMIGR should use
c           for the intermediate transposed data.  The purpose of this
c           parameter is to allow the user to specify the exact disk
c           partition to use in case the "current" partition does not
c           have enough space.
c           preset = a scratch file in the current directory
c           e.g.    path /user/scratch/moreroom
c  VPATH  - The pathname (filename) of a scratch file SSMIGR should use
c           for the transposed velocity slices.  The purpose of this
c           parameter is to allow the user to specify the exact disk
c           partition to use in case the "current" partition does not
c           have enough space.
c           preset = a scratch file in the current directory
c           e.g.    vpath /user/scratch/vmoreroomc
c  SGYPATH - The pathname (filename) of an additional SEGY compatible
c           velocity file to be output for external purposes. Includes
c           the smoothing operators.
c           preset = none
c           e.g.    sgypath /user/scratch/vmoreroomc.segy
c
c  Copyright (C) 1995 by:
c  Woods Hole Oceanographic Institution and
c  The Regents of The University of California, 1995
c
c  Written by Dan Lizarralde, WHOI, and adapted by Paul Henkart, SIO
c  Modifications by Graham Kent & Dan Lizarralde, WHOI
c  ALL RIGHTS RESERVED.
c
c  Present limitations include 8192 traces, 5000 depth steps & 800 frequencies.
c  If the number of depths * number of traces exceed 500,000 use sioseis.BIG
c  which allows a product of nz*nx = 3,500,000, also remember padding when
c  estimating your nx.
c
      PARAMETER ( npars = 21 )                                          ! the number of user parameters
      PARAMETER ( maxvel = 55 )                                         ! the maximum number of velocity-depth pairs
      CHARACTER*80 token
      CHARACTER*7 names(npars)
      CHARACTER*1 types(npars)                                          ! the type of parameter
      REAL vals(npars)                                                  ! holds the REAL parameter values
      DIMENSION lvals(npars)                                            ! holds the INTEGER parameter values
      EQUIVALENCE (vals(1),lvals(1))                                    ! must be the same so that wrdisc scr works!
      DIMENSION vels(maxvel*2)
c
      COMMON /edits/ ierror, iwarn, irun, now, icompt
      COMMON /ssmigr/ junit, nlists, nwrds
c
      INTEGER fno, bpad, epad, ref, vdp, vintpl
      INTEGER nvsmth, mtap
c
      EQUIVALENCE ( lprint, lvals(1) ),
     2            ( fno, lvals(2) ),
     3            ( lno, lvals(3) ),
     4            ( nvels, lvals(4) ),
     5            ( bpad, lvals(5) ),
     6            ( epad, lvals(6) ),
     7            ( zskip, vals(7) ),
     8            ( fmin, vals(8) ),
     9            ( fmax, vals(9) ),
     *            ( deltax, vals(10) ),
     1            ( deltaz, vals(11) ),
     2            ( ez, vals(12) ),                                     ! I couldn't resist
     3            ( lunscr, lvals(13) ),
     4            ( lunvel, lvals(14) ),
     5            ( vskip, vals(15) ),
     6            ( ref, lvals(16) ),
     7            ( vintpl, lvals(17) ),
     8            ( nvsmth, lvals(18) ),
     9            ( twinlen, vals(19) )
      EQUIVALENCE ( mtap, lvals(20) ),
     1            ( lunsegy, lvals(21) )

      DATA names /'LPRINT ', 'FNO    ', 'LNO    ', 'VDP    ', 'BPAD   ',
     1            'EPAD   ', 'ZSKIP  ', 'FMIN   ', 'FMAX   ', 'DELTAX ',
     2            'DELTAZ ', 'EZ     ', 'PATH   ', 'VPATH  ', 'VSKIP  ',
     3            'REF    ', 'VINTPL ', 'NVSMTH ', 'TWINLEN', 'MTAP   ',
     4            'SGYPATH'/
      DATA types / 6*'L',6*'F',2*'A','F',2*'L','L','F','L','A'/
c****
c****    Set the parameter presets and various variable presets
c****
      lprint = 0
      fno  = 0
      lno = 0
      nvels = 0
      bpad = 0
      epad = 0
      zskip = 0.
      fmin = 0.
      fmax = 0.
      deltax = 0.
      deltaz = 0.
      ez = 0.
      lunscr = 0
      lunvel = 0
      lunsegy = 0
      nz = 0
      vskip = 0.
      ref = 1
      vdp = 0
      vintpl = 1
      vsmth = 3
      twinlen = .25
      mtap = 25
      CALL getfil( 1, junit, token, istat )                             ! get a file for the DISK parameters
c****
c****     get the user's parameters
c****
      ntokes = 0                                                        ! count the tokens
  100 CONTINUE
      CALL getoke( token, nchars )                                      ! get a token and it's length
      CALL upcase( token, nchars )                                      ! convert parameter names to upper case
      IF( nchars .EQ. 0 ) THEN                                          ! anything there?
          CALL rdline                                                   ! nope, get another line
          ntokes = 0
          GOTO 100
      ENDIF
      ntokes = ntokes + 1
  110 DO 120 i = 1, npars
         nparam = i
         IF( token(1:nchars) .EQ. names(nparam) ) GOTO 150
  120 CONTINUE
      IF( token(1:nchars) .EQ. 'END' ) GOTO 200
      IF( vdp .EQ. 1 ) GOTO 160
      PRINT *,' ***  ERROR  ***  No such parameter as ',token(1:nchars)
      ierror = ierror + 1
      GOTO 100
  150 CONTINUE
      vdp = 0
      lparam = nparam
c****
c****   Got the parameter name, now get the value
c****
      CALL getoke( token, nchars )                                      ! get the value
      ntokes = ntokes + 1
      IF( nchars .EQ. 0 ) THEN
          CALL rdline
          ntokes = 0
          GOTO 150
      ENDIF
      IF( types(nparam) .EQ. 'A' ) THEN
          IF( names(nparam) .EQ. 'PATH' ) THEN
              CALL getfil( 3, lunscr, token(1:nchars), istat )
              IF( istat .NE. 0 ) THEN
                  PRINT *,' ***  ERROR  ***   Can not open file ',
     &                  token(1:nchars)
                  ierror = ierror + 1
              ENDIF
          ENDIF
          IF( names(nparam) .EQ. 'VPATH' ) THEN
              CALL getfil( 3, lunvel,  token(1:nchars), istat )         ! create the velocity file
                  IF( istat .NE. 0 ) THEN
                    PRINT *,' ***  ERROR  ***   Can not open file ',
     &                  token(1:nchars)
                    ierror = ierror + 1
                  ENDIF
          ENDIF
           IF( names(nparam) .EQ. 'SGYPATH' ) THEN
               CALL getfil( 3, lunsegy,  token(1:nchars), istat )        ! create segy velocity file
                  IF( istat .NE. 0 ) THEN
                    PRINT *,' ***  ERROR  ***   Can not open file ',
     &                  token(1:nchars)
                    ierror = ierror + 1
                  ENDIF
          ENDIF
          GOTO 100
      ENDIF
  160 CALL dcode( token, nchars, areal, istat )                         ! convert the alpha number to an internal machine number
      IF( istat .NE. 2 ) ierror = ierror + 1                            ! was the an error decoding it?
      IF( names(lparam) .EQ. 'VDP' ) THEN
          nvels = nvels + 1
          vels(nvels) = areal
          vdp = 1
          GOTO 100
      ENDIF
      IF( types(lparam) .EQ. 'L' ) THEN
          lvals(lparam) = areal                                         ! convert the real to INTEGER*4
      ELSE
          vals(lparam) = areal                                          ! move the real to the parameter
      ENDIF
      GOTO 100
c****
c****    Do the parameter validity checks
c****
  200 CONTINUE
      IF( fno .LT. 0 ) THEN
          PRINT *,' ***  ERROR  ***  FNO must be positive.'
          ierror = ierror + 1
      ENDIF
      IF( lno .LT. 0 ) THEN
          PRINT *,' ***  ERROR  ***  LNO must be positive.'
          ierror = ierror + 1
      ENDIF
      IF( deltax .LT. 1. .OR. deltax .GT. 500. ) THEN
          PRINT *,' ***  ERROR  ***  DELTAX must be between 1 and 500.'
          ierror = ierror + 1
      ENDIF
      IF( nvels .EQ. 0 ) THEN
          PRINT *,' ***  ERROR  ***  A velocity function must be given.'
          ierror = ierror + 1
      ENDIF
      IF( ref .LT. 0 .OR. ref .GT. 2 ) THEN
          PRINT *,' ***  ERROR  ***  REF must be 0, 1, or 2'
          ierror = ierror + 1
      ENDIF
      IF( nvels/2*2 .NE. nvels ) THEN
          PRINT *,' ***  ERROR  ***  Velocities must be in pairs.'
          ierror = ierror + 1
      ENDIF
      DO 1234 i = 1, nvels, 2
         IF( vels(i) .LT. 350. .OR. vels(i) .GT. 32000. ) THEN
             PRINT *,' ***  ERROR  ***  Velocities must be between',
     *          ' 350 and 32000.'
             ierror = ierror + 1
         ENDIF
         IF( i .NE. 1 .AND. vels(i+1) .LE. vels(i-1) ) THEN
             PRINT *,' ***  ERROR  ***  The depths within the velocity',
     *          ' depth pair must increase.'
             ierror = ierror + 1
         ENDIF
 1234 CONTINUE
      IF( bpad .LT. 0 .OR. bpad .GT. 500 ) THEN
          PRINT *,' ***  ERROR  ***  BPAD must be between 1 and 500.'
          ierror = ierror + 1
      ENDIF
      IF( epad .LT. 0 .OR. epad .GT. 500 ) THEN
          PRINT *,' ***  ERROR  ***  EPAD must be between 1 and 500.'
          ierror = ierror + 1
      ENDIF
      IF( deltaz .LT. 1. .OR. deltaz .GT. 32. ) THEN
          PRINT *,' ***  WARNING  *** DELTAZ must be between 1. and 32.'
          iwarn = iwarn + 1
      ENDIF
      IF( ez .LT. 100 .OR. ez .GT. 30000 ) THEN
          PRINT *,' ***  ERROR  *** EZ must be between 100 & 30000'
          ierror = ierror + 1
      ENDIF
c****
c****    Write the parameters to a disc file  and get another list!
c****
      IF( lno .EQ. 0 ) lno = fno
      nwrds = npars
      CALL wrdisc( junit, lvals, nwrds )
      IF( nvels .GT. 0 ) CALL wrdisc( junit, vels, nvels )
      nlists = nlists + 1
c
      IF( IAND(lprint,1) .NE. 0 ) THEN
          PRINT *,(lvals(i),i=1,6)
          PRINT *,(vals(i),i=7,12)
          PRINT *,(lvals(i), i=13,npars)
          IF( nvels .NE. 0 ) PRINT *,' vels:',(vels(i),i=1,nvels)
      ENDIF
c****
c****    finish up the parameter reading
c****
 2000 CONTINUE
      CALL getoke( token, nchars )                                       ! get the next token
      CALL upcase( token, nchars )
      ntokes = ntokes + 1
      IF( nchars .LE. 0 ) THEN
          IF( now .EQ. 1 ) PRINT *,' <  ENTER PARAMETERS  >'
          CALL rdline                                                   ! get a new line of parameters
          ntokes = 0
          GOTO 2000
      ENDIF
      lno = 0
      nvels = 0
      IF( token(1:nchars) .NE. 'END' .OR. nchars .NE. 3 ) GOTO 110

      RETURN
      END


subroutine jacobi_solve(k,xscheb,S1,S2,S3,aintl,nints,ab,dnu,da,db,psivals,avals)
use chebyshev
implicit double precision (a-h,o-z)

integer                        :: nints
double precision               :: ab(2,nints),avals(k,nints),psivals(k,nints)
double precision               :: xscheb(k),S1(k,k),S2(k,k),S3(k,k),aintl(k,k)

!
!  Construct the nonoscillatory phase and amplitude functions for Jacobi's differential
!  equation.  These functions are represented via piecewise Chebyshev expansions given
!  on a collection of intervals.
!
!  Input parameters:
!
!  Output parameters:
!

double precision      :: valsmp(k,nints),valsmpp(k,nints),alphap(k,nints)
data  pi        / 3.14159265358979323846264338327950288d0 /
data  piovertwo / 1.57079632679489661923132169163975144d0 /

p = dnu + (da+db+1)/2

!
!  Compute the value of the amplitude function and its first two derivatives
!  at the point pi/2.
!

call jacobi_asym3(dnu,da,db,c3,c2,c1)

!
!  Solve the third order linear ODE for the square of the amplitude function
!

do int=nints,1,-1
a = ab(1,int)
b = ab(2,int)
call jacobi_solve_int(dnu,da,db,a,b,k,xscheb,S1,S2,S3, &
  c1,c2,c3,avals(:,int),valsmp(:,int),valsmpp(:,int))
c1 = valsmpp(1,int)
c2 = valsmp(1,int)
c3 = avals(1,int)
end do

!
!  Record the values of alpha' and alpha'' at the left endpoint for later
!  user
!

apval  = 1/avals(1,1)
appval = -valsmp(1,1)/avals(1,1)**2

!
!  Construct the phase function and translate it appropriately
!

alphap = 1.0d0/avals
avals  = sqrt(avals)

c1     = 0
do int=1,nints
a              = ab(1,int)
b              = ab(2,int)
psivals(:,int) = c1 + matmul((b-a)/2*aintl,alphap(:,int))
c1             = psivals(k,int)
end do

a = ab(1,1)
call jacobi_asym0(dnu,da,db,a,y,yp)
c1 = y*sqrt(apval)
c2 = yp/sqrt(apval)+y*appval/(2*apval**(1.5d0))
a2  = atan2(c1,c2)

if(a2 .gt. pi) then
a2 = a2-pi
endif

if(a2 .le. 0) then
a2 = a2 + pi
endif

psivals = psivals+a2-piovertwo

!
!  Subtract p*t
!

do int=1,nints
a = ab(1,int)
b = ab(2,int)
do i=1,k
t = (b-a)/2 * xscheb(i) + (b+a)/2
psivals(i,int) = psivals(i,int)   - p*t
end do
end do

psivals = psivals/p

end subroutine

subroutine jacobi_solve_int(dnu,dalpha,dbeta,a,b,k,xs,S1,S2,S3,c1,c2,c3,valsm,valsmp,valsmpp)
use utils
use chebyshev
implicit double precision (a-h,o-z)

double precision       :: S1(k,k),S2(k,k),S3(k,k),xs(k)
double precision       :: valsm(k),valsmp(k),valsmpp(k)

double precision :: qvals(k),qpvals(k),amatr(k,k),xx(k),xx2(k)
double precision :: sigma(k)

dd   = (b-a)/2
xx = xs*(b-a)/2 + (a-b)/2
xx2 = 0.125d0 * (a-b)**2 * (xs-1)**2

!
!  Evaluate Q and its derivative at the specified points
!

do i=1,k
t         = (b-a)/2 * xs(i) + (b+a)/2
qvals(i)  = dnu*(1+dalpha+dbeta+dnu)+((1+dalpha+dbeta-1.d0*(-dalpha+  &
dbeta)*Cos(t))*1/sin(t)**2)/2.d0-((1+dalpha+dbeta)*cos(t)/sin(t)-1.d0*(-dalpha+dbeta)*&
  1/sin(t))**2/4.d0

qpvals(i) = (((-2+4.d0*dalpha**2+4.d0*dbeta**2)*Cos(t)+(dalpha-1.d0*dbeta)*(dalpha  &
+dbeta)*(3+Cos(2.d0*t)))*1/sin(t)**3)/4.d0
end do

!
!  Form the linear system to solve
!

do i=1,k
amatr(i,:) = 4*qvals(i) *S2(i,:)*dd**2 + 2 * qpvals(i) * S3(i,:)*dd**3
end do
do i=1,k
amatr(i,i) = amatr(i,i)+ 1.0d0
end do

sigma = -4*qvals * ( c2 + c1 * xx ) - 2 * qpvals * (c3 + c2 * xx + c1 * xx2)

!
!  Solve it
!

call qrsolv(amatr,k,sigma)

valsmpp = c1 + matmul(S1*dd,sigma)
valsmp  = c2 + matmul(S1*dd,valsmpp)
valsm   = c3 + matmul(S1*dd,valsmp)

end subroutine

! program test_solve_jacobi
! use utils
! use chebyshev
! implicit double precision (a-h,o-z)

! type(chebexps_data)           :: chebdata
! double precision, allocatable :: avals(:,:),psivals(:,:)
! double precision, allocatable :: ab(:,:),valsm(:,:),valsmp(:,:),valsmpp(:,:)
! double precision, allocatable :: S1(:,:),S2(:,:),S3(:,:),coefs(:,:)

! dnu    = 100.0d0
! da     = 0.25d0
! db     = 0.00d0
! pi     = acos(-1.0d0)
! k      = 30

! call chebexps2(k,chebdata)

! allocate(S1(k,k),S2(k,k),S3(k,k))
! S1 = chebdata%aintr
! S2 = matmul(S1,S1)
! S3 = matmul(S1,S2)

! nints = 13
! allocate(ab(2,nints),valsm(k,nints),valsmp(k,nints),valsmpp(k,nints),coefs(k,nints))
! allocate(avals(k,nints),psivals(k,nints))

! !
! !  Set the list of intervals
! !
! p = dnu+(da+db+1)/2
! a = 2/p
! b = pi/2

! do int=1,nints-1
! ab(1,nints-1-int+2) = a + (b-a) * 2.0d0**(-int)
! ab(2,nints-1-int+2) = a + (b-a) * 2.0d0**(1-int)
! end do

! ab(1,1) = a
! ab(2,1) = ab(1,2)

! call elapsed(t1)
! call jacobi_solve(chebdata%k,chebdata%xs,S1,S2,S3,chebdata%aintl,nints,ab,dnu,da,db,psivals,avals)
! call elapsed(t2)
! print *,"time = ", (t2-t1)
! print *,""

! x = 1.0d0
! call chebpw_eval(nints,ab,k,chebdata%xs,avals,x,aval)
! call chebpw_eval(nints,ab,k,chebdata%xs,psivals,x,psival)

! print *,aval,0.0996882556288365776669641708501876308d0
! print *,psival
! print *,aval*cos(psival)
! print *,0.0465831293979657797330681514249250580d0
! stop

! ! call jacobi_asym3(dnu,da,db,c3,c2,c1)
! ! do int=nints,1,-1
! ! a = ab(1,int)
! ! b = ab(2,int)
! ! call jacobi_solve_int(dnu,da,db,a,b,k,chebdata%xs,S1,S2,S3,&
! !   c1,c2,c3,valsm(:,int),valsmp(:,int),valsmpp(:,int))
! ! call elapsed(t2)
! ! c1 = valsmpp(1,int)
! ! c2 = valsmp(1,int)
! ! c3 = valsm(1,int)
! ! end do
! ! print *,"time = ",t2-t1

! ! print *,c1
! ! print *,c2
! ! print *,c3

! end program


module mod_mom
  use mpi
  use mod_param     , only: dims, bforce
  use mod_common_mpi, only: ierr
  use mod_types
  implicit none
  private
  public momxad,momyad,momzad,momxp,momyp,momzp
  contains
  subroutine momxad(nx,ny,nz,dxi,dyi,dzi,dzci,dzfi,dzflzi,visc,u,v,w,dudt,taux)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in) :: dxi,dyi,dzi,visc
    real(rp), intent(in), dimension(0:) :: dzci,dzfi,dzflzi
    real(rp), dimension(0:,0:,0:), intent(in) :: u,v,w
    real(rp), dimension(:,:,:), intent(out) :: dudt
    real(rp), dimension(3)  , intent(out) :: taux
    integer :: im,ip,jm,jp,km,kp,i,j,k
    real(rp) :: uuip,uuim,uvjp,uvjm,uwkp,uwkm
    real(rp) :: dudxp,dudxm,dudyp,dudym,dudzp,dudzm
    integer :: nxg,nyg,nzg
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,im,jm,km,ip,jp,kp) &
    !$OMP PRIVATE(uuip,uuim,uvjp,uvjm,uwkp,uwkm) &
    !$OMP PRIVATE(dudxp,dudxm,dudyp,dudym,dudzp,dudzm) &
    !$OMP SHARED(nx,ny,nz,dxi,dyi,dzi,visc,u,v,w,dudt,dzci,dzfi,bforce)
    do k=1,nz
      kp = k + 1
      km = k - 1
      do j=1,ny
        jp = j + 1
        jm = j - 1
        do i=1,nx
          ip = i + 1
          im = i - 1
          uuip  = 0.25*( u(ip,j,k)+u(i,j,k) )*( u(ip,j ,k )+u(i,j ,k ) )
          uuim  = 0.25*( u(im,j,k)+u(i,j,k) )*( u(im,j ,k )+u(i,j ,k ) )
          uvjp  = 0.25*( u(i,jp,k)+u(i,j,k) )*( v(ip,j ,k )+v(i,j ,k ) )
          uvjm  = 0.25*( u(i,jm,k)+u(i,j,k) )*( v(ip,jm,k )+v(i,jm,k ) )
          uwkp  = 0.25*( u(i,j,kp)+u(i,j,k) )*( w(ip,j ,k )+w(i,j ,k ) )
          uwkm  = 0.25*( u(i,j,km)+u(i,j,k) )*( w(ip,j ,km)+w(i,j ,km) )
          dudxp = (u(ip,j,k)-u(i,j,k))*dxi
          dudxm = (u(i,j,k)-u(im,j,k))*dxi
          dudyp = (u(i,jp,k)-u(i,j,k))*dyi
          dudym = (u(i,j,k)-u(i,jm,k))*dyi
          dudzp = (u(i,j,kp)-u(i,j,k))*dzci(k)
          dudzm = (u(i,j,k)-u(i,j,km))*dzci(km)
          !
          ! Momentum balance
          !
          dudt(i,j,k) = dxi*(     -uuip + uuim ) + (dudxp-dudxm)*visc*dxi + &
                        dyi*(     -uvjp + uvjm ) + (dudyp-dudym)*visc*dyi + &
                        dzfi(k)*( -uwkp + uwkm ) + (dudzp-dudzm)*visc*dzfi(k) + &
                        bforce(1)
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    taux(:) = 0.
    do k=1,nz
      do i=1,nx
        dudyp = (u(i,1 ,k)-u(i,0   ,k))*dyi*visc*dzflzi(k)
        dudym = (u(i,ny,k)-u(i,ny+1,k))*dyi*visc*dzflzi(k)
        taux(2) = taux(2) + (dudyp+dudym)
      enddo
    enddo
    do j=1,ny
      do i=1,nx
        dudzp = (u(i,j,1 )-u(i,j,0   ))*dzci(0)*visc
        dudzm = (u(i,j,nz)-u(i,j,nz+1))*dzci(nz)*visc
        taux(3) = taux(3) + (dudzp+dudzm)
      enddo
    enddo
    call mpi_allreduce(MPI_IN_PLACE,taux(1),3,MPI_REAL_RP,MPI_SUM,MPI_COMM_WORLD,ierr)
    nxg = nx*dims(1)
    nyg = ny*dims(2)
    nzg = nz
    taux(1) = taux(1)/(1.*nyg)
    taux(2) = taux(2)/(1.*nxg)
    taux(3) = taux(3)/(1.*nxg*nyg)
    return
  end subroutine momxad
  !
  subroutine momyad(nx,ny,nz,dxi,dyi,dzi,dzci,dzfi,dzflzi,visc,u,v,w,dvdt,tauy)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in) :: dxi,dyi,dzi,visc
    real(rp), intent(in), dimension(0:) :: dzci,dzfi,dzflzi
    real(rp), dimension(0:,0:,0:), intent(in) :: u,v,w
    real(rp), dimension(:,:,:), intent(out) :: dvdt
    real(rp), dimension(3), intent(out) :: tauy
    integer :: im,ip,jm,jp,km,kp,i,j,k
    real(rp) :: uvip,uvim,vvjp,vvjm,wvkp,wvkm
    real(rp) :: dvdxp,dvdxm,dvdyp,dvdym,dvdzp,dvdzm
    integer :: nxg,nyg,nzg
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,im,jm,km,ip,jp,kp) &
    !$OMP PRIVATE(uvip,uvim,vvjp,vvjm,wvkp,wvkm) &
    !$OMP PRIVATE(dvdxp,dvdxm,dvdyp,dvdym,dvdzp,dvdzm) &
    !$OMP SHARED(nx,ny,nz,dxi,dyi,dzi,visc,u,v,w,dvdt,dzci,dzfi,bforce)
    do k=1,nz
      kp = k + 1
      km = k - 1
      do j=1,ny
        jp = j + 1
        jm = j - 1
        do i=1,nx
          ip = i + 1
          im = i - 1
          uvip  = 0.25*( u(i ,j,k)+u(i ,jp,k) )*( v(i,j,k )+v(ip,j ,k) )
          uvim  = 0.25*( u(im,j,k)+u(im,jp,k) )*( v(i,j,k )+v(im,j ,k) )
          vvjp  = 0.25*( v(i,j,k )+v(i,jp,k)  )*( v(i,j,k )+v(i ,jp,k) )
          vvjm  = 0.25*( v(i,j,k )+v(i,jm,k)  )*( v(i,j,k )+v(i ,jm,k) )
          wvkp  = 0.25*( w(i,j,k )+w(i,jp,k)  )*( v(i,j,kp)+v(i ,j ,k) )
          wvkm  = 0.25*( w(i,j,km)+w(i,jp,km) )*( v(i,j,km)+v(i ,j ,k) )
          dvdxp = (v(ip,j,k)-v(i,j,k))*dxi
          dvdxm = (v(i,j,k)-v(im,j,k))*dxi
          dvdyp = (v(i,jp,k)-v(i,j,k))*dyi
          dvdym = (v(i,j,k)-v(i,jm,k))*dyi
          dvdzp = (v(i,j,kp)-v(i,j,k))*dzci(k)
          dvdzm = (v(i,j,k)-v(i,j,km))*dzci(km)
          !
          ! Momentum balance
          !
          dvdt(i,j,k) = dxi*(     -uvip + uvim ) + (dvdxp-dvdxm)*visc*dxi+ &
                        dyi*(     -vvjp + vvjm ) + (dvdyp-dvdym)*visc*dyi+ &
                        dzfi(k)*( -wvkp + wvkm ) + (dvdzp-dvdzm)*visc*dzfi(k)+ &
                        bforce(2)
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    tauy(:) = 0.
    do k=1,nz
      do j=1,ny
        dvdxp = (v(1 ,j,k)-v(0   ,j,k))*dxi*visc*dzflzi(k)
        dvdxm = (v(nx,j,k)-v(nx+1,j,k))*dxi*visc*dzflzi(k)
        tauy(1) = tauy(1) + (dvdxp+dvdxm)
      enddo
    enddo
    do j=1,ny
      do i=1,nx
        dvdzp = (v(i,j,1 )-v(i,j,0   ))*dzci(0)*visc
        dvdzm = (v(i,j,nz)-v(i,j,nz+1))*dzci(nz)*visc
        tauy(3) = tauy(3) + (dvdzp+dvdzm)
      enddo
    enddo
    call mpi_allreduce(MPI_IN_PLACE,tauy(1),3,MPI_REAL_RP,MPI_SUM,MPI_COMM_WORLD,ierr)
    nxg = nx*dims(1)
    nyg = ny*dims(2)
    nzg = nz
    tauy(1) = tauy(1)/(1.*nyg)
    tauy(2) = tauy(2)/(1.*nxg)
    tauy(3) = tauy(3)/(1.*nxg*nyg)
    return
  end subroutine momyad
  !
  subroutine momzad(nx,ny,nz,dxi,dyi,dzi,dzci,dzfi,dzflzi,visc,u,v,w,dwdt,tauz)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in) :: dxi,dyi,dzi,visc
    real(rp), intent(in), dimension(0:) :: dzci,dzfi,dzflzi
    real(rp), dimension(0:,0:,0:), intent(in) :: u,v,w
    real(rp), dimension(:,:,:), intent(out) :: dwdt
    real(rp), dimension(3), intent(out) :: tauz
    integer :: im,ip,jm,jp,km,kp,i,j,k
    real(rp) :: uwip,uwim,vwjp,vwjm,wwkp,wwkm
    real(rp) :: dwdxp,dwdxm,dwdyp,dwdym,dwdzp,dwdzm
    integer :: nxg,nyg,nzg
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,im,jm,km,ip,jp,kp) &
    !$OMP PRIVATE(uwip,uwim,vwjp,vwjm,wwkp,wwkm) &
    !$OMP PRIVATE(dwdxp,dwdxm,dwdyp,dwdym,dwdzp,dwdzm) &
    !$OMP SHARED(nx,ny,nz,dxi,dyi,dzi,visc,u,v,w,dwdt,dzci,dzfi,bforce)
    do k=1,nz
      kp = k + 1
      km = k - 1
      do j=1,ny
        jp = j + 1
        jm = j - 1
        do i=1,nx
          ip = i + 1
          im = i - 1
          uwip  = 0.25*( w(i,j,k)+w(ip,j,k) )*( u(i ,j ,k)+u(i ,j ,kp) )
          uwim  = 0.25*( w(i,j,k)+w(im,j,k) )*( u(im,j ,k)+u(im,j ,kp) )
          vwjp  = 0.25*( w(i,j,k)+w(i,jp,k) )*( v(i ,j ,k)+v(i ,j ,kp) )
          vwjm  = 0.25*( w(i,j,k)+w(i,jm,k) )*( v(i ,jm,k)+v(i ,jm,kp) )
          wwkp  = 0.25*( w(i,j,k)+w(i,j,kp) )*( w(i ,j ,k)+w(i ,j ,kp) )
          wwkm  = 0.25*( w(i,j,k)+w(i,j,km) )*( w(i ,j ,k)+w(i ,j ,km) )
          dwdxp = (w(ip,j,k)-w(i,j,k))*dxi
          dwdxm = (w(i,j,k)-w(im,j,k))*dxi
          dwdyp = (w(i,jp,k)-w(i,j,k))*dyi
          dwdym = (w(i,j,k)-w(i,jm,k))*dyi
          dwdzp = (w(i,j,kp)-w(i,j,k))*dzfi(kp)
          dwdzm = (w(i,j,k)-w(i,j,km))*dzfi(k)
          !
          ! Momentum balance
          !
          dwdt(i,j,k) = dxi*(     -uwip + uwim ) + (dwdxp-dwdxm)*visc*dxi+ &
                        dyi*(     -vwjp + vwjm ) + (dwdyp-dwdym)*visc*dyi+ &
                        dzci(k)*( -wwkp + wwkm ) + (dwdzp-dwdzm)*visc*dzci(k)+ &
                        bforce(3)
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    tauz(:) = 0.
    do k=1,nz
      do j=1,ny
        dwdxp = (w(1 ,j,k)-w(0   ,j,k))*dxi*visc*dzflzi(k)
        dwdxm = (w(nx,j,k)-w(nx+1,j,k))*dxi*visc*dzflzi(k)
        tauz(1) = tauz(1) + (dwdxp+dwdxm)
      enddo
    enddo
    do k=1,nz
      do i=1,nx
        dwdyp = (w(i,1,k )-w(i,0   ,k))*dyi*visc*dzflzi(k)
        dwdym = (w(i,ny,k)-w(i,ny+1,k))*dyi*visc*dzflzi(k)
        tauz(2) = tauz(2) + (dwdyp+dwdym)
      enddo
    enddo
    call mpi_allreduce(MPI_IN_PLACE,tauz(1),3,MPI_REAL_RP,MPI_SUM,MPI_COMM_WORLD,ierr)
    nxg = nx*dims(1)
    nyg = ny*dims(2)
    nzg = nz
    tauz(1) = tauz(1)/(1.*nyg)
    tauz(2) = tauz(2)/(1.*nxg)
    tauz(3) = tauz(3)/(1.*nxg*nyg)
    return
  end subroutine momzad
  !
  subroutine momxp(nx,ny,nz,dxi,p,dudt)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in) :: dxi
    real(rp), dimension(0:,0:,0:), intent(in) :: p
    real(rp), dimension(:,:,:), intent(out) :: dudt
    integer :: i,j,k
    integer :: ip
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,ip) &
    !$OMP SHARED(nx,ny,nz,dxi,p,dudt)
    do k=1,nz
      do j=1,ny
        do i=1,nx
          ip = i + 1
          dudt(i,j,k) = - dxi*( p(ip,j,k)-p(i,j,k) )
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    return
  end subroutine momxp
  !
  subroutine momyp(nx,ny,nz,dyi,p,dvdt)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in) :: dyi
    real(rp), dimension(0:,0:,0:), intent(in) :: p
    real(rp), dimension(:,:,:), intent(out) :: dvdt
    integer :: i,j,k
    integer :: jp
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,jp) &
    !$OMP SHARED(nx,ny,nz,dyi,p,dvdt)
    do k=1,nz
      do j=1,ny
        jp = j + 1
        do i=1,nx
          dvdt(i,j,k) = - dyi*( p(i,jp,k)-p(i,j,k) )
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    return
  end subroutine momyp
  !
  subroutine momzp(nx,ny,nz,dzci,p,dwdt)
    implicit none
    integer , intent(in) :: nx,ny,nz
    real(rp), intent(in), dimension(0:) :: dzci
    real(rp), dimension(0:,0:,0:), intent(in) :: p
    real(rp), dimension(:,:,:), intent(out) :: dwdt
    integer :: kp
    integer :: i,j,k
    !
    !$OMP PARALLEL DO DEFAULT(none) &
    !$OMP PRIVATE(i,j,k,kp) &
    !$OMP SHARED(nx,ny,nz,p,dwdt,dzci)
    do k=1,nz
      kp = k + 1
      do j=1,ny
        do i=1,nx
          dwdt(i,j,k) = - dzci(k)*( p(i,j,kp)-p(i,j,k) )
        enddo
      enddo
    enddo
    !$OMP END PARALLEL DO
    return
  end subroutine momzp
end module mod_mom


module mathroutines_module
use constants_module

double precision, allocatable :: exp_x(:),exp_y(:)
double precision :: exp_fact
integer :: exp_nx

contains

!-------------------------------------------------------------------
!                   REMAP FUNCTION ONTO NEW GRID
!
! This is a general purpose routine for remapping onto a new grid.
! Interpolation is done in log(f) space. Care is taken that in case
! of going to lower resolution, the average of the high-res function
! is taken as the low-res value.
!
! ARGUMENTS:
!  nold        Nr of grid points of old function
!  xold        Array with old x coordinates
!  fold        Array with old function values
!  nnew        Nr of grid points of new function
!  xnew        Array with new x coordinates
!  av          =0  Do not use the averaging trick in case of lowering resolution
!              =1  Use averaging trick: Make sure that the value fnew in case
!                  of lower resolution equals the average of fold over the
!                  region of x belonging to that grid point.
!  elow        Way to treat if xnew has lower lowest value than xold
!              =0  Do not allow out of bound
!              =1  Take boundary value of fold
!              =2  Logarithmic extrapolation
!              =3  Out of bound -> 0
!              =4  Smooth version of out of bound -> 0
!  eup         Way to treat if xnew has higher highest value than xold
!              =0  Do not allow out of bound
!              =1  Take boundary value of fold
!              =2  Logarithmic extrapolation
!              =3  Out of bound -> 0
!              =4  Smooth version of out of bound -> 0
!
! RETURNS:
!  fnew        Array with new function values
!  ierror      =0  means everything went fine. Else: error:
!              =-1 xnew has lower lowest value than xold (out of bound),
!                  Aborted.
!              =+1 xnew has higher highest value than xold (out of bound)
!                  Aborted.
!-------------------------------------------------------------------
subroutine remap_function(nold,xold,fold,nnew,xnew,fnew,av,elow,eup,ierror)
  implicit none
  integer :: nold,nnew,elow,eup,ierror,av
  double precision :: xold(1:nold),fold(1:nold)
  double precision :: xnew(1:nnew),fnew(1:nnew)
  integer :: iold,inew,sgnold,sgnnew,ioldstart,ioldend,inewstart,inewend
  double precision :: eps
  !
  ! Reset ierror
  !
  ierror = 0
  !
  ! Check if x is ascending or descending, and figure out the
  ! minimum and maximum values of xold and xnew
  !
  if(xold(nold).ge.xold(1)) then
     sgnold    = 1
     ioldstart = 1
     ioldend   = nold
  else
     sgnold    = -1
     ioldstart = nold
     ioldend   = 1
  endif
  if(xnew(nnew).ge.xnew(1)) then
     sgnnew    = 1
     inewstart = 1
     inewend   = nnew
  else
     sgnnew    = -1
     inewstart = nnew
     inewend   = 1
  endif
  !
  ! Now do a loop over the new x values, from low to high
  !
  do inew=inewstart,inewend,sgnnew
     if(xnew(inew).lt.xold(ioldstart)) then
        !
        ! Lower than lowest boundary
        !
        if(elow.eq.0) then
           ierror = -1
           return
        elseif(elow.eq.1) then
           fnew(inew) = fold(ioldstart)
        elseif(elow.eq.2) then
           if(nold.gt.1) then
              if((fold(ioldstart+sgnold).gt.0.d0).and. &
                   (fold(ioldstart).gt.0.d0)) then
                 fnew(inew) = fold(ioldstart) *                               &
                        (fold(ioldstart+sgnold)/fold(ioldstart))**            &
                        ((log(xnew(inew))-log(xold(ioldstart)))/              &
                         (log(xold(ioldstart+sgnold))-log(xold(ioldstart))))
              else
                 fnew(inew) = 0.d0
              endif
           else
              fnew(inew) = fold(ioldstart)
           endif
        elseif(elow.eq.3) then
           fnew(inew) = 0.d0
        elseif(elow.eq.4) then
           if(nold.gt.1) then
              eps = ((xnew(inew)-xold(ioldstart))/                     &
                         (xold(ioldstart+sgnold)-xold(ioldstart)))
              if(eps.gt.-1.d0) then
                 fnew(inew) = (1-abs(eps))*fold(ioldstart)
              else
                 fnew(inew) = 0.d0
              endif
           else
              fnew(inew) = fold(ioldstart)
           endif
        else
           stop 1349
        endif
     elseif(xnew(inew).gt.xold(ioldend)) then
        !
        ! higher than highest boundary
        !
        if(eup.eq.0) then
           ierror = -1
           return
        elseif(eup.eq.1) then
           fnew(inew) = fold(ioldend)
        elseif(eup.eq.2) then
           if(nold.gt.1) then
              if((fold(ioldend-sgnold).gt.0.d0).and. &
                   (fold(ioldend).gt.0.d0)) then
                 fnew(inew) = fold(ioldend) *                               &
                        (fold(ioldend-sgnold)/fold(ioldend))**              &
                        ((log(xnew(inew))-log(xold(ioldend)))/              &
                         (log(xold(ioldend-sgnold))-log(xold(ioldend))))
              else
                 fnew(inew) = 0.d0
              endif
           else
              fnew(inew) = fold(ioldend)
           endif
        elseif(eup.eq.3) then
           fnew(inew) = 0.d0
        elseif(eup.eq.4) then
           if(nold.gt.1) then
              eps = ((xnew(inew)-xold(ioldend))/                     &
                         (xold(ioldend-sgnold)-xold(ioldend)))
              if(eps.gt.-1.d0) then
                 fnew(inew) = (1-abs(eps))*fold(ioldend)
              else
                 fnew(inew) = 0.d0
              endif
           else
              fnew(inew) = fold(ioldend)
           endif
        else
           stop 1349
        endif
     else
        !
        ! Within the domain of xold
        !
        if(av.eq.0) then
           !
           ! Simple remapping: just interpolation
           !
           call hunt(xold,nold,xnew(inew),iold)
           if(xnew(inew).eq.xold(1)) then
              fnew(inew) = fold(1)
           elseif(xnew(inew).eq.xold(nold)) then
              fnew(inew) = fold(nold)
           else
              if((iold.le.0).or.(iold.ge.nold)) stop 1350
              eps = (xnew(inew)-xold(iold)) / (xold(iold+1)-xold(iold))
              if((fold(iold).gt.0.d0).and.(fold(iold+1).gt.0.d0)) then
                 fnew(inew) = exp((1.d0-eps)*log(fold(iold))+eps*log(fold(iold+1)))
              else
                 fnew(inew) = (1.d0-eps)*fold(iold)+eps*fold(iold+1)
              endif
           endif
        elseif(av.eq.1) then
           !
           ! Remapping with assurance that the integral over the function
           ! remains roughly OK even if much lower resolution is used
           !
           write(stdo,*) 'SORRY: The integral-conserving remapping of functions'
           write(stdo,*) '       is not yet implemented...'
           stop 4722
        else
           stop 4721
        endif
     endif
  enddo
  !
end subroutine remap_function

!--------------------------------------------------------------------------
!                     PREPARE TABULATED EXPONENTIAL
!--------------------------------------------------------------------------
subroutine prep_tabulated_exp(x0,x1,nx)
  implicit none
  double precision :: x0,x1
  integer :: nx,ix,ierr
  exp_nx = nx
  if(allocated(exp_x)) deallocate(exp_x)
  if(allocated(exp_y)) deallocate(exp_y)
  allocate(exp_x(nx),STAT=ierr)
  if(ierr.ne.0) then
     write(stdo,*) 'ERROR: Could not allocate tabulated exponential array'
     stop
  endif
  allocate(exp_y(nx),STAT=ierr)
  if(ierr.ne.0) then
     write(stdo,*) 'ERROR: Could not allocate tabulated exponential array'
     stop
  endif
  do ix=1,nx
     exp_x(ix) = x0 - (x1-x0)*((ix-1.d0)/(nx-1.d0))
     exp_y(ix) = exp(exp_x(ix))
  enddo
  exp_fact = (exp_nx-1)/(exp_x(exp_nx)-exp_x(1))
end subroutine prep_tabulated_exp

!--------------------------------------------------------------------------
!                   THE EXPONENTIAL WITH LOOKUP
!--------------------------------------------------------------------------
function tabexp(x)
  implicit none
  double precision :: x,tabexp,eps
  integer :: ix
  if(exp_nx.eq.0) then
     tabexp = exp(x)
     return
  else
     if(x.le.exp_x(1)) then
        tabexp = exp(x)
        return
     endif
     if(x.ge.exp_x(exp_nx)) then
        tabexp = exp(x)
        return
     endif
     ix  = exp_fact*(x-exp_x(1)) + 1
     if((ix.lt.1).or.(ix.ge.exp_nx)) stop 3234
     eps = (x-exp_x(ix))/(exp_x(ix+1)-exp_x(ix))
     if((eps.lt.0.d0).or.(eps.gt.1.d0)) stop 3235
     tabexp = (1.d0-eps)*exp_y(ix)+eps*exp_y(ix+1)
     return
  endif
end function tabexp

!--------------------------------------------------------------------------
!                     DEALLOCATE TABULATED EXPONENTIALS
!--------------------------------------------------------------------------
subroutine tabulated_exp_cleanup()
  implicit none
  if(allocated(exp_x)) deallocate(exp_x)
  if(allocated(exp_y)) deallocate(exp_y)
end subroutine tabulated_exp_cleanup

!--------------------------------------------------------------------------
!                       NUMERICAL RECIPES ROUTINES
!--------------------------------------------------------------------------
FUNCTION ran2(idum)
  INTEGER idum,IM1,IM2,IMM1,IA1,IA2,IQ1,IQ2,IR1,IR2,NTAB,NDIV
  doubleprecision ran2,AM,EPS,RNMX
  PARAMETER (IM1=2147483563,IM2=2147483399,AM=1./IM1,IMM1=IM1-1, &
     IA1=40014,IA2=40692,IQ1=53668,IQ2=52774,IR1=12211,IR2=3791, &
     NTAB=32,NDIV=1+IMM1/NTAB,EPS=1.2e-7,RNMX=1.-EPS)
  INTEGER idum2,j,k,iv(NTAB),iy
  SAVE iv,iy,idum2
  !$OMP THREADPRIVATE(iv,iy,idum2)
  DATA idum2/123456789/, iv/NTAB*0/, iy/0/
  if (idum.le.0) then
     idum=max(-idum,1)
     idum2=idum
     do j=NTAB+8,1,-1
        k=idum/IQ1
        idum=IA1*(idum-k*IQ1)-k*IR1
        if (idum.lt.0) idum=idum+IM1
        if (j.le.NTAB) iv(j)=idum
     enddo
     iy=iv(1)
  endif
  k=idum/IQ1
  idum=IA1*(idum-k*IQ1)-k*IR1
  if (idum.lt.0) idum=idum+IM1
  k=idum2/IQ2
  idum2=IA2*(idum2-k*IQ2)-k*IR2
  if (idum2.lt.0) idum2=idum2+IM2
  j=1+iy/NDIV
  iy=iv(j)-idum2
  iv(j)=idum
  if(iy.lt.1)iy=iy+IMM1
  ran2=min(AM*iy,RNMX)
  return
END FUNCTION ran2
! C  (C) Copr. 1986-92 Numerical Recipes Software =v1.9"217..

SUBROUTINE hunt(xx,n,x,jlo)
  INTEGER jlo,n
  doubleprecision x,xx(n)
  INTEGER inc,jhi,jm
  LOGICAL ascnd
  ascnd=xx(n).gt.xx(1)
  if(jlo.le.0.or.jlo.gt.n)then
     jlo=0
     jhi=n+1
     goto 3
  endif
  inc=1
  if(x.ge.xx(jlo).eqv.ascnd)then
1    jhi=jlo+inc
     if(jhi.gt.n)then
        jhi=n+1
     else if(x.ge.xx(jhi).eqv.ascnd)then
        jlo=jhi
        inc=inc+inc
        goto 1
     endif
  else
     jhi=jlo
2    jlo=jhi-inc
     if(jlo.lt.1)then
        jlo=0
     else if(x.lt.xx(jlo).eqv.ascnd)then
        jhi=jlo
        inc=inc+inc
        goto 2
     endif
  endif
3 if(jhi-jlo.eq.1)return
  jm=(jhi+jlo)/2
  if(x.gt.xx(jm).eqv.ascnd)then
     jlo=jm
  else
     jhi=jm
  endif
  goto 3
END SUBROUTINE hunt
! C  (C) Copr. 1986-92 Numerical Recipes Software =v1.9"217..

SUBROUTINE lubksb(a,n,np,indx,b)
  INTEGER n,np,indx(n)
  double precision a(np,np),b(n)
  INTEGER i,ii,j,ll
  double precision sum
  ii=0
  do i=1,n
     ll=indx(i)
     sum=b(ll)
     b(ll)=b(i)
     if (ii.ne.0)then
        do j=ii,i-1
           sum=sum-a(i,j)*b(j)
        enddo
     else if (sum.ne.0.) then
        ii=i
     endif
     b(i)=sum
  enddo
  do i=n,1,-1
     sum=b(i)
     do j=i+1,n
        sum=sum-a(i,j)*b(j)
     enddo
     b(i)=sum/a(i,i)
  enddo
  return
END SUBROUTINE lubksb
! (C) Copr. 1986-92 Numerical Recipes Software =v1.9"217..

SUBROUTINE ludcmp(a,n,np,indx,d,success)
  INTEGER n,np,indx(n),NMAX
  double precision d,a(np,np),TINY
  PARAMETER (NMAX=500,TINY=1.0e-20)
  INTEGER i,imax,j,k
  double precision aamax,dum,sum,vv(NMAX)
  logical,optional :: success
  if(present(success)) success = .true.
  d=1.
  do i=1,n
     aamax=0.
     do j=1,n
        if (abs(a(i,j)).gt.aamax) aamax=abs(a(i,j))
     enddo
     if (aamax.eq.0.) then
        if(present(success)) then
           success=.false.
           return
        else
           stop 'singular matrix in ludcmp'
        endif
     endif
     vv(i)=1./aamax
  enddo
  do j=1,n
     do i=1,j-1
        sum=a(i,j)
        do k=1,i-1
           sum=sum-a(i,k)*a(k,j)
        enddo
        a(i,j)=sum
     enddo
     aamax=0.
     do i=j,n
        sum=a(i,j)
        do k=1,j-1
           sum=sum-a(i,k)*a(k,j)
        enddo
        a(i,j)=sum
        dum=vv(i)*abs(sum)
        if (dum.ge.aamax) then
           imax=i
           aamax=dum
        endif
     enddo
     if (j.ne.imax)then
        do k=1,n
           dum=a(imax,k)
           a(imax,k)=a(j,k)
           a(j,k)=dum
        enddo
        d=-d
        vv(imax)=vv(j)
     endif
     indx(j)=imax
     if(a(j,j).eq.0.)a(j,j)=TINY
     if(j.ne.n)then
        dum=1./a(j,j)
        do i=j+1,n
           a(i,j)=a(i,j)*dum
        enddo
     endif
  enddo
  return
END SUBROUTINE ludcmp
!  (C) Copr. 1986-92 Numerical Recipes Software =v1.9"217..

end module mathroutines_module


      subroutine buoflx(bf, mask, hf,wf,sst,sss,n,m, itype)
      implicit none
c
      integer                 :: n,m,itype
      integer, dimension(n,m) :: mask
      real,    dimension(n,m) :: bf, hf,wf,sst,sss
c
c --- calculate buoyancy flux.
c
      real, parameter :: flag = 2.0**100
c
      integer i,j
      real    dsgds,dsgdt,g,thref,spcifh,sf
c
c-----------------------------------------------------------------------------
c
c --- -----------------
c --- equation of state
c --- -----------------
c
      real dsigdt,dsigds
      real s,t
c
c --- coefficients for sigma-0 (based on Brydon & Sun fit)
      real       c1,c2,c3,c4,c5,c6,c7
      parameter (c1=-1.36471E-01, c2= 4.68181E-02, c3= 8.07004E-01,
     &           c4=-7.45353E-03, c5=-2.94418E-03,
     &           c6= 3.43570E-05, c7= 3.48658E-05)
      real       pref
      parameter (pref=0.0)
c
c --- sigma-theta as a function of temp (deg c) and salinity (mil)
c --- (friedrich-levitus 3rd degree polynomial fit)
c
*     sig(t,s)=(c1+c3*s+t*(c2+c5*s+t*(c4+c7*s+c6*t)))
c
c --- d(sig)/dt
      dsigdt(t,s)=(c2+c5*s+2.*t*(c4+c7*s+1.5*c6*t))
c
c --- d(sig)/ds
      dsigds(t,s)=(c3+t*(c5+t*c7))
c-----------------------------------------------------------------------------
c
      spcifh = 3990.0    !specific heat of sea water         (j/kg/deg)
      g      =    9.806  !gravitational acceleration         (m/s**2)
      thref  =    0.001  !reference value of specific volume (m**3/kg)
c
      if     (itype.eq.1) then
c
c ---   temperature (heat) buoyancy flux
c
        do j= 1,m
          do i= 1,n
            if     (mask(i,j).ne.0) then
              dsgdt   = dsigdt(sst(i,j),sss(i,j))
              bf(i,j) = g*thref*(dsgdt*hf(i,j)*thref/spcifh)
            else
              bf(i,j)=flag
            endif
          enddo
        enddo
      elseif (itype.eq.2) then
c
c ---   salinity (E-P) buoyancy flux
c
        do j= 1,m
          do i= 1,n
            if     (mask(i,j).ne.0) then
              dsgds   = dsigds(sst(i,j),sss(i,j))
              sf      = -wf(i,j)*sss(i,j)
              bf(i,j) = g*thref*(dsgds*sf*thref)
            else
              bf(i,j)=flag
            endif
          enddo
        enddo
      else
c
c ---   total buoyancy flux
c
        do j= 1,m
          do i= 1,n
            if     (mask(i,j).ne.0) then
              dsgds   = dsigds(sst(i,j),sss(i,j))
              dsgdt   = dsigdt(sst(i,j),sss(i,j))
              sf      = -wf(i,j)*sss(i,j)
              bf(i,j) = g*thref*(dsgds*sf     *thref) +
     &                  g*thref*(dsgdt*hf(i,j)*thref/spcifh)
            else
              bf(i,j)=flag
            endif
          enddo
        enddo
      endif
      return
      end subroutine buoflx


subroutine mapc2p(xc, yc, zc, xp, yp, zp)

    use fault_module, only: center, theta, xcb, ycb

    implicit none

    real(kind=8), intent(in) :: xc, yc, zc
    real(kind=8), intent(out) :: xp, yp, zp

    ! Local variables
    real (kind=8) :: ls, alpha, xrot, zrot

    if (xc < xcb(1)) then
      ls = dsqrt((xc-xcb(1))**2 + (zc-center(3))**2)
    elseif (xc > xcb(2)) then
      ls = dsqrt((xc-xcb(2))**2 + (zc-center(3))**2)
    else
      ls = dabs(zc - center(3))
    end if

    alpha = ls/(-center(3))
    xrot = center(1) + dcos(theta)*(xc-center(1)) + dsin(theta)*(zc-center(3))
    zrot = center(3) - dsin(theta)*(xc-center(1)) + dcos(theta)*(zc-center(3))

    yp = yc
    if (alpha < 1.d0) then
      xp = (1.d0-alpha)*xrot + alpha*xc
      zp = (1.d0-alpha)*zrot + alpha*zc
    else
      xp = xc
      zp = zc
    end if

    return
end


      SUBROUTINE MB04DS( JOB, N, A, LDA, QG, LDQG, ILO, SCALE, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To balance a real skew-Hamiltonian matrix
C
C                   [  A   G  ]
C              S =  [       T ] ,
C                   [  Q   A  ]
C
C     where A is an N-by-N matrix and G, Q are N-by-N skew-symmetric
C     matrices. This involves, first, permuting S by a symplectic
C     similarity transformation to isolate eigenvalues in the first
C     1:ILO-1 elements on the diagonal of A; and second, applying a
C     diagonal similarity transformation to rows and columns
C     ILO:N, N+ILO:2*N to make the rows and columns as close in 1-norm
C     as possible. Both steps are optional.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             Specifies the operations to be performed on S:
C             = 'N':  none, set ILO = 1, SCALE(I) = 1.0, I = 1 .. N;
C             = 'P':  permute only;
C             = 'S':  scale only;
C             = 'B':  both permute and scale.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix A. N >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the matrix A of the balanced skew-Hamiltonian. In
C             particular, the strictly lower triangular part of the
C             first ILO-1 columns of A is zero.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,N).
C
C     QG      (input/output) DOUBLE PRECISION array, dimension
C                            (LDQG,N+1)
C             On entry, the leading N-by-N+1 part of this array must
C             contain in columns 1:N the strictly lower triangular part
C             of the matrix Q and in columns 2:N+1 the strictly upper
C             triangular part of the matrix G. The parts containing the
C             diagonal and the first supdiagonal of this array are not
C             referenced.
C             On exit, the leading N-by-N+1 part of this array contains
C             the strictly lower and strictly upper triangular parts of
C             the matrices Q and G, respectively, of the balanced
C             skew-Hamiltonian. In particular, the strictly lower
C             triangular part of the first ILO-1 columns of QG is zero.
C
C     LDQG    INTEGER
C             The leading dimension of the array QG.  LDQG >= MAX(1,N).
C
C     ILO     (output) INTEGER
C             ILO-1 is the number of deflated eigenvalues in the
C             balanced skew-Hamiltonian matrix.
C
C     SCALE   (output) DOUBLE PRECISION array of dimension (N)
C             Details of the permutations and scaling factors applied to
C             S.  For j = 1,...,ILO-1 let P(j) = SCALE(j). If P(j) <= N,
C             then rows and columns P(j) and P(j)+N are interchanged
C             with rows and columns j and j+N, respectively. If
C             P(j) > N, then row and column P(j)-N are interchanged with
C             row and column j+N by a generalized symplectic
C             permutation. For j = ILO,...,N the j-th element of SCALE
C             contains the factor of the scaling applied to row and
C             column j.
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
C     REFERENCES
C
C     [1] Benner, P.
C         Symplectic balancing of Hamiltonian matrices.
C         SIAM J. Sci. Comput., 22 (5), pp. 1885-1904, 2001.
C
C     CONTRIBUTORS
C
C     D. Kressner, Technical Univ. Berlin, Germany, and
C     P. Benner, Technical Univ. Chemnitz, Germany, December 2003.
C
C     REVISIONS
C
C     V. Sima, June 2008 (SLICOT version of the HAPACK routine DSHBAL).
C     V. Sima, Oct. 2012, Mar. 2016.
C
C     KEYWORDS
C
C     Balancing, skew-Hamiltonian matrix.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
      DOUBLE PRECISION  FACTOR
      PARAMETER         ( FACTOR = 0.95D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOB
      INTEGER           ILO, INFO, LDA, LDQG, N
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), QG(LDQG,*), SCALE(*)
C     .. Local Scalars ..
      LOGICAL           CONV, LPERM, LSCAL
      INTEGER           I, IC, ILOOLD, J
      DOUBLE PRECISION  C, F, G, MAXC, MAXR, R, S, SCLFAC, SFMAX1,
     $                  SFMAX2, SFMIN1, SFMIN2
C     .. External Functions ..
      LOGICAL           LSAME
      INTEGER           IDAMAX
      DOUBLE PRECISION  DASUM, DLAMCH
      EXTERNAL          DASUM, DLAMCH, IDAMAX, LSAME
C     .. External Subroutines ..
      EXTERNAL          DRSCL, DSCAL, DSWAP, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, DBLE, MAX, MIN
C
C     .. Executable Statements ..
C
C     Check the scalar input parameters.
C
      INFO = 0
      LPERM = LSAME( JOB, 'P' ) .OR. LSAME( JOB, 'B' )
      LSCAL = LSAME( JOB, 'S' ) .OR. LSAME( JOB, 'B' )
C
      IF ( .NOT.LPERM .AND. .NOT.LSCAL .AND.
     $     .NOT.LSAME( JOB, 'N' ) ) THEN
         INFO = -1
      ELSE IF ( N.LT.0 ) THEN
         INFO = -2
      ELSE IF ( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      ELSE IF ( LDQG.LT.MAX( 1, N ) ) THEN
         INFO = -6
      END IF
C
C     Return if there were illegal values.
C
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB04DS', -INFO )
         RETURN
      END IF
C
      ILO = 1
C
C     Quick return if possible.
C
      IF ( N.EQ.0 )
     $   RETURN
      IF ( .NOT.LPERM .AND. .NOT.LSCAL ) THEN
         DO 10  I = 1, N
            SCALE(I) = ONE
   10    CONTINUE
         RETURN
      END IF
C
C     Permutations to isolate eigenvalues if possible.
C
      IF ( LPERM ) THEN
         ILOOLD = 0
C        WHILE ( ILO.NE.ILOOLD )
   20    IF ( ILO.NE.ILOOLD ) THEN
            ILOOLD = ILO
C
C           Scan columns ILO .. N.
C
            I = ILO
C           WHILE ( I.LE.N .AND. ILO.EQ.ILOOLD )
   30       IF ( I.LE.N .AND. ILO.EQ.ILOOLD ) THEN
               DO 40  J = ILO, I-1
                  IF ( A(J,I).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 30
                  END IF
   40          CONTINUE
               DO 50  J = I+1, N
                  IF ( A(J,I).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 30
                  END IF
   50          CONTINUE
               DO 60  J = ILO, I-1
                  IF ( QG(I,J).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 30
                  END IF
   60          CONTINUE
               DO 70  J = I+1, N
                  IF ( QG(J,I).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 30
                  END IF
   70          CONTINUE
C
C              Exchange columns/rows ILO <-> I.
C
               SCALE(ILO) = DBLE( I )
               IF ( ILO.NE.I ) THEN
C
                  CALL DSWAP( N, A(1,ILO), 1, A(1,I), 1 )
                  CALL DSWAP( N-ILO+1, A(ILO,ILO), LDA, A(I,ILO), LDA )
C
                  IF ( I.LT.N )
     $               CALL DSWAP( N-I, QG(I+1,I), 1, QG(I+1,ILO), 1 )
                  IF ( I.GT.ILO+1 ) THEN
                     CALL DSCAL( I-ILO-1, -ONE, QG(ILO+1,ILO), 1 )
                     CALL DSWAP( I-ILO-1, QG(ILO+1,ILO), 1, QG(I,ILO+1),
     $                           LDQG )
                  END IF
C
                  CALL DSWAP( ILO-1, QG(1,I+1), 1, QG(1,ILO+1), 1 )
                  IF ( N.GT.I )
     $               CALL DSWAP( N-I, QG(I,I+2), LDQG, QG(ILO,I+2),
     $                           LDQG )
                  IF ( I.GT.ILO+1 ) THEN
                     CALL DSCAL( I-ILO-1, -ONE, QG(ILO+1,I+1), 1 )
                     CALL DSWAP( I-ILO-1, QG(ILO,ILO+2), LDQG,
     $                           QG(ILO+1,I+1), 1 )
                  END IF
                  CALL DSCAL( I-ILO, -ONE, QG(ILO,I+1), 1 )
               END IF
               ILO = ILO + 1
            END IF
C           END WHILE 30
C
C           Scan columns N+ILO .. 2*N.
C
            I = ILO
C           WHILE ( I.LE.N .AND. ILO.EQ.ILOOLD )
   80       IF ( I.LE.N .AND. ILO.EQ.ILOOLD ) THEN
               DO 90  J = ILO, I-1
                  IF ( A(I,J).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 80
                  END IF
   90          CONTINUE
               DO 100  J = I+1, N
                  IF ( A(I,J).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 80
                  END IF
  100          CONTINUE
               DO 110  J = ILO, I-1
                  IF ( QG(J,I+1).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 80
                  END IF
  110          CONTINUE
               DO 120  J = I+1, N
                  IF ( QG(I,J+1).NE.ZERO ) THEN
                     I = I + 1
                     GOTO 80
                  END IF
  120          CONTINUE
               SCALE(ILO) = DBLE( N+I )
C
C              Exchange columns/rows I <-> I+N with a symplectic
C              generalized permutation.
C
               CALL DSWAP( I-ILO, A(I,ILO), LDA, QG(I,ILO), LDQG )
               CALL DSCAL( I-ILO, -ONE, A(I,ILO), LDA )
               CALL DSWAP( N-I, A(I,I+1), LDA, QG(I+1,I), 1 )
               CALL DSCAL( N-I, -ONE, QG(I+1,I), 1 )
               CALL DSWAP( I-1, A(1,I), 1, QG(1,I+1), 1 )
               CALL DSCAL( I-1, -ONE, A(1,I), 1 )
               CALL DSCAL( N-I, -ONE, A(I+1,I), 1 )
               CALL DSWAP( N-I, A(I+1,I), 1, QG(I,I+2), LDQG )
C
C              Exchange columns/rows ILO <-> I.
C
               IF ( ILO.NE.I ) THEN
C
                  CALL DSWAP( N, A(1,ILO), 1, A(1,I), 1 )
                  CALL DSWAP( N-ILO+1, A(ILO,ILO), LDA, A(I,ILO), LDA )
C
                  IF ( I.LT.N )
     $               CALL DSWAP( N-I, QG(I+1,I), 1, QG(I+1,ILO), 1 )
                  IF ( I.GT.ILO+1 ) THEN
                     CALL DSCAL( I-ILO-1, -ONE, QG(ILO+1,ILO), 1 )
                     CALL DSWAP( I-ILO-1, QG(ILO+1,ILO), 1, QG(I,ILO+1),
     $                           LDQG )
                  END IF
C
                  CALL DSWAP( ILO-1, QG(1,I+1), 1, QG(1,ILO+1), 1 )
                  IF ( N.GT.I )
     $               CALL DSWAP( N-I, QG(I,I+2), LDQG, QG(ILO,I+2),
     $                           LDQG )
                  IF ( I.GT.ILO+1 ) THEN
                     CALL DSCAL( I-ILO-1, -ONE, QG(ILO+1,I+1), 1 )
                     CALL DSWAP( I-ILO-1, QG(ILO,ILO+2), LDQG,
     $                           QG(ILO+1,I+1), 1 )
                  END IF
                  CALL DSCAL( I-ILO, -ONE, QG(ILO,I+1), 1 )
               END IF
               ILO = ILO + 1
            END IF
C           END WHILE 80
            GOTO 20
         END IF
C        END WHILE 20
      END IF
C
      DO 130  I = ILO, N
         SCALE(I) = ONE
  130 CONTINUE
C
C     Scale to reduce the 1-norm of the remaining blocks.
C
      IF ( LSCAL ) THEN
         SCLFAC = DLAMCH( 'B' )
         SFMIN1 = DLAMCH( 'S' ) / DLAMCH( 'P' )
         SFMAX1 = ONE / SFMIN1
         SFMIN2 = SFMIN1*SCLFAC
         SFMAX2 = ONE / SFMIN2
C
C        Scale the rows and columns one at a time to minimize the
C        1-norm of the skew-Hamiltonian submatrix.
C        Stop when the 1-norm is very roughly minimal.
C
  140    CONTINUE
            CONV = .TRUE.
            DO 190  I = ILO, N
C
C              Compute 1-norm of row and column I without diagonal
C              elements.
C
               R = DASUM( I-ILO, A(I,ILO), LDA ) +
     $             DASUM( N-I, A(I,I+1), LDA ) +
     $             DASUM( I-ILO, QG(ILO,I+1), 1 ) +
     $             DASUM( N-I, QG(I,I+2), LDQG )
               C = DASUM( I-ILO, A(ILO,I), 1 ) +
     $             DASUM( N-I, A(I+1,I), 1 ) +
     $             DASUM( I-ILO, QG(I,ILO), LDQG ) +
     $             DASUM( N-I, QG(I+1,I), 1 )
C
C              Compute inf-norms of row and column I.
C
               IC = IDAMAX( N-ILO+1, A(I,ILO), LDA )
               MAXR = ABS( A(I,IC+ILO-1) )
               IF ( I.GT.1 ) THEN
                  IC = IDAMAX( I-1, QG(1,I+1), 1 )
                  MAXR = MAX( MAXR, ABS( QG(IC,I+1) ) )
               END IF
               IF ( N.GT.I ) THEN
                  IC = IDAMAX( N-I, QG(I,I+2), LDQG )
                  MAXR = MAX( MAXR, ABS( QG(I,IC+I+1) ) )
               END IF
               IC = IDAMAX( N, A(1,I), 1 )
               MAXC = ABS( A(IC,I) )
               IF ( I.GT.ILO ) THEN
                  IC = IDAMAX( I-ILO, QG(I,ILO), LDQG )
                  MAXC = MAX( MAXC, ABS( QG(I,IC+ILO-1) ) )
               END IF
               IF ( N.GT.I ) THEN
                  IC = IDAMAX( N-I, QG(I+1,I), 1 )
                  MAXC = MAX( MAXC, ABS( QG(IC+I,I) ) )
               END IF
C
               IF ( C.EQ.ZERO .OR. R.EQ.ZERO )
     $            GOTO 190
               G = R / SCLFAC
               F = ONE
               S = C + R
  150          CONTINUE
               IF ( C.GE.G .OR. MAX( F, C, MAXC ).GE.SFMAX2 .OR.
     $              MIN( R, G, MAXR ).LE.SFMIN2 )
     $            GOTO 160
               F = F*SCLFAC
               G = G / SCLFAC
               C = C*SCLFAC
               R = R / SCLFAC
               MAXC = MAXC*SCLFAC
               MAXR = MAXR / SCLFAC
               GOTO 150
C
  160          CONTINUE
               G = C / SCLFAC
  170          CONTINUE
               IF ( G.LT.R .OR. MAX( R, MAXR ).GE.SFMAX2 .OR.
     $              MIN( F, C, G, MAXC ).LE.SFMIN2 )
     $            GOTO 180
               F = F / SCLFAC
               G = G / SCLFAC
               C = C / SCLFAC
               R = R*SCLFAC
               MAXC = MAXC / SCLFAC
               MAXR = MAXR*SCLFAC
               GOTO 170
C
  180          CONTINUE
C
C              Now balance if necessary.
C
               IF ( ( C+R ).GE.FACTOR*S )
     $            GOTO 190
               IF ( F.LT.ONE .AND. SCALE(I).LT.ONE ) THEN
                  IF ( F*SCALE(I).LE.SFMIN1 )
     $               GOTO 190
               END IF
               IF ( F.GT.ONE .AND. SCALE(I).GT.ONE ) THEN
                  IF ( SCALE(I).GE.SFMAX1 / F )
     $               GOTO 190
               END IF
               CONV = .FALSE.
               SCALE(I) = SCALE(I)*F
               CALL DRSCL( I-ILO, F, A(I,ILO), LDA )
               CALL DRSCL( N-I, F, A(I,I+1), LDA )
               CALL DSCAL( I-1, F, A(1,I), 1 )
               CALL DSCAL( N-I, F, A(I+1,I), 1 )
               CALL DRSCL( I-1, F, QG(1,I+1), 1 )
               CALL DRSCL( N-I, F, QG(I,I+2), LDQG )
               CALL DSCAL( I-ILO, F, QG(I,ILO), LDQG )
               CALL DSCAL( N-I, F, QG(I+1,I), 1 )
  190       CONTINUE
         IF ( .NOT.CONV ) GOTO 140
      END IF
      RETURN
C *** Last line of MB04DS ***
      END


!=======================================================================
! Created by
!     Keurfon Luu <keurfon.luu@mines-paristech.fr>
!     MINES ParisTech - Centre de Géosciences
!     PSL - Research University
!=======================================================================

module dispcurve

  use omp_lib

  implicit none

  real(kind = 8), parameter :: pi = 3.141592653589793238460d0

contains

  function thomson_haskell(w, k, alpha, beta, rho, d, nl, wtype) result(panel)
    integer(kind = 4), intent(in) :: nl
    real(kind = 8), intent(in) :: w, k, alpha(nl), beta(nl), rho(nl), d(nl)
    character(len = *), optional :: wtype
    complex(kind = 8) :: panel
    integer(kind = 4) :: i
    real(kind = 8) :: c, X(5)
    real(kind = 8), dimension(nl) :: mu, gam, t
    complex(kind = 8) :: Tl(2,2), Tli(2,2,nl-1), U(2), V(2)
    complex(kind = 8), dimension(nl) :: Ca, Sa, Cb, Sb, r, s
    complex(kind = 8), dimension(nl-1) :: eps, eta, a, ap, b, bp
    complex(kind = 8) :: p1, p2, p3, p4
    complex(kind = 8) :: q1, q2, q3, q4
    complex(kind = 8) :: y1, y2, z1, z2

    ! Model layer parameters
    c = w / k                 ! Phase velocity
    mu = rho * beta**2        ! Layers' rigidity

    ! Layer eigenfunctions
    do i = 1, nl
      if ( trim(wtype) .eq. "rayleigh" ) then
        if ( c .lt. alpha(i) ) then
          r(i) = sqrt( 1.0d0 - c**2 / alpha(i)**2 )
          Ca(i) = cosh( k * r(i) * d(i) )
          Sa(i) = sinh( k * r(i) * d(i) )
        elseif ( c .gt. alpha(i) ) then
          r(i) = cmplx( 0.0d0, sqrt( c**2 / alpha(i)**2 - 1.0d0 ) )
          Ca(i) = cos( k * imag(r(i)) * d(i) )
          Sa(i) = cmplx( 0.0d0, sin( k * imag(r(i)) * d(i) ) )
        else
          r(i) = 0.0d0
          Ca(i) = 1.0d0
          Sa(i) = 0.0d0
        end if
      end if

      if ( c .lt. beta(i) ) then
        s(i) = sqrt( 1.0d0 - c**2 / beta(i)**2 )
        Cb(i) = cosh( k * s(i) * d(i) )
        Sb(i) = sinh( k * s(i) * d(i) )
      elseif ( c .gt. beta(i) ) then
        s(i) = cmplx( 0.0d0, sqrt( c**2 / beta(i)**2  - 1.0d0 ) )
        Cb(i) = cos( k * imag(s(i)) * d(i) )
        Sb(i) = cmplx( 0.0d0, sin( k * imag(s(i)) * d(i) ) )
      else
        s(i) = 0.0d0
        Cb(i) = 1.0d0
        Sb(i) = 0.0d0
      end if
    end do

    ! Thomson-Haskell propagator method
    select case(trim(wtype))

    ! Rayleigh-wave reduced fast Delta matrix
    case("rayleigh")
      gam = beta**2 / c**2
      t = 2.0d0 - c**2 / beta**2

      do i = 1, nl-1
        eps(i) = rho(i+1) / rho(i)
        eta(i) = 2.0d0 * ( gam(i) - eps(i) * gam(i+1) )
      end do

      a = eps + eta
      ap = a - 1.0d0
      b = 1.0d0 - eta
      bp = b - 1.0d0

      X = mu(1)*mu(1) * [ 2.0d0*t(1), -t(1)**2, 0.0d0, 0.0d0, -4.0d0 ]

      do i = 1, nl-1
        X = X
        p1 = Cb(i) * X(2) + s(i) * Sb(i) * X(3)
        p2 = Cb(i) * X(4) + s(i) * Sb(i) * X(5)
        if ( c .ne. beta(i) ) then
          p3 = 1.0d0/s(i) * Sb(i) * X(2) + Cb(i) * X(3)
          p4 = 1.0d0/s(i) * Sb(i) * X(4) + Cb(i) * X(5)
        else
          p3 = k * d(i) * X(2) + Cb(i) * X(3)
          p4 = k * d(i) * X(4) + Cb(i) * X(5)
        end if

        q1 = Ca(i) * p1 - r(i) * Sa(i) * p2
        q3 = Ca(i) * p3 - r(i) * Sa(i) * p4
        if ( c .ne. alpha(i) ) then
          q2 = -1.0d0/r(i) * Sa(i) * p3 + Ca(i) * p4
          q4 = -1.0d0/r(i) * Sa(i) * p1 + Ca(i) * p2
        else
          q2 = -k * d(i) * p3 + Ca(i) * p4
          q4 = -k * d(i) * p1 + Ca(i) * p2
        end if

        y1 = ap(i) * X(1) + a(i) * q1
        y2 = a(i) * X(1) + ap(i) * q2
        z1 = b(i) * X(1) + bp(i) * q1
        z2 = bp(i) * X(1) + b(i) * q2

        X(1) = bp(i) * y1 + b(i) * y2
        X(2) = a(i) * y1 + ap(i) * y2
        X(3) = eps(i) * q3
        X(4) = eps(i) * q4
        X(5) = bp(i) * z1 + b(i) * z2
      end do
      panel = X(2) + s(nl) * X(3) - r(nl) * ( X(4) + s(nl) * X(5) )

    ! Love-wave propagator matrix
    case("love")
      Tli(1,1,:) = Cb(:nl-1)
      Tli(1,2,:) = merge(Sb(:nl-1) / mu(:nl-1) / s(:nl-1), &
                         dcmplx(k) * dcmplx(d(:nl-1)) / mu(:nl-1), &
                         mask = c .ne. beta(:nl-1))
      Tli(2,1,:) = mu(:nl-1) * s(:nl-1) * Sb(:nl-1)
      Tli(2,2,:) = Cb(:nl-1)
      U = [ 0.d0, 1.d0 ]
      V(1) = 1.d0
      V(2) = mu(nl) * s(nl)

      Tl = Tli(:,:,1)
      do i = 2, nl-1
        Tl = Tl
        Tl = matmul(Tl, Tli(:,:,i))
      end do
      panel = dot_product(matmul(U, Tl), V)

    case default
      print *, "Error: unkown surface wave type '" // trim(wtype) // "'"
      stop
    end select
    return
  end function thomson_haskell

  function fcpanel(f, c, alpha, beta, rho, d, nf, nc, nl, wtype, n_threads) result(panel)
    integer(kind = 4), intent(in) :: nf, nc, nl
    real(kind = 8), intent(in) :: f(nf), c(nc), alpha(nl), beta(nl), rho(nl), d(nl)
    character(len = *), intent(in), optional :: wtype
    integer(kind = 4), intent(in), optional :: n_threads
    complex(kind = 8) :: panel(nc,nf)
    integer(kind = 4) :: i, j
    real(kind = 8) :: w
    character(len = 8) :: opt_wtype

    opt_wtype = "rayleigh"
    if ( present(wtype) ) opt_wtype = trim(wtype)
    if ( present(n_threads) ) call omp_set_num_threads(n_threads)

    !$omp parallel default(shared) private(w)
    !$omp do schedule(runtime)
    do j = 1, nf
      w = 2. * pi * f(j)
      do i = 1, nc
        panel(i,j) = thomson_haskell(w, w/c(i), alpha, beta, rho, d, nl, opt_wtype)
      end do
    end do
    !$omp end parallel
    return
  end function fcpanel

  function fkpanel(f, k, alpha, beta, rho, d, nf, nk, nl, wtype, n_threads) result(panel)
    integer(kind = 4), intent(in) :: nf, nk, nl
    real(kind = 8), intent(in) :: f(nf), k(nk), alpha(nl), beta(nl), rho(nl), d(nl)
    character(len = *), intent(in), optional :: wtype
    integer(kind = 4), intent(in), optional :: n_threads
    complex(kind = 8) :: panel(nk,nf)
    integer(kind = 4) :: i, j
    real(kind = 8) :: w
    character(len = 8) :: opt_wtype

    opt_wtype = "rayleigh"
    if ( present(wtype) ) opt_wtype = trim(wtype)
    if ( present(n_threads) ) call omp_set_num_threads(n_threads)

    !$omp parallel default(shared) private(w)
    !$omp do schedule(runtime)
    do j = 1, nf
      w = 2. * pi * f(j)
      do i = 1, nk
        panel(i,j) = thomson_haskell(w, k(i), alpha, beta, rho, d, nl, opt_wtype)
      end do
    end do
    !$omp end parallel
    return
  end function fkpanel

  function fkpanel_feasible(f, nk, alpha, beta, rho, d, nf, nl, wtype, n_threads) result(panel)
    integer(kind = 4), intent(in) :: nf, nk, nl
    real(kind = 8), intent(in) :: f(nf), alpha(nl), beta(nl), rho(nl), d(nl)
    character(len = *), intent(in), optional :: wtype
    integer(kind = 4), intent(in), optional :: n_threads
    complex(kind = 8) :: panel(nk,nf)
    integer(kind = 4) :: i, j, l
    real(kind = 8) :: Vmin, Vmax, kmin, kmax, dk, w, k(nk)
    character(len = 8) :: opt_wtype

    opt_wtype = "rayleigh"
    if ( present(wtype) ) opt_wtype = trim(wtype)
    if ( present(n_threads) ) call omp_set_num_threads(n_threads)

    Vmin = floor( minval( rayleigh_velocity(alpha, beta, nl) ) )
    Vmax = maxval(beta)

    !$omp parallel default(shared) private(w, kmin, kmax, dk, k)
    !$omp do schedule(runtime)
    do j = 1, nf
      w = 2. * pi * f(j)
      kmin = w / Vmax
      kmax = w / Vmin
      dk = ( kmax - kmin ) / ( nk - 1. )
      k = kmin + dk * [ ( l-1., l = 1, nk ) ]
      do i = 1, nk
        panel(i,j) = thomson_haskell(w, k(i), alpha, beta, rho, d, nl, opt_wtype)
      end do
    end do
    !$omp end parallel
    return
  end function fkpanel_feasible

  function rayleigh_velocity(alpha, beta, nl) result(v)
    integer(kind = 4), intent(in) :: nl
    real(kind = 8), intent(in) :: alpha(nl), beta(nl)
    real(kind = 8) :: v(nl), ksi(nl), nu(nl), Vr(nl)

    ksi = alpha**2 / beta**2
    nu = ( 1.0d0 - 0.5d0 * ksi ) / ( 1.0d0 - ksi )
    Vr = ( 0.87d0 + 1.12d0 * nu ) / ( 1.0d0 + nu )
    v = Vr * beta
    return
  end function rayleigh_velocity

  function pick(panel, faxis, yaxis, modes, ny, nf, nm, n_threads) result(dcurve)
    integer(kind = 4), intent(in) :: ny, nf, nm
    real(kind = 8), intent(in) :: panel(ny,nf), faxis(nf), yaxis(ny)
    integer(kind = 4), intent(in) :: modes(nm)
    integer(kind = 4), intent(in), optional :: n_threads
    integer(kind = 4) :: i, j, m
    real(kind = 8) :: dcurve(nm,2,nf)
    integer(kind = 4), dimension(:), allocatable :: idx, iy
    real(kind = 8), dimension(:), allocatable :: tmp

    if ( present(n_threads) ) call omp_set_num_threads(n_threads)

    ! Initialize variables
    dcurve = 0.d0                       ! Picked dispersion curves for each mode
    iy = [ ( i, i = 1, ny ) ]           ! Phase velocity axis (index)

    !$omp parallel default(shared) private(tmp, idx, m)
    !$omp do schedule(runtime)
    do i = 1, nf
      tmp = panel(:,i) / dabs( maxval(panel(:,i)) )
      idx = pack(iy, mask = tmp(:ny-1) * tmp(2:) .lt. 0.d0)
      do j = 1, nm
        m = modes(j) + 1
        if ( size(idx) .ge. m ) then
          dcurve(j,1,i) = ( yaxis(idx(m)) * tmp(idx(m)+1) - yaxis(idx(m)+1) * tmp(idx(m)) ) &
                          / ( tmp(idx(m)+1) - tmp(idx(m)) )
          dcurve(j,2,i) = faxis(i)
        end if
      end do
      deallocate(tmp, idx)
    end do
    !$omp end parallel
    return
  end function pick

end module dispcurve


************************************************************************
*
*     Subroutine DLSQRT                               Called by: DGQTST
*
*     compute rows 1 through N of the Cholesky factor L of
*     A = L*(L**T), where L and the lower triangle of A are both
*     stored compactly by rows (and may occupy the same storage).
*
*     IRC = 0 means all went well.  IRC = J means the leading
*     principal JxJ submatrix of A is not positive definite -- and
*     L(J*(J+1)/2) contains the (nonpos.) reduced Jth diagonal.
*
************************************************************************
      SUBROUTINE DLSQRT(N,L,A,IRC)
*
*     subroutine arguments
*
      INTEGER*4 N,IRC
      DOUBLE PRECISION L(*),A(*)
*
*     local vars
*
      INTEGER*4 I,J,K,I0,J0
      DOUBLE PRECISION T,TD
*
*
*
      I0 = 0
      DO 30 I=1,N
         TD = 0.D0
         J0 = 0
         DO 20 J=1,I-1
            T = 0.D0
            DO 10 K=1,J-1
               T = T + L(I0+K)*L(J0+K)
 10         CONTINUE
            J0 = J0 + J
            T = (A(I0+J) - T) / L(J0)
            L(I0+J) = T
            TD = TD + T*T
 20      CONTINUE
         I0 = I0 + I
         T = A(I0) - TD
         IF (T .LE. 0.D0) THEN
            L(I0) = T
            IRC = I
            RETURN
         ENDIF
         L(I0) = DSQRT(T)
 30   CONTINUE

      IRC = 0

      RETURN
      END


      SUBROUTINE MB02JD( JOB, K, L, M, N, P, S, TC, LDTC, TR, LDTR, Q,
     $                   LDQ, R, LDR, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute a lower triangular matrix R and a matrix Q with
C     Q^T Q = I such that
C                                    T
C                           T  =  Q R ,
C
C     where T is a K*M-by-L*N block Toeplitz matrix with blocks of size
C     (K,L). The first column of T will be denoted by TC and the first
C     row by TR. It is assumed that the first MIN(M*K, N*L) columns of T
C     have full rank.
C
C     By subsequent calls of this routine the factors Q and R can be
C     computed block column by block column.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             Specifies the output of the routine as follows:
C             = 'Q':  computes Q and R;
C             = 'R':  only computes R.
C
C     Input/Output Parameters
C
C     K       (input)  INTEGER
C             The number of rows in one block of T.  K >= 0.
C
C     L       (input)  INTEGER
C             The number of columns in one block of T.  L >= 0.
C
C     M       (input)  INTEGER
C             The number of blocks in one block column of T.  M >= 0.
C
C     N       (input)  INTEGER
C             The number of blocks in one block row of T.  N >= 0.
C
C     P       (input)  INTEGER
C             The number of previously computed block columns of R.
C             P*L < MIN( M*K,N*L ) + L and P >= 0.
C
C     S       (input)  INTEGER
C             The number of block columns of R to compute.
C             (P+S)*L < MIN( M*K,N*L ) + L and S >= 0.
C
C     TC      (input) DOUBLE PRECISION array, dimension (LDTC, L)
C             On entry, if P = 0, the leading M*K-by-L part of this
C             array must contain the first block column of T.
C
C     LDTC    INTEGER
C             The leading dimension of the array TC.
C             LDTC >= MAX(1,M*K).
C
C     TR      (input)  DOUBLE PRECISION array, dimension (LDTR,(N-1)*L)
C             On entry, if P = 0, the leading K-by-(N-1)*L part of this
C             array must contain the first block row of T without the
C             leading K-by-L block.
C
C     LDTR    INTEGER
C             The leading dimension of the array TR.
C             LDTR >= MAX(1,K).
C
C     Q       (input/output)  DOUBLE PRECISION array, dimension
C                             (LDQ,MIN( S*L, MIN( M*K,N*L )-P*L ))
C             On entry, if JOB = 'Q'  and  P > 0, the leading M*K-by-L
C             part of this array must contain the last block column of Q
C             from a previous call of this routine.
C             On exit, if JOB = 'Q'  and  INFO = 0, the leading
C             M*K-by-MIN( S*L, MIN( M*K,N*L )-P*L ) part of this array
C             contains the P-th to (P+S)-th block columns of the factor
C             Q.
C
C     LDQ     INTEGER
C             The leading dimension of the array Q.
C             LDQ >= MAX(1,M*K), if JOB = 'Q';
C             LDQ >= 1,          if JOB = 'R'.
C
C     R       (input/output)  DOUBLE PRECISION array, dimension
C                             (LDR,MIN( S*L, MIN( M*K,N*L )-P*L ))
C             On entry, if P > 0, the leading (N-P+1)*L-by-L
C             part of this array must contain the nozero part of the
C             last block column of R from a previous call of this
C             routine.
C             One exit, if INFO = 0, the leading
C             MIN( N, N-P+1 )*L-by-MIN( S*L, MIN( M*K,N*L )-P*L )
C             part of this array contains the nonzero parts of the P-th
C             to (P+S)-th block columns of the lower triangular
C             factor R.
C             Note that elements in the strictly upper triangular part
C             will not be referenced.
C
C     LDR     INTEGER
C             The leading dimension of the array R.
C             LDR >= MAX( 1, MIN( N, N-P+1 )*L )
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C             On exit, if INFO = -17,  DWORK(1) returns the minimum
C             value of LDWORK.
C             If JOB = 'Q', the first 1 + ( (N-1)*L + M*K )*( 2*K + L )
C             elements of DWORK should be preserved during successive
C             calls of the routine.
C             If JOB = 'R', the first 1 + (N-1)*L*( 2*K + L ) elements
C             of DWORK should be preserved during successive calls of
C             the routine.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             JOB = 'Q':
C                LDWORK >= 1 + ( M*K + ( N - 1 )*L )*( L + 2*K ) + 6*L
C                            + MAX( M*K,( N - MAX( 1,P )*L ) );
C             JOB = 'R':
C                If P = 0,
C                   LDWORK >= MAX( 1 + ( N - 1 )*L*( L + 2*K ) + 6*L
C                                    + (N-1)*L, M*K*( L + 1 ) + L );
C                If P > 0,
C                   LDWORK >= 1 + (N-1)*L*( L + 2*K ) + 6*L + (N-P)*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     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             = 1:  the full rank condition for the first MIN(M*K, N*L)
C                   columns of T is (numerically) violated.
C
C     METHOD
C
C     Block Householder transformations and modified hyperbolic
C     rotations are used in the Schur algorithm [1], [2].
C
C     REFERENCES
C
C     [1] Kailath, T. and Sayed, A.
C         Fast Reliable Algorithms for Matrices with Structure.
C         SIAM Publications, Philadelphia, 1999.
C
C     [2] Kressner, D. and Van Dooren, P.
C         Factorizations and linear system solvers for matrices with
C         Toeplitz structure.
C         SLICOT Working Note 2000-2, 2000.
C
C     NUMERICAL ASPECTS
C
C     The implemented method yields a factor R which has comparable
C     accuracy with the Cholesky factor of T^T * T. Q is implicitly
C     computed from the formula Q = T * inv(R^T R) * R, i.e., for ill
C     conditioned problems this factor is of very limited value.
C                                 2
C     The algorithm requires 0(K*L *M*N) floating point operations.
C
C     CONTRIBUTOR
C
C     D. Kressner, Technical Univ. Berlin, Germany, May 2001.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, June 2001.
C     D. Kressner, Technical Univ. Berlin, Germany, July 2002.
C     V. Sima, Research Institute for Informatics, Bucharest, Mar. 2004,
C     Aug. 2011.
C
C     KEYWORDS
C
C     Elementary matrix operations, Householder transformation, matrix
C     operations, Toeplitz matrix.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOB
      INTEGER           INFO, K, L, LDQ, LDR, LDTC, LDTR, LDWORK,
     $                  M, N, P, S
C     .. Array Arguments ..
      DOUBLE PRECISION  DWORK(LDWORK), Q(LDQ,*), R(LDR,*), TC(LDTC,*),
     $                  TR(LDTR,*)
C     .. Local Scalars ..
      INTEGER           COLR, I, IERR, KK, LEN, NB, NBMIN, PDQ, PDW,
     $                  PNQ, PNR, PRE, PT, RNK, SHFR, STPS, WRKMIN,
     $                  WRKOPT
      LOGICAL           COMPQ, LQUERY
C     .. Local Arrays ..
      INTEGER           IPVT(1)
C     .. External Functions ..
      LOGICAL           LSAME
      INTEGER           ILAENV
      EXTERNAL          ILAENV, LSAME
C     .. External Subroutines ..
      EXTERNAL          DGELQF, DGEQRF, DLACPY, DLASET, DORGQR, MA02AD,
     $                  MB02CU, MB02CV, MB02KD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, INT, MAX, MIN
C
C     .. Executable Statements ..
C
C     Decode the scalar input parameters.
C
      INFO  = 0
      COMPQ = LSAME( JOB, 'Q' )
      IF ( COMPQ ) THEN
         WRKMIN = 1 + ( M*K + ( N - 1 )*L )*( L + 2*K ) + 6*L
     $              + MAX( M*K, ( N - MAX( 1, P ) )*L )
      ELSE
         WRKMIN = 1 + ( N - 1 )*L*( L + 2*K ) + 6*L
     $              + ( N - MAX( P, 1 ) )*L
         IF ( P.EQ.0 ) THEN
            WRKMIN = MAX( WRKMIN, M*K*( L + 1 ) + L )
         END IF
      END IF
C
C     Check the scalar input parameters.
C
      IF ( .NOT.( COMPQ .OR. LSAME( JOB, 'R' ) ) ) THEN
         INFO = -1
      ELSE IF ( K.LT.0 ) THEN
         INFO = -2
      ELSE IF ( L.LT.0 ) THEN
         INFO = -3
      ELSE IF ( M.LT.0 ) THEN
         INFO = -4
      ELSE IF ( N.LT.0 ) THEN
         INFO = -5
      ELSE IF ( P*L.GE.MIN( M*K, N*L ) + L .OR. P.LT.0 ) THEN
         INFO = -6
      ELSE IF ( ( P + S )*L.GE.MIN( M*K, N*L ) + L .OR. S.LT.0  ) THEN
         INFO = -7
      ELSE IF ( LDTC.LT.MAX( 1, M*K ) ) THEN
         INFO = -9
      ELSE IF ( LDTR.LT.MAX( 1, K ) ) THEN
         INFO = -11
      ELSE IF ( LDQ.LT.1 .OR. ( COMPQ .AND. LDQ.LT.M*K ) ) THEN
         INFO = -13
      ELSE IF ( LDR.LT.MAX( 1, MIN( N, N - P + 1 )*L ) ) THEN
         INFO = -15
      ELSE
         LQUERY = LDWORK.EQ.-1
         IF ( LQUERY ) THEN
            WRKOPT = 1
            IF ( M*K.LE.L ) THEN
               PDW = M*K*L + M*K
               CALL DGEQRF( M*K, L, DWORK, M*K, DWORK, DWORK, -1, IERR )
               WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW )
               CALL DORGQR( M*K, M*K, M*K, DWORK, M*K, DWORK, DWORK, -1,
     $                      IERR )
               WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW )
               IF ( N.GT.1 ) THEN
                  PDW = M*K*M*K
                  CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, M*K,
     $                         ONE, ZERO, TC, LDTC, TR, LDTR, DWORK,
     $                         M*K, R, LDR, DWORK, -1, IERR )
                  WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW )
               END IF
C
            ELSE IF ( P.EQ.0 ) THEN
               IF ( COMPQ ) THEN
                  CALL DGEQRF( M*K, L, Q, LDQ, DWORK, DWORK, -1, IERR )
                  WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + L )
                  CALL DORGQR( M*K, L, L, Q, LDQ, DWORK, DWORK, -1,
     $                        IERR )
                  WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + L )
                  IF ( N.GT.1 ) THEN
                     CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, L,
     $                            ONE, ZERO, TC, LDTC, TR, LDTR, Q, LDQ,
     $                            R, LDR, DWORK, LDWORK, IERR )
                     WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
                  END IF
               ELSE
                  PDW = M*K*L
                  CALL DGEQRF( M*K, L, DWORK, M*K, DWORK, DWORK, -1,
     $                        IERR )
                  WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW + L )
                  CALL DORGQR( M*K, L, L, DWORK, M*K, DWORK, DWORK, -1,
     $                         IERR )
                  WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW + L )
                  IF ( N.GT.1 ) THEN
                     CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, L,
     $                            ONE, ZERO, TC, LDTC, TR, LDTR, DWORK,
     $                            M*K, R, LDR, DWORK, -1, IERR )
                     WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + PDW )
                  END IF
               END IF
               PRE = 1
            END IF
C
            IF ( M*K.GT.L .OR. P.GT.0 .OR.
     $         ( P.EQ.0 .AND. N.GT.1 ) ) THEN
               PRE = MAX( 1, P )
               IF ( COMPQ ) THEN
                  PDW = ( 2*K + L )*( ( N - 1 )*L + M*K ) + 1
                  LEN = MAX( L + M*K, ( N - PRE + 1 )*L )
               ELSE
                  PDW = ( 2*K + L )*( N - 1 )*L + 1
                  LEN = ( N - PRE + 1 )*L
               END IF
C
               CALL DGELQF( LEN, L, DWORK, MAX( 1, LEN ), DWORK, DWORK,
     $                      -1, IERR )
               WRKOPT = MAX( WRKOPT, PDW + 6*L + INT( DWORK(1) ) )
            END IF
         ELSE IF ( LDWORK.LT.WRKMIN ) THEN
            DWORK(1) = DBLE( WRKMIN )
            INFO = -17
         END IF
      END IF
C
C     Return if there were illegal values.
C
      IF ( INFO .NE. 0 ) THEN
         CALL XERBLA( 'MB02JD', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( MIN( M, N, K*L, S ) .EQ.0 ) THEN
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Catch M*K <= L.
C
      WRKOPT = 1
      IF ( M*K.LE.L ) THEN
         CALL DLACPY( 'All', M*K, L, TC, LDTC, DWORK, M*K )
         PDW = M*K*L + 1
         CALL DGEQRF( M*K, L, DWORK, M*K, DWORK(PDW),
     $                DWORK(PDW+M*K), LDWORK-PDW-M*K+1, IERR )
         WRKOPT = MAX( WRKOPT, INT( DWORK(PDW+M*K) ) + PDW + M*K - 1 )
         CALL MA02AD( 'Upper part', M*K, L, DWORK, M*K, R, LDR )
         CALL DORGQR( M*K, M*K, M*K, DWORK, M*K, DWORK(PDW),
     $                DWORK(PDW+M*K), LDWORK-PDW-M*K+1, IERR )
         WRKOPT = MAX( WRKOPT, INT( DWORK(PDW+M*K) ) + PDW + M*K - 1 )
         IF ( COMPQ ) THEN
            CALL DLACPY( 'All', M*K, M*K, DWORK, M*K, Q, LDQ )
         END IF
         PDW = M*K*M*K + 1
         IF ( N.GT.1 ) THEN
            CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, M*K, ONE,
     $                   ZERO, TC, LDTC, TR, LDTR, DWORK, M*K, R(L+1,1),
     $                   LDR, DWORK(PDW), LDWORK-PDW+1, IERR )
            WRKOPT = MAX( WRKOPT, INT( DWORK(PDW) ) + PDW - 1 )
         END IF
         DWORK(1) = DBLE( WRKOPT )
         RETURN
      END IF
C
C     Compute the generator if P = 0.
C
      IF ( P.EQ.0 ) THEN
C
C        1st column of the generator.
C
         IF ( COMPQ ) THEN
            CALL DLACPY( 'All', M*K, L, TC, LDTC, Q, LDQ )
            CALL DGEQRF( M*K, L, Q, LDQ, DWORK, DWORK(L+1),
     $                   LDWORK-L, IERR )
            WRKOPT = MAX( WRKOPT, INT( DWORK(L+1) ) + L )
            CALL MA02AD( 'Upper part', L, L, Q, LDQ, R, LDR )
            CALL DORGQR( M*K, L, L, Q, LDQ, DWORK, DWORK(L+1), LDWORK-L,
     $                  IERR )
            WRKOPT = MAX( WRKOPT, INT( DWORK(L+1) ) + L )
            IF ( N.GT.1 ) THEN
               CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, L, ONE,
     $                      ZERO, TC, LDTC, TR, LDTR, Q, LDQ, R(L+1,1),
     $                      LDR, DWORK, LDWORK, IERR )
               WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
            END IF
         ELSE
            PDW = M*K*L + 1
            CALL DLACPY( 'All', M*K, L, TC, LDTC, DWORK, M*K )
            CALL DGEQRF( M*K, L, DWORK, M*K, DWORK(PDW), DWORK(PDW+L),
     $                   LDWORK-PDW-L+1, IERR )
            WRKOPT = MAX( WRKOPT, INT( DWORK(PDW+L) ) + PDW + L - 1 )
            CALL MA02AD( 'Upper part', L, L, DWORK, M*K, R, LDR )
            CALL DORGQR( M*K, L, L, DWORK, M*K, DWORK(PDW),
     $                   DWORK(PDW+L), LDWORK-PDW-L+1, IERR )
            WRKOPT = MAX( WRKOPT, INT( DWORK(PDW+L) ) + PDW + L - 1 )
            IF ( N.GT.1 ) THEN
               CALL MB02KD( 'Row', 'Transpose', K, L, M, N-1, L, ONE,
     $                      ZERO, TC, LDTC, TR, LDTR, DWORK, M*K,
     $                      R(L+1,1), LDR, DWORK(PDW), LDWORK-PDW+1,
     $                      IERR )
               WRKOPT = MAX( WRKOPT, INT( DWORK(PDW) ) + PDW - 1 )
            END IF
         END IF
C
C        Quick return if N = 1.
C
         IF ( N.EQ.1 ) THEN
            DWORK(1) = DBLE( WRKOPT )
            RETURN
         END IF
C
C        2nd column of the generator.
C
         PNR = ( N - 1 )*L*K + 2
         CALL MA02AD( 'All', K, (N-1)*L, TR, LDTR, DWORK(2), (N-1)*L )
C
C        3rd and 4th column of the generator.
C
         CALL DLACPY( 'All', (N-1)*L, L, R(L+1,1), LDR, DWORK(PNR),
     $                (N-1)*L )
         PT  = ( M - 1 )*K + 1
         PDW = PNR + ( N - 1 )*L*L
C
         DO 10  I = 1, MIN( M, N-1 )
            CALL MA02AD( 'All', K, L, TC(PT,1), LDTC, DWORK(PDW),
     $                   (N-1)*L )
            PT  = PT  - K
            PDW = PDW + L
   10    CONTINUE
C
         PT = 1
C
         DO 20  I = M + 1, N - 1
            CALL MA02AD( 'All', K, L, TR(1,PT), LDTR, DWORK(PDW),
     $                   (N-1)*L )
            PT  = PT  + L
            PDW = PDW + L
   20    CONTINUE
C
         IF ( COMPQ ) THEN
            PDQ = ( 2*K + L )*( N - 1 )*L + 2
            PDW = ( 2*K + L )*( ( N - 1 )*L + M*K ) + 2
            PNQ = PDQ + M*K*K
            CALL DLASET( 'All', K, K, ZERO, ONE, DWORK(PDQ), M*K )
            CALL DLASET( 'All', (M-1)*K, K, ZERO, ZERO, DWORK(PDQ+K),
     $                   M*K )
            CALL DLACPY( 'All', M*K, L, Q, LDQ, DWORK(PNQ), M*K )
            CALL DLASET( 'All', M*K, K, ZERO, ZERO, DWORK(PNQ+M*L*K),
     $                   M*K )
         ELSE
            PDW = ( 2*K + L )*( N - 1 )*L + 2
         END IF
         PRE  = 1
         STPS = S - 1
      ELSE
C
C        Set workspace pointers.
C
         PNR = ( N - 1 )*L*K + 2
         IF ( COMPQ ) THEN
            PDQ = ( 2*K + L )*( N - 1 )*L + 2
            PDW = ( 2*K + L )*( ( N - 1 )*L + M*K ) + 2
            PNQ = PDQ + M*K*K
         ELSE
            PDW = ( 2*K + L )*( N - 1 )*L + 2
         END IF
         PRE  = P
         STPS = S
      END IF
C
C     Determine suitable size for the block Housholder reflectors.
C
      IF ( COMPQ ) THEN
         LEN = MAX( L + M*K, ( N - PRE + 1 )*L )
      ELSE
         LEN = ( N - PRE + 1 )*L
      END IF
      NB    = MIN( ( LDWORK - PDW - 6*L + 1 ) / LEN, L )
      NBMIN = MAX( 2, ILAENV( 2, 'DGELQF', ' ', LEN, L, -1, -1 ) )
      IF ( NB.LT.NBMIN )  NB = 0
      COLR = L + 1
C
C     Generator reduction process.
C
      LEN  = ( N - PRE )*L
      SHFR = ( PRE - 1 )*L
      DO 30  I = PRE, PRE + STPS - 1
C
C        IF M*K < N*L the last block might have less than L columns.
C
         KK = MIN( L, M*K - I*L )
         CALL DLACPY( 'Lower', LEN, KK, R(COLR-L,COLR-L), LDR,
     $                R(COLR,COLR), LDR )
         CALL MB02CU( 'Column', KK, KK+K, L+K, NB, R(COLR,COLR), LDR,
     $                 DWORK(SHFR+2), (N-1)*L, DWORK(PNR+SHFR), (N-1)*L,
     $                 RNK, IPVT, DWORK(PDW), ZERO, DWORK(PDW+6*L),
     $                 LDWORK-PDW-6*L+1, IERR )
         IF ( IERR.NE.0 )  THEN
C
C           Error return:  The rank condition is (numerically) not
C                          satisfied.
C
            INFO = 1
            RETURN
         END IF
         IF ( LEN.GT.KK ) THEN
            CALL MB02CV( 'Column', 'NoStructure', KK, LEN-KK, KK+K, L+K,
     $                   NB, -1, R(COLR,COLR), LDR, DWORK(SHFR+2),
     $                   (N-1)*L, DWORK(PNR+SHFR), (N-1)*L,
     $                   R(COLR+KK,COLR), LDR, DWORK(SHFR+KK+2),
     $                   (N-1)*L, DWORK(PNR+SHFR+KK), (N-1)*L,
     $                   DWORK(PDW), DWORK(PDW+6*L), LDWORK-PDW-6*L+1,
     $                   IERR )
         END IF
         IF ( COMPQ ) THEN
            CALL DLASET( 'All', K, KK, ZERO, ZERO, Q(1,COLR), LDQ )
            IF ( M.GT.1 ) THEN
               CALL DLACPY( 'All', (M-1)*K, KK, Q(1,COLR-L), LDQ,
     $                      Q(K+1,COLR), LDQ )
            END IF
            CALL MB02CV( 'Column', 'NoStructure', KK, M*K, KK+K, L+K,
     $                   NB, -1, R(COLR,COLR), LDR, DWORK(SHFR+2),
     $                   (N-1)*L, DWORK(PNR+SHFR), (N-1)*L, Q(1,COLR),
     $                   LDQ, DWORK(PDQ), M*K, DWORK(PNQ), M*K,
     $                   DWORK(PDW), DWORK(PDW+6*L), LDWORK-PDW-6*L+1,
     $                   IERR )
         END IF
         LEN  = LEN  - L
         COLR = COLR + L
         SHFR = SHFR + L
   30 CONTINUE
C
      DWORK(1) = DBLE( WRKOPT )
      RETURN
C
C *** Last line of MB02JD ***
      END


program main
	USE Plotter
	implicit none
	integer :: i
	real :: x,y,phi,r,PI=DACOS(-1.D0)
	real,dimension(:,:,:),allocatable :: matrix
	logical,dimension(:,:),allocatable :: perf
	phi = 0
	x = 0
	y = 0
	r = 0
	allocate(matrix(3,8400,2))
	allocate(perf(8400,2))
	do i=1,8400
		phi = phi+PI/4096.
		r=(1+sin(phi))*(1-0.9*abs(sin(4*phi)))*(0.9+0.05*cos(phi))
		matrix(1,i,1) = cos(phi)*r*5
		matrix(2,i,1) = sin(phi)*r*5
		matrix(3,i,1) = 0.
		matrix(1,i,2) = cos(phi)*r*5
		matrix(2,i,2) = -sin(phi)*r*5
		matrix(3,i,2) = 0.
		perf(i,1) = .FALSE.
		perf(i,2) = .FALSE.
		if(i>1000 .AND. i<2000) then
			perf(i,1) = .TRUE.
		end if
		if(i>4000 .AND. i<5000) then
			perf(i,2) = .TRUE.
		end if
	end do
	!Передаём размеры окна, массив точек и их количество
	call DrawGraph(960,720,matrix,8400,2,perf)
end