!
!     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!     *                                                               *
!     *                  copyright (c) 1998 by UCAR                   *
!     *                                                               *
!     *       University Corporation for Atmospheric Research         *
!     *                                                               *
!     *                      all rights reserved                      *
!     *                                                               *
!     *                          Spherepack                           *
!     *                                                               *
!     *       A Package of Fortran Subroutines and Programs           *
!     *                                                               *
!     *              for Modeling Geophysical Processes               *
!     *                                                               *
!     *                             by                                *
!     *                                                               *
!     *                  John Adams and Paul Swarztrauber             *
!     *                                                               *
!     *                             of                                *
!     *                                                               *
!     *         the National Center for Atmospheric Research          *
!     *                                                               *
!     *                Boulder, Colorado  (80307)  U.S.A.             *
!     *                                                               *
!     *                   which is sponsored by                       *
!     *                                                               *
!     *              the National Science Foundation                  *
!     *                                                               *
!     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!
submodule(grid_transfer_routines) grid_transfer_vector_shift

contains

    !     subroutine vshifte(ioff, nlon, nlat, uoff, voff, ureg, vreg, wsave, ierror)
    !
    ! *** purpose
    !
    !     subroutine vshifte does a highly accurate 1/2 grid increment shift
    !     in both longitude and latitude of equally spaced vector data on the
    !     sphere. data is transferred between the nlon by nlat "offset grid"
    !     in (uoff, voff) (which excludes poles) and the nlon by nlat+1 "regular
    !     grid" in (ureg, vreg) (which includes poles).  the transfer can go from
    !     (uoff, voff) to (ureg, vreg) or vice versa (see ioff).  the grids which
    !     underly the vector fields are described below.  the north and south
    !     pole are at HALF*pi and-HALF*pi radians respectively.
    !     uoff and ureg are the east longitudinal vector data components.  voff
    !     and vreg are the latitudinal vector data components.
    !
    !     subroutine sshifte can be used to shift scalar data on the sphere.
    !     notice that scalar and vector quantities are fundamentally different
    !     on the sphere.  for example, vectors are discontinuous and multiple
    !     valued at the poles.  scalars are continuous and single valued at the
    !     poles. erroneous results would be produced if one attempted to shift
    !     vector fields with subroutine sshifte applied to each component of
    !     of the vector.
    !
    ! *** grid descriptions
    !
    !     let dlon = TWO_PI/nlon and dlat = pi/nlat be the uniform grid
    !     increments in longitude and latitude
    !
    !     offset grid
    !
    !     the "1/2 increment offset" grid (long(j), lat(i)) on which uoff(j, i)
    !     and voff(j, i) are given (ioff=0) or generated (ioff=1) is
    !
    !          long(j) =HALF*dlon + (j-1)*dlon  (j=1, ..., nlon)
    !
    !     and
    !
    !          lat(i) = -HALF*pi + HALF*dlat + (i-1)*dlat (i=1, ..., nlat)
    !
    !     the data in (uoff, voff) is "shifted" one half a grid increment in both
    !     longitude and latitude and excludes the poles.  each uoff(j, 1), voff(j, 1)
    !     is given at latitude -pi/2+dlat/2.  uoff(j, nlat), voff(j, nlat) is
    !     given at pi/2-dlat/2 (1/2 a grid increment away from the poles).
    !     uoff(1, i), voff(1, i) is given at longitude dlon/2.  each uoff(nlon, i),
    !     voff(nlon, i) is given at longitude 2*pi-dlon/2.
    !
    !     regular grid
    !
    !     let dlat, dlon be as above.  then the nlon by nlat+1 grid on which
    !     ureg(j, i), vreg(j, i) are generated (ioff=0) or given (ioff=1) is
    !
    !          lone(j) = (j-1)*dlon (j=1, ..., nlon)
    !
    !      and
    !
    !          late(i) = -HALF*pi + (i-1)*dlat (i=1, ..., nlat+1)
    !
    !     values in ureg, vreg include the poles and start at zero degrees
    !     longitude and at the south pole this is the "usual" equally spaced
    !     grid in geophysical coordinates.
    !
    ! *** remark
    !
    !     subroutine vshifte can be used in conjunction with subroutine trvsph
    !     when transferring vector data from an equally spaced "1/2 increment
    !     offset" grid to a gaussian or equally spaced grid (which includes poles)
    !     of any resolution.  this problem (personal communication with dennis
    !     shea) is encountered in geophysical modeling and data analysis.
    !
    ! *** method
    !
    !     fast fourier transform software from spherepack2 and trigonometric
    !     identities are used to accurately "shift" periodic vectors half a
    !     grid increment in latitude and longitude.  latitudinal shifts are
    !     accomplished by setting periodic 2*nlat vectors over the pole for each
    !     longitude.  vector values must be negated on one side of the pole
    !     to maintain periodicity prior to the 2*nlat shift over the poles.
    !     when nlon is odd, the 2*nlat latitudinal shift requires an additional
    !     longitude shift to obtain symmetry necessary for full circle shifts
    !     over the poles.  finally longitudinal shifts are executed for each
    !     shifted latitude.
    !
    ! *** argument description
    !
    ! ... ioff
    !
    !     ioff = 0 if values on the offset grid in (uoff, voff) are given and
    !              values on the regular grid in (ureg, vreg) are to be generated.
    !
    !     ioff = 1 if values on the regular grid in (ureg, vreg) are given and
    !              values on the offset grid in (uoff, voff) are to be generated.
    !
    ! ... nlon
    !
    !     the number of longitude points on both the "offset" and "regular"
    !     uniform grid in longitude (see "grid description" above).  nlon
    !     is also the first dimension of uoff, voff, ureg, vreg.  nlon determines
    !     the grid increment in longitude as dlon = 2.*pi/nlon.  for example,
    !     nlon = 144 for a 2.5 degree grid.  nlon can be even or odd and must
    !     be greater than or equal to 4.  the efficiency of the computation
    !     is improved when nlon is a product of small primes.
    !
    ! ... nlat
    !
    !     the number of latitude points on the "offset" uniform grid.  nlat+1
    !     is the number of latitude points on the "regular" uniform grid (see
    !     "grid description" above).  nlat is the second dimension of uoff, voff.
    !     nlat+1 must be the second dimension of ureg, vreg in the program
    !     calling vshifte.  nlat determines the grid in latitude as pi/nlat.
    !     for example, nlat = 36 for a five degree grid.  nlat must be at least 3.
    !
    ! ... uoff
    !
    !     a nlon by nlat array that contains the east longitudinal vector
    !     data component on the offset grid described above.  uoff is a
    !     given input argument if ioff=0.  uoff is a generated output
    !     argument if ioff=1.
    !
    ! ... voff
    !
    !     a nlon by nlat array that contains the latitudinal vector data
    !     component on the offset grid described above.  voff is a given
    !     input argument if ioff=0.  voff is a generated output argument
    !     if ioff=1.
    !
    ! ... ureg
    !
    !     a nlon by nlat+1 array that contains the east longitudinal vector
    !     data component on the regular grid described above.  ureg is a given
    !     input argument if ioff=1.  ureg is a generated output argument
    !     if ioff=0.
    !
    ! ... vreg
    !
    !     a nlon by nlat+1 array that contains the latitudinal vector data
    !     component on the regular grid described above.  vreg is a given
    !     input argument if ioff=1.  vreg is a generated output argument
    !     if ioff=0.
    !
    ! ... wsav
    !
    !     a real saved workspace array that must be initialized by calling
    !     subroutine vshifti(ioff, nlon, nlat, wsav, ier) before calling vshifte.
    !     wsav can then be used repeatedly by vshifte as long as ioff, nlon,
    !     and nlat do not change.  this bypasses redundant computations and
    !     saves time.  undetectable errors will result if vshifte is called
    !     without initializing wsav whenever ioff, nlon, or nlat change.
    !
    ! ... lsav
    !
    !     the length of the saved workspace wsav in the routine calling vshifte
    !     and sshifti.  lsave must be greater than or equal to 2*(2*nlat+nlon+16).
    !
    ! ... ier
    !
    !     indicates errors in input parameters
    !
    !     = 0 if no errors are detected
    !
    !     = 1 if ioff is not equal to 0 or 1
    !
    !     = 2 if nlon < 4
    !
    !     = 3 if nlat < 3
    !
    !     = 4 if lsave < 2*(2*nlat+nlon+16)
    !
    module subroutine vshifte(ioff, nlon, nlat, uoff, voff, ureg, vreg, wsav, ier)

        ! Dummy arguments
        integer(ip), intent(in)    :: ioff
        integer(ip), intent(in)    :: nlon
        integer(ip), intent(in)    :: nlat
        real(wp),    intent(inout) :: uoff(:,:), voff(:,:)
        real(wp),    intent(inout) :: ureg(:,:), vreg(:,:)
        real(wp),    intent(in)    :: wsav(:)
        integer(ip), intent(out)   :: ier

        ! Local variables
        integer(ip) :: n2, nr, two_nlat, nlatp1
        integer(ip) :: iw1, iw2, iw3, lwork

        ! Check calling arguments
        call check_calling_arguments(ioff, nlon, nlat, wsav, ier)

        ! Check error flag
        if (ier /= 0) return

        two_nlat = 2 * nlat
        nlatp1 = nlat+1
        n2 = (nlon + 1)/2

        if (2*n2 == nlon) then
            lwork = 2*nlon*(nlat + 1)
            nr = n2
            iw1 = 1
            iw2 = 1
            iw3 = iw2+nlon*nlatp1
        else
            lwork = nlon*(5*nlat+1)
            nr = nlon
            iw1 = 1
            iw2 = iw1+two_nlat*nlon
            iw3 = iw2+nlatp1*nlon
        end if

        block
            real(wp) :: work(lwork)

            if (ioff == 0) then
                ! Shift (uoff, voff) to (ureg, vreg)
                call vhftoff(nlon, nlat, uoff, ureg, wsav, nr, two_nlat, &
                    nlatp1, work(iw1:), work(iw2:), work(iw2:), work(iw3:))
                call vhftoff(nlon, nlat, voff, vreg, wsav, nr, two_nlat, &
                    nlatp1, work(iw1:), work(iw2:), work(iw2:), work(iw3:))
            else
                ! Shift (ureg, vreg) to (uoff, voff)
                call vhftreg(nlon, nlat, uoff, ureg, wsav, nr, two_nlat, &
                    nlatp1, work(iw1:), work(iw2:), work(iw2:), work(iw3:))
                call vhftreg(nlon, nlat, voff, vreg, wsav, nr, two_nlat, &
                    nlatp1, work(iw1:), work(iw2:), work(iw2:), work(iw3:))
            end if
        end block

    end subroutine vshifte

    ! Purpose:
    !
    !     Initialize wsav for vshifte
    !
    !     subroutine vshifti(ioff, nlon, nlat, lsav, wsav, ier)
    !
    !     subroutine vshifti initializes the saved workspace wsav
    !     for ioff and nlon and nlat (see documentation for vshifte).
    !     vshifti must be called before vshifte whenever ioff or nlon
    !     or nlat change.
    !
    ! ... ier
    !
    !     = 0 if no errors with input arguments
    !
    !     = 1 if ioff is not 0 or 1
    !
    !     = 2 if nlon < 4
    !
    !     = 3 if nlat < 3
    !
    !     = 4 if lsav < 2*(2*nlat+nlon+16)
    !
    module subroutine vshifti(ioff, nlon, nlat, wsav, ier)

        ! Dummy arguments
        integer(ip), intent(in)  :: ioff
        integer(ip), intent(in)  :: nlon
        integer(ip), intent(in)  :: nlat
        real(wp),    intent(out) :: wsav(:)
        integer(ip), intent(out) :: ier

        ! Local variables
        integer(ip) :: two_nlat, iw
        real(wp)    :: dlat, dlon, dp

        ! Check calling arguments
        call check_calling_arguments(ioff, nlon, nlat, wsav, ier)

        ! Check error flag
        if (ier /= 0) return

        ! Set lat, lon increments
        dlat = PI/nlat
        dlon = TWO_PI/nlon

        ! Set left or right latitude shifts
        if (ioff == 0) then
            dp = -HALF * dlat
        else
            dp = HALF * dlat
        end if

        two_nlat = 2 * nlat

        call vhifthi(two_nlat, dp, wsav)

        ! Set left or right longitude shifts
        if (ioff == 0) then
            dp = -HALF * dlon
        else
            dp = HALF * dlon
        end if

        iw = (4 * nlat) + 17

        call vhifthi(nlon, dp, wsav(iw:))

    end subroutine vshifti

    subroutine vhftoff(nlon, nlat, uoff, ureg, wsav, nr, &
        nlat2, nlatp1, rlatu, rlonu, rlou, wrk)
        !
        !     generate ureg from uoff (a vector component!)
        !

        integer(ip) :: nlon, nlat, nlat2, nlatp1, n2, nr, j, i, js, isav
        real(wp) :: uoff(nlon, nlat), ureg(nlon, nlatp1)
        real(wp) :: rlatu(nr, nlat2), rlonu(nlatp1, nlon), rlou(nlat, nlon)
        real(wp) :: wsav(:), wrk(:)
        isav = 4*nlat+17
        n2 = (nlon + 1)/2
        !
        !     execute full circle latitude shifts for nlon odd or even
        !
        if (2*n2 > nlon) then
                 !
                 !     odd number of longitudes
                 !
            do i=1, nlat
                do j=1, nlon
                    rlou(i, j) = uoff(j, i)
                end do
            end do
            !
            !       half shift in longitude
            !
            call vhifth(nlat, nlon, rlou, wsav(isav:), wrk)
                !
                !       set full 2*nlat circles in rlatu using shifted values in rlonu
                !
            do j=1, n2-1
                js = j+n2
                do i=1, nlat
                    rlatu(j, i)      = uoff(j, i)
                    rlatu(j, nlat+i) = -rlou(nlat+1-i, js)
                end do
            end do
            do j=n2, nlon
                js = j-n2+1
                do i=1, nlat
                    rlatu(j, i)      = uoff(j, i)
                    rlatu(j, nlat+i) = -rlou(nlat+1-i, js)
                end do
            end do
            !
            !       shift the nlon rlat vectors one half latitude grid
            !
            call vhifth(nlon, nlat2, rlatu, wsav, wrk)
                    !
                    !       set in ureg
                    !
            do j=1, nlon
                do i=1, nlat+1
                    ureg(j, i) =  rlatu(j, i)
                end do
            end do
        else
                !
                !     even number of longitudes (no initial longitude shift necessary)
                !     set full 2*nlat circles (over poles) for each longitude pair (j, js)
                !     negating js vector side for periodicity
                !
            do j=1, n2
                js = n2+j
                do i=1, nlat
                    rlatu(j, i)      = uoff(j, i)
                    rlatu(j, nlat+i) =-uoff(js, nlatp1-i)
                end do
            end do
            !
            !       shift the n2=(nlon + 1)/2 rlat vectors one half latitude grid
            !
            call vhifth(n2, nlat2, rlatu, wsav, wrk)
            !
            !       set ureg, vreg shifted in latitude
            !
            do j=1, n2
                js = n2+j
                ureg(j, 1) =   rlatu(j, 1)
                ureg(js, 1) = -rlatu(j, 1)
                do i=2, nlatp1
                    ureg(j, i) =  rlatu(j, i)
                    ureg(js, i) =-rlatu(j, nlat2-i+2)
                end do
            end do
        end if
        !
        !     execute full circle longitude shift
        !
        do j=1, nlon
            do i=1, nlatp1
                rlonu(i, j) = ureg(j, i)
            end do
        end do
        call vhifth(nlatp1, nlon, rlonu, wsav(isav:), wrk)
        do j=1, nlon
            do i=1, nlatp1
                ureg(j, i) = rlonu(i, j)
            end do
        end do
    end subroutine vhftoff

    subroutine vhftreg(nlon, nlat, uoff, ureg, wsav, nr, nlat2, &
        nlatp1, rlatu, rlonu, rlou, wrk)
        !
        !     generate uoff vector component from ureg
        !

        integer(ip) :: nlon, nlat, nlat2, nlatp1, n2, nr, j, i, js, isav
        real(wp) :: uoff(nlon, nlat), ureg(nlon, nlatp1)
        real(wp) :: rlatu(nr, nlat2), rlonu(nlatp1, nlon), rlou(nlat, nlon)
        real(wp) :: wsav(:), wrk(:)
        isav = 4*nlat+17
        n2 = (nlon + 1)/2
        !
        !     execute full circle latitude shifts for nlon odd or even
        !
        if (2*n2 > nlon) then
            !
            !     odd number of longitudes
            !
            do i=1, nlatp1
                do j=1, nlon
                    rlonu(i, j) = ureg(j, i)
                end do
            end do
            !
            !       half shift in longitude in rlon
            !
            call vhifth(nlatp1, nlon, rlonu, wsav(isav:), wrk)
                !
                !       set full 2*nlat circles in rlat using shifted values in rlon
                !
            do j=1, n2
                js = j+n2-1
                rlatu(j, 1) = ureg(j, 1)
                do i=2, nlat
                    rlatu(j, i) = ureg(j, i)
                    rlatu(j, nlat+i) =-rlonu(nlat+2-i, js)
                end do
                rlatu(j, nlat+1) = ureg(j, nlat+1)
            end do
            do j=n2+1, nlon
                js = j-n2
                rlatu(j, 1) = ureg(j, 1)
                do i=2, nlat
                    rlatu(j, i) = ureg(j, i)
                    rlatu(j, nlat+i) =-rlonu(nlat+2-i, js)
                end do
                rlatu(j, nlat+1) = ureg(j, nlat+1)
            end do
            !
            !       shift the nlon rlat vectors one halflatitude grid
            !
            call vhifth(nlon, nlat2, rlatu, wsav, wrk)
            !
            !       set values in uoff
            !
            do j=1, nlon
                do i=1, nlat
                    uoff(j, i) = rlatu(j, i)
                end do
            end do
        else
                !
                !     even number of longitudes (no initial longitude shift necessary)
                !     set full 2*nlat circles (over poles) for each longitude pair (j, js)
                !
            do j=1, n2
                js = n2+j
                rlatu(j, 1) = ureg(j, 1)
                do i=2, nlat
                    rlatu(j, i) = ureg(j, i)
                    rlatu(j, nlat+i) =-ureg(js, nlat+2-i)
                end do
                rlatu(j, nlat+1) = ureg(j, nlat+1)
            end do
            !
            !       shift the n2=(nlon + 1)/2 rlat vectors one half latitude grid
            !
            call vhifth(n2, nlat2, rlatu, wsav, wrk)
                !
                !       set values in uoff
                !
            do j=1, n2
                js = n2+j
                do i=1, nlat
                    uoff(j, i) =  rlatu(j, i)
                    uoff(js, i) =-rlatu(j, nlat2+1-i)
                end do
            end do
        end if

        !  Execute full circle longitude shift for all latitude circles
        do j=1, nlon
            do i=1, nlat
                rlou(i, j) = uoff(j, i)
            end do
        end do

        call vhifth(nlat, nlon, rlou, wsav(isav:), wrk)

        do j=1, nlon
            do i=1, nlat
                uoff(j, i) = rlou(i, j)
            end do
        end do

    end subroutine vhftreg

    subroutine vhifth(m, n, r, wsav, work)

        type(RealPeriodicFastFourierTransform) :: hfft
        integer(ip) :: m, n, n2, j, i, iw1
        real(wp) :: r(m, n), wsav(:), work(:), r2km2, r2km1

        n2 = (n+1)/2

        ! Set workspace index pointer
        iw1 = n + 2

        ! Compute fourier coefficients for r on shifted grid
        call hfft%forward(m, n, r, m, wsav(iw1:))

        do i=1, m
            do j=2, n2
                r2km2 = r(i, j+j-2)
                r2km1 = r(i, j+j-1)
                r(i, j+j-2) = r2km2 * wsav(n2+j) - r2km1 * wsav(j)
                r(i, j+j-1) = r2km2 * wsav(j) + r2km1 * wsav(n2+j)
            end do
        end do

        ! Shift r with fourier synthesis and normalization
        call hfft%backward(m, n, r, m, wsav(iw1:))

        do i=1, m
            do j=1, n
                r(i, j) = r(i, j)/n
            end do
        end do

    end subroutine vhifth

    ! Initialize wsav for subroutine vhifth
    subroutine vhifthi(n, dp, wsav)

        ! Dummy arguments
        integer(ip), intent(in)  :: n
        real(wp),    intent(in)  :: dp
        real(wp),    intent(out) :: wsav(:)

        ! Local variables
        integer(ip) :: n2, i, iw1
        real(wp)    :: arg
        type(RealPeriodicFastFourierTransform) :: hfft

        n2 = (n+1)/2

        do i=2, n2
            arg = real(i - 1, kind=wp) * dp
            wsav(i) = sin(arg)
            wsav(i+n2) = cos(arg)
        end do

        ! Set workspace index pointer
        iw1 = n + 2

        call hfft%initialize(n, wsav(iw1:))

    end subroutine vhifthi

end submodule grid_transfer_vector_shift


module chemotaxis_module

  use boxlib
  use multifab_module
  use mt19937_module

  use dtypes_module
  use advance_module
  use sdcquad_module
  use write_plotfile_module

  implicit none

contains

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  subroutine chemotaxis(ctx, opts, sdc)
    type(cht_ctx_t),  intent(inout) :: ctx
    type(cht_opts_t), intent(inout) :: opts
    type(sdcquad),    intent(in)    :: sdc

    integer        :: lo(dim), hi(dim), n
    logical        :: is_periodic(dim)
    real(dp_t)     :: dt, time

    type(box)      :: bx
    type(boxarray) :: ba
    type(layout)   :: la
    type(multifab) :: q

    ! physical problem is a box on (-1,-1) to (1,1), periodic on all sides
    ctx%prob_lo     = -10.0d0
    ctx%prob_hi     =  10.0d0
    is_periodic = .true.

    ! create a box from (0,0) to (n_cell-1,n_cell-1)
    lo = 0
    hi = ctx%n_cell-1
    bx = make_box(lo,hi)

    ctx%dx    = (ctx%prob_hi(1) - ctx%prob_lo(1)) / ctx%n_cell
    ctx%invdx = 1.0d0 / ctx%dx

    ! build layout
    call build(ba,bx)
    call build(la,ba,bx,pmask=is_periodic)

    ! build q (solution) multifab
    call build(q,la,ctx%nc,ctx%ng)

    ! set initial condition and write plot 0
    call initial(q,ctx)
    call write_plotfile(la,q,0,ctx%dx,0.0d0,ctx%prob_lo,ctx%prob_hi)

    ! run
    dt = ctx%dt
    time = 0.0d0
    do n = 1, opts%nsteps
       select case(opts%method)
       case("fe")
          call advance_fe(q,dt,ctx)
       case("sdc")
          call advance_sdc(q,dt,ctx,sdc)
       end select

       time = time + dt
       if (mod(n, opts%plot_int) .eq. 0 .or. n .eq. opts%nsteps) then
          print *, "OUTPUT: step:", n, "time:", time
          call write_plotfile(la,q,n,ctx%dx,time,ctx%prob_lo,ctx%prob_hi)
       end if
    end do

    ! destroy/deallocate
    call destroy(ba)
    call destroy(la)
    call destroy(q)

  end subroutine chemotaxis

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  subroutine initial(q,ctx)
    type(multifab),  intent(inout) :: q
    type(cht_ctx_t), intent(in)    :: ctx

    integer        :: n, p, i, j, lo(2), hi(2)

    double precision :: x, y, px, py, d, a
    double precision, pointer, dimension(:,:,:,:) :: qp

    ! double precision, parameter :: pi  = 3.141592653589793d0
    ! double precision, parameter :: pad = 4.0d0
    double precision, parameter :: pad = 2.0d0

    call init_genrand(368)

    do n=1, nfabs(q)

       qp => dataptr(q,n)

       lo = lwb(get_box(q,n))
       hi = upb(get_box(q,n))

       qp(:,:,1,iu) = 1.0d0
       qp(:,:,1,iv) = 0.5d0

       do p = 1, 800
          call mt_random_number(px)
          call mt_random_number(py)
          call mt_random_number(a)
          a = -1.0d0 + 2.0d0 * a

          px = ctx%prob_lo(1) + pad + px * (ctx%prob_hi(1) - ctx%prob_lo(1) - 2*pad)
          py = ctx%prob_lo(2) + pad + py * (ctx%prob_hi(2) - ctx%prob_lo(2) - 2*pad)

          do j = lo(2), hi(2)
             y = ctx%prob_lo(2) + j * ctx%dx
             do i = lo(1), hi(2)
                x = ctx%prob_lo(1) + i * ctx%dx

                d = (x - px)**2 + (y - py)**2
                qp(i,j,1,iv) = qp(i,j,1,iv) + 0.01d0 * a * dexp(-4.0d0*d)
             end do
          end do
       end do

    end do
  end subroutine initial

end module chemotaxis_module


!==============================================================================!
  subroutine Field_Mod_Grad_Variable(flow, var)
!------------------------------------------------------------------------------!
!   Calculates gradient of a variable from field flow                          !
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  type(Field_Type) :: flow
  type(Var_Type)   :: var
!==============================================================================!

  call Field_Mod_Grad_Component(flow, var % n, 1, var % x)  ! dp/dx
  call Field_Mod_Grad_Component(flow, var % n, 2, var % y)  ! dp/dy
  call Field_Mod_Grad_Component(flow, var % n, 3, var % z)  ! dp/dz

  end subroutine


      double complex function Fvf(st,j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
      integer j1,j2,j3,j4,j5,j6
      include 'constants.f'
      include 'zprods_decl.f'
      include 'sprods_com.f'
      character*9 st
      double complex Lsm1_2mh,Lsm1_2me,I3m,zab2,I3m123456,I3m563412
      double precision t
C---statement function
      zab2(j1,j2,j3,j4)=za(j1,j2)*zb(j2,j4)+za(j1,j3)*zb(j3,j4)
      I3m123456=I3m(s(j1,j2),s(j3,j4),s(j5,j6))
      I3m563412=I3m123456

      if(st.eq.'q+qb-g+g-') then
      Fvf=
     & -zab2(j5,j2,j4,j1)**2/(za(j5,j6)*zb(j1,j2)*zab2(j3,j1,j2,j4)**2)
     & *Lsm1_2mh(s(j3,j4),t(j1,j2,j4),s(j1,j2),s(j5,j6))
     & -zab2(j2,j1,j3,j6)**2/(za(j1,j2)*zb(j5,j6)*zab2(j3,j1,j2,j4)**2)
     & *Lsm1_2mh(s(j3,j4),t(j1,j2,j3),s(j1,j2),s(j5,j6))

     & -I3m123456*za(j4,j5)*zb(j1,j3)
     & *zab2(j2,j1,j3,j6)/(2d0*zab2(j3,j1,j2,j4)*t(j1,j2,j3))

     & -I3m123456*za(j2,j4)*zb(j3,j6)
     & *zab2(j5,j2,j4,j1)/(2d0*zab2(j3,j1,j2,j4)*t(j1,j2,j4))

     & +I3m563412*za(j2,j4)*zb(j3,j6)
     & *zab2(j5,j3,j6,j1)/(2d0*zab2(j3,j1,j2,j4)*t(j3,j5,j6))

     & +I3m563412*za(j4,j5)*zb(j1,j3)
     & *zab2(j2,j4,j5,j6)/(2d0*zab2(j3,j1,j2,j4)*t(j4,j5,j6))
      elseif(st.eq.'q+qb-g+g+') then
      Fvf=-za(j2,j5)**2/(za(j1,j2)*za(j5,j6)*za(j3,j4)**2)
     & *Lsm1_2me(t(j1,j2,j3),t(j1,j2,j4),s(j1,j2),s(j5,j6))
      endif
      return
      end


      subroutine conic(xm,b)
!     to obtain mach number derivatives in radial flow
      use kinddefine
      use gg, only:g5,g6,g8,ga,qt
      implicit none
      real(kind=K8),dimension(4),intent(out) :: b
      real(kind=K8),intent(in) :: xm
      real(kind=K8) :: area,bmm,c2,c4,cmm,dmm,four,one,thr,two
      real(kind=K8) :: xmm,xmm1,xmm2
      data one/1.d+0/,two/2.d+0/,thr/3.d+0/,four/4.d+0/
      xmm=xm*xm
      xmm1=xmm-one
      xmm2=xmm1**2
      bmm=one+g8*xmm
      area=(g6+g5*xmm)**ga/xm
      b(1)=area**qt
      b(2)=xm*bmm/qt/xmm1/b(1)
      c2=two-(one+thr*g8)/qt
      c4=g8/qt-one
      cmm=xmm*(c2+xmm*c4)-one-one/qt
      b(3)=b(2)*cmm/xmm2/b(1)
      dmm=(four*c4*xmm+two*c2)/cmm-four/xmm1
      b(4)=b(3)*(b(3)/b(2)+xm*b(2)*dmm-one/b(1))
      end subroutine conic


*DECK CACOSH
      COMPLEX FUNCTION CACOSH (Z)
C***BEGIN PROLOGUE  CACOSH
C***PURPOSE  Compute the arc hyperbolic cosine.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C4C
C***TYPE      COMPLEX (ACOSH-S, DACOSH-D, CACOSH-C)
C***KEYWORDS  ACOSH, ARC HYPERBOLIC COSINE, ELEMENTARY FUNCTIONS, FNLIB,
C             INVERSE HYPERBOLIC COSINE
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C CACOSH(Z) calculates the complex arc hyperbolic cosine of Z.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  CACOS
C***REVISION HISTORY  (YYMMDD)
C   770401  DATE WRITTEN
C   861211  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C***END PROLOGUE  CACOSH
      COMPLEX Z, CI, CACOS
      SAVE CI
      DATA CI /(0.,1.)/
C***FIRST EXECUTABLE STATEMENT  CACOSH
      CACOSH = CI*CACOS(Z)
C
      RETURN
      END


      subroutine outar(c,s,a,rt,in,r2,se,su,sr,pv,p,po,iunit,io)
c*******************************************
c  Displays AR coefficients and their t stats.
c  Input: c - AR coefficients, s - std errors, se - sig of AR fit
c   sr - sampling rate, p - in no. of lags, po - out no. of lags
c  Output: rt - roots of the characteristic polynomial for AR model
c===========================================
      integer p,po,iunit,in(p),ep
      real*8 c(po)
      complex a(p+1),rt(p)
      real s(po),amp,per,pi,sr
      character*10 su
      logical*1 plsh
      intent(in) c,s,r2,se,su,sr,pv,p,po,iunit,io
      intent(out) rt
c********************
c  Trap zero s's
      ier=0
      do k=1,po
       if(s(k)<=0.) then
        write(io,*) 'Standard error s(',k,') = ',s(k),' < 0 in outar'
        ier=ier+1
       endif
      enddo
      if(ier>0) then
       return
      endif
      ep=in(po)
      do k=1,ep
       a(k)=cmplx(0.,0.)
      enddo
c-------
      pi=acos(-1.)
      plsh=.true.
c  Put coefficients of AR into a
      do k=1,po
       a(ep-in(k)+1)=cmplx(-1.*sngl(c(k)),0.)
      enddo
      a(ep+1)=cmplx(1.,0.)
      call zroots(a,ep,rt,plsh,ier)
c-------
      do k=1,ep
       amp=cabs(rt(k))
       if(abs(aimag(rt(k)))<1.e-5) then
        per=0.
         else
          if(iunit==1) then
           per=1.e3*sr*abs(atan2(aimag(rt(k)),real(rt(k))))/(2.*pi)
            elseif(iunit==2) then
             per=sr*abs(atan2(aimag(rt(k)),real(rt(k))))/(2.*pi)
              else
               per=2.*pi/(sr*abs(atan2(aimag(rt(k)),real(rt(k)))))
               if(per>1.e+4) then
                per=1.e4
               endif
          endif
       endif
       rt(k)=cmplx(amp,per)
      enddo
c-------
      write(io,'(/17x,''AR('',i3,'') parameters / t values'')') in(po)
      write(io,'(/6x,''Adjusted R Square = '',f5.3,2x,''Std Error of AR
     *Fit = '',g10.3)') r2,se
      write(io,'(7x,''p Value Threshold for the Iterative AR Prewhitenin
     *g Method = '',f5.3)') pv
      write(io,'(6x,47(''='')/)')
      write(io,'(70(:,i3,'' - '',f8.2,2x))') (in(k),c(k),k=1,po)
      write(io,'(70(:,6x,f8.2,2x))') (sngl(c(k))/s(k),k=1,po)
      write(io,*)
c-------
      if(iunit==2) then
       write(io,'(1x,''Amplitudes & Frequencies of the Roots of the AR P
     *olynomial in units of Hz'')')
       elseif(iunit==1) then
        write(io,'(1x,''Amplitudes & Frequencies of the Roots of the AR
     *polynomial in units of kHz'')')
         elseif(iunit==0) then
          write(io,'(1x,''Amplitudes & Periods of the Roots of the AR Po
     *lynomial in units of '',a5)') su
      endif
      write(io,'(11x,''A real root is given a period = 0.'')')
      write(io,'(6x,47(''='')/)')
      write(io,'(70(:,i3,'' - '',f8.2,2x))') (k,real(rt(k)),k=1,ep)
      write(io,'(70(:,6x,f8.2,2x))') (aimag(rt(k)),k=1,ep)
      write(io,*)
c-------
      return
      end


      SUBROUTINE SEMST(ISTK,ISEM,ANS,IDIV,N,N2,NDOWS,char,IFMT)
!      TRACE SEMBL,TEMP,K
!    SEMST IS THE VERSION OF SEMSTK THAT RUNS IN HOST MEMORY RATHER THAN IN THE AP.
!  SEMST PERFORMS THE FOLLOWING TASKS: 1) COMPUTES THE ENERGY OF THE STACKED
! TRACE (THE ENERGY WINDOWS ARE N TRACE SAMPLES LONG AND ARE COMPUTED EVERY
! N2 TRACE SAMPLES).  2) DIVIDES THE STACKED TRACE ENERGY BY THE NUMBER OF
! WINDOWS THAT WENT INTO THE STACKED TRACE, FOR EACH WINDOW.  THIS IS THE MEAN
! ENERGY OF THE STACKED TRACE.  3) DIVIDES THE STACKED TRACE MEAN ENERGY BY THE
! MEAN ENERGY OF THE INPUT WINDOWS.  4) MULTIPLIES THIS SEMBLANCE BY char (9).
!
! ARGUMENTS:
! ISTK - THE APMEM INDEX OF THE STACKED TRACE.
! ISEM - THE APMEM INDEX OF THE MEAN ENERGY OF THE INPUT TRACES.
! ANS  - THE ARRAY TO RECEIVE THE REAL SEMBLANCE VALUES.
! IDIV - THE APMEM INDEX OF THE NUMBER OF WINDOWS FORMING THE STACK.
! N    - THE NUMBER OF SAMPLES IN EACH WINDOW.
! N2   - THE DISTANCE BETWEEN WINDOWS (USUALLY N/2).
! NDOWS - THE NUMBER OF WINDOWS.
! IFMT - THE OUTPUT FORMAT - BETTER BE 1 - INCLUDED FOR COMPATIBILITY WITH SEMSTK
!
! COPYRIGHTED BY PAUL HENKART, 23 JANUARY 1983
!
      COMMON /APMEM/ A(32766)
      DIMENSION ans(1)
!
      IF(IFMT.EQ.1) GO TO 2
      PRINT 1
1     FORMAT(' ***  ERROR  ***  SEMST CAN NOT DO A FORMAT ',
     *  'OTHER THAN 1.')
      STOP
2     CONTINUE
      IDIV1=IDIV
      ISEM1=ISEM
      K=ISTK-1
      DO 20 I=1,NDOWS
         ENERGY=0.
         IF(A(IDIV1).LE.0.) A(IDIV1)=1.
         DO 10 J=1,N                                                    ! DO A WINDOW LENGTH
            IF(A(K+J).EQ.0.) GO TO 10                                   ! NO SENSE IN PLAYING WITH ZEROES!
            TEMP=A(K+J)/A(IDIV1)                                        !  THE STACKED TRACE HAS NOT BEEN DIVIDED YET!!!
            ENERGY=ENERGY+TEMP*TEMP                                     !  SUM OF THE SQUARES!
10       CONTINUE
!         PRINT 111,I,IDIV1,ISEM1,A(IDIV1),A(ISEM1),ENERGY
!  111    FORMAT(1X,3I10,3F12.5)
         IF(A(ISEM1).EQ.0.) A(ISEM1)=1.
         SEMBL=(ENERGY/(A(ISEM1)/A(IDIV1))) * char
         ANS(I)=SEMBL
!         IDIV1=IDIV1+1
         ISEM1=ISEM1+1
         K=K+N                                                          ! FIND THE BEGINNING OF THE NEXT WINDOW
   20 CONTINUE
      RETURN
      END


!---------------------------------------------------------------------------
!       Calculatess the first and second derivatives of
!       Legendre Polynomials.
!---------------------------------------------------------------------------
subroutine legendre

    implicit none

    include 'mpintp.inc'
    include 'spp.inc'

    real X
    integer l
    !
    X = csthcm
    !
    pol(0, 1) = 0.
    pol(1, 1) = 1.
    pol(2, 1) = 3. * X
    pol(3, 1) = (15. * X * X - 3.) / 2.
    pol(4, 1) = (35. * X * X * X - 15. * X) / 2.
    pol(5, 1) = (315. * X * X * X * X - 210. * X * X + 15.) / 8.
    pol(6, 1) = (693. * X * X * X * X * X - 630. * X * X * X + 105. * X) / 8.
    pol(7, 1) = (3003. * X * X * X * X * X * X - 3465. * X * X * X * X + 945 * X * X - 35.) / 16.
    pol(0, 2) = 0.
    pol(1, 2) = 0.
    pol(2, 2) = 3.
    pol(3, 2) = 15. * X
    pol(4, 2) = (105. * X * X - 15.) / 2.
    pol(5, 2) = (315. * X * X * X - 105. * X) / 2.
    pol(6, 2) = (3465. * X * X * X * X - 1890. * X * X + 105.) / 8.
    pol(7, 2) = (9009. * X * X * X * X * X - 6930. * X * X * X + 945. * X) / 8.
    !
    !      do l = 0, 6
    !          print *, 'legedre', 'l=',l
    !          print *, pol(l,1), pol(l,2)
    !      enddo
    return
end


c=======================================================================
c
c     subroutine BUILDRBIT
c
c     Uses the inputs of riskbudgetIT to compute the suitable inputs for
c     the solver riskbudegting
c
c-----------------------------------------------------------------------
      SUBROUTINE buildrbit ( n, cov, kappa, rho, covb, varb,
     &                       neq, nineq, ccst, bcst, cinf, csup,
     &                       nbc, class, sigma,
     &                       iwork, dwork, omegabench,
     &                       rhoIT, bcstIT, cinfIT, csupIT, sigmaIT,
     &                       info )
c-----------------------------------------------------------------------
c
c     INPUT
c
c       n      : portfolio size                                  integer
c       cov    : covariance matrix (n*n)                          double
c       rho    : expected returns vector (n)                      double
c       covb   : covariance assets-index (n)                      double
c       varb   : variance of the benchmark                        double
c       neq    : number equality constraints                     integer
c       nin    : number inequality constraints                   integer
c       ccst   : matrix of constraints (nasset*(neq+nin))         double
c       bcst   : vector initial of constraints (neq+nin)          double
c       cinf   : lower bound (n)                                  double
c       csup   : upper bound (n)                                  double
c       nbc    : number of risk budgeting constraints (>=1)      integer
c       class  : block definition (n)                            integer
c       sigma  : volatilities budgets constraints (nbc)           double
c
c     WORKSPACE
c       iwork  : 12*n + nbc                                        integer
c       dwork  : n*(4*n+28) + nbc + nineq + neq                     double
c
c     OUTPUT
c       omegabench:  variable omega benchmark (n)                   double
c       rhoIT: n-double, equal to -(rho + kappa* Gamma_b)           double
c       bcstIT: (neq-nineq)-double
c       binfIT   : modified lower bound (n)                         double
c       bsupIT   : modifief upper bound (n)                         double
c       sigmaIT  : nbc-vector of Index tracking risk constraints    double
c
c     CALL
c        IVI, IMX, SCHURS, OMCDMCT, PMV, OMEGAB
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, info, neq, nineq, nbc, class(*)
      DOUBLE PRECISION cov(*), covb(*), rhoIT(*), bcstIT(*), cinfIT(*)
      DOUBLE PRECISION sigmaIT(*), csupIT(*), sigma(*), varb
      DOUBLE PRECISION kappa, rho(*), ccst(*), bcst(*), cinf(*), csup(*)
      DOUBLE PRECISION omegabench(*)
c
c     workspaces
      DOUBLE PRECISION dwork(*)
      INTEGER iwork(*)
c
c     local variables
      INTEGER i, piw, pdsigmabench, pdw, pdbcstIT
      DOUBLE PRECISION NEGONE, EPS
      PARAMETER (EPS = 1.E-10, NEGONE=-1.E0)
c
c     external function
      EXTERNAL IVI, IMX, SCHURS, OMCDMCT, PMV, omegab
c
c-----------------------------------------------------------------------
c
c     pointers for integer work space : iwork
c     ---------------------------------------
      piw = 1
c
c     Total size of iwork array = 12*n+nbc
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdsigmabench = 1
c     needs nbc
      pdbcstIT = pdsigmabench + nbc
c     needs nineq + neq
      pdw = pdbcstIT +  nineq + neq
c     needs n*(4*n+28)
c
c     Total size of dwork array = n*(4*n+28) + nbc + nineq + neq
c
c------------------------------------------------------------------------
c
      CALL omegab ( n, cov, covb, nbc, class,
     &              iwork(piw), dwork(pdw),
     &              omegabench, dwork(pdsigmabench), info)
      IF (info .LT. 0) RETURN
      DO i = 1,nbc
        sigmaIT(i)=sigma(i)**2-varb+dwork(pdsigmabench+i-1)
        IF (sigmaIT(i) .LT. EPS) THEN
            info = -112
            RETURN
        ENDIF
        sigmaIT(i) = SQRT(sigmaIT(i))
      ENDDO
      CALL PMV(n, n, cov, omegabench, rhoIT)
c
c     rhoIt = -rho + kappa( Covb + Cov*omegabench)
      IF (kappa .LT. EPS) THEN
        CALL YV(n, rho, rhoIT)
      ELSE
        DO i = 1,n
            rhoIT(i) = rhoIT(i)+covb(i)
            rhoIT(i) = -kappa*rhoIT(i)
            rhoIT(i) = rho(i)+rhoIT(i)
        ENDDO
      ENDIF
c
c     bcstIT = bcst - ccst*omegaB
      CALL PMTV(n, (neq+nineq), ccst, omegabench, dwork(pdbcstIT))
      CALL SVVX((neq+nineq), bcst, dwork(pdbcstIT), NEGONE, bcstIT)

c     bsupIT = bsup - omegab
      CALL SVVX(n, csup, omegabench, NEGONE, csupIT)

c     binfIT = binf - omegab
      CALL SVVX(n, cinf, omegabench, NEGONE, cinfIT)
      RETURN
      END


      subroutine growop(imngmt,month,ityp,extinc,sumgd,alai,dm)
c
c     + + + PURPOSE + + +
c     Computes optimum biomass growth (dm) for a given day.
c
c     Called from YLDOPT and YOPTP.
c     Author(s): Arnold
c     Reference in User Guide:
c
c     Changes:
c           1) Substituted GDD for (TAVE - BTEMP(ITYP)) in EPIC
c              Equation 2.235.
c           2) Changed order of parameters to copnform to WEPP Coding
c              Convention.  (Swapped 1st & 5th (last).)
c
c     Version: This module recoded from Arnold's WEPP Post-version 92.25.
c     Date recoded: 07/09/92.
c     Recoded by: Charles R. Meyer.
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
      include 'pmxcrp.inc'
      include 'pmxpln.inc'
      include 'pmxres.inc'
      include 'pmxtil.inc'
      include 'pmxtls.inc'
      include 'pntype.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
      real sumgd, alai, dm, extinc
      integer imngmt, month, ityp
c
c     + + + ARGUMENT DEFINITIONS + + +
c     sumgd  - gdd accumulated thus far in the growing season
c     alai   - adjusted leaf area index
c     dm     - optimum above-ground biomass (kg/m^2)
c     extinc - radiation extinction coefficient for current crop
c     imngmt - current cropping system (annual or perennial)
c     month  - current month
c     ityp   - current crop
c
c     + + + COMMON BLOCKS + + +
      include 'ccrpgro.inc'
c       read: beinp(ntype), otemp(ntype)
c
      include 'ccrpprm.inc'
c -- XXX -- NOTE: DECLARATION in CCRPPRM.INC needs to be changed from
c                 "10" to a *PARAMETER* !! -- CRM -- 7/31/92
c       read: btemp(10)
c
      include 'ccrpvr3.inc'
c       read: xmxlai(ntype)
c     modify: gdd
c
      include 'cobclim.inc'
c       read: radave(12), obmint(12), obmaxt(12)
c
c     + + + END SPECIFICATIONS + + +
c
      real par, tave, temstr
      tave = (obmaxt(month)+obmint(month)) / 2.0
c
      if (tave.gt.btemp(ityp)) then
        gdd = tave - btemp(ityp)
        sumgd = sumgd + gdd
c
c       ------ Compute temperature stress parameter (temstr) from
c       ratio of growing degree days (gdd) to difference between
c       optimum temperature for plant growth (otemp) and base
c       daily air temperature (btemp).  TEMSTR is really the
c       *complement* of temperature stress; ie, 1 - temperature
c       stress.  By the way it is defined, TEMSTR must range
c       between zero and one.
c       (EPIC Equation 2.235)
c
c       Original Code:
c       temstr = sin(1.5708 * (tave - btemp(ityp)) / (
c       1           otemp(ityp) - btemp(ityp)))
        temstr = sin(1.5708*gdd/(otemp(ityp)-btemp(ityp)))
        if (temstr.lt.0.0) temstr = 0.0
c
c       ------ Compute photosynthetically active radiation (par) from
c       observed average monthly solar radiation (radave) and leaf
c       area index (?) (dlai).
c       (EPIC Equation 2.192 with modifications.  0.2092 is used to
c       convert the units from Langleys to MJ/m**2.  0.05 is added
c       to LAI to "get the equation started"; otherwise, the values
c       never seem to depart from the baseline.  CRM -- 6/25/92.)
c
        par = 0.02092 * radave(month) * (1.0-exp(-extinc*(alai+0.05)))
c
c       ------ Compute dry matter (dm) from inputted biomass conversion factor
c       (beinp) and PAR.
c       (EPIC Equation 2.193 with modifications.  Change in daylength
c       is ignored.  The right side is divided by 10 to implement a
c       change in units.  CRM -- 6/25/92.)
c
c       Original Code:
c       dm = dm + (beinp(ityp) * par / 10000.0) * temstr
        dm = dm + 0.0001 * beinp(ityp) * par * temstr
c
c       Annuals
        if (imngmt.eq.1) then
          alai = (xmxlai(ityp)*dm) / (dm+0.5512*exp(-6.8*dm))
c       Perennials
        else
          alai = (xmxlai(ityp)*dm) / (dm+0.2756*exp(-13.6*dm))
        end if
c
      end if
c
      return
      end


Subroutine MC_interp(IX,IY,IZ,WX,WY,WZ,RBIN,LBOX,confineType,DEL)
IMPLICIT NONE
Double PRECISION RBIN(3)
Double PRECISION LBox
INTEGER IX(2), IY(2), IZ(2)
DOUBLE PRECISION WX(2), WY(2), WZ(2)
INTEGER NBINX
Double precision DEL
INTEGER confineType

SELECT CASE (confineType)
CASE (0) ! Box from 0-LBOX, Bins split by boundaries
    ! Periodic BC
    RBIN(1)=RBIN(1)-nint(RBIN(1)/LBOX-0.5)*LBOX
    RBIN(2)=RBIN(2)-nint(RBIN(2)/LBOX-0.5)*LBOX
    RBIN(3)=RBIN(3)-nint(RBIN(3)/LBOX-0.5)*LBOX

    ! Binning
    IX(1)=nint(RBIN(1)/DEL+0.5)
    IY(1)=nint(RBIN(2)/DEL+0.5)
    IZ(1)=nint(RBIN(3)/DEL+0.5)

    IX(2)=IX(1)-1
    IY(2)=IY(1)-1
    IZ(2)=IZ(1)-1

    ! Calculate the bin weighting
    WX(2)=(DEL*IX(1)-RBIN(1))/DEL   ! WX(2)=(RBIN(1)-IX(1)*DEL)/(IX(2)*DEL-IX(1)*DEL)
    WX(1)=1-WX(2)                   ! WX(1)=(IX(2)*DEL-RBIN(1))/(IX(2)*DEL-IX(1)*DEL)
    WY(1)=(DEL*IY(1)-RBIN(2))/DEL   ! WY(2)=(RBIN(2)-IY(1)*DEL)/(IY(2)*DEL-IY(1)*DEL)
    WY(2)=1-WY(1)                   ! WY(1)=(IY(2)*DEL-RBIN(2))/(IY(2)*DEL-IY(1)*DEL)
    WZ(2)=(DEL*IZ(1)-RBIN(3))/DEL   ! WZ(2)=(RBIN(3)-IZ(1)*DEL)/(IZ(2)*DEL-IZ(1)*DEL)
    WZ(1)=1-WZ(2)                   ! WZ(1)=(IZ(2)*DEL-RBIN(3))/(IZ(2)*DEL-IZ(1)*DEL)

    ! Periodic BC on Bins:
    IX(1)=IX(1)-floor(REAL((IX(1)-1))/REAL(NBINX)) * NBINX
    IX(2)=IX(2)-floor(REAL((IX(2)-1))/REAL(NBINX)) * NBINX
    IY(1)=IY(1)-floor(REAL((IY(1)-1))/REAL(NBINX)) * NBINX
    IY(2)=IY(2)-floor(REAL((IY(2)-1))/REAL(NBINX)) * NBINX
    IZ(1)=IZ(1)-floor(REAL((IZ(1)-1))/REAL(NBINX)) * NBINX
    IZ(2)=IZ(2)-floor(REAL((IZ(2)-1))/REAL(NBINX)) * NBINX
CASE (1)
    ! Periodic BC
    RBIN(1)=RBIN(1)-nint(RBIN(1)/LBOX-0.5)*LBOX
    RBIN(2)=RBIN(2)-nint(RBIN(2)/LBOX-0.5)*LBOX

    ! Binning
    IX(1)=nint(RBIN(1)/DEL+0.5)
    IY(1)=nint(RBIN(2)/DEL+0.5)
    IZ(1)=nint(RBIN(3)/DEL+1.0) ! Note 1.0 so that box centers are on half intigers

    IX(2)=IX(1)-1
    IY(2)=IY(1)-1
    IZ(2)=IZ(1)-1

    ! Calculate the bin weighting
    WX(2)=(DEL*IX(1)-RBIN(1))/DEL   ! WX(2)=(RBIN(1)-IX(1)*DEL)/(IX(2)*DEL-IX(1)*DEL)
    WX(1)=1-WX(2)                   ! WX(1)=(IX(2)*DEL-RBIN(1))/(IX(2)*DEL-IX(1)*DEL)
    WY(1)=(DEL*IY(1)-RBIN(2))/DEL   ! WY(2)=(RBIN(2)-IY(1)*DEL)/(IY(2)*DEL-IY(1)*DEL)
    WY(2)=1-WY(1)                   ! WY(1)=(IY(2)*DEL-RBIN(2))/(IY(2)*DEL-IY(1)*DEL)
    WZ(2)=(DEL*IZ(1)-0.5*DEL-RBIN(3))/DEL   ! WZ(2)=(RBIN(3)-IZ(1)*DEL)/(IZ(2)*DEL-IZ(1)*DEL)
    WZ(1)=1-WZ(2)                   ! WZ(1)=(IZ(2)*DEL-RBIN(3))/(IZ(2)*DEL-IZ(1)*DEL)

    if ((WZ(1).lt.0).OR.(WZ(2).lt.0)) then
        print*, "negitive W"
        stop 1
    endif

    ! Periodic BC on Bins:
    IX(1)=IX(1)-floor(REAL((IX(1)-1))/REAL(NBINX)) * NBINX
    IX(2)=IX(2)-floor(REAL((IX(2)-1))/REAL(NBINX)) * NBINX
    IY(1)=IY(1)-floor(REAL((IY(1)-1))/REAL(NBINX)) * NBINX
    IY(2)=IY(2)-floor(REAL((IY(2)-1))/REAL(NBINX)) * NBINX
CASE (2) ! Box confinement
    ! Binning
    IX(1)=nint(RBIN(1)/DEL+1.0)
    IY(1)=nint(RBIN(2)/DEL+1.0)
    IZ(1)=nint(RBIN(3)/DEL+1.0) ! Note 1.0 so that box centers are on half intigers

    IX(2)=IX(1)-1
    IY(2)=IY(1)-1
    IZ(2)=IZ(1)-1

    ! Calculate the bin weighting
    WX(2)=(DEL*IX(1)-0.5*DEL-RBIN(1))/DEL   ! WX(2)=(RBIN(1)-IX(1)*DEL)/(IX(2)*DEL-IX(1)*DEL)
    WX(1)=1-WX(2)                   ! WX(1)=(IX(2)*DEL-RBIN(1))/(IX(2)*DEL-IX(1)*DEL)
    WY(1)=(DEL*IY(1)-0.5*DEL-RBIN(2))/DEL   ! WY(2)=(RBIN(2)-IY(1)*DEL)/(IY(2)*DEL-IY(1)*DEL)
    WY(2)=1-WY(1)                   ! WY(1)=(IY(2)*DEL-RBIN(2))/(IY(2)*DEL-IY(1)*DEL)
    WZ(2)=(DEL*IZ(1)-0.5*DEL-RBIN(3))/DEL   ! WZ(2)=(RBIN(3)-IZ(1)*DEL)/(IZ(2)*DEL-IZ(1)*DEL)
    WZ(1)=1-WZ(2)                   ! WZ(1)=(IZ(2)*DEL-RBIN(3))/(IZ(2)*DEL-IZ(1)*DEL)
CASE (3)
    print*, "DEL", DEL," RBIN(1)",RBIN(1)
    ! Binning
    IX(2)=nint(RBIN(1)/DEL)
    IY(2)=nint(RBIN(2)/DEL)
    IZ(2)=nint(RBIN(3)/DEL) ! Note 1.0 so that box centers are on half intigers
    !write(*,"(A,I4,A,I4,A,I4)"),"IX(1):",IX(1),"  IY(1):",IY(1),"  IZ(1)",IZ(1)

    IX(1)=IX(2)+1
    IY(1)=IY(2)+1
    IZ(1)=IZ(2)+1

    ! Calculate the bin weighting
    WX(2)=(DEL*IX(1)-0.5*DEL-RBIN(1))/DEL   ! WX(2)=(RBIN(1)-IX(1)*DEL)/(IX(2)*DEL-IX(1)*DEL)
    WX(1)=1-WX(2)                   ! WX(1)=(IX(2)*DEL-RBIN(1))/(IX(2)*DEL-IX(1)*DEL)
    WY(1)=(DEL*IY(1)-0.5*DEL-RBIN(2))/DEL   ! WY(2)=(RBIN(2)-IY(1)*DEL)/(IY(2)*DEL-IY(1)*DEL)
    WY(2)=1-WY(1)                   ! WY(1)=(IY(2)*DEL-RBIN(2))/(IY(2)*DEL-IY(1)*DEL)
    WZ(2)=(DEL*IZ(1)-0.5*DEL-RBIN(3))/DEL   ! WZ(2)=(RBIN(3)-IZ(1)*DEL)/(IZ(2)*DEL-IZ(1)*DEL)
    WZ(1)=1-WZ(2)                   ! WZ(1)=(IZ(2)*DEL-RBIN(3))/(IZ(2)*DEL-IZ(1)*DEL)
    !write(*,"(A,f4.3,A,f4.3,A,f4.3)"),"WX(1):",WX(1),"  WY(1):",WY(1),"  WZ(1)",WZ(1)
END SELECT

end subroutine


c gmm01f
c  THIS FILE IS PART OF THE ORIGINAL GMM01F DISTRIBUTION
      subroutine cofd0(nmax)
      implicit double precision (a-h,o-z)
      include 'gmm01f.par'
      parameter (nmp=np*(np+2))
      parameter (ni0=np*(np+1)*(2*np+1)/3+np*np)
      integer v
      double precision lnfacd
      common/cofmnv0/cof0(ni0)
      common/crot/cofsr(nmp)
          common/fnr/fnr(0:2*(np+2))
      i=0
      sm=-0.5d0*dble((-1)**nmax)
      do m=-nmax,nmax
         ns=max(1,iabs(m))
         sm=-sm
         do n=ns,nmax
            inm=n*(n+1)-m
            do v=ns,nmax
               i=i+1
               ivm=v*(v+1)+m
               c=cofsr(inm)+cofsr(ivm)
               c=sm*dexp(c)
               c0=fnr(2*n+1)*fnr(2*v+1)
               c1=fnr(n)*fnr(v)*fnr(n+1)*fnr(v+1)
               c0=c0/c1
                   cof0(i)=c*c0
                enddo
             enddo
          enddo
      return
      end


SUBROUTINE angle_sat2toa ( earth_curv, ers2_alt, atm_hght, n_arr_pix, sza, vza, relazm )

  USE OMSAO_precision_module,  ONLY: i4, r4, r8
  USE OMSAO_parameters_module, ONLY: &
       r4_missval, deg2rad, rad2deg, pi, min_zenith, max_zenith, max_azimuth
  IMPLICIT NONE

  ! Input variables
  ! ===============
  INTEGER (KIND=i4), INTENT (IN) :: n_arr_pix
  REAL    (KIND=r4), INTENT (IN) :: earth_curv, ers2_alt, atm_hght

  ! ==================
  ! Modified variables
  ! ==================
  REAL (KIND=r4), DIMENSION (n_arr_pix), INTENT (INOUT) :: sza, vza, relazm

  ! ===============
  ! Local variables
  ! ===============
  REAL (KIND=r4)                        :: pihalf
  REAL (KIND=r4), DIMENSION (n_arr_pix) :: &
       earth_zen, locazm, szarad, vzarad, razmrad, tmp_sza, tmp_vza

  ! ==================================
  ! Compute GOME viewing angles at TOA
  ! ==================================
  ! (Adopted in part from R.Spurr's geometry module)

  pihalf = REAL(pi/2.0_r8, KIND=r4)

  ! -------------------------------------------------------
  ! Modify a local copy of SZA and VZA so that we don't go
  ! out-of-bounds for the computation of TOA angles.
  ! -------------------------------------------------------
  tmp_sza = sza  ;  tmp_vza = vza
  WHERE ( tmp_sza < min_zenith .OR. tmp_sza > max_zenith )
     tmp_sza = r4_missval
  ENDWHERE
  WHERE ( tmp_vza < min_zenith .OR. tmp_vza > max_zenith )
     tmp_vza = r4_missval
  ENDWHERE

  ! ----------------------------------
  ! First convert everything to Radian
  ! ----------------------------------
  szarad  = sza    * REAL ( deg2rad, KIND=r4 )
  vzarad  = vza    * REAL ( deg2rad, KIND=r4 )
  razmrad = relazm * REAL ( deg2rad, KIND=r4 )

  ! ----------------------------------
  ! Step 1: Compute Earth center angle
  ! ----------------------------------
  earth_zen = ASIN( SIN (vzarad) * (earth_curv+ers2_alt) / (earth_curv+atm_hght) )
  WHERE ( earth_zen <= pihalf )
     earth_zen = earth_zen - vzarad
  ELSEWHERE
     earth_zen = r4_missval
  END WHERE

  ! ------------------------------------------------------------
  ! Step 2: Correction of relative azimuth angle, save as LOCAZM
  ! ------------------------------------------------------------
  WHERE ( earth_zen /= r4_missval )
     locazm = COS(earth_zen) * SIN(szarad) * COS(razmrad) - SIN(earth_zen) * COS(szarad)
  ELSEWHERE
     locazm = r4_missval
  END WHERE

  ! -----------------------------------
  ! Step 3: Correction of zenith angles
  ! -----------------------------------
  WHERE ( earth_zen /= r4_missval )
     szarad = ACOS( COS(szarad) * COS(earth_zen) + SIN(earth_zen) * SIN(szarad) * COS(razmrad) )
     vzarad  = vzarad + earth_zen
  ELSEWHERE
     szarad = r4_missval
     vzarad = r4_missval
  END WHERE
  ! ---------------------------------------------------
  ! Step 4: Final computation of relative azimuth angle
  ! ---------------------------------------------------
  WHERE ( (szarad /= r4_missval) .AND. (SIN(szarad) /= 0.0_r4 ) )
     razmrad = locazm / SIN(szarad)
  ELSEWHERE
     razmrad = r4_missval
  END WHERE

  ! -----------------------------------------------------------------
  ! Make sure the COS of the relative azimuth angle is within [-1,+1]
  ! (i.e., beware of rounding errors), and then take ACOS.
  ! -----------------------------------------------------------------
  WHERE ( (razmrad > 1.0_r4) .AND. (razmrad /= r4_missval) )
     razmrad = 1.0_r4
  ENDWHERE
  WHERE ( (razmrad < -1.0_r4) .AND. (razmrad /= r4_missval) )
     razmrad = -1.0_r4
  ENDWHERE
  razmrad = ACOS ( razmrad )

  ! ----------------------------------------------
  ! Final step: Convert everything back to Degrees
  ! ----------------------------------------------
  WHERE ( ABS(szarad) <= pihalf )
     sza   = szarad * REAL ( rad2deg, KIND=r4 )
  ELSEWHERE
     sza = r4_missval
  END WHERE
  WHERE ( ABS(vzarad) <= pihalf )
     vza    = vzarad  * REAL ( rad2deg, KIND=r4 )
  ELSEWHERE
     vza  = r4_missval
  ENDWHERE
  WHERE ( ABS(razmrad) <= REAL(pi, KIND=r4) )
     relazm = razmrad * REAL ( rad2deg, KIND=r4 )
  ELSEWHERE
     relazm = r4_missval
  END WHERE

  RETURN
END SUBROUTINE angle_sat2toa


      subroutine coefsdk(s12,mtsq,ct,cv,c1)
      implicit none
      include 'constants.f'
      include 'epinv.f'
      include 'epinv2.f'
      include 'scheme.f'
      include 'scale.f'
      include 'alfacut.f'
      include 'includect.f'
      double precision cv,ct,c1,s12,mtsq,ddilog,rsq,omrsq,eta,Kfun,
     & wlog,rlog,mulog

      if (scheme .eq.'dred') then
C------        eta=0 4d-hel
         eta=0d0
      elseif (scheme .eq. 'tH-V') then
C------       eta=1 t'Hooft Veltman
         eta=1d0
      endif

      rsq=s12/mtsq
      omrsq=1d0-rsq
      wlog=dlog(omrsq)
      rlog=dlog(rsq)

c--- epinv stands for (4*pi)^ep/Gamma(1-ep)/ep  (as usual)
c----  ct is the integrated counter-term, including alpha-dependence,
c----  see Eq. (10) of arXiv:1102.1967
      mulog=dlog(musq/mtsq)
      if (includect) then
      ct=(epinv2*epinv+epinv*mulog+0.5d0*mulog**2)
     & +(epinv+mulog)*(2.5d0-2d0*wlog)
     & +25d0/4d0+0.5d0*(1d0/omrsq**2-8d0/omrsq+7d0)*rlog
     & +0.5d0/omrsq+2d0*ddilog(omrsq)-5d0*pisqo6
     & -5d0*wlog+2d0*wlog**2+eta/2d0
     & -2d0*dlog(aff)**2-(3.5d0-4d0*aff+aff**2/2d0)*dlog(aff)
     & +2d0*(1d0-aff)*rsq/omrsq*dlog(rsq/(1d0-aff+rsq*aff))
      else
      ct=0d0
      endif

C---- this routine has been constructed from
C---- %\cite{Gottschalk:1980rv}
C---- \bibitem{Gottschalk:1980rv}
C---- T.~Gottschalk,
C---- %``Chromodynamic Corrections To Neutrino Production Of Heavy Quarks,''
C---- Phys.\ Rev.\ D {\bf 23}, 56 (1981).
C---- %%CITATION = PHRVA,D23,56;%%
C----- Adapted from Eqs.(A8,A9)

      Kfun=1d0/rsq*wlog
      c1=2d0*Kfun
      cv=-(epinv2*epinv+epinv*mulog+0.5d0*mulog**2)
     & -(epinv+mulog)*(2.5d0-2d0*wlog)
     & -0.5d0*(11d0+eta)-pisqo6-2d0*ddilog(rsq)
     &  +3d0*wlog-2d0*wlog**2-Kfun

      return
      end


      SUBROUTINE SG03BW( TRANS, M, N, A, LDA, C, LDC, E, LDE, D, LDD, X,
     $                   LDX, SCALE, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve for X the generalized Sylvester equation
C
C         T            T
C        A  * X * C + E  * X * D  =  SCALE * Y,                      (1)
C
C     or the transposed equation
C
C                 T            T
C        A * X * C  + E * X * D   =  SCALE * Y,                      (2)
C
C     where A and E are real M-by-M matrices, C and D are real N-by-N
C     matrices, X and Y are real M-by-N matrices. N is either 1 or 2.
C     The pencil A - lambda * E must be in generalized real Schur form
C     (A upper quasitriangular, E upper triangular). SCALE is an output
C     scale factor, set to avoid overflow in X.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     TRANS   CHARACTER*1
C             Specifies whether the transposed equation is to be solved
C             or not:
C             = 'N':  Solve equation (1);
C             = 'T':  Solve equation (2).
C
C     Input/Output Parameters
C
C     M       (input) INTEGER
C             The order of the matrices A and E.  M >= 0.
C
C     N       (input) INTEGER
C             The order of the matrices C and D.  N = 1 or N = 2.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,M)
C             The leading M-by-M part of this array must contain the
C             upper quasitriangular matrix A. The elements below the
C             upper Hessenberg part are not referenced.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,M).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading N-by-N part of this array must contain the
C             matrix C.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= MAX(1,N).
C
C     E       (input) DOUBLE PRECISION array, dimension (LDE,M)
C             The leading M-by-M part of this array must contain the
C             upper triangular matrix E. The elements below the main
C             diagonal are not referenced.
C
C     LDE     INTEGER
C             The leading dimension of the array E.  LDE >= MAX(1,M).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,N)
C             The leading N-by-N part of this array must contain the
C             matrix D.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= MAX(1,N).
C
C     X       (input/output) DOUBLE PRECISION array, dimension (LDX,N)
C             On entry, the leading M-by-N part of this array must
C             contain the right hand side matrix Y.
C             On exit, the leading M-by-N part of this array contains
C             the solution matrix X.
C
C     LDX     INTEGER
C             The leading dimension of the array X.  LDX >= MAX(1,M).
C
C     SCALE   (output) DOUBLE PRECISION
C             The scale factor set to avoid overflow in X.
C             0 < SCALE <= 1.
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 generalized Sylvester equation is (nearly)
C                   singular to working precision;  perturbed values
C                   were used to solve the equation (but the matrices
C                   A, C, D, and E are unchanged).
C
C     METHOD
C
C     The method used by the routine is based on a generalization of the
C     algorithm due to Bartels and Stewart [1]. See also [2] and [3] for
C     details.
C
C     REFERENCES
C
C     [1] Bartels, R.H., Stewart, G.W.
C         Solution of the equation A X + X B = C.
C         Comm. A.C.M., 15, pp. 820-826, 1972.
C
C     [2] Gardiner, J.D., Laub, A.J., Amato, J.J., Moler, C.B.
C         Solution of the Sylvester Matrix Equation
C         A X B**T + C X D**T = E.
C         A.C.M. Trans. Math. Soft., vol. 18, no. 2, pp. 223-231, 1992.
C
C     [3] Penzl, T.
C         Numerical solution of generalized Lyapunov equations.
C         Advances in Comp. Math., vol. 8, pp. 33-48, 1998.
C
C     NUMERICAL ASPECTS
C
C     The routine requires about 2 * N * M**2 flops. Note that we count
C     a single floating point arithmetic operation as one flop.
C
C     The algorithm is backward stable if the eigenvalues of the pencil
C     A - lambda * E are real. Otherwise, linear systems of order at
C     most 4 are involved into the computation. These systems are solved
C     by Gauss elimination with complete pivoting. The loss of stability
C     of the Gauss elimination with complete pivoting is rarely
C     encountered in practice.
C
C     FURTHER COMMENTS
C
C     When near singularity is detected, perturbed values are used
C     to solve the equation (but the given matrices are unchanged).
C
C     CONTRIBUTOR
C
C     T. Penzl, Technical University Chemnitz, Germany, Aug. 1998.
C
C     REVISIONS
C
C     Sep. 1998 (V. Sima).
C
C     KEYWORDS
C
C     Lyapunov equation
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  MONE, ONE, ZERO
      PARAMETER         ( MONE = -1.0D+0, ONE = 1.0D+0, ZERO = 0.0D+0 )
C     .. Scalar Arguments ..
      CHARACTER         TRANS
      DOUBLE PRECISION  SCALE
      INTEGER           INFO, LDA, LDC, LDD, LDE, LDX, M, N
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), C(LDC,*), D(LDD,*), E(LDE,*), X(LDX,*)
C     .. Local Scalars ..
      DOUBLE PRECISION  SCALE1
      INTEGER           DIMMAT, I, INFO1, J, MA, MAI, MAJ, MB, ME
      LOGICAL           NOTRNS
C     .. Local Arrays ..
      DOUBLE PRECISION  MAT(4,4), RHS(4), TM(2,2)
      INTEGER           PIV1(4), PIV2(4)
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DGEMM, DSCAL, MB02UU, MB02UV, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX
C
C     Decode input parameters.
C
      NOTRNS = LSAME( TRANS, 'N' )
C
C     Check the scalar input parameters.
C
      IF ( .NOT.( NOTRNS .OR. LSAME( TRANS, 'T' ) ) ) THEN
         INFO = -1
      ELSEIF ( M .LT. 0 ) THEN
         INFO = -2
      ELSEIF ( N .NE. 1 .AND. N .NE. 2 ) THEN
         INFO = -3
      ELSEIF ( LDA .LT. MAX( 1, M ) ) THEN
         INFO = -5
      ELSEIF ( LDC .LT. MAX( 1, N ) ) THEN
         INFO = -7
      ELSEIF ( LDE .LT. MAX( 1, M ) ) THEN
         INFO = -9
      ELSEIF ( LDD .LT. MAX( 1, N ) ) THEN
         INFO = -11
      ELSEIF ( LDX .LT. MAX( 1, M ) ) THEN
         INFO = -13
      ELSE
         INFO = 0
      END IF
      IF ( INFO .NE. 0 ) THEN
         CALL XERBLA( 'SG03BW', -INFO )
         RETURN
      END IF
C
      SCALE = ONE
C
C     Quick return if possible.
C
      IF ( M .EQ. 0 )
     $    RETURN
C
      IF ( NOTRNS ) THEN
C
C        Solve equation (1).
C
C        Compute block row X(MA:ME,:). MB denotes the number of rows in
C        this block row.
C
         ME = 0
C        WHILE ( ME .NE. M ) DO
   20    IF ( ME .NE. M ) THEN
            MA = ME + 1
            IF ( MA .EQ. M ) THEN
               ME = M
               MB = 1
            ELSE
               IF ( A(MA+1,MA) .EQ. ZERO ) THEN
                  ME = MA
                  MB = 1
               ELSE
                  ME = MA + 1
                  MB = 2
               END IF
            END IF
C
C           Assemble Kronecker product system of linear equations with
C           matrix
C
C              MAT = kron(C',A(MA:ME,MA:ME)') + kron(D',E(MA:ME,MA:ME)')
C
C           and right hand side
C
C              RHS = vec(X(MA:ME,:)).
C
            IF ( N .EQ. 1 ) THEN
               DIMMAT = MB
               DO 60 I = 1, MB
                  MAI = MA + I - 1
                  DO 40 J = 1, MB
                     MAJ = MA + J - 1
                     MAT(I,J) = C(1,1)*A(MAJ,MAI)
                     IF ( MAJ .LE. MAI )
     $                  MAT(I,J) = MAT(I,J) + D(1,1)*E(MAJ,MAI)
   40             CONTINUE
                  RHS(I) = X(MAI,1)
   60          CONTINUE
            ELSE
               DIMMAT = 2*MB
               DO 100 I = 1, MB
                  MAI = MA + I - 1
                  DO 80 J = 1, MB
                     MAJ = MA + J - 1
                     MAT(I,J) = C(1,1)*A(MAJ,MAI)
                     MAT(MB+I,J) = C(1,2)*A(MAJ,MAI)
                     MAT(I,MB+J) = C(2,1)*A(MAJ,MAI)
                     MAT(MB+I,MB+J) = C(2,2)*A(MAJ,MAI)
                     IF ( MAJ .LE. MAI ) THEN
                        MAT(I,J) = MAT(I,J) + D(1,1)*E(MAJ,MAI)
                        MAT(MB+I,J) = MAT(MB+I,J) + D(1,2)*E(MAJ,MAI)
                        MAT(I,MB+J) = MAT(I,MB+J) + D(2,1)*E(MAJ,MAI)
                        MAT(MB+I,MB+J) = MAT(MB+I,MB+J) +
     $                                   D(2,2)*E(MAJ,MAI)
                     END IF
   80             CONTINUE
                  RHS(I) = X(MAI,1)
                  RHS(MB+I) = X(MAI,2)
  100          CONTINUE
            END IF
C
C           Solve the system of linear equations.
C
            CALL MB02UV( DIMMAT, MAT, 4, PIV1, PIV2, INFO1 )
            IF ( INFO1 .NE. 0 )
     $         INFO = 1
            CALL MB02UU( DIMMAT, MAT, 4, RHS, PIV1, PIV2, SCALE1 )
            IF ( SCALE1 .NE. ONE ) THEN
               SCALE = SCALE1*SCALE
               DO 120 I = 1, N
                  CALL DSCAL( M, SCALE1, X(1,I), 1 )
  120          CONTINUE
            END IF
C
            IF ( N .EQ. 1 ) THEN
               DO 140 I = 1, MB
                  MAI = MA + I - 1
                  X(MAI,1) = RHS(I)
  140          CONTINUE
            ELSE
               DO 160 I = 1, MB
                  MAI = MA + I - 1
                  X(MAI,1) = RHS(I)
                  X(MAI,2) = RHS(MB+I)
  160          CONTINUE
            END IF
C
C           Update right hand sides.
C
C           X(ME+1:M,:) = X(ME+1:M,:) - A(MA:ME,ME+1:M)'*X(MA:ME,:)*C
C
C           X(ME+1:M,:) = X(ME+1:M,:) - E(MA:ME,ME+1:M)'*X(MA:ME,:)*D
C
            IF ( ME .LT. M ) THEN
               CALL DGEMM( 'N', 'N', MB, N, N, ONE, X(MA,1), LDX, C,
     $                     LDC, ZERO, TM, 2 )
               CALL DGEMM( 'T', 'N', M-ME, N, MB, MONE, A(MA,ME+1),
     $                     LDA, TM, 2, ONE, X(ME+1,1), LDX )
               CALL DGEMM( 'N', 'N', MB, N, N, ONE, X(MA,1), LDX, D,
     $                     LDD, ZERO, TM, 2 )
               CALL DGEMM( 'T', 'N', M-ME, N, MB, MONE, E(MA,ME+1), LDE,
     $                     TM, 2, ONE, X(ME+1,1), LDX )
            END IF
C
         GOTO 20
         END IF
C        END WHILE 20
C
      ELSE
C
C        Solve equation (2).
C
C        Compute block row X(MA:ME,:). MB denotes the number of rows in
C        this block row.
C
         MA = M + 1
C        WHILE ( MA .NE. 1 ) DO
  180    IF ( MA .NE. 1 ) THEN
            ME = MA - 1
            IF ( ME .EQ. 1 ) THEN
               MA = 1
               MB = 1
            ELSE
               IF ( A(ME,ME-1) .EQ. ZERO ) THEN
                  MA = ME
                  MB = 1
               ELSE
                  MA = ME - 1
                  MB = 2
               END IF
            END IF
C
C           Assemble Kronecker product system of linear equations with
C           matrix
C
C              MAT = kron(C,A(MA:ME,MA:ME)) + kron(D,E(MA:ME,MA:ME))
C
C           and right hand side
C
C              RHS = vec(X(MA:ME,:)).
C
            IF ( N .EQ. 1 ) THEN
               DIMMAT = MB
               DO 220 I = 1, MB
                  MAI = MA + I - 1
                  DO 200 J = 1, MB
                     MAJ = MA + J - 1
                     MAT(I,J) = C(1,1)*A(MAI,MAJ)
                     IF ( MAJ .GE. MAI )
     $                  MAT(I,J) = MAT(I,J) + D(1,1)*E(MAI,MAJ)
  200             CONTINUE
                  RHS(I) = X(MAI,1)
  220          CONTINUE
            ELSE
               DIMMAT = 2*MB
               DO 260 I = 1, MB
                  MAI = MA + I - 1
                  DO 240 J = 1, MB
                     MAJ = MA + J - 1
                     MAT(I,J) = C(1,1)*A(MAI,MAJ)
                     MAT(MB+I,J) = C(2,1)*A(MAI,MAJ)
                     MAT(I,MB+J) = C(1,2)*A(MAI,MAJ)
                     MAT(MB+I,MB+J) = C(2,2)*A(MAI,MAJ)
                     IF ( MAJ .GE. MAI ) THEN
                        MAT(I,J) = MAT(I,J) + D(1,1)*E(MAI,MAJ)
                        MAT(MB+I,J) = MAT(MB+I,J) + D(2,1)*E(MAI,MAJ)
                        MAT(I,MB+J) = MAT(I,MB+J) + D(1,2)*E(MAI,MAJ)
                        MAT(MB+I,MB+J) = MAT(MB+I,MB+J) +
     $                                   D(2,2)*E(MAI,MAJ)
                     END IF
  240             CONTINUE
                  RHS(I) = X(MAI,1)
                  RHS(MB+I) = X(MAI,2)
  260          CONTINUE
            END IF
C
C           Solve the system of linear equations.
C
            CALL MB02UV( DIMMAT, MAT, 4, PIV1, PIV2, INFO1 )
            IF ( INFO1 .NE. 0 )
     $         INFO = 1
            CALL MB02UU( DIMMAT, MAT, 4, RHS, PIV1, PIV2, SCALE1 )
            IF ( SCALE1 .NE. ONE ) THEN
               SCALE = SCALE1*SCALE
               DO 280 I = 1, N
                  CALL DSCAL( M, SCALE1, X(1,I), 1 )
  280          CONTINUE
            END IF
C
            IF ( N .EQ. 1 ) THEN
               DO 300 I = 1, MB
                  MAI = MA + I - 1
                  X(MAI,1) = RHS(I)
  300          CONTINUE
            ELSE
               DO 320 I = 1, MB
                  MAI = MA + I - 1
                  X(MAI,1) = RHS(I)
                  X(MAI,2) = RHS(MB+I)
  320          CONTINUE
            END IF
C
C           Update right hand sides.
C
C              X(1:MA-1,:) = X(1:MA-1,:) - A(1:MA-1,MA:ME)*X(MA:ME,:)*C'
C
C              X(1:MA-1,:) = X(1:MA-1,:) - E(1:MA-1,MA:ME)*X(MA:ME,:)*D'
C
            IF ( MA .GT. 1 ) THEN
               CALL DGEMM( 'N', 'T', MB, N, N, ONE, X(MA,1), LDX, C,
     $                     LDC, ZERO, TM, 2 )
               CALL DGEMM( 'N', 'N', MA-1, N, MB, MONE, A(1,MA), LDA,
     $                     TM, 2, ONE, X, LDX )
               CALL DGEMM( 'N', 'T', MB, N, N, ONE, X(MA,1), LDX, D,
     $                     LDD, ZERO, TM, 2 )
               CALL DGEMM( 'N', 'N', MA-1, N, MB, MONE, E(1,MA), LDE,
     $                     TM, 2, ONE, X, LDX )
            END IF
C
         GOTO 180
         END IF
C        END WHILE 180
C
      END IF
C
      RETURN
C *** Last line of SG03BW ***
      END


! This file is a part of JuliaFEM.
! License is MIT: see https://github.com/JuliaFEM/UMAT.jl/blob/master/LICENSE
!***********************************************************************
! File drucker_prager_plasticity_deriv.f90
!
!   Subroutine DF_Derivatives
!   Subroutine DFDSIGMA
!   Subroutine DEVSTRESS
!   Function   YIELDF
!   Function   J2INVARIANT
!   Function   HARDENING_K
!   Function   HARDENING_K_DERIV
!   Function   DPHIDF
!   subroutine invert
!   Subroutine WriteOut
!
!***********************************************************************
!=======================================================================
! Subroutine DF_Derivatives
!
! to calculate Derivates of Yield function F
!-----------------------------------------------------------------------
!
! Input parameters
! ----------------
! M           = number of stress components
! STRESS      = stress vector
! DEV_STRESS  = deviatoric stress vector
! PARAM       = some material parameters
! HIDDEN      = hidden variables
! IFLAG       = flag for yield function type
!             = 'von_Mises'
!             = 'Drucker-Prager_Overstress_A'
!             = 'Drucker-Prager_Consistency_A'
! HFLAG       = flag for Hardening function type
!     1       = Linear
!     2       = Exponential
! LambdaDot   = Lambda/DT
!
! Output parameters
! ------------------
!   DF_DLambda
!   DF_DLambdaDot
!   DDF_DDSigma
!   DDF_DSigmaDLambda
!   DDF_DSigmaDLambdaDot
!   ALPHA
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Subroutine DF_Derivatives(M, IFLAG,HFLAG,STRESS,DEV_STRESS,PARAM,      &
                          HIDDEN,LambdaDot,DF_DLambda,DF_DLambdaDot,   &
                          DDF_DDSigma,DDF_DSigmaDLambda,               &
                          DDF_DSigmaDLambdaDot,ALPHA)
  Implicit None
  Integer, Parameter      :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)     :: M,HFLAG
  Character(*), Intent(in):: IFLAG
  Real(dp), Intent(in)    :: STRESS(M),DEV_STRESS(M),PARAM(*),HIDDEN(*),&
                             LambdaDot
  Real(dp), Intent(out)   :: DF_DLambda,DF_DLambdaDot,DDF_DDSigma(M,M),  &
                             DDF_DSigmaDLambda(M),DDF_DSigmaDLambdaDot(M),&
                             ALPHA
!- local variables -----------------------------------------------------
  Integer :: II,JJ
  Real(dp) :: nvec(M),dum,SLEN,J2INVARIANT,DHardeningDeffStrain,&
              Sigma_t,Sigma_C,BEETA,N_Power, A_Const, B_Const,ETA

  IF (M /= 6) Then
    Write (*,*) ' *** Error **** in Subroutine DF_Derivatives, 6 Stress components needed'
    Call XIT
  Endif

  If(INDEX(IFLAG,'von_Mises') /= 0) Then

    SLEN    = SQRT(2.0_dp * J2INVARIANT(M, DEV_STRESS) )
    DF_DLambda = DHardeningDeffStrain(PARAM, HIDDEN, HFLAG)
    DF_DLambdaDot        = 0.0_dp
    DDF_DSigmaDLambda    = 0.0_dp
    DDF_DSigmaDLambdaDot = 0.0_dp

  Elseif (INDEX(IFLAG,'Drucker-Prager_Overstress_A') /= 0 ) Then

    SIGMA_T = PARAM(1)
    SIGMA_C = PARAM(2)
    ALPHA   = (SIGMA_C - SIGMA_T) / (SIGMA_C + SIGMA_T)
    BEETA   = (1.0_dp + ALPHA) * SIGMA_T
    SLEN    = SQRT(2.0_dp * J2INVARIANT(M, DEV_STRESS) )

    DF_DLambda = DHardeningDeffStrain(PARAM, HIDDEN, HFLAG)*(1.0_dp+Alpha)
    DF_DLambdaDot        = 0.0_dp
    DDF_DSigmaDLambda    = 0.0_dp
    DDF_DSigmaDLambdaDot = 0.0_dp

  Elseif (INDEX(IFLAG,'Drucker-Prager_Consistency_A') /= 0    ) Then

    SIGMA_T = PARAM(1)
    SIGMA_C = PARAM(2)
    ETA     = PARAM(6)
    N_Power = PARAM(5)
    A_Const = PARAM(7)
    B_Const = PARAM(8)
    ALPHA   = (SIGMA_C - SIGMA_T) / (SIGMA_C + SIGMA_T)
    BEETA   = (1.0_dp + ALPHA) * SIGMA_T
    SLEN    = SQRT(2.0_dp * J2INVARIANT(M, DEV_STRESS) )

    DF_DLambda = DHardeningDeffStrain(PARAM, HIDDEN, HFLAG)*(1.0_dp+Alpha)
    DF_DLambdaDot = -A_Const/ETA/N_Power*(LambdaDot/ETA)**(1.0_dp/N_Power-1.0_dp)
    DDF_DSigmaDLambda    = 0.0_dp
    DDF_DSigmaDLambdaDot = 0.0_dp

  Endif

  If(INDEX(IFLAG,'von_Mises') /= 0                   .or. &
     INDEX(IFLAG,'Drucker-Prager_Overstress_A') /= 0 .or. &
     INDEX(IFLAG,'Drucker-Prager_Consistency_A') /= 0    ) Then

    nvec(1) = DEV_STRESS(1)/SLEN
    nvec(2) = DEV_STRESS(2)/SLEN
    nvec(3) = DEV_STRESS(3)/SLEN
    nvec(4) = 2.0_dp*DEV_STRESS(4)/SLEN
    nvec(5) = 2.0_dp*DEV_STRESS(5)/SLEN
    nvec(6) = 2.0_dp*DEV_STRESS(6)/SLEN
    dum = SQRT(1.5)/SLEN
    DO JJ=1,M
      DO II = 1,M
        DDF_DDSigma(II,JJ) = -dum*nvec(II)*nvec(JJ)
      Enddo
    Enddo
    dum = SQRT(1.5)/SLEN/3.0_dp
    DDF_DDSigma(1,1) = DDF_DDSigma(1,1) + dum * 2.0_dp
    DDF_DDSigma(2,2) = DDF_DDSigma(2,2) + dum * 2.0_dp
    DDF_DDSigma(3,3) = DDF_DDSigma(3,3) + dum * 2.0_dp
    DDF_DDSigma(4,4) = DDF_DDSigma(4,4) + dum * 6.0_dp
    DDF_DDSigma(5,5) = DDF_DDSigma(5,5) + dum * 6.0_dp
    DDF_DDSigma(6,6) = DDF_DDSigma(6,6) + dum * 6.0_dp

    dum = SQRT(1.5)/SLEN/3.0_dp
    DDF_DDSigma(1,2) = DDF_DDSigma(1,2) - dum
    DDF_DDSigma(1,3) = DDF_DDSigma(1,3) - dum
    DDF_DDSigma(2,1) = DDF_DDSigma(2,1) - dum
    DDF_DDSigma(2,3) = DDF_DDSigma(2,3) - dum
    DDF_DDSigma(3,1) = DDF_DDSigma(3,1) - dum
    DDF_DDSigma(3,2) = DDF_DDSigma(3,2) - dum

  Endif
!
End Subroutine DF_Derivatives
!
!***********************************************************************
!=======================================================================
! Subroutine DFDSIGMA
!
! to evaluate plastic potential derivate over stress
!-----------------------------------------------------------------------
! input parameters
! ----------------
! M           = number of stress components
! STRESS      = stress vector
! DEV_STRESS  = deviatoric stress vector
! PARAM       = some material parameters
! HIDDEN      = hidden variables
! IFLAG       = flag for yield function type
!             = 'von_Mises'
!             = 'Drucker-Prager_Overstress_A'
!             = 'Drucker-Prager_Consistency_A'
! HFLAG  = flag for Hardening function type
!     1  = Linear
!     2  = Exponential
! Output parameters
!
! ------------------
! DF_DSIGMA(M)
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Subroutine DFDSIGMA(M, STRESS, DEV_STRESS, PARAM, HIDDEN, &
                  IFLAG,HFLAG,DF_DSIGMA)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: M,HFLAG
  Character(*), Intent(in)  :: IFLAG
  Real(dp), Intent(in)      :: STRESS(M), DEV_STRESS(M), PARAM(*), &
                               HIDDEN(*)
  Real(dp), Intent(out)     :: DF_DSIGMA(M)
!- local variables -----------------------------------------------------
  Real(dp) :: SIGMA_T,SIGMA_C,ALPHA,BEETA,dum,HARDENING_K,&
              SM,K,SLEN,J2INVARIANT
!
  IF (M /= 6) Then
    Write (*,*) ' *** Error **** in Function DFDSIGMA, 6 Stress components needed'
    Call XIT
  Endif

  SM    = (STRESS(1) + STRESS(2) + STRESS(3)) / 3.0_dp

  If(INDEX(IFLAG,'von_Mises') /= 0) Then

    K = HARDENING_K(PARAM, HIDDEN, HFLAG)
    SLEN    = SQRT(2.0_dp * J2INVARIANT(M, DEV_STRESS) )
    dum     = SQRT(1.5_dp)/SLEN
    DF_DSIGMA(1) = dum*DEV_STRESS(1)
    DF_DSIGMA(2) = dum*DEV_STRESS(2)
    DF_DSIGMA(3) = dum*DEV_STRESS(3)
    DF_DSIGMA(4) = dum*DEV_STRESS(4)  * 2.0_dp
    DF_DSIGMA(5) = dum*DEV_STRESS(5)  * 2.0_dp
    DF_DSIGMA(6) = dum*DEV_STRESS(6)  * 2.0_dp

  Elseif (INDEX(IFLAG,'Drucker-Prager_Overstress_A') /= 0 .or. &
          INDEX(IFLAG,'Drucker-Prager_Consistency_A') /= 0    ) Then

    SIGMA_T = PARAM(1)
    SIGMA_C = PARAM(2)
    !C_const = PARAM(4)
    !K_Zero  = PARAM(3)
    !N_Power = PARAM(5)
    !A_Const = PARAM(7)
    !B_Const = PARAM(8)
    K = HARDENING_K(PARAM, HIDDEN, HFLAG)
    ALPHA   = (SIGMA_C - SIGMA_T) / (SIGMA_C + SIGMA_T)
    BEETA   = (1.0_dp + ALPHA) * SIGMA_T
    SLEN    = SQRT(2.0_dp * J2INVARIANT(M, DEV_STRESS) )
    dum     = SQRT(1.5_dp)/SLEN
    DF_DSIGMA(1) = dum*DEV_STRESS(1)  + ALPHA
    DF_DSIGMA(2) = dum*DEV_STRESS(2)  + ALPHA
    DF_DSIGMA(3) = dum*DEV_STRESS(3)  + ALPHA
    DF_DSIGMA(4) = dum*DEV_STRESS(4)  * 2.0_dp
    DF_DSIGMA(5) = dum*DEV_STRESS(5)  * 2.0_dp
    DF_DSIGMA(6) = dum*DEV_STRESS(6)  * 2.0_dp

  End If

End Subroutine DFDSIGMA

!***********************************************************************
!=======================================================================
! Subroutine DEVSTRESS
!
! to calculate deviatoric stress vector
!-----------------------------------------------------------------------
!
! Input parameters
! ----------------
!   M      = number of stress components
!   STRESS = stress vector
!
! Output parameters
! ------------------
!   DEV_STRESS
!   SM          = Mean Stress
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Subroutine DEVSTRESS(M, STRESS,SM,DEV_STRESS)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: M
  Real(dp), Intent(in)      :: STRESS(M)
  Real(dp), Intent(out)     :: SM,DEV_STRESS(M)
!- local variables -----------------------------------------------------

  IF (M /= 6) Then
    Write (*,*) ' *** Error **** in Subroutine DEVSTRESS, 6 Stress components needed'
    Call XIT
  Endif

  SM    = (STRESS(1) + STRESS(2) + STRESS(3)) / 3.0_dp
  DEV_STRESS(1) = STRESS(1) - SM
  DEV_STRESS(2) = STRESS(2) - SM
  DEV_STRESS(3) = STRESS(3) - SM
  DEV_STRESS(4) = STRESS(4)
  DEV_STRESS(5) = STRESS(5)
  DEV_STRESS(6) = STRESS(6)
!
End Subroutine DEVSTRESS
!
!***********************************************************************
!=======================================================================
! function YIELDF
!
! to evaluate the value of yield function
!-----------------------------------------------------------------------
! input parameters
! ----------------
! M      = number of stress components
! STRESS = stress vector
! PARAM  = some material parameters
! HIDDEN = hidden variables
! HFLAG       = flag for Hardening function type
!     1       = Linear
!     2       = Exponential
! IFLAG  = flag for yield function type
!        = 'von_Mises'
!        = 'Drucker-Prager_Overstress_A'
!        = 'Drucker-Prager_Consistency_A'
!-----------------------------------------------------------------------
! Originally coded RK (2001)
! last modified 19/03/2003 KKO
!=======================================================================
!
Function YIELDF(M, STRESS, PARAM, HIDDEN, IFLAG,HFLAG) Result(YIELD)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: M,HFLAG
  Character(*), Intent(in)  :: IFLAG
  Real(dp), Intent(in)      :: STRESS(M), PARAM(*), HIDDEN(*)
!- local variables -----------------------------------------------------
  Real(dp) :: SM,SX,SY,SZ,SXY,SYZ,SXZ,YLD2,YLD,YIELD,SIGMA_T,&
              SIGMA_C,ALPHA,BEETA,RJ2,K,K_Zero,C_const,HARDENING_K
!
  IF (M /= 6) Then
    Write (*,*) ' *** Error **** in Function YIELDF, 6 Stress components needed'
    Call XIT
  Endif

  SM    = (STRESS(1) + STRESS(2) + STRESS(3)) / 3.0_dp
  SX    = STRESS(1) - SM
  SY    = STRESS(2) - SM
  SZ    = STRESS(3) - SM
  SXY   = STRESS(4)
  SYZ   = STRESS(5)
  SXZ   = STRESS(6)

  If(INDEX(IFLAG,'von_Mises') /= 0) Then
    YLD   = PARAM(1)
    YLD2  = YLD*YLD
    YIELD = 1.5_dp*(SX*SX+SY*SY+SZ*SZ+2.0_dp*(SXY*SXY+SYZ*SYZ+SXZ*SXZ))-YLD2

  Elseif (INDEX(IFLAG,'Drucker-Prager_Overstress_A') /= 0 .or. &
          INDEX(IFLAG,'Drucker-Prager_Consistency_A') /= 0) Then
    SIGMA_T = PARAM(1)
    SIGMA_C = PARAM(2)
    K_Zero  = PARAM(3)
    C_const = PARAM(4)
    !N_Power = PARAM(5)
    !A_Const = PARAM(7)
    !B_Const = PARAM(8)
    K = HARDENING_K(PARAM, HIDDEN, HFLAG)
    ALPHA   = (SIGMA_C - SIGMA_T) / (SIGMA_C + SIGMA_T)
    BEETA    = (1.0_dp + ALPHA) * SIGMA_T
    RJ2     = 0.5*(SX*SX+SY*SY+SZ*SZ)+SXY*SXY+SXZ*SXZ+SYZ*SYZ
    YIELD  = Sqrt(3.0_dp*RJ2) + ALPHA*3.0_dp*SM - BEETA - K
  End If
End Function YIELDF
!
!***********************************************************************
!=======================================================================
! function J2INVARIANT
!
! to evaluate Deviatoric stress 2nd invariant
!-----------------------------------------------------------------------
! input parameters
! ----------------
! M           = number of stress components
! DEV_STRESS  = deviatoric stress vector
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Function J2INVARIANT(M, DEV_STRESS) Result(J2Inv)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: M
  Real(dp), Intent(in)      :: DEV_STRESS(M)
!- local variables -----------------------------------------------------
  Real(dp) :: J2Inv
!
  IF (M /= 6) Then
    Write (*,*) ' *** Error **** in Function J_INVARIANTS, 6 Stress components needed'
    Call XIT
  Endif

    J2Inv = 0.5_dp*(Dev_Stress(1)**2+Dev_Stress(2)**2+Dev_Stress(3)**2)&
                 +  Dev_Stress(4)**2+Dev_Stress(5)**2+Dev_Stress(6)**2

End Function J2INVARIANT
!
!***********************************************************************
!=======================================================================
! function HARDENING_K
!
! to evaluate Hardening parameter K
!-----------------------------------------------------------------------
! input parameters
! ----------------
! PARAM  = some material parameters
! HIDDEN = hidden variables
! HFLAG  = flag for Hardening function type
!     1  = Linear
!     2  = Exponential
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Function HARDENING_K(PARAM, HIDDEN, HFLAG)  Result(K)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: HFLAG
  Real(dp), Intent(in)      :: PARAM(*),HIDDEN(*)
!- local variables -----------------------------------------------------
  Real(dp) :: K
!

  If(HFLAG == 1) Then
    K = -PARAM(3) *HIDDEN(1)
  Elseif(HFLAG == 2) Then
    K = -PARAM(3) *(1.0_dp-EXP(-PARAM(4)*HIDDEN(1)))
  Endif

End Function HARDENING_K
!
!***********************************************************************
!=======================================================================
! function DHardeningDeffStrain
!
! to evaluate derivatives of parameter K
!-----------------------------------------------------------------------
! input parameters
! ----------------
! PARAM  = some material parameters
! HIDDEN = hidden variables
! HFLAG  = flag for Hardening function type
!     1  = Linear
!     2  = Exponential
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Function DHardeningDeffStrain(PARAM, HIDDEN, HFLAG)  Result(DK)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Integer, Intent(in)       :: HFLAG
  Real(dp), Intent(in)      :: PARAM(*),HIDDEN(*)
!- local variables -----------------------------------------------------
  Real(dp) :: DK
!

  If(HFLAG == 1) Then
    DK = -PARAM(3)
  Elseif(HFLAG == 2) Then
    DK = -PARAM(4)*PARAM(3) *EXP(-PARAM(4)*HIDDEN(1))
  Endif

End Function DHardeningDeffStrain
!
!***********************************************************************
!=======================================================================
! function DPHIDF
!
! to evaluate derivative of the function Phi inside the Heaviside step
! function in Perzyna's viscoplastic modell
!-----------------------------------------------------------------------
! input parameters
! ----------------
!
! PARAM  = some material parameters
! STRESS = stress vector
! M      = number of stress components
! HIDDEN = hidden variables
! HFLAG  = flag for Hardening function type
!     1  = Linear
!     2  = Exponential
! IFLAG  = flag for yield function type
!        = 'von_Mises'
!        = 'Drucker-Prager_Overstress_A'
!        = 'Drucker-Prager_Consistency_A'
!-----------------------------------------------------------------------
! Originally coded 19/03/2003 KKO
! last modified
!=======================================================================
!
Function DPHIDF(F,PARAM,STRESS,M, HIDDEN, IFLAG,HFLAG)  Result(DPhi_Df)
  Implicit None
  Integer, Parameter        :: dp = Selected_real_kind(12,50)
  Character(*), Intent(in)  :: IFLAG
  Integer , Intent(in)      :: M,HFLAG
  Real(dp), Intent(in)      :: PARAM(*),HIDDEN(*),STRESS(M)
!- local variables -----------------------------------------------------
  Real(dp) :: DPhi_Df,N_Power, A_Const, B_Const,F,YIELDF
!

  If(INDEX(IFLAG,'von_Mises') /= 0) Then

    DPhi_Df = 0.0_dp ! ei m��ritelty

  Elseif (INDEX(IFLAG,'Drucker-Prager_Overstress_A') /= 0 ) Then

    N_Power = PARAM(5)
    A_Const = PARAM(7)
    B_Const = PARAM(8)
    F = YIELDF(M, STRESS, PARAM, HIDDEN, IFLAG,HFLAG)
    DPhi_Df = N_Power / A_Const *(F/A_Const-B_Const)**(N_Power-1.0_dp)

  Elseif (INDEX(IFLAG,'Drucker-Prager_Consistency_A') /= 0    ) Then
  Endif
End Function DPHIDF
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
!c
!c    routine to invert a matrix by Gaussian elimination
!c     Ainv=inverse(A)    A(n,n)   Ainv(n,n)
!c           D matrix is extended.
!ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
       subroutine invert( A, Ainv, n )

       Implicit none
       Integer, Parameter :: dp = Selected_real_kind(12,50)
       Integer :: n
       Real(dp), Intent(in) :: A(n,n)
       Real(dp), Intent(out) :: Ainv(n,n)

       Integer :: n2,i,j,k
       Real(dp) :: D(n,2*n),alpha,beta

!  initialize the reduction matrix
       n2 = 2*n
       do 1 i = 1,n
        do 2 j = 1,n
         D(i,j) = A(i,j)
         d(i,n+j) = 0.
 2      continue
        D(i,n+i) = 1.
 1     continue

!  do the reduction
       do 3 i = 1,n
        alpha = D(i,i)
        if(alpha .eq. 0.) go to 300
        do 4 j = 1,n2
         D(i,j) = D(i,j)/alpha
 4      continue
        do 5 k = 1,n
         if((k-i).eq.0) go to 5
         beta = D(k,i)
         do 6 j = 1,n2
          D(k,j) = D(k,j) - beta*D(i,j)
 6       continue
 5      continue
 3     continue

!  copy result into output matrix
       do 7 i = 1,n
        do 8 j = 1,n
         Ainv(i,j) = D(i,j+n)
 8      continue
 7     continue

       return
 300   print *,'*** ERROR: Singular matrix ***'
       return
       end
!  ============================================================================================
!
!   Write to dat,log and msg-files
!
!  ============================================================================================
!
  SUBROUTINE WriteOut(string,form,IND,KSTEP,KINC,NOEL,NPT)

  character*(*), Intent(in) :: string,form
  Integer, Intent(in) :: IND,KSTEP,KINC,NOEL,NPT
  Integer, Parameter :: DATF=6,LOGF=5,MSGF=7

  !Form = '(/,'''//Trim(Task)//LCHR(1:JL)//''','//XCHR(1:XL)//'X'
  !Form = '('''//Trim(Task)//LCHR(1:JL)//''','//XCHR(1:XL)//'X'

  If(KSTEP > 0 ) Then

    If(IND == 1 .or. IND == 4) &
    Write (DATF,'(/,A,I2,A,I5,A,I6,A,I2)') &
      ' Step =',KSTEP,' Increment =',KINC,' Element =',NOEL,' Integration point =',NPT
!    If(IND == 2 .or. IND == 4) &
!    Write (LOGF,'(/,A,I2,A,I5,A,I6,A,I2)') &
!      ' Step =',KSTEP,' Increment =',KINC,' Element =',NOEL,' Integration point =',NPT
    If(IND == 2 .or. IND == 4) &
    Write (MSGF,'(/,A,I2,A,I5,A,I6,A,I2)') &
      ' Step =',KSTEP,' Increment =',KINC,' Element =',NOEL,' Integration point =',NPT
  Endif

  If(IND == 1 .or. IND == 4) &
    Write(DATF,form),string
! If(IND == 2 .or. IND == 4) &
!   Write(LOGF,form),string
  If(IND == 3 .or. IND == 4) &
    Write(MSGF,form),string

  RETURN
  END SUBROUTINE WriteOut


      SUBROUTINE ASYCON
C
C     SUBROUTINE FOR COMPUTING CONSTANT TERM IN KAPPA MINUS
C
      COMPLEX         BSYCON,AI,C1,C1TEST,ALP,ALN,ARAT1,ARAT2
      CHARACTER       UFM*23
      COMMON /XMSSG / UFM
      COMMON /BLK2  / BSYCON
      COMMON /SYSTEM/ SYSBUF,IBBOUT
      COMMON /BLK1  / SCRK,SPS,SNS,DSTR,AI,PI,DEL,SIGMA,BETA,RES
C
      C1   = 1.0
      PI2  = 2.0*PI
      A1   = PI2/(SPS-SNS)
      GAM0 = SPS*DEL - SIGMA
      A2   =-A1
      B1   = GAM0/(SPS-SNS)
      S1   = SPS/(DSTR**2)
      S2   = SNS/DSTR
      C1TEST = 0.0
      DO 10 I = 1,200
      R    = I
      GAMP = PI2*R + GAM0
      GAMN =-PI2*R + GAM0
      C2P  = GAMP/DSTR - SCRK
      C2Q  = GAMP/DSTR + SCRK
      C2N  = GAMN/DSTR - SCRK
      C3Q  = GAMN/DSTR + SCRK
      NN   = 0
      CSEC = C2P*C2Q
      IF (CSEC .LT. 0.0) NN = 1
      T1   = GAMP*S1
      T2   = S2*SQRT(ABS(CSEC))
      IF (C2P.LT.0.0 .AND. C2Q.LT.0.0) T2 =-T2
      IF (NN .EQ. 0) ALP = T1 + T2
      IF (NN .EQ. 1) ALP = CMPLX(T1,T2)
      NN   = 0
      CSEC = C2N*C3Q
      IF (CSEC .LT. 0.0) NN = 1
      T1   = GAMN*S1
      T2   = S2*SQRT(ABS(CSEC))
      IF (C2N.LT.0.0 .AND. C3Q.LT.0.0) T2 =-T2
      IF (NN .EQ. 0) ALN = T1 + T2
      IF (NN .EQ. 1) ALN = CMPLX(T1,T2)
      ARAT1 = (A1*R+B1)/ALP
      ARAT2 = (A2*R+B1)/ALN
      C1    = C1*ARAT1*ARAT2
      IF (CABS((C1-C1TEST)/C1) .LT. 0.0001)  GO TO 60
      C1TEST = C1
   10 CONTINUE
      GO TO 9999
   60 CONTINUE
      BSYCON = C1
      RETURN
C
 9999 WRITE  (IBBOUT,1000) UFM
 1000 FORMAT (A23,' - AMG MODULE - SUBROUTINE ASYCON')
      CALL MESAGE (-61,0,0)
      RETURN
      END


      SUBROUTINE AKAPPA (ARG,BKAPPA)
C
C     SUBROUTINE FOR COMPUTING KAPPA
C
      COMPLEX AI
C
      COMMON/BLK1/SCRK,SPS,SNS,DSTR,AI,PI,DEL,SIGMA,BETA,RES
C
      SCRK1 =  ABS (SCRK)
      ARG1  =  ABS (ARG)
      IF ( SCRK1 .GT. ARG1)  GO TO 10
      GAM=SQRT(ARG**2-SCRK**2)
      S1=SNS*GAM
      C1=BETA*GAM*SIN(S1)
      C2=COS(S1)-COS((ARG-DEL)*SPS+SIGMA)
      BKAPPA=C1/C2
      RETURN
   10 CONTINUE
      GAM=SQRT(SCRK**2-ARG**2)
      S1=SNS*GAM
      C1=-BETA*GAM*SINH(S1)
      C2=COSH(S1)-COS((ARG-DEL)*SPS+SIGMA)
      BKAPPA=C1/C2
      RETURN
      END


program test
      real :: a
      a = -10.12
      print *, a
      a = abs(a)
      print *, a
end program test


	SUBROUTINE SMTHPL(Y,NP,NA)

c	SMOOTHS OUT ARRAY Y BY A MEAN RUNNING AVERAGE OF NA POINTS.
c	TAKES CARE OF BORDER EFFECTS.
c	NP IS THE TOTAL NUMBER OF POINTS IN THE ARRAY. WORKS FOR UP TO
c	200000 POINTS.
c	RETURNS SMOOTHED ARRAY INTO SAME ARRAY.

	REAL Y(NP),BUF(200000)
	NB=NA/2
	DO I=1,NB
	BUF(I)=AVERAG(Y,I,I-1)
	M=NP-I+1
	BUF(M)=AVERAG(Y,M,I-1)
	ENDDO
	I1=NB+1
	I2=NP-NB
	DO I=I1,I2
	BUF(I)=AVERAG(Y,I,NB)
	ENDDO
	DO I=1,NP
	Y(I)=BUF(I)
	ENDDO
	RETURN
	END

	FUNCTION AVERAG(Y,I,J)
c	TAKES AVERAGE OF (2*J+1) POINTS OF ARRAY Y CENTERED ON Y(I)
	REAL Y(20000)
	AVERAG=0.
	L1=I-J
	L2=I+J
	DO L=L1,L2
	AVERAG=AVERAG+Y(L)
	ENDDO
	AVERAG=AVERAG/(2*J+1.)
	RETURN
	END


subroutine build_table(transition_cost, penalty, cost, prev_node, n_beats, n_length)
    implicit none
    integer, intent(in) :: n_beats, n_length
    real(8), intent(in), dimension(0:n_beats - 1, 0:n_beats - 1) :: transition_cost
    real(8), intent(in), dimension(0:n_beats - 1, 0:n_length - 1) :: penalty
    real(8), intent(out), dimension(0:n_beats - 1, 0:n_length - 1) :: cost
    integer, intent(out), dimension(0:n_beats - 1, 0:n_length - 1) :: prev_node
    real(8), dimension(0:n_beats - 1) :: temp_cost
    real(8) :: min_val
    integer :: l, i, j, min_i
    cost(:, 0) = penalty(:, 0)
    do l = 1, n_length - 1
        do i = 0, n_beats - 1
            min_val = penalty(i, l) + transition_cost(0, i) + cost(0, l - 1)
            min_i = 0
            do j = 1,  n_beats - 1
                temp_cost(j) = penalty(i, l) + transition_cost(j, i) + cost(j, l - 1)
                if (temp_cost(j) < min_val) then
                    min_val = temp_cost(j)
                    min_i = j
                end if
            enddo
            cost(i, l) = min_val
            prev_node(i, l) = min_i
        enddo
    enddo
end subroutine