module utilities
  !>
  !> This module contains several simple general-purpose utilities
  !>

  use constants
  implicit none

contains

  subroutine error( routine, message, ierr )

    !    This is a simple routine which writes an error message to
    !    standard output. 
    !    If ierr = 0 it does nothing, 
    !    if ierr < 0 it writes the message but does not stop, 
    !    if ierr > 0 it writes the message and stops.

    character*(*), intent(in) :: routine, & ! name of the calling routine
                                 message    ! output message
    integer, intent(in)   :: ierr       ! error flag
    integer               :: i

    if( ierr == 0 ) return
    write(*,*) " "
    write(*,"(1x,78a)") ("-",i=1,78)
    write(*,"(5x,3a,i10)") "from ",routine,": error #",ierr
    write(*,"(5x,a)") message
    write(*,"(1x,78a)") ("-",i=1,78)

    if(ierr > 0) then
       write(*,"(a)") "     stopping ... "
       stop
    else
       write(*,*) " "
       return
    end if
  end subroutine error

  subroutine inv3x3(a,b,det)
    !>
    !> This routine calculates the inverse of a $3 \times 3$ real matrix 
    !> and, optionally, it also returns the determinant.
    !>
    !> When the columns of the input matrix, {\tt a}, coincide with the 
    !> basis vectors of a Bravais lattice (in units of {\tt alat}), the 
    !> rows of inverse, {\tt b}, coincide with the basis vectors of the 
    !> reciprocal lattice (in units of $2 \pi \over {\tt alat}$), and the
    !> determinant with the volume of the unit cell (in units of ${\tt 
    !> alat}^3$).
    !>
    !> The {\tt b} and {\tt det} output variables are optional. If {\tt b} 
    !> is not present, the inverse is overwritten on {\tt a}.
    !>

    real(kind=DP), intent(inout), dimension(:,:)         :: a
    real(kind=DP), intent(out), optional, dimension(:,:) :: b
    real(kind=DP), intent(out), optional                 :: det 
    real(kind=DP), dimension(3,3)                        :: inv
    real(kind=DP)                                        :: omg
    integer   :: iperm,       &! counter on the permutations
                 i,j,k,l       ! Auxiliary variables

    i = 1
    j = 2
    k = 3
    do iperm=1,3
       inv(i,1) = (a(2,j)*a(3,k)-a(2,k)*a(3,j))
       inv(i,2) = (a(3,j)*a(1,k)-a(3,k)*a(1,j))
       inv(i,3) = (a(1,j)*a(2,k)-a(1,k)*a(2,j))
       l = i
       i = j
       j = k
       k = l
    end do

    omg = a(1,1)*inv(1,1) + a(2,1)*inv(1,2) + a(3,1)*inv(1,3)
    inv(:,:)  = inv(:,:) / omg

    if(present(b)) then
       b(:,:) = inv(:,:)
    else
       a(:,:) = inv(:,:)
    end if

    if(present(det)) det = abs(omg)

    return
  end subroutine inv3x3

  subroutine hpsort(ra,ind) 

    !>
    !> This routine sorts an array {\tt ra(1:n)} into ascending order using 
    !> the {\sl heapsort} algorithm. {\tt ra} is replaced on output by its 
    !> sorted rearrangement. It also creates an index table, {\tt ind}, by 
    !> making an exchange in the index array whenever an exchange is made 
    !> on the sorted data array, {\tt ra}. In case of equal values in the 
    !> input data array, {\tt ra} the values in the index array, {\tt ind} 
    !> are used to order the entries. If on input $\tt ind(1)  = 0$ then 
    !> indices are initialized in the routine; if on input 
    !> $\tt ind(1) \ne  0$, then indices are assumed to have been 
    !> initialized before entering the routine and these indices are 
    !> carried around during the sorting process.
    !>
    !> No work space is needed!
    !>
    !> Adapted from Numerical Recipes pg. 329 (new edition) and ported to 
    !> f90 on Nov 11 2002 by SB
    !>
    
    integer, intent(out), dimension(:)    :: ind    ! in-out variables  
    real(DP), intent(inout), dimension(:) :: ra     !
  
    integer                               :: n, i, ir, j, l, iind ! local
    real(DP)                              :: rra                  ! variables

    n = size(ra)
    if( n > size(ind) ) stop "   hpsort: wrong dimensions for the input data"

    if ( ind(1) == 0 ) ind(1:n) = (/ (i, i=1,n) /) ! initialize index array

    if (n < 2) return                              ! nothing to order

    ! initialize indices for hiring and retirement-promotion phase

    l  = n/2 + 1
    ir = n
    outer: do
       if (l > 1) then    ! still in hiring phase
          l    = l-1
          rra  = ra(l)
          iind = ind(l)
       else                  ! in retirement-promotion phase.
          rra     = ra(ir)   ! clear a space at the end of the array
          iind    = ind(ir)  !  
          ra(ir)  = ra(1)    ! retire the top of the heap into it
          ind(ir) = ind(1)   !
          ir      = ir -1    ! decrease the size of the corporation
          if (ir == 1) then  ! done with the last promotion
             ra(1) = rra     ! the least competent worker at all !
             ind(1)= iind    !
             exit outer
          end if
       end if
       i = l               ! wheter in hiring or promotion phase, we 
       j = l + l           ! set up to place rra in its proper level
       
       do while (j <= ir) 
          if (j < ir) then
             if (ra(j) < ra(j+1)) then ! compare to better underling
                j = j+1 
             else if (ra(j) == ra(j+1)) then
                if (ind(j) < ind(j+1)) j = j+1
             end if
          end if
          if (rra < ra(j)) then ! demote rra
             ra(i) = ra(j)
             ind(i)= ind(j)
             i = j
             j = j + j
          else if (rra == ra(j)) then
             if (iind < ind(j)) then ! demote rra
                ra(i) = ra(j)
                ind(i)= ind(j)
                i = j
                j = j + j
             else
                j = ir + 1       ! set j to terminate do-while loop
             end if
          else                   ! this is the right place for rra
             j = ir + 1          ! set j to terminate do-while loop
          end if
       end do
       ra(i) = rra
       ind(i)= iind
       
    end do outer

    return
  end subroutine hpsort

!---------------------------------------------------------------------
real(dp) function erf (x)  
  !---------------------------------------------------------------------
  !
  !     Error function - computed from the rational approximations of
  !     W. J. Cody, Math. Comp. 22 (1969), pages 631-637.
  !
  !     for abs(x) le 0.47 erf is calculated directly
  !     for abs(x) gt 0.47 erf is calculated via erf(x)=1-erfc(x)
  !
  real(kind=dp) :: x, x2, p1 (4), q1 (4)
  data p1 / 2.42667955230532d2, 2.19792616182942d1, &
       6.99638348861914d0, - 3.56098437018154d-2 /
  data q1 / 2.15058875869861d2, 9.11649054045149d1, &
       1.50827976304078d1, 1.00000000000000d0 /
  !
  if (abs (x) .gt.6.d0) then  
     !
     !  erf(6)=1-10^(-17) cannot be distinguished from 1 with 16-byte words
     !
     erf = sign (1.d0, x)  
  else  
     if (abs (x) .le.0.47d0) then  
        x2 = x**2  
        erf = x * (p1 (1) + x2 * (p1 (2) + x2 * (p1 (3) + x2 * p1 ( &
             4) ) ) ) / (q1 (1) + x2 * (q1 (2) + x2 * (q1 (3) + x2 * q1 ( &
             4) ) ) )
     else  
        erf = 1.d0 - erfc (x)  
     endif
  endif
  !
  return  
end function erf
!
!---------------------------------------------------------------------
real(dp) function erfc (x)  
  !---------------------------------------------------------------------
  !
  !     erfc(x) = 1-erf(x)  - See comments in erf
  !

  real(kind=dp) :: x, ax, x2, xm2, p2 (8), q2 (8), p3 (5), q3 (5), pim1
  data p2 / 3.00459261020162d2, 4.51918953711873d2, &
       3.39320816734344d2, 1.52989285046940d2, 4.31622272220567d1, &
       7.21175825088309d0, 5.64195517478974d-1, - 1.36864857382717d-7 /
  data q2 / 3.00459260956983d2, 7.90950925327898d2, &
       9.31354094850610d2, 6.38980264465631d2, 2.77585444743988d2, &
       7.70001529352295d1, 1.27827273196294d1, 1.00000000000000d0 /
  data p3 / - 2.99610707703542d-3, - 4.94730910623251d-2, - &
       2.26956593539687d-1, - 2.78661308609648d-1, - 2.23192459734185d-2 &
       /
  data q3 / 1.06209230528468d-2, 1.91308926107830d-1, &
       1.05167510706793d0, 1.98733201817135d0, 1.00000000000000d0 /

  data pim1 / 0.564189583547756d0 /  
  !        ( pim1= sqrt(1/pi) )
  ax = abs (x)  
  if (ax.gt.26.d0) then  
     !
     !  erfc(26.0)=10^(-296); erfc( 9.0)=10^(-37);
     !
     erfc = 0.d0  
  elseif (ax.gt.4.d0) then  
     x2 = x**2  
     xm2 = (1.d0 / ax) **2  
     erfc = (1.d0 / ax) * exp ( - x2) * (pim1 + xm2 * (p3 (1) &
          + xm2 * (p3 (2) + xm2 * (p3 (3) + xm2 * (p3 (4) + xm2 * p3 (5) &
          ) ) ) ) / (q3 (1) + xm2 * (q3 (2) + xm2 * (q3 (3) + xm2 * &
          (q3 (4) + xm2 * q3 (5) ) ) ) ) )
  elseif (ax.gt.0.47d0) then  
     x2 = x**2  
     erfc = exp ( - x2) * (p2 (1) + ax * (p2 (2) + ax * (p2 (3) &
          + ax * (p2 (4) + ax * (p2 (5) + ax * (p2 (6) + ax * (p2 (7) &
          + ax * p2 (8) ) ) ) ) ) ) ) / (q2 (1) + ax * (q2 (2) + ax * &
          (q2 (3) + ax * (q2 (4) + ax * (q2 (5) + ax * (q2 (6) + ax * &
          (q2 (7) + ax * q2 (8) ) ) ) ) ) ) )
  else  
     erfc = 1.d0 - erf (ax)  
  endif
  !
  ! erf(-x)=-erf(x)  =>  erfc(-x) = 2-erfc(x)
  !
  if (x.lt.0.d0) erfc = 2.d0 - erfc  
  !
  return  
end function erfc
!---------------------------------------------------------------------
real(dp) function gauss_freq (x)  
  !---------------------------------------------------------------------
  !
  !     gauss_freq(x) = (1+erf(x/sqrt(2)))/2 = erfc(-x/sqrt(2))/2
  !             - See comments in erf
  !
  real(kind=dp) :: x, c

  data c / 0.707106781186548d0 /  
  !        ( c= sqrt(1/2) )
  gauss_freq = 0.5d0 * erfc ( - x * c)  
  !
  return  
end function gauss_freq
end module utilities
