!     Cuba Integration Library Header File
! ******************************************************

  module cubaint
    use settings, only: dp => mcp
    use NLRSD_settings, only: pi, pisqr
    implicit none

    !integer gridno   !this is notreally used
    real(kind=dp)             :: xmin, xmax
    !to fix rank missmatch with oneloop.f90
    integer, parameter        :: ncomp = 1

    contains

    subroutine integrate4D(integrand, method, epsrel, epsabs, result, error, prob)
      implicit none
      integer             :: method
      real(kind=dp)       :: epsabs, epsrel
      real(kind=dp)       :: result(1)
      external            :: integrand
! *******************************************************
      integer, parameter  :: ndim = 4    !chosen dimension
      integer, parameter  :: last = 4    !only last set of samples used
      integer, parameter  :: mineval = 0 !minimum integrand evaluations
      integer, parameter  :: maxeval = 110000000 !max evaluations
      !!VEGAS specific flags
!************  Verbosity flag for all integrals **********
      !0*8: sobol quasi random numbers used for sampling
      !1*4: only the last set of smples is used in the final
      !     result
      !1*2: echoes input parameters, verbosity flag
      integer, parameter  :: verbose   = 0*8+0*4+0*2+0*1
      integer, parameter  :: nstart    = 4000    !! 4000
      integer, parameter  :: nincrease = 700
      !! Suave specific flags
      integer, parameter  :: nnew = 200000
      real(kind=dp), parameter :: flatness = 50._dp
      !! Divonne specific flags !!
      !real(kind=dp), parameter    :: epsrel = 1D-2
      !real(kind=dp), parameter    :: epsabs = 1D-4
      !$$$      parameter (verbose = 0*8+0*4+1*2+0*1)
      integer, parameter     :: key1 = 9, key2 = 1, key3 = 1, maxpass = 25
      integer, parameter     :: ngiven = 0, ldxgiven = ndim, nextra = 0
      real(kind=dp)          :: border = 0.D0, maxchisq = 0.10D0, mindeviation = 0.125D0
!      double precision xgiven(ldxgiven,ngiven)
      !!Cuhre specific flags
      integer, parameter     :: key = 0 !default cubature rule for each dimension
      !!Output variables
      integer                        :: nregions, neval, fail
      integer                        :: ic !integer used to access and print the output arrays
      real(kind=dp),dimension(ncomp) ::  integral, error, prob
! *******************************************************

      if(method == 1 .or. method == 0) then
        call vegas(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval, &
           nstart, nincrease, &
           neval, fail, integral, error, prob)

        if (fail /= 0) then
          write(*,*) 'Failure to meet required accuracy'
          write(*,*) "VEGAS: neval =", neval," fail =", fail
          write(*,*) '("VEGAS:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
               (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
          if(method /=  0) stop
        endif
!      endif

      elseif(method == 2 .or. method == 0) then

        call suave(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,nnew,flatness, &
           nregions, neval, fail, integral, error, prob)

        if (fail .ne. 0) then
          write(*,*) 'Failure to meet required accuracy'
          write(*,*) "SUAVE: neval =", neval," fail =", fail
          write(*,*) '("SUAVE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
          if(method /= 0) then
!           stop
          endif
        endif

      elseif(method == 3 .or. method == 0) then

        call cuhre(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,key, &
           nregions, neval, fail, integral, error, prob)

        if (fail /= 0) then
          write(*,*) 'Failure to meet required accuracy'
          write(*,*) "CUHRE: neval =", neval," fail =", fail
          write(*,*) '("CUHRE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
          if(method /= 0) then
!                stop
          endif
        endif
      elseif (method == 4 .or. method == 0) then
         call divonne(ndim, ncomp, integrand, &
              epsrel, epsabs, verbose, mineval, maxeval, &
              key1, key2, key3, maxpass, &
              border, maxchisq, mindeviation, &
              ngiven, ldxgiven, 0, nextra, 0, &
              nregions, neval, fail, integral, error, prob)
         if (method == 0 .or. fail == 1) then
            write(*,*) "DIVONNE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            write(*,*) '("DIVONNE: ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif
      endif
      result = integral(1)
    end subroutine integrate4D

! *******************************************************
! *******************************************************
! ******************************************************

! *******************************************************
! *******************************************************
! ******************************************************

    subroutine integrate3D(integrand, method, epsrel, epsabs, result, error,prob)
      implicit none
      integer       :: verbose, ndim,mineval,maxeval,last
      integer       :: nstart,nincrease,method
      real(kind=dp) :: epsabs,epsrel
      real(kind=dp) :: result(1)
      external integrand

! *******************************************************
      parameter (ndim = 3) !chosen dimension
      parameter (last = 4) !only last set of samples used
      parameter (mineval = 0) !minimum integrand evaluations
!      parameter (maxeval = 100000) !max evaluations
      parameter (maxeval = 110000000) !max evaluations

!************  Verbosity flag for all integrals **********
      !0*8: sobol quasi random numbers used for sampling
      !1*4: only the last set of smples is used in the final
      !     result
      !1*2: echoes input parameters, verbosity flag
      parameter (verbose = 0*8+0*4+0*2+0*1)

      !!VEGAS specific flags
      parameter (nstart = 4000)    !! 4000
      parameter (nincrease = 700)

      !! Suave specific flags
      integer nnew
      double precision flatness
      parameter (nnew = 1000)
      parameter (flatness = 50D0)
      !! Divonne specific flags !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)

      integer key1, key2, key3, maxpass
      double precision border, maxchisq, mindeviation
      integer ngiven, ldxgiven, nextra
!      parameter (key1 = 47)
      parameter (key1 = 9)
!      parameter (key2 = 100)
      parameter (key2 = 1)
! default key2=67
      parameter (key3 = 1)
      parameter (maxpass = 25) ! was 25
      parameter (border = 1.D-7) ! thickness of border. Important for
                                  ! kernels with angular singularities
      parameter (maxchisq = .10D0)  !!default 10_dp
      parameter (mindeviation = .25D0)  !!default .25_dp
      parameter (ngiven = 0)
      parameter (ldxgiven = ndim)
      parameter (nextra = 0)
!      double precision xgiven(ldxgiven,ngiven)

      !! Cuhre specific flags
      integer key
      parameter (key = 0) ! default cubature rule for each dimension

      !! Output variables
      real(kind=dp) integral(ncomp), error(ncomp), prob(ncomp)
      integer nregions, neval, fail

      integer ic !integer used to access and print the output arrays
! *******************************************************

      if(method .eq. 1 .or. method .eq.0) then

      call vegas(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval, &
           nstart, nincrease, &
           neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "VEGAS: neval =", neval," fail =", fail
            print '("VEGAS:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
            integral(1)=0._dp
      endif
!      endif

      elseif(method .eq. 2 .or. method .eq.0) then

      call suave(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,nnew,flatness, &
           nregions, neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "SUAVE: neval =", neval," fail =", fail
            print '("SUAVE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
!      endif
      endif

      elseif(method .eq. 3 .or. method .eq.0) then

      call cuhre(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,key, &
           nregions, neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "CUHRE: neval =", neval," fail =", fail
            print '("CUHRE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
            integral(1)=0._dp
      endif

      elseif (method.eq.4 .or. method.eq.0) then
         call divonne(ndim, ncomp, integrand, &
              epsrel, epsabs, verbose, mineval, maxeval, &
              key1, key2, key3, maxpass, &
              border, maxchisq, mindeviation, &
              ngiven, ldxgiven, 0, nextra, 0, &
              nregions, neval, fail, integral, error, prob)
         if (method.eq.0 .or. fail.eq.1) then
            print *, "DIVONNE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("DIVONNE: ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif
      endif

      result = integral(1)
    end subroutine integrate3D

! *******************************************************
! *******************************************************
! ******************************************************
! *******************************************************
! *******************************************************
! ******************************************************

    subroutine integrate2D(integrand, method, epsrel, epsabs, result, error,prob)
      implicit none
      integer        :: verbose, ndim,mineval,maxeval,last
      integer        :: nstart,nincrease,method
      real(kind=dp)  :: epsabs,epsrel
      real(kind=dp)  :: result(1)
      external       :: integrand

! *******************************************************
      parameter (ndim = 2) !chosen dimension
      parameter (last = 4) !only last set of samples used
      parameter (mineval = 0) !minimum integrand evaluations
      parameter (maxeval = 110000000) !max evaluations

!************  Verbosity flag for all integrals **********
      !0*8: sobol quasi random numbers used for sampling
      !1*4: only the last set of smples is used in the final
      !     result
      !1*2: echoes input parameters, verbosity flag
      parameter (verbose = 0*8+0*4+0*2+0*1)

      !!VEGAS specific flags
      parameter (nstart = 4000)    !! 4000
      parameter (nincrease = 700)

      !! Suave specific flags
      integer nnew
      double precision flatness
      parameter (nnew = 1000)
      parameter (flatness = 50D0)

      !! Divonne specific flags !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)

      integer key1, key2, key3, maxpass
      double precision border, maxchisq, mindeviation
      integer ngiven, ldxgiven, nextra
!      parameter (key1 = 47)
      parameter (key1 = 9)
!      parameter (key2 = 100)
      parameter (key2 = 1)
! default key2=67
      parameter (key3 = 1)
      parameter (maxpass = 25)
      parameter (border = 0D0)
      parameter (maxchisq = .10D0)  !!default 10_dp
      parameter (mindeviation = .125D0)  !!default .25_dp
      parameter (ngiven = 0)
      parameter (ldxgiven = ndim)
      parameter (nextra = 0)
!      double precision xgiven(ldxgiven,ngiven)

      !! Cuhre specific flags
      integer key
      parameter (key = 13) ! default cubature rule for each dimension

      !! Output variables
      real(kind=dp) integral(ncomp), error(ncomp), prob(ncomp)
      integer nregions, neval, fail

      integer ic !integer used to access and print the output arrays
! *******************************************************

      if(method .eq. 1 .or. method .eq.0) then

      call vegas(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval, &
           nstart, nincrease, &
           neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "VEGAS: neval =", neval," fail =", fail
            print '("VEGAS:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
      endif
!      endif

      elseif(method .eq. 2 .or. method .eq.0) then

      call suave(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,nnew,flatness, &
           nregions, neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "SUAVE: neval =", neval," fail =", fail
            print '("SUAVE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
!      endif
      endif

      elseif(method .eq. 3 .or. method .eq.0) then

      call cuhre(ndim, ncomp, integrand, &
           epsrel, epsabs, verbose, mineval, maxeval,key, &
           nregions, neval, fail, integral, error, prob)

      if (fail .ne. 0) then
            write(*,*) 'Failure to meet required accuracy'
            print *, "CUHRE: neval =", neval," fail =", fail
            print '("CUHRE:   ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
            if(method .ne. 0) then
!                  stop
            endif
            integral(1)=0._dp
      endif
      elseif (method.eq.4 .or. method.eq.0) then
         call divonne(ndim, ncomp, integrand, &
              epsrel, epsabs, verbose, mineval, maxeval, &
              key1, key2, key3, maxpass, &
              border, maxchisq, mindeviation, &
              ngiven, ldxgiven, 0, nextra, 0, &
              nregions, neval, fail, integral, error, prob)
         if (method.eq.0 .or. fail.eq.1) then
            print *, "DIVONNE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("DIVONNE: ",E20.12," +- ",E20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif
      endif
      result = integral(1)
    end subroutine integrate2D

! *******************************************************
!     Emiliano's 2D integral

    subroutine integrale2D(subint,iint,xepsrel,xepsabs,pout)
      implicit none
      integer xverbose,iint
      real(kind=dp) pout,xepsrel,xepsabs
      external subint

            !! Common arguments !! (last = ??)

      integer ndim,  mineval, maxeval, verbose, last
      double precision epsrel, epsabs
      parameter (ndim = 2)
!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-6)
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)
      parameter (last = 4)
      parameter (mineval = 0)
      parameter (maxeval = 110000000)

      !! VEGAS !!

      parameter (epsrel = 1D-2)
      parameter (epsabs = 0)
!      parameter (epsrel = 1D-3)
!      parameter (epsabs = 0)

      parameter (verbose = 0*8+0*4+0*2+0*1)

      integer nstart, nincrease
      parameter (nstart = 4000)    !! 4000
      parameter (nincrease = 700)

      !! SUAVE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)
!$$$      parameter (verbose = 0*8+1*4+1*2+0*1)

      integer nnew
      double precision flatness
      parameter (nnew = 1000)
      parameter (flatness = 50D0)

      !! DIVONNE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)

      integer key1, key2, key3, maxpass
      double precision border, maxchisq, mindeviation
      integer ngiven, ldxgiven, nextra
!      parameter (key1 = 47)
      parameter (key1 = 9)
!      parameter (key2 = 100)
      parameter (key2 = 1)
! default key2=67
      parameter (key3 = 1)
      parameter (maxpass = 25)
      parameter (border = 0D0)
      parameter (maxchisq = .10D0)  !!default 10_dp
      parameter (mindeviation = .125D0)  !!default .25_dp
      parameter (ngiven = 0)
      parameter (ldxgiven = ndim)
      parameter (nextra = 0)
!      double precision xgiven(ldxgiven,ngiven)

      !! CUHRE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+1*4+1*2+0*1)

      integer key
      parameter (key = 0)

      !! Output !!

!      external integrand

      real(kind=dp) integral(ncomp), error(ncomp), prob(ncomp)
      integer nregions, neval, fail

      integer ic

      xverbose=0*8+0*4+0*2+0*1

      if (iint.eq.1 .or. iint.eq.0) then

         call vegas(ndim, ncomp, subint,xepsrel, xepsabs, xverbose, &
              mineval,maxeval,nstart, nincrease,neval,fail,integral, &
              error,prob)
         if (iint.eq.0) then
            print *, "VEGAS: neval =", neval," fail =", fail
            print '("VEGAS: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      elseif (iint.eq.2 .or. iint.eq.0) then

            call suave(ndim, ncomp, subint,xepsrel, xepsabs, &
                 xverbose + last, mineval, maxeval,nnew, flatness, &
                 nregions, neval, fail, integral, error, prob)
         if (iint.eq.0 .or. fail.eq.1) then
            print *, "SUAVE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("SUAVE: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      elseif (iint.eq.3 .or. iint.eq.0) then

         call divonne(ndim,ncomp,subint,xepsrel,xepsabs,xverbose, &
              mineval,maxeval,key1,key2,key3,maxpass,border,maxchisq, &
              mindeviation,ngiven,ldxgiven,0,nextra,0,nregions,neval, &
              fail,integral,error,prob)
         if (iint.eq.0 .or. fail.eq.1) then
            print *, "DIVON: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("DIVON: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      elseif (iint.eq.4 .or. iint.eq.0) then

         call cuhre(ndim,ncomp,subint,xepsrel,xepsabs,xverbose+last, &
              mineval,maxeval,key,nregions,neval,fail,integral,error, &
              prob)
         if (iint.eq.0 .or. fail.eq.1) then
            print *, "CUHRE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("CUHRE: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      endif
      pout=integral(1)
      return
    end subroutine integrale2D

! *******************************************************
!     Emiliano's 1D integral

    subroutine integrate1D(subint,method,xepsrel,xepsabs,result, error,prob)
      implicit none
      integer       :: xverbose,method
      real(kind=dp) :: xepsrel,xepsabs
      real(kind=dp) :: result(1)
      external      :: subint

            !! Common arguments !! (last = ??)

      integer ndim, mineval, maxeval, verbose, last
      double precision epsrel, epsabs
      parameter (ndim = 1)
!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-6)
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)
      parameter (last = 4)
      parameter (mineval = 0)
      parameter (maxeval = 110000000)

      !! VEGAS !!

      parameter (epsrel = 1D-2)
      parameter (epsabs = 0)
!      parameter (epsrel = 1D-3)
!      parameter (epsabs = 0)

      parameter (verbose = 0*8+0*4+0*2+0*1)

      integer nstart, nincrease
      parameter (nstart = 4000)    !! 4000
      parameter (nincrease = 700)

      !! SUAVE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)
!$$$      parameter (verbose = 0*8+1*4+1*2+0*1)

      integer nnew
      double precision flatness
      parameter (nnew = 1000)
      parameter (flatness = 50D0)

      !! DIVONNE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+0*4+1*2+0*1)

      integer key1, key2, key3, maxpass
      double precision border, maxchisq, mindeviation
      integer ngiven, ldxgiven, nextra
!      parameter (key1 = 47)
      parameter (key1 = 9)
!      parameter (key2 = 100)
      parameter (key2 = 1)
! default key2=67
      parameter (key3 = 1)
      parameter (maxpass = 25)
      parameter (border = 0D0)
      parameter (maxchisq = .10D0)  !!default 10_dp
      parameter (mindeviation = .125D0)  !!default .25_dp
      parameter (ngiven = 0)
      parameter (ldxgiven = ndim)
      parameter (nextra = 0)
!      double precision xgiven(ldxgiven,ngiven)

      !! CUHRE !!

!$$$      parameter (epsrel = 1D-2)
!$$$      parameter (epsabs = 1D-4)       !!-6
!$$$      parameter (verbose = 0*8+1*4+1*2+0*1)

      integer key
      parameter (key = 0)

      !! Output !!

!      external integrand

      real(kind=dp) integral(ncomp), error(ncomp), prob(ncomp)
      integer nregions, neval, fail

      integer ic

      xverbose=0*8+0*4+0*2+0*1

      if (method.eq.1 .or. method.eq.0) then

         call vegas(ndim, ncomp, subint,xepsrel, xepsabs, xverbose, &
              mineval,maxeval,nstart, nincrease,neval,fail,integral, &
              error,prob)
         if (method.eq.0) then
            print *, "VEGAS: neval =", neval," fail =", fail
            print '("VEGAS: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      elseif (method.eq.2 .or. method.eq.0) then

            call suave(ndim, ncomp, subint,xepsrel, xepsabs, &
                 xverbose + last, mineval, maxeval,nnew, flatness, &
                 nregions, neval, fail, integral, error, prob)
         if (method.eq.0 .or. fail.eq.1) then
            print *, "SUAVE: nregions =", nregions," neval =", neval, &
                 " fail =", fail
            print '("SUAVE: ",F20.12," +- ",F20.12,"   p = ",F8.3)', &
                 (integral(ic), error(ic), prob(ic), ic = 1, ncomp)
         endif

      elseif (method.eq.4 .or. method.eq.3) then
            write(*,*) 'integrate 1-D: method',method,' out of bounds'
            stop
      endif

      result=integral(1)

    end subroutine integrate1D

! **************************** TESTING SUBROUTINE **********************
    subroutine test_cuba()
      IMPLICIT NONE
      real(kind=dp)   :: t1, t2
      real(kind=dp)   :: relacc, absacc
      real(kind=dp)   :: result(1), error(1), prob(1)
      integer         :: method, i
      xmin=0._dp
      xmax=100._dp
      relacc=1.d-3
      absacc=1.d-10
      method=1
      write(*,*) '****************** CUBA TEST *********************'
      write(*,*) '      testing 1-D integrators...'
      do i=1,2
        method=i
        call cpu_time(t1)
        call integrate1D(testc1,method,relacc,absacc,result, error,prob)
        if (abs(result(1)-5000._dp) .gt. 5000*relacc) then
          write(*,*) 'Bad f(x)=x accuracy!'
          write(*,*) 'result',result(1)
          stop
        endif
        call cpu_time(t2)
        write(*,*) '            method:',method,t2-t1,' seconds'
      enddo
      write(*,*) '      testing 2-D integrators...'
      do i=1,4
        method=i
        call cpu_time(t1)
        call integrate2D(testc2,method,relacc,absacc,result, error,prob)
!            write(*,*) 'result=',result(1)
        if (abs(result(1)-4.18879d6) .gt. 4.18879d6*relacc) then
          write(*,*) 'Bad accuracy for method:',method
          stop
        endif
        call cpu_time(t2)
        write(*,*) '            method:',method,t2-t1,' seconds'
      enddo
      write(*,*) '      testing 3-D integrators...'
      do i=1,4
        method=i
        call cpu_time(t1)
        call integrate3D(testc3,method,relacc,absacc,result, error,prob)
!        write(*,*) 'result=',result(1)
        if (abs(result(1)-4.18879d6) .gt. 4.18879d6*relacc) then
          write(*,*) 'Bad accuracy for method:',method
          stop
        endif
        call cpu_time(t2)
        write(*,*) '            method:',method,t2-t1,' seconds'
      enddo
      write(*,*) '      testing 4-D integrators...'
      do i=1,4
        method=i
        call cpu_time(t1)
        call integrate4D(testc4,method,relacc,absacc,result, error,prob)
!        write(*,*) 'result=',result(1)
        if (abs(result(1)-2.09413d10) .gt. 2.09413d10*relacc) then
          write(*,*) 'Bad accuracy for method:',method
          write(*,*) 'result',result(1)
          stop
        endif
        call cpu_time(t2)
        write(*,*) '            method:',method,t2-t1,' seconds'
      enddo

    end subroutine test_cuba

    subroutine testc1(ndim,x,nncomp,f)
      implicit none
      integer ndim,nncomp,i
      real(kind=dp) x(ndim),f(nncomp),q(ndim),j
      do i=1,ndim
        q(i)=xmin+(xmax-xmin)*x(i)
      enddo
      f(1)=q(1)
      j=(xmax-xmin)
      f(1)=f(1)*j
    end subroutine testc1

    subroutine testc2(ndim,x,nncomp,f)
      implicit none
      integer ndim,nncomp
      real(kind=dp) x(ndim),f(nncomp),q(ndim),j
      q(1)=xmin+(xmax-xmin)*x(1)    !  r
      q(2)=pi*x(2)                 ! theta
      f(1)=1._dp
      j=2._dp*pisqr*(xmax-xmin)*q(1)**2*sin(q(2))
      f(1)=f(1)*j
    end subroutine testc2

    subroutine testc3(ndim,x,nncomp,f)
      implicit none
      integer ndim,nncomp
      real(kind=dp) x(ndim),f(nncomp),q(ndim),j
      q(1) = xmin + (xmax - xmin)*x(1)    !  r
      q(2) = pi*x(2)                 ! theta
      q(3) = 2._dp*pi*x(3)             ! phi
      f(1) = 1._dp
      j    = 2._dp*pisqr*(xmax - xmin)*q(1)**2*sin(q(2))
      f(1) = f(1)*j
    end subroutine testc3

    subroutine testc4(ndim,x,nncomp,f)
      implicit none
      integer ndim,nncomp
      real(kind=dp) x(ndim),f(nncomp),q(ndim),j
      q(1)=xmin+(xmax-xmin)*x(1)    !  r
      q(2)=pi*x(2)                 ! theta
      q(3)=2._dp*pi*x(3)             ! phi
      q(4)=xmin+(xmax-xmin)*x(4)
      f(1)=q(4)!1._dp
      j=2._dp*pisqr*(xmax-xmin)**2*q(1)**2*sin(q(2))
      f(1)=f(1)*j
    end subroutine testc4

  end module cubaint


      subroutine runY_00i1i2(k,l,i1,i2,Xtwiddle,Gtwiddle,Shat4,N0)
      implicit none
C---  Expression for Eq. 5.58c
C---  Calculates D00i1i2, requires D00li1,D00li2
C---  Small terms of order Xtwiddle(0,k)*Diii,Xtwiddle(0,0)*Diiii
C---  Denominator Gtwiddle(k,l)
      include 'pvDnames.f'
      include 'pvDv.f'
      include 'Darraydef.f'
      include 'Darrays.f'
      integer ep,N0,k,l,i1,i2,np
      parameter(np=3)
      double precision Xtwiddle(0:np,0:np),Gtwiddle(np,np)
      double complex Shat4(np,z3max,-2:0)

      if (  (i1 .eq. l) .or. (i2 .eq. l)
     . .or. (i1 .eq. 0) .or. (i2 .eq. 0)) then
      return
      endif

      do ep=-2,0

      Dv(dzzii(z2(i1,i2))+N0,ep)=
     .(-2*Gtwiddle(k,i1)*Dv(dzzii(z2(l,i2))+N0,ep)
     . -2*Gtwiddle(k,i2)*Dv(dzzii(z2(l,i1))+N0,ep)
     . +Gtwiddle(k,1)*Shat4(1,z3(l,i1,i2),ep)
     . +Gtwiddle(k,2)*Shat4(2,z3(l,i1,i2),ep)
     . +Gtwiddle(k,3)*Shat4(3,z3(l,i1,i2),ep)
     . +Xtwiddle(0,k)*Dv(diii(z3(l,i1,i2))+N0,ep)
     . -Xtwiddle(0,0)*Dv(diiii(z4(k,l,i1,i2))+N0,ep))/(2*Gtwiddle(k,l))
      enddo

      return
      end


use ContactMechanicsClass
implicit none

type(FEMDomain_),allocatable :: domains(:)
type(ContactMechanics_) :: contact
type(Time_) :: time
integer(int32),allocatable :: FixBoundary(:)
integer(int32) :: contactList(3,3)=0

call time%start()

allocate(domains(3) )

! contact of three cubes
call domains(1)%create(meshtype="Cube3D",x_num=10,y_num=10,z_num=10)
call domains(1)%resize(x=2.0d0,y=2.0d0,z=2.0d0)

call domains(2)%create(meshtype="Cube3D",x_num=10,y_num=10,z_num=10)
call domains(2)%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domains(2)%move(x=1.9d0)

call domains(3)%create(meshtype="Cube3D",x_num=10,y_num=10,z_num=10)
call domains(3)%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domains(3)%move(x=3.9d0)

! contact domain#1 => domain#2
contactList(1,2) = 1
! contact domain#2 => domain#3
contactList(2,3) = 1
! initialize simulator
call contact%init(femdomains=domains,contactlist=contactlist)
contact%YoungModulus(1) = 1000.0d0
contact%YoungModulus(2) = 1000.0d0
contact%YoungModulus(3) = 1000.0d0

! setup solver
! Caution >> GaussPointProjection=.true. >> bug exists.
call contact%setup(penaltyparameter=1000.0d0,GaussPointProjection=.False.)
! fix displacement
call time%show()

call contact%fix(direction="x",disp= 0.0d0, DomainID=1,x_max=0.10d0)
call contact%fix(direction="y",disp= 0.0d0, DomainID=1,x_max=0.10d0)
call contact%fix(direction="z",disp= 0.0d0, DomainID=1,x_max=0.10d0)
call contact%fix(direction="z",disp=-0.50d0,DomainID=3,x_min=5.80d0)

! solve > get displacement
call time%show()
call contact%solver%solve("BiCGSTAB")
call time%show()
! update mesh
call contact%updateMesh()
! show results
call domains(1)%vtk("domains(1)_result")
call domains(2)%vtk("domains(2)_result")
call domains(3)%vtk("domains(3)_result")

print *, "Predicted computation time (sec.)"
print *, 10.0d0 ** (1.395460d0*log10(10000000.0d0) - 4.645620d0)/60.0d0/60.0d0

end


module local_stage_NR

  USE OMP_LIB
  use Structure
  use Matrix_op
  use bar
  !use global_stage

  implicit none

  !type :: local

  ! double precision, dimension(:,:,:), allocatable :: sigma
  !  double precision, dimension(:,:,:), allocatable :: d_Ep

  !end type local

  !type(local) :: L

  contains

  function local_stage_vp_NR(I,Sop,sigma) result(d_Ep)

    type(Input), intent(in) :: I
    !type(global), intent(inout) :: G
    type(Space_op), intent(in) :: Sop
    double precision, dimension(:,:,:) :: sigma
    integer, dimension(3) :: siz
    double precision, dimension(Sop%comp,I%ngps,I%Nx) :: sigma_D,d_Ep,tau
    double precision, dimension(Sop%comp,I%ngps,I%Nx) :: J2_sigma,iJ2_sigma,N,nlaw

    integer :: j
    double precision, dimension(1,I%ngps,I%Nx) :: f_K

    !!$OMP PARALLEL

    siz = (/1,I%ngps,I%Nx/)
    !L%sigma      = sigma
    sigma_D      = sigma - sigma/3

    tau = sigma_D
    J2_sigma = reshape( sqrt(1.5*sum((tau)**2,1)), siz )

    iJ2_sigma = 1/J2_sigma ! only the positive parts.
    where(J2_sigma == 0.D0) iJ2_sigma = 0.D0  !iJ2_sigma(J2_sigma==0) = 0;

    N = iJ2_sigma*tau
    f_K = J2_sigma  - I%sigma_y
    where(f_K<0.D0) f_K = 0.D0

    nlaw = I%ks*(f_K**I%np)
    !nlaw = Norton_law(I,Sop)
    d_Ep  = 3/2*(nlaw*N)

    !!$OMP END PARALLEL

  end function local_stage_vp_NR

end module local_stage_NR


!***********************************************************************
!                                                                      *
      SUBROUTINE MATELT(I1, K, I2, APART, GJPART, DGJPART)
!                                                                      *
!   This  routine computes  the angular part  of the  reduced matrix   *
!   elements of the magnetic and electric multipole operators as       *
!   well as for th operators associated with the g_j factor            *
!                                                                      *
!   Calls to: [LIB92]: CLRX                                            *
!                                                                      *
!   Written by Per Jonsson                Last revision: 22 Oct 1999   *
!                                                                      *
!***********************************************************************
!...Translated by Pacific-Sierra Research 77to90  4.3E  14:06:03   1/ 3/07
!...Modified by Charlotte Froese Fischer
!                     Gediminas Gaigalas  11/01/17
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY: DOUBLE
      USE parameter_def,   ONLY: NNNW
      USE orb_C,           ONLY: np, nak
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE clrx_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER, INTENT(IN) :: I1
      INTEGER  :: K
      INTEGER, INTENT(IN) :: I2
      REAL(DOUBLE), INTENT(OUT) :: APART
      REAL(DOUBLE), INTENT(OUT) :: GJPART
      REAL(DOUBLE), INTENT(OUT) :: DGJPART
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER      :: KAP1, KAP2, L1, L2
      REAL(DOUBLE) :: FASE, OVLFAC
!-----------------------------------------------
!
!
!   Set KAP1 and KAP2
!
      KAP1 = NAK(I1)
!
      IF (MOD(K,2) == 1) THEN
         KAP2 = -NAK(I2)
      ELSE
         KAP2 = NAK(I2)
      ENDIF
!
!   Determine the l quantum numbers
!
      IF (KAP1 > 0) THEN
         L1 = KAP1
      ELSE
         L1 = (-KAP1) - 1
      ENDIF
!
      IF (KAP2 > 0) THEN
         L2 = KAP2
      ELSE
         L2 = (-KAP2) - 1
      ENDIF
!
      IF (MOD(L1 + K + L2,2) == 0) THEN
!
!   Parity selection rule satisfied
!
!   Determine the phase factor
!
         IF (MOD(KAP1 + 1,2) == 0) THEN
            FASE = 1.0D00
         ELSE
            FASE = -1.0D00
         ENDIF
!
!   The other factor is \sqrt (2 j_2 + 1); since j = | \kappa | - 1/2,
!   we have 2 j_2 + 1 = 2 | \kappa |; the factor \sqrt (2 j_2 + 1)
!   has been accounted for in MCT
!
         OVLFAC = FASE*SQRT(DBLE(2*ABS(KAP2)))
!
         IF (MOD(K,2) == 1) THEN
!
!   These are for the magnetic multipole moments and the two operators of the g_j factor
!
            APART = (KAP1 + NAK(I2))*CLRX(KAP1,K,KAP2)*OVLFAC
!
            GJPART = APART
            DGJPART = -(KAP1 + NAK(I2)-1.0D00)*CLRX(KAP1,K,KAP2)*OVLFAC
         ELSE
!
!   These are for the electric multipole moments
!
            APART = CLRX(KAP1,K,KAP2)*OVLFAC
!
         ENDIF
!
      ELSE
!
         APART = 0.0D00
         GJPART = 0.0D00
         DGJPART = 0.0D00
!
      ENDIF
!
      RETURN
      END SUBROUTINE MATELT


!==========================================================================
elemental function gsw_hill_ratio_at_sp2 (t)
!==========================================================================
!
!  Calculates the Hill ratio, which is the adjustment needed to apply for
!  Practical Salinities smaller than 2.  This ratio is defined at a
!  Practical Salinity = 2 and in-situ temperature, t using PSS-78. The Hill
!  ratio is the ratio of 2 to the output of the Hill et al. (1986) formula
!  for Practical Salinity at the conductivity ratio, Rt, at which Practical
!  Salinity on the PSS-78 scale is exactly 2.
!--------------------------------------------------------------------------

use gsw_mod_sp_coefficients

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: t

real (r8) :: gsw_hill_ratio_at_sp2

real (r8), parameter :: g0 = 2.641463563366498e-1_r8
real (r8), parameter :: g1 = 2.007883247811176e-4_r8
real (r8), parameter :: g2 = -4.107694432853053e-6_r8
real (r8), parameter :: g3 = 8.401670882091225e-8_r8
real (r8), parameter :: g4 = -1.711392021989210e-9_r8
real (r8), parameter :: g5 = 3.374193893377380e-11_r8
real (r8), parameter :: g6 = -5.923731174730784e-13_r8
real (r8), parameter :: g7 = 8.057771569962299e-15_r8
real (r8), parameter :: g8 = -7.054313817447962e-17_r8
real (r8), parameter :: g9 = 2.859992717347235e-19_r8
real (r8), parameter :: sp2 = 2.0_r8

real (r8) :: t68, ft68, rtx0, dsp_drtx, sp_est, rtx, rtxm, x, part1, part2
real (r8) :: sqrty, sp_hill_raw_at_sp2

t68 = t*1.00024_r8
ft68 = (t68 - 15.0_r8)/(1.0_r8 + k*(t68 - 15.0_r8))

!--------------------------------------------------------------------------
! Find the initial estimates of Rtx (Rtx0) and of the derivative dSP_dRtx
! at SP = 2.
!--------------------------------------------------------------------------
rtx0 = g0 + t68*(g1 + t68*(g2 + t68*(g3 + t68*(g4 + t68*(g5 &
          + t68*(g6 + t68*(g7 + t68*(g8 + t68*g9))))))))

dsp_drtx =  a1 + (2.0_r8*a2 + (3.0_r8*a3 + (4.0_r8*a4 + 5.0_r8*a5*rtx0)*rtx0)* &
                  rtx0)*rtx0 + &
      ft68*(b1 + (2.0_r8*b2 + (3.0_r8*b3 + (4.0_r8*b4 + 5.0_r8*b5*rtx0)*rtx0)* &
                  rtx0)*rtx0)

!--------------------------------------------------------------------------
! Begin a single modified Newton-Raphson iteration to find Rt at SP = 2.
!--------------------------------------------------------------------------
sp_est = a0 + (a1 + (a2 + (a3 + (a4 + a5*rtx0)*rtx0)*rtx0)*rtx0)*rtx0 &
 + ft68*(b0 + (b1 + (b2 + (b3 + (b4 + b5*rtx0)*rtx0)*rtx0)*rtx0)*rtx0)
rtx = rtx0 - (sp_est - sp2)/dsp_drtx
rtxm = 0.5_r8*(rtx + rtx0)
dsp_drtx =  a1 + (2.0_r8*a2 + (3.0_r8*a3 + (4.0_r8*a4 + 5.0_r8*a5*rtxm)*rtxm)* &
                  rtxm)*rtxm &
    + ft68*(b1 + (2.0_r8*b2 + (3.0_r8*b3 + (4.0_r8*b4 + 5.0_r8*b5*rtxm)*rtxm)* &
                  rtxm)*rtxm)
rtx = rtx0 - (sp_est - sp2)/dsp_drtx

! This is the end of one full iteration of the modified Newton-Raphson
! iterative equation solver.  The error in Rtx at this point is equivalent
! to an error in SP of 9e-16 psu.

x = 400.0_r8*rtx*rtx
sqrty = 10.0_r8*rtx
part1 = 1.0_r8 + x*(1.5_r8 + x)
part2 = 1.0_r8 + sqrty*(1.0_r8 + sqrty*(1.0_r8 + sqrty))
sp_hill_raw_at_sp2 = SP2 - a0/part1 - b0*ft68/part2

gsw_hill_ratio_at_sp2 = 2.0_r8/sp_hill_raw_at_sp2

return
end function

!--------------------------------------------------------------------------


c
c
c     ###################################################
c     ##  COPYRIGHT (C)  1990  by  Jay William Ponder  ##
c     ##              All Rights Reserved              ##
c     ###################################################
c
c     ################################################################
c     ##                                                            ##
c     ##  subroutine ocvm  --  variable metric optimization method  ##
c     ##                                                            ##
c     ################################################################
c
c
c     "ocvm" is an optimally conditioned variable metric nonlinear
c     optimization routine without line searches
c
c     literature references:
c
c     W. C. Davidon, "Optimally Conditioned Optimization Algorithms
c     Without Line Searches", Mathematical Programming, 9, 1-30 (1975)
c
c     D. F. Shanno and K-H. Phua, "Matrix Conditioning and Nonlinear
c     Optimization", Mathematical Programming, 14, 149-16 (1977)
c
c     D. F. Shanno and K-H. Phua, "Numerical Comparison of Several
c     Variable-Metric Algorithms", Journal of Optimization Theory
c     and Applications, 25, 507-518 (1978)
c
c     variables and parameters:
c
c     nvar      number of parameters in the objective function
c     x0        contains starting point upon input, upon return
c                 contains the best point found
c     f0        during optimization contains best current function
c                 value; returns final best function value
c     grdmin    normal exit if rms gradient gets below this value
c     ncalls    total number of function/gradient evaluations
c
c     required external routines:
c
c     fgvalue    function to evaluate function and gradient values
c     optsave    subroutine to write out info about current status
c
c
      subroutine ocvm (nvar,x0,f0,grdmin,fgvalue,optsave)
      implicit none
      include 'sizes.i'
      include 'inform.i'
      include 'iounit.i'
      include 'keys.i'
      include 'linmin.i'
      include 'math.i'
      include 'minima.i'
      include 'output.i'
      include 'potent.i'
      include 'scales.i'
      integer i,j,nvar
      integer mvar,next
      integer niter,ncalls
      integer nbig,nstep
      integer maxbig,maxstep
      real*8 fgvalue,eps
      real*8 grdmin,precise
      real*8 f,f0,f0old
      real*8 fprime,f0prime
      real*8 srchnorm
      real*8 sgangle,sg,snorm
      real*8 zeta,cosang
      real*8 fmove,xmove
      real*8 gnorm,grms,rms
      real*8 m2,n2,u2,v
      real*8 micron,mw,us,qk0
      real*8 a,b,b0,c
      real*8 alpha,gamma,delta
      real*8 x0(*)
      real*8, allocatable :: x0old(:)
      real*8, allocatable :: x(:)
      real*8, allocatable :: g(:)
      real*8, allocatable :: hq(:)
      real*8, allocatable :: search(:)
      real*8, allocatable :: s(:)
      real*8, allocatable :: w(:)
      real*8, allocatable :: k(:)
      real*8, allocatable :: k0(:)
      real*8, allocatable :: m(:)
      real*8, allocatable :: n(:)
      real*8, allocatable :: p(:)
      real*8, allocatable :: q(:)
      real*8, allocatable :: u(:)
      real*8, allocatable :: h(:,:)
      logical restart,done
      character*9 status
      character*20 keyword
      character*120 record
      character*120 string
      external fgvalue,optsave
c
c
c     initialization and set-up for the optimization
c
      if (nvar .gt. maxopt) then
         write (iout,10)
   10    format (/,' OCVM  --  Too many Parameters,',
     &              ' Increase the Value of MAXOPT')
         return
      end if
      mvar = nvar
      rms = sqrt(dble(nvar))
      if (coordtype .eq. 'CARTESIAN') then
         rms = rms / sqrt(3.0d0)
      else if (coordtype .eq. 'RIGIDBODY') then
         rms = rms / sqrt(6.0d0)
      end if
      maxbig = 2
      maxstep = 10
      eps = precise (2)
      restart = .true.
      done = .false.
c
c     set default values for variable scale factors
c
      if (.not. set_scale) then
         do i = 1, nvar
            if (scale(i) .eq. 0.0d0)  scale(i) = 1.0d0
         end do
      end if
c
c     set default parameters for the optimization
c
      if (fctmin .eq. 0.0d0)  fctmin = -10000000.0d0
      if (maxiter .eq. 0)  maxiter = 1000000
      if (nextiter .eq. 0)  nextiter = 1
      if (iprint .lt. 0)  iprint = 1
      if (iwrite .lt. 0)  iwrite = 1
      if (stpmax .eq. 0.0d0)  stpmax = 5.0d0
      if (hguess .eq. 0.0d0)  hguess = 0.4d0
      angmax = 180.0d0
c
c     search the keywords for optimization parameters
c
      do i = 1, nkey
         next = 1
         record = keyline(i)
         call gettext (record,keyword,next)
         call upcase (keyword)
         string = record(next:120)
         if (keyword(1:7) .eq. 'FCTMIN ') then
            read (string,*,err=20,end=20)  fctmin
         else if (keyword(1:8) .eq. 'MAXITER ') then
            read (string,*,err=20,end=20)  maxiter
         else if (keyword(1:9) .eq. 'NEXTITER ') then
            read (string,*,err=20,end=20)  nextiter
         else if (keyword(1:7) .eq. 'HGUESS ') then
            read (string,*,err=20,end=20)  hguess
         else if (keyword(1:8) .eq. 'STEPMAX ') then
            read (string,*,err=20,end=20)  stpmax
         else if (keyword(1:7) .eq. 'ANGMAX ') then
            read (string,*,err=20,end=20)  angmax
         end if
   20    continue
      end do
c
c     print initial information prior to first iteration
c
      if (iprint .gt. 0) then
         write (iout,30)
   30    format (/,' Optimally Conditioned Variable Metric',
     &             ' Optimization :')
         write (iout,40)
   40    format (/,' VM Iter     F Value       G RMS     F Move',
     &              '    X Move      Angle   FG Call',/)
      end if
c
c     perform dynamic allocation of some local arrays
c
      allocate (x0old(nvar))
      allocate (x(nvar))
      allocate (g(nvar))
      allocate (hq(nvar))
      allocate (search(nvar))
      allocate (s(mvar))
      allocate (w(mvar))
      allocate (k(mvar))
      allocate (k0(mvar))
      allocate (m(mvar))
      allocate (n(mvar))
      allocate (p(mvar))
      allocate (q(mvar))
      allocate (u(mvar))
      allocate (h(nvar,mvar))
c
c     evaluate the function and get the initial gradient
c
      niter = nextiter - 1
      maxiter = niter + maxiter
      do i = 1, nvar
         x0old(i) = x0(i)
      end do
      ncalls = 1
      f0 = fgvalue (x0,g)
      f0old = f0
c
c     set the "h" matrix to a diagonal upon restarting
c
      do while (.not. done)
         if (restart) then
            do j = 1, mvar
               do i = 1, nvar
                  h(i,j) = 0.0d0
               end do
            end do
            do j = 1, mvar
               h(j,j) = hguess
            end do
            do j = 1, mvar
               k0(j) = 0.0d0
               do i = 1, nvar
                  k0(j) = k0(j) + h(i,j)*g(i)
               end do
               w(j) = k0(j)
            end do
            restart = .false.
         end if
c
c     start the next iteration using either an updated "h"
c     matrix or the "h" matrix from the previous iteration
c
         gnorm = 0.0d0
         grms = 0.0d0
         do i = 1, nvar
            gnorm = gnorm + g(i)**2
            grms = grms + (g(i)*scale(i))**2
         end do
         gnorm = sqrt(gnorm)
         grms = sqrt(grms) / rms
         xmove = 0.0d0
         if (niter .ne. 0) then
            do i = 1, nvar
               xmove = xmove + ((x0(i)-x0old(i))/scale(i))**2
               x0old(i) = x0(i)
            end do
            xmove = sqrt(xmove) / rms
            if (coordtype .eq. 'INTERNAL') then
               xmove = radian * xmove
            end if
            fmove = f0old - f0
            f0old = f0
         end if
c
c     print intermediate results for the current iteration
c
         if (iprint .gt. 0) then
            if (niter .eq. 0) then
               if (f0.lt.1.0d8 .and. f0.gt.-1.0d7 .and.
     &                    grms.lt.1.0d6) then
                  write (iout,50)  niter,f0,grms,ncalls
   50             format (i6,f14.4,f12.4,32x,i9)
               else
                  write (iout,60)  niter,f0,grms,ncalls
   60             format (i6,d14.4,d12.4,32x,i9)
               end if
            else if (mod(niter,iprint) .eq. 0) then
               if (f0.lt.1.0d8 .and. f0.gt.-1.0d7 .and.
     &             grms.lt.1.0d6 .and. fmove.lt.1.0d5) then
                  write (iout,70)  niter,f0,grms,fmove,
     &                             xmove,sgangle,ncalls
   70             format (i6,f14.4,f12.4,f11.4,f10.4,f11.4,i9)
               else
                  write (iout,80)  niter,f0,grms,fmove,
     &                             xmove,sgangle,ncalls
   80             format (i6,d14.4,d12.4,d11.4,f10.4,f11.4,i9)
               end if
            end if
         end if
c
c     write intermediate results for the current iteration
c
         if (iwrite .gt. 0) then
            if (mod(niter,iwrite) .eq. 0) then
               call optsave (niter,f0,x0)
            end if
         end if
c
c     before starting the next iteration, check to see whether
c     the gradient norm, function decrease or iteration limit
c     termination criteria have been satisfied
c
         if (grms.lt.grdmin .or. f0.lt.fctmin
     &          .or. niter.ge.maxiter) then
            if (iprint .gt. 0) then
               if (niter.ne.0 .and. mod(niter,iprint).ne.0) then
                  if (f0.lt.1.0d8 .and. f0.gt.-1.0d7 .and.
     &                grms.lt.1.0d6 .and. fmove.lt.1.0d5) then
                     write (iout,90)  niter,f0,grms,fmove,
     &                                xmove,sgangle,ncalls
   90                format (i6,f14.4,f12.4,f11.4,f10.4,f11.4,i9)
                  else
                     write (iout,100)  niter,f0,grms,fmove,
     &                                 xmove,sgangle,ncalls
  100                format (i6,d14.4,d12.4,d11.4,f10.4,f11.4,i9)
                  end if
               end if
               if (niter .ge. maxiter)  status = 'IterLimit'
               if (f0 .lt. fctmin)  status = 'SmallFct '
               if (grms .lt. grdmin)  status = 'SmallGrad'
               if (status .eq. 'IterLimit') then
                  write (iout,110)  status
  110             format (/,' OCVM  --  Incomplete Convergence',
     &                       ' due to ',a9)
               else
                  write (iout,120)  status
  120             format (/,' OCVM  --  Normal Termination',
     &                       ' due to ',a9)
               end if
            end if
            if (iwrite .gt. 0) then
               if (mod(niter,iwrite) .ne. 0) then
                  call optsave (niter,f0,x)
               end if
            end if
            done = .true.
            goto 170
         end if
c
c     start of the next iteration
c
         niter = niter + 1
         sg = 0.0d0
         snorm = 0.0d0
         do j = 1, mvar
            s(j) = -k0(j)
            snorm = snorm + s(j)**2
            sg = sg - s(j)*g(j)
         end do
         f0prime = -snorm
         snorm = sqrt(snorm)
         cosang = sg / (snorm*gnorm)
         cosang = min(1.0d0,max(-1.0d0,cosang))
         sgangle = radian * acos(cosang)
         if (sgangle .gt. angmax) then
            nbig = nbig + 1
         else
            nbig = 0
         end if
         zeta = 2.0d0
         if (4.0d0*(f0-fctmin) .lt. -f0prime) then
            do j = 1, mvar
               s(j) = -s(j) * (4.0d0*(f0-fctmin)/f0prime)
            end do
            f0prime = -4.0d0 * (f0-fctmin)
         end if
c
c     location of the next starting point
c
         nstep = 0
  130    continue
         do i = 1, nvar
            search(i) = 0.0d0
         end do
         do j = 1, mvar
            do i = 1, nvar
               search(i) = search(i) + h(i,j)*s(j)
            end do
         end do
         srchnorm = 0.0d0
         do i = 1, nvar
            srchnorm = srchnorm + search(i)**2
         end do
         srchnorm = sqrt(srchnorm)
         if (srchnorm .gt. stpmax) then
            do j = 1, mvar
               s(j) = (stpmax/srchnorm) * s(j)
            end do
            do i = 1, nvar
               search(i) = (stpmax/srchnorm) * search(i)
            end do
            f0prime = (stpmax/srchnorm) * f0prime
            zeta = 0.5d0
         end if
c
c     invoke abnormal termination if -f0prime is too small
c
         if (-f0prime .lt. eps) then
            if (iprint .gt. 0) then
               if (niter.ne.0 .and. mod(niter,iprint).ne.0) then
                  if (f0.lt.1.0d8 .and. f0.gt.-1.0d7 .and.
     &                       grms.lt.1.0d6) then
                     write (iout,140)  niter,f0,grms,0.0,0.0,
     &                                 sgangle,ncalls
  140                format (i6,f14.4,f12.4,f11.4,f10.4,f11.4,i9)
                  else
                     write (iout,150)  niter,f0,grms,0.0,0.0,
     &                                 sgangle,ncalls
  150                format (i6,d14.4,d12.4,f11.4,f10.4,f11.4,i9)
                  end if
               end if
               status = 'SmallMove'
               write (iout,160)  status
  160          format (/,' OCVM  --  Incomplete Convergence',
     &                    ' due to ',a9)
            end if
            if (iwrite .gt. 0) then
               if (mod(niter,iwrite) .ne. 0) then
                  call optsave (niter,f0,x)
               end if
            end if
            done = .true.
            goto 170
         end if
         do i = 1, nvar
            x(i) = x0(i) + search(i)
         end do
         ncalls = ncalls + 1
         f = fgvalue (x,g)
         if (f .ge. f0) then
            do j = 1, mvar
               s(j) = 0.5d0 * s(j)
            end do
            f0prime = 0.5d0 * f0prime
            zeta = 0.5d0
            goto 130
         end if
c
c     decide whether to update or take another step
c
         do j = 1, mvar
            k(j) = 0.0d0
            do i = 1, nvar
               k(j) = k(j) + h(i,j)*g(i)
            end do
         end do
         fprime = 0.0d0
         do j = 1, mvar
            fprime = fprime + k(j)*s(j)
         end do
         b0 = fprime - f0prime
         do j = 1, mvar
            m(j) = s(j) + k0(j) - k(j)
            k0(j) = k(j)
         end do
         do i = 1, nvar
            x0(i) = x(i)
         end do
         f0 = f
         f0prime = fprime
         if (b0 .lt. eps) then
            nstep = nstep + 1
            if (nstep .ge. maxstep) then
               restart = .true.
               goto 170
            end if
            do j = 1, mvar
               s(j) = s(j) * zeta
            end do
            f0prime = f0prime * zeta
            goto 130
         end if
c
c     check to see if we need to update
c
         if (nbig .ge. maxbig) then
            restart = .true.
            goto 170
         end if
         m2 = 0.0d0
         do j = 1, mvar
            m2 = m2 + m(j)**2
         end do
         if (m2 .lt. eps) then
            goto 170
         end if
         v = 0.0d0
         do j = 1, mvar
            v = v + m(j)*s(j)
         end do
         micron = v - m2
         mw = 0.0d0
         do j = 1, mvar
            mw = mw + m(j)*w(j)
         end do
         do j = 1, mvar
            u(j) = w(j) - m(j)*(mw/m2)
         end do
         u2 = 0.0d0
         do j = 1, mvar
            u2 = u2 + u(j)**2
         end do
         if (m2*u2 .ge. eps) then
            us = 0.0d0
            do j = 1, mvar
               us = us + u(j)*s(j)
            end do
            do j = 1, mvar
               n(j) = u(j)*(us/u2)
            end do
            n2 = us * us/u2
         else
            do j = 1, mvar
               n(j) = 0.0d0
            end do
            n2 = 0.0d0
         end if
c
c     test inner product of projected s and del-g
c
         b = n2 + micron * v/m2
         if (b .lt. eps) then
            do j = 1, mvar
               n(j) = s(j) - m(j)*(v/m2)
            end do
            n2 = b0 - micron * v/m2
            b = b0
         end if
c
c     set "gamma" and "delta" for the update
c
         if (micron*v .ge. m2*n2) then
            gamma = 0.0d0
            delta = sqrt(v/micron)
         else
            a = b - micron
            c = b + v
            gamma = sqrt((1.0d0-micron*v/(m2*n2))/(a*b))
            delta = sqrt(c/a)
            if (c .lt. a) then
               gamma = -gamma
            end if
         end if
c
c     perform the update of the "h" matrix
c
         alpha = v + micron*delta + m2*n2*gamma
         do j = 1, mvar
            p(j) = m(j)*(delta-n2*gamma) + n(j)*(gamma*v)
            q(j) = m(j)*((1.0d0+n2*gamma)/alpha)
     &             - n(j)*(gamma * micron/alpha)
            w(j) = m(j)*(n2*(1.0d0+gamma*micron*v)/alpha)
     &             - n(j)*((1.0d0+delta)*micron*v/alpha)
         end do
         qk0 = 0.0d0
         do j = 1, mvar
            qk0 = qk0 + q(j)*k0(j)
         end do
         do j = 1, mvar
            k0(j) = k0(j) + p(j)*qk0
         end do
         do i = 1, nvar
            hq(i) = 0.0d0
         end do
         do j = 1, mvar
            do i = 1, nvar
               hq(i) = hq(i) + h(i,j)*q(j)
            end do
         end do
         do j = 1, mvar
            do i = 1, nvar
               h(i,j) = h(i,j) + hq(i)*p(j)
            end do
         end do
         if (n2 .le. 0.0d0) then
            do j = 1, mvar
               w(j) = k0(j)
            end do
         end if
  170    continue
      end do
c
c     perform deallocation of some local arrays
c
      deallocate (x0old)
      deallocate (x)
      deallocate (g)
      deallocate (hq)
      deallocate (search)
      deallocate (s)
      deallocate (w)
      deallocate (k)
      deallocate (k0)
      deallocate (m)
      deallocate (n)
      deallocate (p)
      deallocate (q)
      deallocate (u)
      deallocate (h)
      return
      end


#include "../defines.inc"
pure function in_confinement(RP, NT, IT1, IT2)
   use params, only: dp, wlcsim_params
   use inputparams, only: MAXPARAMLEN

   implicit none
   Interface
      pure function inside_confinement(RR)
         use params, only: dp
         real(dp), intent(in) :: RR(3)
         logical inside_confinement
      end function
   end Interface
   integer, intent(in) :: IT1, IT2, NT
   real(dp), intent(in) :: RP(3, NT)
   logical in_confinement
   integer i

   in_confinement = .TRUE.
   do I = IT1, IT2
      in_confinement = inside_confinement(RP(:, I))
      if (in_confinement .eqv. .False.) return
   enddo
end function
pure function list_confinement()
   use params, only: dp, wlcsim_params, nan, wlc_RP, wlc_pointsMoved, wlc_nPointsMoved
   use inputparams, only: MAXPARAMLEN

   implicit none
   Interface
      pure function inside_confinement(RR)
         use params, only: dp
         real(dp), intent(in) :: RR(3)
         logical inside_confinement
      end function
   end Interface

   logical list_confinement
   integer i, j

   list_confinement = .TRUE.
   do I = 1, wlc_nPointsMoved
      J = wlc_pointsMoved(I)
      list_confinement = inside_confinement(wlc_RP(:, J))
      if (list_confinement .eqv. .False.) return
   enddo

end function
pure function inside_confinement(RR)
   use params, only: dp, wlcsim_params, nan
   use inputparams, only: MAXPARAMLEN

   implicit none

   real(dp), intent(in) :: RR(3)
   logical inside_confinement
   real(dp) rad, length, r2
   real(dp), parameter :: center(3) = [WLC_P__LBOX_X/2.0_dp, &
                                       WLC_P__LBOX_Y/2.0_dp, &
                                       WLC_P__LBOX_Z/2.0_dp]

   real(dp) centers(3)
   integer ix, iy, iz
   inside_confinement = .True.
   if (WLC_P__CONFINETYPE == 'platesInZperiodicXY') then
      ! Confinement only in the z-direction
      ! limits: 0 and LBox(3)
      if (WLC_P__BOUNDARY_TYPE == "ExtendBinsPast") then
         if ((RR(3) < 0.0_dp + WLC_P__DBIN) .or. &
             (RR(3) > WLC_P__CONFINEMENT_SLIT_WIDTH - WLC_P__DBIN)) then
            inside_confinement = .False.
         endif
      elseif (WLC_P__BOUNDARY_TYPE == "SolidEdgeBin") then
         if ((RR(3) < 0.0_dp) .or. &
             (RR(3) > WLC_P__CONFINEMENT_SLIT_WIDTH)) then
            inside_confinement = .False.
         endif
      endif
   elseif (WLC_P__CONFINETYPE == 'cube') then
      if (WLC_P__BOUNDARY_TYPE == "ExtendBinsPast") then
         if ((RR(1) < 0.0_dp + WLC_P__DBIN) &
             .or. (RR(1) > WLC_P__CONFINEMENT_CUBE_LENGTH - WLC_P__DBIN) &
             .or. (RR(2) < 0.0_dp + WLC_P__DBIN) &
             .or. (RR(2) > WLC_P__CONFINEMENT_CUBE_LENGTH - WLC_P__DBIN) &
             .or. (RR(3) < 0.0_dp + WLC_P__DBIN) &
             .or. (RR(3) > WLC_P__CONFINEMENT_CUBE_LENGTH - WLC_P__DBIN)) then
            inside_confinement = .False.
         endif
      elseif (WLC_P__BOUNDARY_TYPE == "SolidEdgeBin") then
         if ((RR(1) < 0.0_dp) &
             .or. (RR(1) > WLC_P__CONFINEMENT_CUBE_LENGTH) &
             .or. (RR(2) < 0.0_dp) &
             .or. (RR(2) > WLC_P__CONFINEMENT_CUBE_LENGTH) &
             .or. (RR(3) < 0.0_dp) &
             .or. (RR(3) > WLC_P__CONFINEMENT_CUBE_LENGTH)) then
            inside_confinement = .False.
         endif
      endif
   elseif (WLC_P__CONFINETYPE == 'sphere') then
      ! sphere with given diameter
      rad = (WLC_P__CONFINEMENT_SPHERE_DIAMETER/2.0_dp)**2
      if ((RR(1) - center(1))**2 + (RR(2) - center(2))**2 + &
          (RR(3) - center(3))**2 > rad) then
         inside_confinement = .False.
      endif
   elseif (WLC_P__CONFINETYPE == 'excludedShpereInPeriodic') then
      ! Periodic boundary conditions with an excluded sphere
      do ix = 1, WLC_P__N_SPHERES_TO_SIDE
      do iy = 1, WLC_P__N_SPHERES_TO_SIDE
      do iz = 1, WLC_P__N_SPHERES_TO_SIDE
         centers(1) = (real(ix, dp) - 0.5_DP)*WLC_P__LBOX_X/real(WLC_P__N_SPHERES_TO_SIDE, dp)
         centers(2) = (real(ix, dp) - 0.5_DP)*WLC_P__LBOX_Y/real(WLC_P__N_SPHERES_TO_SIDE, dp)
         centers(3) = (real(ix, dp) - 0.5_DP)*WLC_P__LBOX_Z/real(WLC_P__N_SPHERES_TO_SIDE, dp)
         rad = (WLC_P__CONFINEMENT_SPHERE_DIAMETER/2.0_dp)**2
         if ((modulo(RR(1), WLC_P__LBOX_X) - centers(1))**2 + &
             (modulo(RR(2), WLC_P__LBOX_Y) - centers(2))**2 + &
             (modulo(RR(3), WLC_P__LBOX_Z) - centers(3))**2 < rad) then
            inside_confinement = .False.
         endif
      enddo
      enddo
      enddo

   elseif (WLC_P__CONFINETYPE == 'ecoli') then
      ! cylinder with hemispherical caps, one tip at origin
      ! full length - lbox(1)/CONFINEMENT_ECOLI_LENGTH
      ! diameter - lbox(2:3)/CONFINEMENT_ECOLI_DIAMETER
      length = WLC_P__CONFINEMENT_ECOLI_LENGTH
      rad = WLC_P__CONFINEMENT_ECOLI_DIAMETER/2
      r2 = RR(2)**2 + RR(3)**2
      if (r2 > rad &
          .or. RR(1) > length &
          .or. RR(1) < 0.0_dp) then
         inside_confinement = .False.
         return
      elseif (RR(1) >= rad .and. RR(1) <= length - rad) then
         ! we're inside the main cylinder section, no need to check
         ! intersection with caps
         return
         ! if we are inside cap touching origin
      elseif (RR(1) < rad) then
         r2 = r2 + (RR(1) - rad)**2
         ! we are inside cap far from origin
      else
         r2 = r2 + (RR(1) - length + rad)**2
      endif
      if (r2 > rad*rad) then
         inside_confinement = .False.
      endif
      ! always inside confinement otherwise
      ! print*, "Undefined comfone Type"
      ! stop 1
   endif
   return

end function


      subroutine phase3(r1,r2,r3,r6,r7,p1,p2,p3,p4,p5,p6,p7,wt)
c----generate phase space for 2-->3 process
c----r(mxdim),p1(4),p2(4) are inputs
c----incoming p1 and p2 reversed in sign from physical values
c----i.e. phase space for -p1-p2 --> p3+p4+p5
c----with all 2 pi's (ie 1/(2*pi)^5)
c----(p4,p5) are dummies
      implicit none
      include 'constants.f'
      include 'limits.f'
      include 'masses.f'
      include 'process.f'
      include 'hdecaymode.f'

      integer j
      double precision r1,r2,r3,r6,r7
      double precision p1(4),p2(4),p3(4),p4(4),p5(4),p6(4),p7(4)
      double precision p12(4),p34(4),smin
      double precision wt,wt125,wt34,wt0,m5
      parameter(wt0=1d0/twopi)

      m5=0d0
      if     ((case .eq. 'W_cjet') .or. (case .eq. 'Wcs_ms')) then
         m5=mc
      elseif  (case .eq. 'Wbfrmc') then
         m5=mb
      elseif ((case .eq. 'W_tndk') .or. (case .eq. 'vlchwn')) then
         m5=mt
      endif

      do j=1,4
      p12(j)=-p1(j)-p2(j)
      p6(j)=0d0
      p7(j)=0d0
      enddo
c      smin=0d0
      smin=wsqmin ! for more efficient generation

c---generate p5 and p34,
c---smin is the minimum inv mass of 34 system
c---m5 is the mass of p5
      call phi1_2m(m5,r1,r2,r3,smin,p12,p5,p34,wt125,*99)
c---decay 34-system
      if (hdecaymode == 'tlta') then
      call phi3m(r6,r7,p34,p3,p4,mtau,mtau,wt34,*99)
      elseif (hdecaymode == 'bqba') then
      call phi3m(r6,r7,p34,p3,p4,mb,mb,wt34,*99)
      else
      call phi3m0(r6,r7,p34,p3,p4,wt34,*99)
      endif
      wt=wt0*wt125*wt34
      return
 99   continue
      wt=0d0
      return
      end


      subroutine voltage(y,bandw,a)
************************************************************************
*
*   VOLTAGE -- Solve the voltage equation.
*
************************************************************************
      include "parameters.h"

      integer nepn
      parameter(nepn=npde+2)

      double precision g7w,g7a0,g7a1,g7a2
      common /g2d7/ g7w(7),g7a0(7),g7a1(7),g7a2(7)

      double precision uscf,vscf
      common /data1/ uscf,vscf

      integer nnod,ntri,tvtx
      common /mfe010/ nnod,ntri /mfe011/ tvtx(0:2,mtri)

      double precision bv
      integer nbnod,bnod,bc
      common /mfe030/ bv(nepn,mbnd),nbnod,bnod(2,mbnd),bc(nepn,10)

      double precision lambda,eps,usera
      common /mfe100/ lambda,eps,usera(23)

      integer bandw,i,j,k,vtx0,vtx1,vtx2,jbnod,jbc
      double precision y(nepn,nnod),a(0:bandw,nnod),b(mnod)
      double precision u0,x0,y0,u1,x1,y1,u2,x2,y2
      double precision dx0,dx1,dx2,dy0,dy1,dy2,area
      double precision a00,a10,a20,a11,a21,a22,fa0,fa1,fa2,fatuk,bval
*     ==================================================================
*      Zero the storage for the stiffness matrix and RHS vector.
*     ==================================================================
      do 110 i=0,bandw
      do 110 j=1,nnod
  110 a(i,j) = 0.

      do 120 j=1,nnod
  120 b(j) = 0.
*     ==================================================================
*      Compute the stiffness matrix and RHS vector triangle by triangle.
*     ==================================================================
      do 200 j=1,ntri
        vtx0 = tvtx(0,j)
        vtx1 = tvtx(1,j)
        vtx2 = tvtx(2,j)
*       -----------------------------
*        Gather the local variables.
*       -----------------------------
        u0 = y(1,vtx0)
        x0 = y(3,vtx0)
        y0 = y(4,vtx0)
        u1 = y(1,vtx1)
        x1 = y(3,vtx1)
        y1 = y(4,vtx1)
        u2 = y(1,vtx2)
        x2 = y(3,vtx2)
        y2 = y(4,vtx2)
*       ------------------------------------------------
*        Differences along the edges and triangle area.
*       ------------------------------------------------
        dx0 = x2 - x1
        dx1 = x0 - x2
        dx2 = x1 - x0
        dy0 = y2 - y1
        dy1 = y0 - y2
        dy2 = y1 - y0
        area = .5*(dx1*dy2 - dx2*dy1)
*       ----------------------------------------------------------
*        Lower triangular elements of the local stiffness matrix.
*       ----------------------------------------------------------
        a00 = (.25/area)*(dx0*dx0 + dy0*dy0)
        a10 = (.25/area)*(dx1*dx0 + dy1*dy0)
        a20 = (.25/area)*(dx2*dx0 + dy2*dy0)
        a11 = (.25/area)*(dx1*dx1 + dy1*dy1)
        a21 = (.25/area)*(dx2*dx1 + dy2*dy1)
        a22 = (.25/area)*(dx2*dx2 + dy2*dy2)
*       ------------------------------------------------------------
*        Local RHS vector using a 7-pt gaussian quadrature formula.
*       ------------------------------------------------------------
        fa0 = 0.
        fa1 = 0.
        fa2 = 0.
        do 210 k=1,7
         fatuk = (1.-exp(uscf*(u0*g7a0(k)+u1*g7a1(k)+u2*g7a2(k))))/vscf
         fa0 = fa0 + g7w(k)*fatuk*g7a0(k)
         fa1 = fa1 + g7w(k)*fatuk*g7a1(k)
         fa2 = fa2 + g7w(k)*fatuk*g7a2(k)
  210   continue
        fa0 = area*fa0
        fa1 = area*fa1
        fa2 = area*fa2
*       ----------------------------------------------------
*        Scatter the local stiffness matrix and RHS vector.
*       ----------------------------------------------------
        b(vtx0) = b(vtx0) - fa0
        b(vtx1) = b(vtx1) - fa1
        b(vtx2) = b(vtx2) - fa2

        a(0,vtx0) = a(0,vtx0) + a00
        a(0,vtx1) = a(0,vtx1) + a11
        a(0,vtx2) = a(0,vtx2) + a22

        if(vtx1 .gt. vtx0) then
         a(vtx1-vtx0,vtx0) = a(vtx1-vtx0,vtx0) + a10
        else
         a(vtx0-vtx1,vtx1) = a(vtx0-vtx1,vtx1) + a10
         endif

        if(vtx2 .gt. vtx0) then
         a(vtx2-vtx0,vtx0) = a(vtx2-vtx0,vtx0) + a20
        else
         a(vtx0-vtx2,vtx2) = a(vtx0-vtx2,vtx2) + a20
         endif

        if(vtx2 .gt. vtx1) then
         a(vtx2-vtx1,vtx1) = a(vtx2-vtx1,vtx1) + a21
        else
         a(vtx1-vtx2,vtx2) = a(vtx1-vtx2,vtx2) + a21
         endif
  200 continue
*     ==================================================================
*      Modify the equations for Dirichlet boundary conditions.
*     ==================================================================
      do 300 j=1,nbnod

        jbnod = bnod(1,j)
        jbc   = bnod(2,j)
        if(bc(2,jbc) .ne. 1) go to 300

        bval = bv(2,j)

        do 320 i=1,min(bandw,nnod-jbnod)
         b(jbnod+i) = b(jbnod+i) - bval*a(i,jbnod)
         a(i,jbnod) = 0.
  320   continue

        do 310 i=1,min(bandw,jbnod-1)
         b(jbnod-i) = b(jbnod-i) - bval*a(i,jbnod-i)
         a(i,jbnod-i) = 0.
  310   continue

        a(0,jbnod) = 1.
        b(jbnod) = bval

  300 continue
*     ==================================================================
*      Solve the equations.
*     ==================================================================
      call sbfct(nnod,bandw,a)
      call sbslv(nnod,bandw,a,b)

*     Store the solution.
      do 400 j=1,nnod
  400 y(2,j) = b(j)

      return
      end


*     MB02SD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2020 NICONET e.V.
*
*     .. Parameters ..
      DOUBLE PRECISION ZERO
      PARAMETER        ( ZERO = 0.0D0 )
      INTEGER          NIN, NOUT
      PARAMETER        ( NIN = 5, NOUT = 6 )
      INTEGER          NMAX, NRHMAX
      PARAMETER        ( NMAX = 20, NRHMAX = 20 )
      INTEGER          LDB, LDH
      PARAMETER        ( LDB = NMAX, LDH = NMAX )
      INTEGER          LDWORK
      PARAMETER        ( LDWORK = 3*NMAX )
      INTEGER          LIWORK
      PARAMETER        ( LIWORK = NMAX )
*     .. Local Scalars ..
      DOUBLE PRECISION HNORM, RCOND
      INTEGER          I, INFO, INFO1, J, N, NRHS
      CHARACTER*1      NORM, TRANS
*     .. Local Arrays ..
      DOUBLE PRECISION H(LDH,NMAX), B(LDB,NRHMAX), DWORK(LDWORK)
      INTEGER          IPIV(NMAX), IWORK(LIWORK)
*     .. External Functions ..
      DOUBLE PRECISION DLAMCH, DLANHS
      EXTERNAL         DLAMCH, DLANHS
*     .. External Subroutines ..
      EXTERNAL         DLASET, MB02RD, MB02SD, MB02TD
*     .. Executable Statements ..
*
      WRITE ( NOUT, FMT = 99999 )
*     Skip the heading in the data file and read in the data.
      READ ( NIN, FMT = '()' )
      READ ( NIN, FMT = * ) N, NRHS, NORM, TRANS
      IF ( N.LT.0 .OR. N.GT.NMAX ) THEN
         WRITE ( NOUT, FMT = 99994 ) N
      ELSE
         READ ( NIN, FMT = * ) ( ( H(I,J), J = 1,N ), I = 1,N )
         IF ( NRHS.LT.0 .OR. NRHS.GT.NRHMAX ) THEN
            WRITE ( NOUT, FMT = 99993 ) NRHS
         ELSE
            READ ( NIN, FMT = * ) ( ( B(I,J), J = 1,NRHS ), I = 1,N )
            IF ( N.GT.2 )
     $         CALL DLASET( 'Lower', N-2, N-2, ZERO, ZERO, H(3,1), LDH )
*           Compute the LU factorization of the upper Hessenberg matrix.
            CALL MB02SD( N, H, LDH, IPIV, INFO )
*           Estimate the reciprocal condition number of the matrix.
            HNORM = DLANHS( NORM, N, H, LDH, DWORK )
            CALL MB02TD( NORM, N, HNORM, H, LDH, IPIV, RCOND, IWORK,
     $                   DWORK, INFO1 )
            IF ( INFO.EQ.0 .AND. RCOND.GT.DLAMCH( 'Epsilon' ) ) THEN
*              Solve the linear system.
               CALL MB02RD( TRANS, N, NRHS, H, LDH, IPIV, B, LDB, INFO )
*
               WRITE ( NOUT, FMT = 99997 )
            ELSE
               WRITE ( NOUT, FMT = 99998 ) INFO
            END IF
               DO 10 I = 1, N
                  WRITE ( NOUT, FMT = 99996 ) ( B(I,J), J = 1,NRHS )
   10          CONTINUE
            WRITE ( NOUT, FMT = 99995 ) RCOND
         END IF
      END IF
      STOP
*
99999 FORMAT (' MB02SD EXAMPLE PROGRAM RESULTS',/1X)
99998 FORMAT (' INFO on exit from MB02SD = ',I2)
99997 FORMAT (' The solution matrix is ')
99996 FORMAT (20(1X,F8.4))
99995 FORMAT (/' Reciprocal condition number = ',D12.4)
99994 FORMAT (/' N is out of range.',/' N = ',I5)
99993 FORMAT (/' NRHS is out of range.',/' NRHS = ',I5)
      END


      SUBROUTINE ROM2 (A,B,SUM,DMIN)
C
C     FOR THE SOMMERFELD GROUND OPTION, ROM2 INTEGRATES OVER THE SOURCE
C     SEGMENT TO OBTAIN THE TOTAL FIELD DUE TO GROUND.  THE METHOD OF
C     VARIABLE INTERVAL WIDTH ROMBERG INTEGRATION IS USED.  THERE ARE 9
C     FIELD COMPONENTS - THE X, Y, AND Z COMPONENTS DUE TO CONSTANT,
C     SINE, AND COSINE CURRENT DISTRIBUTIONS.
C
      COMPLEX SUM,G1,G2,G3,G4,G5,T00,T01,T10,T02,T11,T20
      DIMENSION SUM(9), G1(9), G2(9), G3(9), G4(9), G5(9), T01(9), T10(9
     1), T20(9)
      DATA NM,NTS,NX,N/65536,4,1,9/,RX/1.E-4/
      Z=A
      ZE=B
      S=B-A
      IF (S.GE.0.) GO TO 1
      PRINT 18
      STOP
1     EP=S/(1.E4*NM)
      ZEND=ZE-EP
      DO 2 I=1,N
2     SUM(I)=(0.,0.)
      NS=NX
      NT=0
      CALL SFLDS (Z,G1)
3     DZ=S/NS
      IF (Z+DZ.LE.ZE) GO TO 4
      DZ=ZE-Z
      IF (DZ.LE.EP) GO TO 17
4     DZOT=DZ*.5
      CALL SFLDS (Z+DZOT,G3)
      CALL SFLDS (Z+DZ,G5)
5     TMAG1=0.
      TMAG2=0.
C
C     EVALUATE 3 POINT ROMBERG RESULT AND TEST CONVERGENCE.
C
      DO 6 I=1,N
      T00=(G1(I)+G5(I))*DZOT
      T01(I)=(T00+DZ*G3(I))*.5
      T10(I)=(4.*T01(I)-T00)/3.
      IF (I.GT.3) GO TO 6
      TR=REAL(T01(I))
      TI=AIMAG(T01(I))
      TMAG1=TMAG1+TR*TR+TI*TI
      TR=REAL(T10(I))
      TI=AIMAG(T10(I))
      TMAG2=TMAG2+TR*TR+TI*TI
6     CONTINUE
      TMAG1=SQRT(TMAG1)
      TMAG2=SQRT(TMAG2)
      CALL TEST(TMAG1,TMAG2,TR,0.D0,0.D0,TI,DMIN)
      IF(TR.GT.RX)GO TO 8
      DO 7 I=1,N
7     SUM(I)=SUM(I)+T10(I)
      NT=NT+2
      GO TO 12
8     CALL SFLDS (Z+DZ*.25,G2)
      CALL SFLDS (Z+DZ*.75,G4)
      TMAG1=0.
      TMAG2=0.
C
C     EVALUATE 5 POINT ROMBERG RESULT AND TEST CONVERGENCE.
C
      DO 9 I=1,N
      T02=(T01(I)+DZOT*(G2(I)+G4(I)))*.5
      T11=(4.*T02-T01(I))/3.
      T20(I)=(16.*T11-T10(I))/15.
      IF (I.GT.3) GO TO 9
      TR=REAL(T11)
      TI=AIMAG(T11)
      TMAG1=TMAG1+TR*TR+TI*TI
      TR=REAL(T20(I))
      TI=AIMAG(T20(I))
      TMAG2=TMAG2+TR*TR+TI*TI
9     CONTINUE
      TMAG1=SQRT(TMAG1)
      TMAG2=SQRT(TMAG2)
      CALL TEST(TMAG1,TMAG2,TR,0.D0,0.D0,TI,DMIN)
      IF(TR.GT.RX)GO TO 14
10    DO 11 I=1,N
11    SUM(I)=SUM(I)+T20(I)
      NT=NT+1
12    Z=Z+DZ
      IF (Z.GT.ZEND) GO TO 17
      DO 13 I=1,N
13    G1(I)=G5(I)
      IF (NT.LT.NTS.OR.NS.LE.NX) GO TO 3
      NS=NS/2
      NT=1
      GO TO 3
14    NT=0
      IF (NS.LT.NM) GO TO 15
      PRINT 19, Z
      GO TO 10
15    NS=NS*2
      DZ=S/NS
      DZOT=DZ*.5
      DO 16 I=1,N
      G5(I)=G3(I)
16    G3(I)=G2(I)
      GO TO 5
17    CONTINUE
      RETURN
C
18    FORMAT (30H ERROR - B LESS THAN A IN ROM2)
19    FORMAT (33H ROM2 -- STEP SIZE LIMITED AT Z =,E12.5)
      END


module qinit_module

    implicit none
    save

    logical, private :: module_setup = .false.

    ! Type of q initialization
    integer, public :: qinit_type

    integer, public :: min_level_qinit
    integer, public :: max_level_qinit

    ! Geometry
    real(kind=8), public :: x_low_qinit
    real(kind=8), public :: y_low_qinit
    real(kind=8), public :: t_low_qinit
    real(kind=8), public :: x_hi_qinit
    real(kind=8), public :: y_hi_qinit
    real(kind=8), public :: t_hi_qinit
    real(kind=8), public :: dx_qinit
    real(kind=8), public :: dy_qinit

    ! Work array
    real(kind=8), private, allocatable :: qinit(:)

    integer, private :: mx_qinit
    integer, private :: my_qinit

    ! Specifc types of intialization
    ! Type of perturbation to add
    integer, private :: wave_family
    real(kind=8), private :: init_location(2), epsilon
    real(kind=8), private :: angle, sigma

contains

    subroutine set_qinit(fname)

        use geoclaw_module, only: GEO_PARM_UNIT

        implicit none

        ! Subroutine arguments
        character(len=*), optional, intent(in) :: fname

        ! File handling
        integer, parameter :: unit = 7
        character(len=150) :: qinit_fname

        if (.not.module_setup) then

            write(GEO_PARM_UNIT,*) ' '
            write(GEO_PARM_UNIT,*) '--------------------------------------------'
            write(GEO_PARM_UNIT,*) 'SETQINIT:'
            write(GEO_PARM_UNIT,*) '-------------'

            ! Open the data file
            if (present(fname)) then
                call opendatafile(unit,fname)
            else
                call opendatafile(unit,"qinit.data")
            endif

            read(unit,"(i1)") qinit_type
            if (qinit_type == 0) then
                ! No perturbation specified
                write(GEO_PARM_UNIT,*)  '  qinit_type = 0, no perturbation'
                print *,'  qinit_type = 0, no perturbation'
                return
            else if (qinit_type > 0 .and. qinit_type < 5) then
                read(unit,*) qinit_fname
                read(unit,"(2i2)") min_level_qinit, max_level_qinit

                write(GEO_PARM_UNIT,*) '   min_level, max_level, qinit_fname:'
                write(GEO_PARM_UNIT,*)  min_level_qinit, max_level_qinit, qinit_fname

                call read_qinit(qinit_fname)
            else if (qinit_type >= 5) then
                read(unit,*) epsilon
                read(unit,*) init_location
                read(unit,*) wave_family
                read(unit,*) angle
                read(unit,*) sigma

                write(GEO_PARM_UNIT,*) " epsilon = ",  epsilon
                write(GEO_PARM_UNIT,*) " init_location = ",  init_location
                write(GEO_PARM_UNIT,*) " wave_family = ",  wave_family
                write(GEO_PARM_UNIT,*) " angle = ",  angle
                write(GEO_PARM_UNIT,*) " sigma = ",  sigma
            endif

            close(unit)

            module_setup = .true.
        end if

    end subroutine set_qinit

    subroutine add_perturbation(meqn,mbc,mx,my,xlower,ylower,dx,dy,q,maux,aux)

        use geoclaw_module, only: sea_level, pi, g => grav, rho
        use multilayer_module, only: aux_layer_index, r, eta_init

        implicit none

        ! Subroutine arguments
        integer, intent(in) :: meqn,mbc,mx,my,maux
        real(kind=8), intent(in) :: xlower,ylower,dx,dy
        real(kind=8), intent(inout) :: q(meqn,1-mbc:mx+mbc,1-mbc:my+mbc)
        real(kind=8), intent(inout) :: aux(maux,1-mbc:mx+mbc,1-mbc:my+mbc)

        ! Local
        integer :: i,j
        real(kind=8) :: ximc,xim,x,xc,xip,xipc,yjmc,yjm,y,yc,yjp,yjpc,dq

        real(kind=8) :: xmid,m,x_c,y_c, effective_b
        real(kind=8) :: eigen_vector(6),gamma,lambda,alpha,h_1,h_2,deta

        ! Topography integral function
        real(kind=8) :: topointegral
        do i = 1-mbc, mx+mbc
            x = xlower + (i - 0.5d0)*dx
            xim = x - 0.5d0 * dx
            xip = x + 0.5d0 * dx
            do j = 1-mbc, my+mbc
                y = ylower + (j - 0.5d0) * dy
                yjm = y - 0.5d0 * dy
                yjp = y + 0.5d0 * dy

                ! Check to see if we are in the qinit region at this grid point
                if ((xip > x_low_qinit).and.(xim < x_hi_qinit).and.  &
                    (yjp > y_low_qinit).and.(yjm < y_hi_qinit)) then
                    xipc = min(xip, x_hi_qinit)
                    ximc = max(xim, x_low_qinit)
                    xc = 0.5d0 * (xipc + ximc)

                    yjpc=min(yjp,y_hi_qinit)
                    yjmc=max(yjm,y_low_qinit)
                    yc=0.5d0*(yjmc+yjpc)

                    dq = topointegral(ximc,xipc,yjmc,yjpc,x_low_qinit, &
                                      y_low_qinit,dx_qinit,dy_qinit,mx_qinit, &
                                      my_qinit,qinit,1)
                    dq = dq / ((xipc-ximc)*(yjpc-yjmc))

                    effective_b = max(aux(1,i,j), eta_init(2))
                    q(1,i,j) = max((dq - effective_b) * rho(1), 0.d0)
                endif
            enddo
        enddo

    end subroutine add_perturbation

    ! currently only supports one file type:
    ! x,y,z values, one per line in standard order from NW corner to SE
    ! z is perturbation from standard depth h,hu,hv set in qinit_geo,
    ! if iqinit = 1,2, or 3 respectively.
    ! if iqinit = 4, the z column corresponds to the definition of the
    ! surface elevation eta. The depth is then set as q(i,j,1)=max(eta-b,0)
    subroutine read_qinit(fname)

        use geoclaw_module, only: GEO_PARM_UNIT

        implicit none

        ! Subroutine arguments
        character(len=150) :: fname

        ! Data file opening
        integer, parameter :: unit = 19
        integer :: i,num_points,status
        double precision :: x,y

        print *,'  '
        print *,'Reading qinit data from file  ', fname
        print *,'  '

        write(GEO_PARM_UNIT,*) '  '
        write(GEO_PARM_UNIT,*) 'Reading qinit data from'
        write(GEO_PARM_UNIT,*) fname
        write(GEO_PARM_UNIT,*) '  '

        open(unit=unit, file=fname, iostat=status, status="unknown", &
             form='formatted',action="read")
        if ( status /= 0 ) then
            print *,"Error opening file", fname
            stop
        endif

        ! Initialize counters
        num_points = 0
        mx_qinit = 0

        ! Read in first values, determines x_low and y_hi
        read(unit,*) x_low_qinit,y_hi_qinit
        num_points = num_points + 1
        mx_qinit = mx_qinit + 1

        ! Sweep through first row figuring out mx
        y = y_hi_qinit
        do while (y_hi_qinit == y)
            read(unit,*) x,y
            num_points = num_points + 1
            mx_qinit = mx_qinit + 1
        enddo
        ! We over count by one in the above loop
        mx_qinit = mx_qinit - 1

        ! Continue to count the rest of the lines
        do
            read(unit,*,iostat=status) x,y
            if (status /= 0) exit
            num_points = num_points + 1
        enddo
        if (status > 0) then
            print *,"ERROR:  Error reading qinit file ",fname
            stop
        endif

        ! Extract rest of geometry
        x_hi_qinit = x
        y_low_qinit = y
        my_qinit = num_points / mx_qinit
        dx_qinit = (x_hi_qinit - x_low_qinit) / (mx_qinit-1)
        dy_qinit = (y_hi_qinit - y_low_qinit) / (my_qinit-1)

        rewind(unit)
        allocate(qinit(num_points))

        ! Read and store the data this time
        do i=1,num_points
            read(unit,*) x,y,qinit(i)
        enddo
        close(unit)

    end subroutine read_qinit

end module qinit_module


module physics_declarations
   use bl_types, only: dp_t

   public

   ! adjust as necessary, be sure it is PHYSICALLY CORRECT!
   real(dp_t), parameter ::  TIMESCALE =   3155692600.00000     , &
                             LSCALE    =  3.085680249999995E+018, &
                             MSCALE    =  6.077143661962447E+031, &
                             RSCALE    =  2.068458160421109E-024, &
                             VELSCALE  =   977813951.206778     , &
                             PSCALE    =  1.977694471123365E-006, &
                             NSCALE    =   1.00000000000000     , &
                             BSCALE    =  4.985222330659412E-003, &
                             TEMPSCALE =   14324369256.4538     , &
                             SCALEGRAV =  1.374560173618349E-012, &
                             SCALEC    =   30.6594580318688     , &
                             SCALECOOL =  1.595642120700027E+015
   real(dp_t), parameter :: gamma  = 1.6666666666666666666667_dp_t,&
                            gamma1 = gamma - 1.0_dp_t,&
                            gamma7 = 1.0_dp_t/gamma1, &
                            half = 0.5_dp_t, &
                            one = 1.0_dp_t, &
                            zero = 0.0_dp_t
 contains

   !> @brief convert conservative to primitive
   pure function primitive(u) result(q)
     real(dp_t), intent(in) :: u(:)
     real(dp_t) :: q(1:size(u))
     q = u
     q(2:4) = u(2:4)/u(1)
     q(5) = gamma7*(u(5)-half*SUM(u(2:4)**2)/u(1))
   end function primitive

   !> @brief convert primitive to conservative
   pure function conservative(q) result(u)
     real(dp_t), intent(in) :: q(:)
     real(dp_t) :: u(1:size(q))
     u = q
     u(2:4) = q(2:4)*q(1)
     u(5) = q(5)/gamma1 + half*sum(q(2:4)**2)/q(1)
   end function conservative
end module physics_declarations


!
!     CalculiX - A 3-dimensional finite element program
!     Copyright (C) 1998-2021 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
!     This subroutines enables to caclulate a correction term linked to with the radius
!     of the spike as a function of r/s (radius/gap)
!     the parameter Hst ( height of the step ) enable to select either the table for a
!     straight labyrinth (Hst=0) or for a stepped labyrinth
!
!     H.Zimmermann and K.h. Wolff
!     "Air system correlations part 1 Labyrinth seals"
!     asme 98-GT-206
!
!     author: Yannick Muller
!
      subroutine cd_lab_radius(rad,s,hst,cd_radius)
!
      implicit none
!
      integer id,i,number,n9
!
      real*8 rad,s,cd_radius,rzs_tab(9),cd_sharp(9),rzs,hst
!
      real*8 rzs_tab1(9)
      data rzs_tab1
     &     /0d0,0.05d0,0.100d0,0.150d0,0.200d0,0.250d0,0.300d0,0.350d0,
     &      0.400d0/
!
      real*8 cd_sharp1(9)
      data cd_sharp1
     &     /1d0,1.025d0,1.10d0,1.11d0,1.12d0,1.125d0,1.126d0,1.127d0,
     &      1.127d0/
!
      real*8 rzs_tab2(9)
      data rzs_tab2
     &     /0d0,0.05d0,075d0,0.100d0,0.15d0,0.20d0,0.25d0,0.30d0,0.40d0/
!
      real*8 cd_sharp2(9)
      data cd_sharp2
     &     /1d0,1.10d0,1.15d0,1.20d0,1.26d0,1.31d0,1.34d0,1.36d0,1.37d0/
!
      data n9 /9/
!
      rzs=rad/s
!
!     straight labyrinth
!
      if(hst.eq.0d0) then
         call ident(rzs_tab1,rzs,n9,id)
         number=9
         do i=1,9
            rzs_tab(i)=rzs_tab1(i)
            cd_sharp(i)=cd_sharp1(i)
         enddo
!
!     stepped labyrinth
!
      else
         call ident(rzs_tab2,rzs,n9,id)
         number=9
         do i=1,9
            rzs_tab(i)=rzs_tab2(i)
            cd_sharp(i)=cd_sharp2(i)
         enddo
      endif
!
!     linear interpolation
!
!
            if(id.eq.1) then
              cd_radius=cd_sharp(1)
            elseif(id.eq.number) then
               cd_radius=cd_sharp(number)
            else
               cd_radius=cd_sharp(id)+(cd_sharp(id+1)-cd_sharp(id))
     &              *(rzs-rzs_tab(id))/(rzs_tab(id+1)-rzs_tab(id))
            endif
!
            return
!
            end


      subroutine solvecon(y,       x,      iBC,  BC,
     &                     iper,    ilwork, shp,  shgl)

c---------------------------------------------------------------------
c This subroutine is to calculate the constarint for the redistancing
c scalar of the level set method. This is to prevent interface from
c moving by applying the condition that the volume must stay constant
c in each element when the redisatnce step is applied.
c--------------------------------------------------------------------
c
c
      use pointer_data
c
      include "common.h"
      include "mpif.h"
      include "auxmpi.h"
c
      dimension y(nshg,ndof),
     &          x(numnp,nsd),            iBC(nshg),
     &          BC(nshg,ndofBC),         ilwork(nlwork),
     &          iper(nshg)
c
c
      dimension shp(MAXTOP,maxsh,MAXQPT),
     &           shgl(MAXTOP,nsd,maxsh,MAXQPT),
     &           v_lambda(nshg),   hprime(nshg),
     &           v_lambda1(nshg), v_lambda2(nshg),
     &           rmass(nshg)
c
        real*8, allocatable :: tmpshp(:,:),  tmpshgl(:,:,:)
        real*8, allocatable :: tmpshpb(:,:), tmpshglb(:,:,:)

c
c ... intialize
c
      rmass  = zero
      v_lambda = zero
      v_lambda1 = zero
      v_lambda2 = zero
      hprime = zero
c
c ... loop over element blocks
c
      do iblk = 1, nelblk
c
c.... set up the parameters
c
         nenl   = lcblk(5,iblk) ! no. of vertices per element
         iel    = lcblk(1,iblk)
         lelCat = lcblk(2,iblk)
         lcsyst = lcblk(3,iblk)
         iorder = lcblk(4,iblk)
         nenl   = lcblk(5,iblk) ! no. of vertices per element
         nshl   = lcblk(10,iblk)
         mattyp = lcblk(7,iblk)
         ndofl  = lcblk(8,iblk)
         nsymdl = lcblk(9,iblk)
         npro   = lcblk(1,iblk+1) - iel
         ngauss = nint(lcsyst)
c
c.... compute and assemble the constarint factor, and mass matrix
c

         allocate (tmpshp(nshl,MAXQPT))
         allocate (tmpshgl(nsd,nshl,MAXQPT))

         tmpshp(1:nshl,:) = shp(lcsyst,1:nshl,:)
         tmpshgl(:,1:nshl,:) = shgl(lcsyst,:,1:nshl,:)
c
         call volcon (y,          x,             tmpshp,
     &                tmpshgl,    mien(iblk)%p,  rmass,
     &                v_lambda1,  hprime,        v_lambda2)

         deallocate ( tmpshp )
         deallocate ( tmpshgl )
      enddo

c
c ... multiple processor communication
c
      if (numpe > 1) then
         call commu (v_lambda1  , ilwork, 1  , 'in ')
         call commu (v_lambda2  , ilwork, 1  , 'in ')
         call commu (hprime     , ilwork, 1  , 'in ')
         call commu (rmass      , ilwork, 1  , 'in ')
      endif
c
c.... take care of periodic boundary conditions
c
      do j= 1,nshg
         if (btest(iBC(j),10)) then
            i = iper(j)
            rmass(i) = rmass(i) + rmass(j)
            v_lambda1(i) =  v_lambda1(i) +  v_lambda1(j)
            v_lambda2(i) =  v_lambda2(i) +  v_lambda2(j)
            hprime(i)   =  hprime(i) + hprime(j)
         endif
      enddo
c
      do j= 1,nshg
         if (btest(iBC(j),10)) then
            i = iper(j)
            rmass(j) = rmass(i)
            v_lambda1(j) =  v_lambda1(i)
            v_lambda2(j) =  v_lambda2(i)
            hprime(j) = hprime(i)
         endif
      enddo
c
c ... calculation of constraint factor
c
      rmass  = one/rmass
      v_lambda1 = v_lambda1*rmass ! numerator of lambda
      v_lambda2 = v_lambda2*rmass ! denominator of lambda
      v_lambda  = v_lambda1/(v_lambda2+epsM**2)
      hprime    = hprime*rmass
      v_lambda  = v_lambda*hprime
c
c ... commu out for the multiple processor
c
      if(numpe > 1) then
         call commu (v_lambda, ilwork, 1, 'out')
      endif
c
c ... the following commented lines are for the different way of getting
c     the denominator of constraint (lambda) calculation
c$$$                hprime=zero
c$$$                do kk=1, nshg
c$$$                   if (abs (y(kk,6)) .le. epsilon_ls) then
c$$$                      hprime(kk) = (0.5/epsilon_ls) * (1
c$$$     &                   + cos(pi*y(kk,6)/epsilon_ls))
c$$$                   endif
c$$$                enddo
c$$$                y(:,7) = y(:,7)+v_lambda*hprime/dtgl
c
c ... the vlome constraint applied on the second scalar
c
      y(:,7) = y(:,7)+v_lambda/dtgl
c
      return
      end
c
c
c

      subroutine volcon (y,         x,      shp,
     &                   shgl,      ien,    rmass,
     &                   v_lambda1, hprime, v_lambda2)

c---------------------------------------------------------------------
c
c This subroutine is to calculate the element contribution to the
c constraint factor and mass matrix.
c
c---------------------------------------------------------------------
      include "common.h"
c
      dimension y(nshg,ndof),               x(numnp,nsd),
     &            shp(nshl,maxsh),
     &            shgl(nsd,nshl,maxsh),
     &            ien(npro,nshl),
     &            qres(nshg,idflx),         rmass(nshg)
c
c.... element level declarations
c
      dimension ycl(npro,nshl,ndof),      xl(npro,nenl,nsd),
     &          rmassl(npro,nshl)
      dimension sgn(npro,nshape),         v_lambdal1(npro,nshl),
     &          v_lambda1(nshg),          hprimel(npro,nshl),
     &          hprime(nshg),             v_lambdal2(npro,nshl),
     &          v_lambda2(nshg)
c
c local arrays
c
      dimension shg(npro,nshl,nsd),
     &          dxidx(npro,nsd,nsd),      WdetJ(npro)
c
      dimension shape(npro,nshl),
     &          shdrv(npro,nsd,nshl)
c
c
c.... for volume constraint calculation of redistancing step
c
      dimension Sclr(npro),              Sclrtmp(npro),
     &          h_prime(npro),           tmp1(npro),
     &          tmp2(npro),
     &          v_lambdatmp(npro),       v_lambdal(npro,nshl)
c$$$     &          ,hprimel(npro,nshl),      v_lambdal1(npro,nshl),
c$$$     &          v_lambdal2(npro,nshl)
c above arrays must be uncommented for alternate method included below (commented)
      real epsilon_tmp

c
c.... create the matrix of mode signs for the hierarchic basis
c     functions.
c
      if (ipord .gt. 1) then
         call getsgn(ien,sgn)
      endif
c
c.... gather the variables
c

      call localy(y,      ycl,     ien,    ndof,   'gather  ')
      call localx(x,      xl,      ien,    nsd,    'gather  ')
c
c.... get the element contributions of the numerator and denominator
c     of lambda
c
      rmassl = zero
      v_lambdal1= zero
      v_lambdal2= zero
      hprimel = zero
c
c.... loop through the integration points
c
      do intp = 1, ngauss
         if (Qwt(lcsyst,intp) .eq. zero) cycle ! precaution
c
c.... create a matrix of shape functions (and derivatives) for each
c     element at this quadrature point. These arrays will contain
c     the correct signs for the hierarchic basis
c
         call getshp(shp,          shgl,      sgn,
     &               shape,        shdrv)
c
c.... initialize
c
         h_prime   = zero
	 sclr      = zero
	 sclrtmp   = zero
         v_lambdatmp=zero
         tmp1       =zero
         tmp2       =zero

c
c.... --------------------->  Element Metrics  <-----------------------
c
         call e3metric( xl,         shdrv,        dxidx,
     &                  shg,        WdetJ)

c
         do i = 1, nshl
c
c  y(intp)=SUM_{a=1}^nshl (N_a(intp) Ya)
c
            Sclr    = Sclr    + shape(:,i) * ycl(:,i,7) !d^kbar
            sclrtmp = sclrtmp + shape(:,i) * ycl(:,i,6) !d^0
         enddo

         if (isclr .eq. 2) then
            epsilon_tmp = epsilon_lsd
         else
            epsilon_tmp = epsilon_ls
         endif

         do i=1,npro
            if (abs (Sclrtmp(i)) .le. epsilon_tmp) then
               h_prime(i) = (0.5/epsilon_tmp) * (1
     &                    + cos(pi*Sclrtmp(i)/epsilon_tmp))
               tmp1(i)=-h_prime(i)*(sclr(i)-sclrtmp(i))*dtgl
               tmp2(i)=h_prime(i)**2
c              v_lambdatmp(i)=tmp1(i)/(tmp2(i)+ epsM)
            endif
         enddo
c
         do i=1,nshl
            v_lambdal1(:,i)=v_lambdal1(:,i)+ shape(:,i)*WdetJ*tmp1
            v_lambdal2(:,i)=v_lambdal2(:,i)+ shape(:,i)*WdetJ*tmp2
c           v_lambdal(:,i)=v_lambdal(:,i)+ shape(:,i)*WdetJ*v_lambdatmp
            hprimel(:,i) = hprimel(:,i) + shape(:,i)*WdetJ*h_prime
            rmassl(:,i)  =rmassl(:,i) + shape(:,i)*WdetJ
         enddo
c
c.... end of the loop over integration points
c
      enddo
c
      call local (v_lambda1,  v_lambdal1, ien,  1,  'scatter ')
      call local (v_lambda2,  v_lambdal2, ien,  1,  'scatter ')
      call local (hprime,     hprimel,    ien,  1,  'scatter ')
      call local (rmass,      rmassl,     ien,  1,  'scatter ')
c
      return
      end


      SUBROUTINE CURFUN(X, AI, AIP)                                     ABSH0466
C***********************************************************************ABSH0467
C*****CURFUN PROVIDES THE INTEGRATED TOROIDAL CURRENT PROFILE.         *ABSH0468
C*****REFERENCES:                                                      *ABSH0469
C*****L.L.LAO,R.M.WIELAND,W.A.HOULBERG,S.P.HIRSHMAN,ORNL/TM-7871 (1981)*ABSH0470
C*****LAST REVISION: 6/81 L.L.LAO, R.M.WIELAND, AND W.A.HOULBERG ORNL. *ABSH0471
C*****CALCULATED PARAMETERS:                                           *ABSH0472
C*****AI-NORMALIZED INTEGRATED TOROIDAL CURRENT AT X-(DIMENSIONLESS).  *ABSH0473
C*****AIP-DERIVATIVE OF AI WITH RESPECT TO X-(DIMENSIONLESS).          *ABSH0474
C*****INPUT PARAMETERS:                                                *ABSH0475
C*****NRHO-NUMBER OF RADIAL GRID POINTS.                               *ABSH0476
C*****X-RADIAL POSITION-(DIMENSIONLESS).                               *ABSH0477
C*****CURT(I)-TOROIDAL CURRENT INTEGRATED TO NODE I.                   *ABSH0478
C*****RHO(I)-RADIAL COORDINATE FOR NODE I.                             *ABSH0479
C***********************************************************************ABSH0480
      PARAMETER MNMX=61                                                 ABSH0481
      COMMON /CINIT/ INITM, INITC, INITP, ITPR                          ABSH0482
      COMMON /SWORK1/ PRES(MNMX), CURT(MNMX), RHO(MNMX),                ABSH0483
     *     BPRES(MNMX), CPRES(MNMX), DPRES(MNMX), BCURT(MNMX),          ABSH0484
     *     CCURT(MNMX), DCURT(MNMX), NRHO                               ABSH0485
      IF (INITC.NE.0) GO TO 10                                          ABSH0486
      CALL SPLAAN(NRHO, RHO, CURT, BCURT, CCURT, DCURT)                 ABSH0487
      INITC = 1                                                         ABSH0488
   10 R = X*RHO(NRHO)                                                   ABSH0489
      AI = SEVAL(NRHO,R,RHO,CURT,BCURT,CCURT,DCURT)/CURT(NRHO)          ABSH0490
      AIP = SPEVAL(NRHO,R,RHO,CURT,BCURT,CCURT,DCURT)*RHO(NRHO)/        ABSH0491
     *     CURT(NRHO)                                                   ABSH0492
      RETURN                                                            ABSH0493
      END                                                               ABSH0494