math.f90

Go to the documentation of this file.

! Copyright (c) 2008-2020, UCHICAGO ARGONNE, LLC.
!
! The UChicago Argonne, LLC as Operator of Argonne National
! Laboratory holds copyright in the Software. The copyright holder
! reserves all rights except those expressly granted to licensees,
! and U.S. Government license rights.
!
! Redistribution and use in source and binary forms, with or without
! modification, are permitted provided that the following conditions
! are met:
!
! 1. Redistributions of source code must retain the above copyright
! notice, this list of conditions and the disclaimer below.
!
! 2. Redistributions in binary form must reproduce the above copyright
! notice, this list of conditions and the disclaimer (as noted below)
! in the documentation and/or other materials provided with the
! distribution.
!
! 3. Neither the name of ANL nor the names of its contributors
! may be used to endorse or promote products derived from this software
! without specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
! "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
! LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
! FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
! UCHICAGO ARGONNE, LLC, THE U.S. DEPARTMENT OF
! ENERGY OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
! SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
! DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
! THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
! (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
! OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
!
! Additional BSD Notice
! ---------------------
! 1. This notice is required to be provided under our contract with
! the U.S. Department of Energy (DOE). This work was produced at
! Argonne National Laboratory under Contract
! No. DE-AC02-06CH11357 with the DOE.
!
! 2. Neither the United States Government nor UCHICAGO ARGONNE,
! LLC nor any of their employees, makes any warranty,
! express or implied, or assumes any liability or responsibility for the
! accuracy, completeness, or usefulness of any information, apparatus,
! product, or process disclosed, or represents that its use would not
! infringe privately-owned rights.
!
! 3. Also, reference herein to any specific commercial products, process,
! or services by trade name, trademark, manufacturer or otherwise does
! not necessarily constitute or imply its endorsement, recommendation,
! or favoring by the United States Government or UCHICAGO ARGONNE LLC.
! The views and opinions of authors expressed
! herein do not necessarily state or reflect those of the United States
! Government or UCHICAGO ARGONNE, LLC, and shall
! not be used for advertising or product endorsement purposes.
!
module math
  use num_types, only : rp, dp, sp, qp, i4, xp
  use comm, only : neko_comm, mpi_real_precision, mpi_extra_precision
  use mpi_f08, only : mpi_min, mpi_max, mpi_sum, mpi_in_place, mpi_integer, &
       mpi_allreduce
  use utils, only : nonlinear_index
  implicit none
  private
 
  real(kind=rp), public, parameter :: neko_eps = epsilon(1.0_rp)
  real(kind=dp), public, parameter :: neko_eps_dp = epsilon(1.0_dp)
  real(kind=sp), public, parameter :: neko_eps_sp = epsilon(1.0_sp)
 
  real(kind=rp), public, parameter :: neko_m_ln2 = log(2.0_rp)
 
  real(kind=rp), public, parameter :: pi = 4._rp*atan(1._rp)
 
  interface abscmp
     module procedure sabscmp, dabscmp, qabscmp
  end interface abscmp
 
  interface sort
     module procedure sortrp, sorti4
  end interface sort
 
  interface swap
     module procedure swapdp, swapi4
  end interface swap
 
  interface reord
     module procedure reorddp, reordi4
  end interface reord
 
  interface flipv
     module procedure flipvdp, flipvi4
  end interface flipv
 
  interface relcmp
     module procedure srelcmp, drelcmp, qrelcmp
  end interface relcmp
 
  public :: abscmp, rzero, izero, row_zero, rone, copy, cmult, cadd, cfill, &
       glsum, glmax, glmin, chsign, vlmax, vlmin, invcol1, invcol3, invers2, &
       vcross, vdot2, vdot3, vlsc3, vlsc2, add2, add3, add4, sub2, sub3, &
       add2s1, add2s2, addsqr2s2, cmult2, invcol2, col2, col3, subcol3, &
       add3s2, add4s3, add5s4, subcol4, addcol3, addcol4, addcol3s2, ascol5, &
       p_update, x_update, glsc2, glsc3, glsc4, sort, masked_copy_0, &
       cfill_mask, relcmp, glimax, glimin, swap, reord, flipv, cadd2, &
       masked_gather_copy_0, face_masked_gather_copy_0, absval, matinv3, &
       matinv39, &
       pwmax2, pwmax3, cpwmax2, cpwmax3, pwmin2, pwmin3, cpwmin2, cpwmin3, &
       masked_scatter_copy_0, cdiv, cdiv2, glsubnorm, &
       masked_copy, masked_gather_copy, masked_scatter_copy, sabscmp, dabscmp, &
       math_dstepf, math_stepf, cwrap, lambert_w0, sqrt_inplace, power, &
       eig_sym2, eig_sym3
 
contains
 
  pure function sabscmp(x, y, tol)
    real(kind=sp), intent(in) :: x
    real(kind=sp), intent(in) :: y
    real(kind=sp), intent(in), optional :: tol
    logical :: sabscmp
 
    if (present(tol)) then
       sabscmp = abs(x - y) .lt. tol
    else
       sabscmp = abs(x - y) .lt. neko_eps
    end if
 
  end function sabscmp
 
  pure function dabscmp(x, y, tol)
    real(kind=dp), intent(in) :: x
    real(kind=dp), intent(in) :: y
    real(kind=dp), intent(in), optional :: tol
    logical :: dabscmp
 
    if (present(tol)) then
       dabscmp = abs(x - y) .lt. tol
    else
       dabscmp = abs(x - y) .lt. neko_eps
    end if
 
  end function dabscmp
 
  pure function qabscmp(x, y, tol)
    real(kind=qp), intent(in) :: x
    real(kind=qp), intent(in) :: y
    real(kind=qp), intent(in), optional :: tol
    logical :: qabscmp
 
    if (present(tol)) then
       qabscmp = abs(x - y) .lt. tol
    else
       qabscmp = abs(x - y) .lt. neko_eps
    end if
 
  end function qabscmp
 
  pure function srelcmp(x, y, eps)
    real(kind=sp), intent(in) :: x
    real(kind=sp), intent(in) :: y
    real(kind=sp), intent(in), optional :: eps
    logical :: srelcmp
    if (present(eps)) then
       srelcmp = abs(x - y) .le. eps*abs(y)
    else
       srelcmp = abs(x - y) .le. neko_eps*abs(y)
    end if
 
  end function srelcmp
 
  pure function drelcmp(x, y, eps)
    real(kind=dp), intent(in) :: x
    real(kind=dp), intent(in) :: y
    real(kind=dp), intent(in), optional :: eps
    logical :: drelcmp
    if (present(eps)) then
       drelcmp = abs(x - y) .le. eps*abs(y)
    else
       drelcmp = abs(x - y) .le. neko_eps*abs(y)
    end if
 
  end function drelcmp
 
 
  pure function qrelcmp(x, y, eps)
    real(kind=qp), intent(in) :: x
    real(kind=qp), intent(in) :: y
    real(kind=qp), intent(in), optional :: eps
    logical :: qrelcmp
    if (present(eps)) then
       qrelcmp = abs(x - y)/abs(y) .lt. eps
    else
       qrelcmp = abs(x - y)/abs(y) .lt. neko_eps
    end if
 
  end function qrelcmp
 
  pure function lambert_w0(x, niter) result(w)
    real(kind=rp), intent(in) :: x
    integer, intent(in) :: niter
    real(kind=rp) :: w
    real(kind=rp) :: a
    integer :: k
 
    if (x == 0.0_rp) then
       w = 0.0_rp
       return
    end if
 
    a = 1.0_rp / (1.0_rp + 0.5_rp * log(1.0_rp + x))
    w = log(1.0_rp + a * x)
 
    do k = 1, max(niter, 0)
       w = w / (1.0_rp + w) * (1.0_rp + log(x / w))
    end do
  end function lambert_w0
 
  subroutine rzero(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = 0.0_rp
    end do
    !$omp end parallel do
 
  end subroutine rzero
 
  subroutine izero(a, n)
    integer, intent(in) :: n
    integer, dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = 0
    end do
    !$omp end parallel do
 
  end subroutine izero
 
  subroutine row_zero(a, m, n, e)
    integer, intent(in) :: m, n, e
    real(kind=rp), intent(inout) :: a(m,n)
    integer :: j
 
    !$omp parallel do
    do j = 1, n
       a(e,j) = 0.0_rp
    end do
    !$omp end parallel do
 
  end subroutine row_zero
 
  subroutine rone(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = 1.0_rp
    end do
    !$omp end parallel do
 
  end subroutine rone
 
  subroutine copy(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i)
    end do
    !$omp end parallel do
 
  end subroutine copy
 
  subroutine masked_copy_0(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(inout) :: a
    integer, dimension(0:n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(j) = b(j)
    end do
    !$omp end parallel do
 
  end subroutine masked_copy_0
 
  subroutine masked_copy(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(inout) :: a
    integer, dimension(n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(j) = b(j)
    end do
    !$omp end parallel do
 
  end subroutine masked_copy
 
  subroutine masked_gather_copy_0(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n_mask), intent(inout) :: a
    integer, dimension(0:n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(i) = b(j)
    end do
    !$omp end parallel do
 
  end subroutine masked_gather_copy_0
 
  subroutine face_masked_gather_copy_0(a, b, mask, facet, lx, ly, lz, n_mask)
    integer, intent(in) :: lx, ly, lz, n_mask
    real(kind=rp), dimension(n_mask), intent(inout) :: a
    real(kind=rp), dimension(:, :, :, :), intent(in) :: b
    integer, dimension(0:n_mask), intent(in) :: mask
    integer, dimension(0:n_mask), intent(in) :: facet
    integer :: l
    integer :: idx(4)
 
    !$omp parallel do private(l, idx)
    do l = 1, n_mask
       idx = nonlinear_index(mask(l), lx, ly, lz)
 
       select case (facet(l))
       case (1, 2)
          a(l) = b(idx(2), idx(3), facet(l), idx(4))
       case (3, 4)
          a(l) = b(idx(1), idx(3), facet(l), idx(4))
       case (5, 6)
          a(l) = b(idx(1), idx(2), facet(l), idx(4))
       end select
    end do
    !$omp end parallel do
 
  end subroutine face_masked_gather_copy_0
 
  subroutine masked_gather_copy(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n_mask), intent(inout) :: a
    integer, dimension(n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(i) = b(j)
    end do
    !$omp end parallel do
 
  end subroutine masked_gather_copy
 
  subroutine masked_scatter_copy_0(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n_mask), intent(in) :: b
    real(kind=rp), dimension(n), intent(inout) :: a
    integer, dimension(0:n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(j) = b(i)
    end do
    !$omp end parallel do
 
  end subroutine masked_scatter_copy_0
 
  subroutine masked_scatter_copy(a, b, mask, n, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n_mask), intent(in) :: b
    real(kind=rp), dimension(n), intent(inout) :: a
    integer, dimension(n_mask) :: mask
    integer :: i, j
 
    !$omp parallel do private(i, j)
    do i = 1, n_mask
       j = mask(i)
       a(j) = b(i)
    end do
    !$omp end parallel do
 
  end subroutine masked_scatter_copy
 
  subroutine cfill_mask(a, c, n, mask, n_mask)
    integer, intent(in) :: n, n_mask
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: c
    integer, dimension(n_mask), intent(in) :: mask
    integer :: i
 
    !$omp parallel do
    do i = 1, n_mask
       a(mask(i)) = c
    end do
    !$omp end parallel do
 
  end subroutine cfill_mask
 
  subroutine cmult(a, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c * a(i)
    end do
    !$omp end parallel do
 
  end subroutine cmult
 
  subroutine cmult2(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c * b(i)
    end do
    !$omp end parallel do
 
  end subroutine cmult2
 
  subroutine cdiv(a, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c / a(i)
    end do
    !$omp end parallel do
 
  end subroutine cdiv
 
  subroutine cdiv2(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c / b(i)
    end do
    !$omp end parallel do
 
  end subroutine cdiv2
 
  subroutine cadd(a, s, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: s
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + s
    end do
    !$omp end parallel do
 
  end subroutine cadd
 
  subroutine cadd2(a, b, s, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: s
    integer :: i
 
    !$omp parallel do
    do i = 1,n
       a(i) = b(i) + s
    end do
    !$omp end parallel do
 
  end subroutine cadd2
 
  subroutine cfill(a, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c
    end do
    !$omp end parallel do
 
  end subroutine cfill
 
  subroutine cwrap(a, min_val, max_val, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: min_val, max_val
    integer :: i
 
    if (n .lt. 1 .or. max_val .le. min_val) return
 
    !$omp parallel do
    do i = 1, n
       a(i) = modulo(a(i) - min_val, max_val - min_val) + min_val
    end do
    !$omp end parallel do
 
  end subroutine cwrap
 
  function glsum(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n) :: a
    real(kind=rp) :: glsum
    real(kind=xp) :: tmp
    integer :: i, ierr
 
    tmp = 0.0_rp
    !$omp parallel do reduction(+:tmp)
    do i = 1, n
       tmp = tmp + a(i)
    end do
    !$omp end parallel do
 
    call mpi_allreduce(mpi_in_place, tmp, 1, &
         mpi_extra_precision, mpi_sum, neko_comm, ierr)
    glsum = tmp
 
  end function glsum
 
  function glmax(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n) :: a
    real(kind=rp) :: tmp, glmax
    integer :: i, ierr
 
    tmp = -huge(0.0_rp)
    !$omp parallel do reduction(max:tmp)
    do i = 1, n
       tmp = max(tmp,a(i))
    end do
    !$omp end parallel do
 
    call mpi_allreduce(tmp, glmax, 1, &
         mpi_real_precision, mpi_max, neko_comm, ierr)
 
  end function glmax
 
  function glimax(a, n)
    integer, intent(in) :: n
    integer, dimension(n) :: a
    integer :: tmp, glimax
    integer :: i, ierr
 
    tmp = -huge(0)
    !$omp parallel do reduction(max:tmp)
    do i = 1, n
       tmp = max(tmp,a(i))
    end do
    !$omp end parallel do
 
    call mpi_allreduce(tmp, glimax, 1, &
         mpi_integer, mpi_max, neko_comm, ierr)
 
  end function glimax
 
  function glmin(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n) :: a
    real(kind=rp) :: tmp, glmin
    integer :: i, ierr
 
    tmp = huge(0.0_rp)
    !$omp parallel do reduction(min:tmp)
    do i = 1, n
       tmp = min(tmp,a(i))
    end do
    !$omp end parallel do
 
    call mpi_allreduce(tmp, glmin, 1, &
         mpi_real_precision, mpi_min, neko_comm, ierr)
 
  end function glmin
 
  function glimin(a, n)
    integer, intent(in) :: n
    integer, dimension(n) :: a
    integer :: tmp, glimin
    integer :: i, ierr
 
    tmp = huge(0)
    !$omp parallel do reduction(min:tmp)
    do i = 1, n
       tmp = min(tmp,a(i))
    end do
    !$omp end parallel do
 
    call mpi_allreduce(tmp, glimin, 1, &
         mpi_integer, mpi_min, neko_comm, ierr)
 
  end function glimin
 
  subroutine chsign(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = -a(i)
    end do
    !$omp end parallel do
 
  end subroutine chsign
 
  function vlmax(vec,n) result(tmax)
    integer :: n, i
    real(kind=rp), intent(in) :: vec(n)
    real(kind=rp) :: tmax
 
    tmax = real(-99d20, rp)
    !$omp parallel do reduction(max:tmax)
    do i = 1, n
       tmax = max(tmax, vec(i))
    end do
    !$omp end parallel do
 
  end function vlmax
 
  function vlmin(vec,n) result(tmin)
    integer, intent(in) :: n
    real(kind=rp), intent(in) :: vec(n)
    real(kind=rp) :: tmin
    integer :: i
 
    tmin = real(99.0e20, rp)
    !$omp parallel do reduction(min:tmin)
    do i = 1, n
       tmin = min(tmin, vec(i))
    end do
    !$omp end parallel do
 
  end function vlmin
 
  subroutine invcol1(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = 1.0_xp / real(a(i), xp)
    end do
    !$omp end parallel do
 
  end subroutine invcol1
 
  subroutine invcol3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b, c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = real(b(i), xp) / c(i)
    end do
    !$omp end parallel do
 
  end subroutine invcol3
 
  subroutine invers2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = 1.0_xp / real(b(i), xp)
    end do
    !$omp end parallel do
 
  end subroutine invers2
 
  subroutine vcross(u1, u2, u3, v1, v2, v3, w1, w2, w3, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: v1, v2, v3
    real(kind=rp), dimension(n), intent(in) :: w1, w2, w3
    real(kind=rp), dimension(n), intent(out) :: u1, u2, u3
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       u1(i) = v2(i)*w3(i) - v3(i)*w2(i)
       u2(i) = v3(i)*w1(i) - v1(i)*w3(i)
       u3(i) = v1(i)*w2(i) - v2(i)*w1(i)
    end do
    !$omp end parallel do
 
  end subroutine vcross
 
  subroutine vdot2(dot, u1, u2, v1, v2, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: u1, u2
    real(kind=rp), dimension(n), intent(in) :: v1, v2
    real(kind=rp), dimension(n), intent(out) :: dot
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       dot(i) = u1(i)*v1(i) + u2(i)*v2(i)
    end do
    !$omp end parallel do
 
  end subroutine vdot2
 
  subroutine vdot3(dot, u1, u2, u3, v1, v2, v3, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: u1, u2, u3
    real(kind=rp), dimension(n), intent(in) :: v1, v2, v3
    real(kind=rp), dimension(n), intent(out) :: dot
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       dot(i) = u1(i)*v1(i) + u2(i)*v2(i) + u3(i)*v3(i)
    end do
    !$omp end parallel do
 
  end subroutine vdot3
 
  function vlsc3(u, v, w, n) result(s)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: u, v, w
    real(kind=rp) :: s
    integer :: i
 
    s = 0.0_rp
    !$omp parallel do reduction(+:s)
    do i = 1, n
       s = s + u(i)*v(i)*w(i)
    end do
    !$omp end parallel do
 
  end function vlsc3
 
  function vlsc2(u, v, n) result(s)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: u, v
    real(kind=rp) :: s
    integer :: i
 
    s = 0.0_rp
    !$omp parallel do reduction(+:s)
    do i = 1, n
       s = s + u(i)*v(i)
    end do
    !$omp end parallel do
 
  end function vlsc2
 
  subroutine add2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + b(i)
    end do
    !$omp end parallel do
 
  end subroutine add2
 
  subroutine add3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i) + c(i)
    end do
    !$omp end parallel do
 
  end subroutine add3
 
  subroutine add4(a, b, c, d, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(out) :: a
    real(kind=rp), dimension(n), intent(in) :: d
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i) + c(i) + d(i)
    end do
    !$omp end parallel do
 
  end subroutine add4
 
  subroutine sub2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) - b(i)
    end do
    !$omp end parallel do
 
  end subroutine sub2
 
  subroutine sub3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i) - c(i)
    end do
    !$omp end parallel do
 
  end subroutine sub3
 
 
  subroutine add2s1(a, b, c1, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c1
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c1 * a(i) + b(i)
    end do
    !$omp end parallel do
 
  end subroutine add2s1
 
  subroutine add2s2(a, b, c1, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c1
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + c1 * b(i)
    end do
    !$omp end parallel do
 
  end subroutine add2s2
 
  subroutine addsqr2s2(a, b, c1, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c1
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + c1 * ( b(i) * b(i) )
    end do
    !$omp end parallel do
 
  end subroutine addsqr2s2
 
  subroutine invcol2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = real(a(i), xp) / b(i)
    end do
    !$omp end parallel do
 
  end subroutine invcol2
 
 
  subroutine col2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) * b(i)
    end do
    !$omp end parallel do
 
  end subroutine col2
 
  subroutine col3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i) * c(i)
    end do
    !$omp end parallel do
 
  end subroutine col3
 
  subroutine subcol3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) - b(i) * c(i)
    end do
    !$omp end parallel do
 
  end subroutine subcol3
 
  subroutine add3s2(a, b, c, c1, c2 ,n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), intent(in) :: c1, c2
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c1 * b(i) + c2 * c(i)
    end do
    !$omp end parallel do
 
  end subroutine add3s2
 
  subroutine add4s3(a, b, c, d, c1, c2, c3, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    real(kind=rp), intent(in) :: c1, c2, c3
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = c1 * b(i) + c2 * c(i) + c3 * d(i)
    end do
    !$omp end parallel do
 
  end subroutine add4s3
 
  subroutine add5s4(a, b, c, d, e, c1, c2, c3, c4, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    real(kind=rp), dimension(n), intent(in) :: e
    real(kind=rp), intent(in) :: c1, c2, c3, c4
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + c1 * b(i) + c2 * c(i) + c3 * d(i) + c4 * e(i)
    end do
    !$omp end parallel do
 
  end subroutine add5s4
 
  subroutine subcol4(a, b, c, d, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) - b(i) * c(i) * d(i)
    end do
    !$omp end parallel do
 
  end subroutine subcol4
 
  subroutine addcol3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + b(i) * c(i)
    end do
    !$omp end parallel do
 
  end subroutine addcol3
 
  subroutine addcol4(a, b, c, d, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + b(i) * c(i) * d(i)
    end do
    !$omp end parallel do
 
  end subroutine addcol4
 
  subroutine addcol3s2(a, b, c, s, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), intent(in) :: s
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + s * b(i) * c(i)
    end do
    !$omp end parallel do
 
  end subroutine addcol3s2
 
  subroutine ascol5(a, b, c, d, e, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    real(kind=rp), dimension(n), intent(in) :: e
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i)*c(i) - d(i)*e(i)
    end do
    !$omp end parallel do
 
  end subroutine ascol5
 
  subroutine p_update(a, b, c, c1, c2, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), intent(in) :: c1, c2
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = b(i) + c1*(a(i)-c2*c(i))
    end do
    !$omp end parallel do
 
  end subroutine p_update
 
  subroutine x_update(a, b, c, c1, c2, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), intent(in) :: c1, c2
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = a(i) + c1*b(i)+c2*c(i)
    end do
    !$omp end parallel do
 
  end subroutine x_update
 
  function glsc2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp) :: glsc2
    real(kind=xp) :: tmp
    integer :: i, ierr
 
    tmp = 0.0_xp
    !$omp parallel do reduction(+:tmp)
    do i = 1, n
       tmp = tmp + a(i) * b(i)
    end do
    !$omp end parallel do
 
    call mpi_allreduce(mpi_in_place, tmp, 1, &
         mpi_extra_precision, mpi_sum, neko_comm, ierr)
    glsc2 = tmp
  end function glsc2
 
  function glsc3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp) :: glsc3
    real(kind=xp) :: tmp
    integer :: i, ierr
 
    tmp = 0.0_xp
    !$omp parallel do reduction(+:tmp)
    do i = 1, n
       tmp = tmp + a(i) * b(i) * c(i)
    end do
    !$omp end parallel do
 
    call mpi_allreduce(mpi_in_place, tmp, 1, &
         mpi_extra_precision, mpi_sum, neko_comm, ierr)
    glsc3 = tmp
 
  end function glsc3
  function glsc4(a, b, c, d, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), dimension(n), intent(in) :: c
    real(kind=rp), dimension(n), intent(in) :: d
    real(kind=rp) :: glsc4
    real(kind=xp) :: tmp
    integer :: i, ierr
 
    tmp = 0.0_xp
    !$omp parallel do reduction(+:tmp)
    do i = 1, n
       tmp = tmp + a(i) * b(i) * c(i) * d(i)
    end do
    !$omp end parallel do
 
    call mpi_allreduce(mpi_in_place, tmp, 1, &
         mpi_extra_precision, mpi_sum, neko_comm, ierr)
    glsc4 = tmp
 
  end function glsc4
 
  function glsubnorm(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(in) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp) :: glsubnorm
    real(kind=xp) :: tmp
    integer :: i, ierr
 
    tmp = 0.0_xp
    !$omp parallel do reduction(+:tmp)
    do i = 1, n
       tmp = tmp + (a(i) - b(i))**2
    end do
    !$omp end parallel do
 
    call mpi_allreduce(mpi_in_place, tmp, 1, &
         mpi_extra_precision, mpi_sum, neko_comm, ierr)
    glsubnorm = sqrt(tmp)
 
  end function glsubnorm
 
  subroutine sortrp(a, ind, n)
    integer, intent(in) :: n
    real(kind=rp), intent(inout) :: a(n)
    integer, intent(out) :: ind(n)
    real(kind=rp) :: aa
    integer :: j, ir, i, ii, l
 
    do j = 1, n
       ind(j) = j
    end do
 
    if (n .le. 1) return
 
 
    l = n/2+1
    ir = n
    do while (.true.)
       if (l .gt. 1) then
          l = l-1
          aa = a(l)
          ii = ind(l)
       else
          aa = a(ir)
          ii = ind(ir)
          a(ir) = a(1)
          ind(ir) = ind(1)
          ir = ir - 1
          if (ir .eq. 1) then
             a(1) = aa
             ind(1) = ii
             return
          end if
       end if
       i = l
       j = l+l
       do while (j .le. ir)
          if (j .lt. ir) then
             if ( a(j) .lt. a(j+1) ) j = j + 1
          end if
          if (aa .lt. a(j)) then
             a(i) = a(j)
             ind(i) = ind(j)
             i = j
             j = j+j
          else
             j = ir+1
          end if
       end do
       a(i) = aa
       ind(i) = ii
    end do
  end subroutine sortrp
 
  subroutine sorti4(a, ind, n)
    integer, intent(in) :: n
    integer(i4), intent(inout) :: a(n)
    integer, intent(out) :: ind(n)
    integer(i4) :: aa
    integer :: j, ir, i, ii, l
 
    do j = 1, n
       ind(j) = j
    end do
 
    if (n .le. 1) return
 
    l = n/2+1
    ir = n
    do while (.true.)
       if (l .gt. 1) then
          l = l - 1
          aa = a(l)
          ii = ind(l)
       else
          aa = a(ir)
          ii = ind(ir)
          a(ir) = a( 1)
          ind(ir) = ind( 1)
          ir = ir - 1
          if (ir .eq. 1) then
             a(1) = aa
             ind(1) = ii
             return
          end if
       end if
       i = l
       j = l + l
       do while (j .le. ir)
          if (j .lt. ir) then
             if ( a(j) .lt. a(j + 1) ) j = j + 1
          end if
          if (aa .lt. a(j)) then
             a(i) = a(j)
             ind(i) = ind(j)
             i = j
             j = j + j
          else
             j = ir + 1
          end if
       end do
       a(i) = aa
       ind(i) = ii
    end do
  end subroutine sorti4
 
  subroutine swapdp(b, ind, n)
    integer, intent(in) :: n
    real(kind=rp), intent(inout) :: b(n)
    integer, intent(in) :: ind(n)
    real(kind=rp) :: temp(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       temp(i) = b(i)
    end do
    !$omp end do
    !$omp do
    do i = 1, n
       jj = ind(i)
       b(i) = temp(jj)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine swapdp
 
  subroutine swapi4(b, ind, n)
    integer, intent(in) :: n
    integer(i4), intent(inout) :: b(n)
    integer, intent(in) :: ind(n)
    integer(i4) :: temp(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       temp(i) = b(i)
    end do
    !$omp end do
    !$omp do
    do i = 1, n
       jj = ind(i)
       b(i) = temp(jj)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine swapi4
 
  subroutine reorddp(b, ind, n)
    integer, intent(in) :: n
    real(kind=rp), intent(inout) :: b(n)
    integer, intent(in) :: ind(n)
    real(kind=rp) :: temp(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       temp(i) = b(i)
    end do
    !$omp end do
    !$omp do
    do i = 1, n
       jj = ind(i)
       b(jj) = temp(i)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine reorddp
 
  subroutine reordi4(b, ind, n)
    integer, intent(in) :: n
    integer(i4), intent(inout) :: b(n)
    integer, intent(in) :: ind(n)
    integer(i4) :: temp(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       temp(i) = b(i)
    end do
    !$omp end do
    !$omp do
    do i = 1, n
       jj = ind(i)
       b(jj) = temp(i)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine reordi4
 
  subroutine flipvdp(b, ind, n)
    integer, intent(in) :: n
    real(kind=rp), intent(inout) :: b(n)
    integer, intent(inout) :: ind(n)
    real(kind=rp) :: temp(n)
    integer :: tempind(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       jj = n+1-i
       temp(jj) = b(i)
       tempind(jj) = ind(i)
    end do
    !$omp end do
    !$omp do
    do i = 1,n
       b(i) = temp(i)
       ind(i) = tempind(i)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine flipvdp
 
  subroutine flipvi4(b, ind, n)
    integer, intent(in) :: n
    integer(i4), intent(inout) :: b(n)
    integer, intent(inout) :: ind(n)
    integer(i4) :: temp(n)
    integer :: tempind(n)
    integer :: i, jj
 
    !$omp parallel private(i, jj)
    !$omp do
    do i = 1, n
       jj = n+1-i
       temp(jj) = b(i)
       tempind(jj) = ind(i)
    end do
    !$omp end do
    !$omp do
    do i = 1,n
       b(i) = temp(i)
       ind(i) = tempind(i)
    end do
    !$omp end do
    !$omp end parallel
 
  end subroutine flipvi4
 
  subroutine absval(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = abs(a(i))
    end do
    !$omp end parallel do
 
  end subroutine absval
 
  ! ========================================================================== !
  ! Point-wise operations
 
  subroutine pwmax2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = max(a(i), b(i))
    end do
    !$omp end parallel do
 
  end subroutine pwmax2
 
  subroutine pwmax3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b, c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = max(b(i), c(i))
    end do
    !$omp end parallel do
 
  end subroutine pwmax3
 
  subroutine cpwmax2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = max(a(i), b)
    end do
    !$omp end parallel do
 
  end subroutine cpwmax2
 
  subroutine cpwmax3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = max(b(i), c)
    end do
    !$omp end parallel do
 
  end subroutine cpwmax3
 
  subroutine pwmin2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = min(a(i), b(i))
    end do
    !$omp end parallel do
 
  end subroutine pwmin2
 
  subroutine pwmin3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b, c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = min(b(i), c(i))
    end do
    !$omp end parallel do
 
  end subroutine pwmin3
 
  subroutine cpwmin2(a, b, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), intent(in) :: b
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = min(a(i), b)
    end do
    !$omp end parallel do
 
  end subroutine cpwmin2
 
  subroutine cpwmin3(a, b, c, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    real(kind=rp), dimension(n), intent(in) :: b
    real(kind=rp), intent(in) :: c
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = min(b(i), c)
    end do
    !$omp end parallel do
 
  end subroutine cpwmin3
 
  ! M33INV and M44INV by David G. Simpson pure function version from
  ! https://fortranwiki.org/fortran/show/Matrix+inversion
  ! Invert 3x3 matrix
  function matinv39(a11, a12, a13, a21, a22, a23, a31, a32, a33) &
       result(b)
    real(kind=rp), intent(in) :: a11, a12, a13, a21, a22, a23, a31, a32, a33
    real(xp) :: a(3,3) !! Matrix
    real(rp) :: b(3,3) !! Inverse matrix
    a(1,1) = a11
    a(1,2) = a12
    a(1,3) = a13
    a(2,1) = a21
    a(2,2) = a22
    a(2,3) = a23
    a(3,1) = a31
    a(3,2) = a32
    a(3,3) = a33
    b = matinv3(a)
  end function matinv39
 
  function matinv3(A) result(B)
    !! Performs a direct calculation of the inverse of a 3×3 matrix.
    real(kind=xp), intent(in) :: a(3,3) !! Matrix
    real(kind=xp) :: b(3,3) !! Inverse matrix
    real(kind=xp) :: detinv
 
    ! Calculate the inverse determinant of the matrix
    ! first index x,y,z, second r, s, t
    detinv = 1.0_xp / real(a(1,1)*a(2,2)*a(3,3) - a(1,1)*a(2,3)*a(3,2) &
         - a(1,2)*a(2,1)*a(3,3) + a(1,2)*a(2,3)*a(3,1)&
         + a(1,3)*a(2,1)*a(3,2) - a(1,3)*a(2,2)*a(3,1), xp)
    ! Calculate the inverse of the matrix
    ! first index r, s, t, second x, y, z
    b(1,1) = +detinv * (a(2,2)*a(3,3) - a(2,3)*a(3,2))
    b(2,1) = -detinv * (a(2,1)*a(3,3) - a(2,3)*a(3,1))
    b(3,1) = +detinv * (a(2,1)*a(3,2) - a(2,2)*a(3,1))
    b(1,2) = -detinv * (a(1,2)*a(3,3) - a(1,3)*a(3,2))
    b(2,2) = +detinv * (a(1,1)*a(3,3) - a(1,3)*a(3,1))
    b(3,2) = -detinv * (a(1,1)*a(3,2) - a(1,2)*a(3,1))
    b(1,3) = +detinv * (a(1,2)*a(2,3) - a(1,3)*a(2,2))
    b(2,3) = -detinv * (a(1,1)*a(2,3) - a(1,3)*a(2,1))
    b(3,3) = +detinv * (a(1,1)*a(2,2) - a(1,2)*a(2,1))
  end function matinv3
 
  function math_stepf(x) result(val)
    real(kind=rp), intent(in) :: x
    real(kind=rp) :: val
    real(kind=rp), parameter :: xdmin = 0.0001_rp
    real(kind=rp), parameter :: xdmax = 0.9999_rp
    real(kind=rp) :: g
 
    if (x <= xdmin) then
       ! Below the lower bound, the function is 0
       val = 0.0_rp
    else if (x >= xdmax) then
       ! Above the upper bound, the function is 1
       val = 1.0_rp
    else
       ! g(x) = 1/(x-1) + 1/x
       g = (1.0_rp / (x - 1.0_rp)) + (1.0_rp / x)
 
       ! The sigmoid: S(x) = 1 / (1 + exp(g))
       val = 1.0_rp / (1.0_rp + exp(g))
    end if
  end function math_stepf
 
  function math_dstepf(x) result(val)
    real(kind=rp), intent(in) :: x
    real(kind=rp) :: val
    real(kind=rp), parameter :: xdmin = 0.0001_rp
    real(kind=rp), parameter :: xdmax = 0.9999_rp
    real(kind=rp) :: arg, g, dg, s_val
 
    if (x <= xdmin .or. x >= xdmax) then
       val = 0.0_rp
    else
       ! The step function is S(x) = 1 / (1 + exp(g(x)))
       ! where g(x) = 1/(x-1) + 1/x
       ! S'(x) = -S(x) * (1 - S(x)) * g'(x)
 
       g = (1.0_rp / (x - 1.0_rp)) + (1.0_rp / x)
 
       ! Derivative of g(x)
       dg = -(1.0_rp / ((x - 1.0_rp)**2)) - (1.0_rp / (x**2))
 
       ! Recompute S(x) locally
       s_val = 1.0_rp / (1.0_rp + exp(g))
 
       val = -s_val * (1.0_rp - s_val) * dg
    end if
  end function math_dstepf
 
  subroutine sqrt_inplace(a, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: a
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       a(i) = sqrt(a(i))
    end do
    !$omp end parallel do
 
  end subroutine sqrt_inplace
 
  subroutine power(ap, a, p, n)
    integer, intent(in) :: n
    real(kind=rp), dimension(n), intent(inout) :: ap
    real(kind=rp), dimension(n), intent(in) :: a
    real(kind=rp), intent(in) :: p
    integer :: i
 
    !$omp parallel do
    do i = 1, n
       ap(i) = a(i)**p
    end do
    !$omp end parallel do
 
  end subroutine power
 
  pure subroutine eig_sym2(a11, a22, a12, e1, e2)
    real(kind=dp), intent(in) :: a11, a22, a12
    real(kind=dp), intent(out) :: e1, e2
    real(kind=dp) :: t, d, s
 
    t = a11 + a22
    d = a11 * a22 - a12 * a12
    s = sqrt(max(0.0_dp, 0.25_dp * t * t - d))
    e1 = 0.5_dp * t + s
 
    if (e1 .gt. 0.0_dp) then
       e2 = d / e1
    else
       e2 = 0.5_dp * t - s
    end if
 
  end subroutine eig_sym2
 
  pure subroutine eig_sym3(a11, a22, a33, a12, a13, a23, e1, e2, e3)
    real(kind=dp), intent(in) :: a11, a22, a33, a12, a13, a23
    real(kind=dp), intent(out) :: e1, e2, e3
    real(kind=dp) :: p1, p2, q, p, r, phi
    real(kind=dp) :: b11, b22, b33, b12, b13, b23
    real(kind=dp), parameter :: third = 1.0_dp / 3.0_dp
    real(kind=dp), parameter :: pi_third = 1.0471975511965976_dp
    real(kind=dp), parameter :: twopi_third = 2.0943951023931953_dp
 
    p1 = a12 * a12 + a13 * a13 + a23 * a23
 
    q = (a11 + a22 + a33) * third
    p2 = (a11 - q)**2 + (a22 - q)**2 + (a33 - q)**2 + 2.0_dp * p1
    p = sqrt(p2 / 6.0_dp)
 
    if (p .le. 0.0_dp) then
       ! diagonal and isotropic, or all zero
       e1 = max(a11, max(a22, a33))
       e3 = min(a11, min(a22, a33))
       e2 = a11 + a22 + a33 - e1 - e3
       return
    end if
 
    b11 = (a11 - q) / p
    b22 = (a22 - q) / p
    b33 = (a33 - q) / p
    b12 = a12 / p
    b13 = a13 / p
    b23 = a23 / p
 
    r = 0.5_dp * (b11 * (b22 * b33 - b23 * b23) &
         - b12 * (b12 * b33 - b23 * b13) &
         + b13 * (b12 * b23 - b22 * b13))
 
    ! r is in [-1, 1] analytically, round-off can push it out
    if (r .le. -1.0_dp) then
       phi = pi_third
    else if (r .ge. 1.0_dp) then
       phi = 0.0_dp
    else
       phi = acos(r) * third
    end if
 
    e1 = q + 2.0_dp * p * cos(phi)
    e3 = q + 2.0_dp * p * cos(phi + twopi_third)
    e2 = 3.0_dp * q - e1 - e3
 
  end subroutine eig_sym3
 
 
 
end module math