c       this file contains the following user-callable routines:
c
c
c       routine idz_snorm estimates the spectral norm
c       of a matrix specified by routines for applying the matrix
c       and its adjoint to arbitrary vectors. This routine uses
c       the power method with a random starting vector.
c
c       routine idz_diffsnorm estimates the spectral norm
c       of the difference between two matrices specified by routines
c       for applying the matrices and their adjoints
c       to arbitrary vectors. This routine uses
c       the power method with a random starting vector.
c
c       routine idz_enorm calculates the Euclidean norm of a vector.
c
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c
c
c
        subroutine idz_snorm(m,n,matveca,p1a,p2a,p3a,p4a,
     1                       matvec,p1,p2,p3,p4,its,snorm,v,u)
c
c       estimates the spectral norm of a matrix a specified
c       by a routine matvec for applying a to an arbitrary vector,
c       and by a routine matveca for applying a^*
c       to an arbitrary vector. This routine uses the power method
c       with a random starting vector.
c
c       input:
c       m -- number of rows in a
c       n -- number of columns in a
c       matveca -- routine which applies the adjoint of a
c                  to an arbitrary vector; this routine must have
c                  a calling sequence of the form
c
c                  matveca(m,x,n,y,p1a,p2a,p3a,p4a),
c
c                  where m is the length of x,
c                  x is the vector to which the adjoint of a
c                  is to be applied,
c                  n is the length of y,
c                  y is the product of the adjoint of a and x,
c                  and p1a, p2a, p3a, and p4a are user-specified
c                  parameters
c       p1a -- parameter to be passed to routine matveca
c       p2a -- parameter to be passed to routine matveca
c       p3a -- parameter to be passed to routine matveca
c       p4a -- parameter to be passed to routine matveca
c       matvec -- routine which applies the matrix a
c                 to an arbitrary vector; this routine must have
c                 a calling sequence of the form
c
c                 matvec(n,x,m,y,p1,p2,p3,p4),
c
c                 where n is the length of x,
c                 x is the vector to which a is to be applied,
c                 m is the length of y,
c                 y is the product of a and x,
c                 and p1, p2, p3, and p4 are user-specified parameters
c       p1 -- parameter to be passed to routine matvec
c       p2 -- parameter to be passed to routine matvec
c       p3 -- parameter to be passed to routine matvec
c       p4 -- parameter to be passed to routine matvec
c       its -- number of iterations of the power method to conduct
c
c       output:
c       snorm -- estimate of the spectral norm of a
c       v -- estimate of a normalized right singular vector
c            corresponding to the greatest singular value of a
c
c       work:
c       u -- must be at least m complex*16 elements long
c
c       reference:
c       Kuczynski and Wozniakowski, "Estimating the largest eigenvalue
c            by the power and Lanczos algorithms with a random start,"
c            SIAM Journal on Matrix Analysis and Applications,
c            13 (4): 1992, 1094-1122.
c
        implicit none
        integer m,n,its,it,n2,k
        real*8 snorm,enorm
        complex*16 p1a,p2a,p3a,p4a,p1,p2,p3,p4,u(m),v(n)
        external matveca,matvec
c
c
c       Fill the real and imaginary parts of each entry
c       of the initial vector v with i.i.d. random variables
c       drawn uniformly from [-1,1].
c
        n2 = 2*n
        call id_srand(n2,v)
c
        do k = 1,n
          v(k) = 2*v(k)-1
        enddo ! k
c
c
c       Normalize v.
c
        call idz_enorm(n,v,enorm)
c
        do k = 1,n
          v(k) = v(k)/enorm
        enddo ! k
c
c
        do it = 1,its
c
c         Apply a to v, obtaining u.
c
          call matvec(n,v,m,u,p1,p2,p3,p4)
c
c         Apply a^* to u, obtaining v.
c
          call matveca(m,u,n,v,p1a,p2a,p3a,p4a)
c
c         Normalize v.
c
          call idz_enorm(n,v,snorm)
c
          if(snorm .ne. 0) then
c
            do k = 1,n
              v(k) = v(k)/snorm
            enddo ! k
c
          endif
c
          snorm = sqrt(snorm)
c
        enddo ! it
c
c
        return
        end
c
c
c
c
        subroutine idz_enorm(n,v,enorm)
c
c       computes the Euclidean norm of v, the square root
c       of the sum of the squares of the absolute values
c       of the entries of v.
c
c       input:
c       n -- length of v
c       v -- vector whose Euclidean norm is to be calculated
c
c       output:
c       enorm -- Euclidean norm of v
c
        implicit none
        integer n,k
        real*8 enorm
        complex*16 v(n)
c
c
        enorm = 0
c
        do k = 1,n
          enorm = enorm+v(k)*conjg(v(k))
        enddo ! k
c
        enorm = sqrt(enorm)
c
c
        return
        end
c
c
c
c
        subroutine idz_diffsnorm(m,n,matveca,p1a,p2a,p3a,p4a,
     1                           matveca2,p1a2,p2a2,p3a2,p4a2,
     2                           matvec,p1,p2,p3,p4,
     3                           matvec2,p12,p22,p32,p42,its,snorm,w)
c
c       estimates the spectral norm of the difference between matrices
c       a and a2, where a is specified by routines matvec and matveca
c       for applying a and a^* to arbitrary vectors,
c       and a2 is specified by routines matvec2 and matveca2
c       for applying a2 and (a2)^* to arbitrary vectors.
c       This routine uses the power method
c       with a random starting vector.
c
c       input:
c       m -- number of rows in a, as well as the number of rows in a2
c       n -- number of columns in a, as well as the number of columns
c            in a2
c       matveca -- routine which applies the adjoint of a
c                  to an arbitrary vector; this routine must have
c                  a calling sequence of the form
c
c                  matveca(m,x,n,y,p1a,p2a,p3a,p4a),
c
c                  where m is the length of x,
c                  x is the vector to which the adjoint of a
c                  is to be applied,
c                  n is the length of y,
c                  y is the product of the adjoint of a and x,
c                  and p1a, p2a, p3a, and p4a are user-specified
c                  parameters
c       p1a -- parameter to be passed to routine matveca
c       p2a -- parameter to be passed to routine matveca
c       p3a -- parameter to be passed to routine matveca
c       p4a -- parameter to be passed to routine matveca
c       matveca2 -- routine which applies the adjoint of a2
c                   to an arbitrary vector; this routine must have
c                   a calling sequence of the form
c
c                   matveca2(m,x,n,y,p1a2,p2a2,p3a2,p4a2),
c
c                   where m is the length of x,
c                   x is the vector to which the adjoint of a2
c                   is to be applied,
c                   n is the length of y,
c                   y is the product of the adjoint of a2 and x,
c                   and p1a2, p2a2, p3a2, and p4a2 are user-specified
c                   parameters
c       p1a2 -- parameter to be passed to routine matveca2
c       p2a2 -- parameter to be passed to routine matveca2
c       p3a2 -- parameter to be passed to routine matveca2
c       p4a2 -- parameter to be passed to routine matveca2
c       matvec -- routine which applies the matrix a
c                 to an arbitrary vector; this routine must have
c                 a calling sequence of the form
c
c                 matvec(n,x,m,y,p1,p2,p3,p4),
c
c                 where n is the length of x,
c                 x is the vector to which a is to be applied,
c                 m is the length of y,
c                 y is the product of a and x,
c                 and p1, p2, p3, and p4 are user-specified parameters
c       p1 -- parameter to be passed to routine matvec
c       p2 -- parameter to be passed to routine matvec
c       p3 -- parameter to be passed to routine matvec
c       p4 -- parameter to be passed to routine matvec
c       matvec2 -- routine which applies the matrix a2
c                  to an arbitrary vector; this routine must have
c                  a calling sequence of the form
c
c                  matvec2(n,x,m,y,p12,p22,p32,p42),
c
c                  where n is the length of x,
c                  x is the vector to which a2 is to be applied,
c                  m is the length of y,
c                  y is the product of a2 and x, and
c                  p12, p22, p32, and p42 are user-specified parameters
c       p12 -- parameter to be passed to routine matvec2
c       p22 -- parameter to be passed to routine matvec2
c       p32 -- parameter to be passed to routine matvec2
c       p42 -- parameter to be passed to routine matvec2
c       its -- number of iterations of the power method to conduct
c
c       output:
c       snorm -- estimate of the spectral norm of a-a2
c
c       work:
c       w -- must be at least 3*m+3*n complex*16 elements long
c
c       reference:
c       Kuczynski and Wozniakowski, "Estimating the largest eigenvalue
c            by the power and Lanczos algorithms with a random start,"
c            SIAM Journal on Matrix Analysis and Applications,
c            13 (4): 1992, 1094-1122.
c
        implicit none
        integer m,n,its,lw,iu,lu,iu1,lu1,iu2,lu2,
     1          iv,lv,iv1,lv1,iv2,lv2
        real*8 snorm
        complex*16 p1a,p2a,p3a,p4a,p1a2,p2a2,p3a2,p4a2,
     1             p1,p2,p3,p4,p12,p22,p32,p42,w(3*m+3*n)
        external matveca,matvec,matveca2,matvec2
c
c
c       Allocate memory in w.
c
        lw = 0
c
        iu = lw+1
        lu = m
        lw = lw+lu
c
        iu1 = lw+1
        lu1 = m
        lw = lw+lu1
c
        iu2 = lw+1
        lu2 = m
        lw = lw+lu2
c
        iv = lw+1
        lv = n
        lw = lw+1
c
        iv1 = lw+1
        lv1 = n
        lw = lw+lv1
c
        iv2 = lw+1
        lv2 = n
        lw = lw+lv2
c
c
        call idz_diffsnorm0(m,n,matveca,p1a,p2a,p3a,p4a,
     1                      matveca2,p1a2,p2a2,p3a2,p4a2,
     2                      matvec,p1,p2,p3,p4,
     3                      matvec2,p12,p22,p32,p42,
     4                      its,snorm,w(iu),w(iu1),w(iu2),
     5                      w(iv),w(iv1),w(iv2))
c
c
        return
        end
c
c
c
c
        subroutine idz_diffsnorm0(m,n,matveca,p1a,p2a,p3a,p4a,
     1                            matveca2,p1a2,p2a2,p3a2,p4a2,
     2                            matvec,p1,p2,p3,p4,
     3                            matvec2,p12,p22,p32,p42,
     4                            its,snorm,u,u1,u2,v,v1,v2)
c
c       routine idz_diffsnorm serves as a memory wrapper
c       for the present routine. (Please see routine idz_diffsnorm
c       for further documentation.)
c
        implicit none
        integer m,n,its,it,n2,k
        real*8 snorm,enorm
        complex*16 p1a,p2a,p3a,p4a,p1a2,p2a2,p3a2,p4a2,
     1             p1,p2,p3,p4,p12,p22,p32,p42,u(m),u1(m),u2(m),
     2             v(n),v1(n),v2(n)
        external matveca,matvec,matveca2,matvec2
c
c
c       Fill the real and imaginary parts of each entry
c       of the initial vector v with i.i.d. random variables
c       drawn uniformly from [-1,1].
c
        n2 = 2*n
        call id_srand(n2,v)
c
        do k = 1,n
          v(k) = 2*v(k)-1
        enddo ! k
c
c
c       Normalize v.
c
        call idz_enorm(n,v,enorm)
c
        do k = 1,n
          v(k) = v(k)/enorm
        enddo ! k
c
c
        do it = 1,its
c
c         Apply a and a2 to v, obtaining u1 and u2.
c
          call matvec(n,v,m,u1,p1,p2,p3,p4)
          call matvec2(n,v,m,u2,p12,p22,p32,p42)
c
c         Form u = u1-u2.
c
          do k = 1,m
            u(k) = u1(k)-u2(k)
          enddo ! k
c
c         Apply a^* and (a2)^* to u, obtaining v1 and v2.
c
          call matveca(m,u,n,v1,p1a,p2a,p3a,p4a)
          call matveca2(m,u,n,v2,p1a2,p2a2,p3a2,p4a2)
c
c         Form v = v1-v2.
c
          do k = 1,n
            v(k) = v1(k)-v2(k)
          enddo ! k
c
c         Normalize v.
c
          call idz_enorm(n,v,snorm)
c
          if(snorm .gt. 0) then
c
            do k = 1,n
              v(k) = v(k)/snorm
            enddo ! k
c
          endif
c
          snorm = sqrt(snorm)
c
        enddo ! it
c
c
        return
        end


!> 通过应用密度求和法更新密度的子程序。详见第 4 章中的论述。
!>   subroutine to calculate the density with sph summation algorithm.
!>   see equ.(4.35)
!>
!>     ntotal : number of particles                                  [in]
!>     hsml   : smoothing length                                     [in]
!>     mass   : particle masses                                      [in]
!>     niac   : number of interaction pairs                          [in]
!>     pair_i : list of first partner of interaction pair            [in]
!>     pair_j : list of second partner of interaction pair           [in]
!>     w      : kernel for all interaction pairs                     [in]
!>     itype   : type of particles                                   [in]
!>     x       : coordinates of all particles                        [in]
!>     rho    : density                                             [out]

subroutine sum_density(ntotal, hsml, mass, niac, pair_i, pair_j, w, itype, rho)

    use sph_kind, only: rk
    use parameter
    implicit none

    !> 在模拟中所使用的粒子总数
    !> number of particles in simulation
    integer, intent(in) :: ntotal
    !> 相互作用对的数目
    integer, intent(in) :: niac
    integer :: pair_i(max_interaction), pair_j(max_interaction), itype(maxn)
    real(rk) :: hsml(maxn), mass(maxn), w(max_interaction), rho(maxn)
    integer :: i, j, k, d
    real(rk) :: selfdens, hv(dim), r, wi(maxn)

    !     wi(maxn)---integration of the kernel itself

    do d = 1, dim
        hv(d) = 0._rk
    end do

    !     self density of each particle: wii (kernel for distance 0)
    !     and take contribution of particle itself:

    r = 0._rk

    !     firstly calculate the integration of the kernel over the space

    do i = 1, ntotal
        call kernel(r, hv, hsml(i), selfdens, hv)
        wi(i) = selfdens*mass(i)/rho(i)
    end do

    do k = 1, niac
        i = pair_i(k)
        j = pair_j(k)
        wi(i) = wi(i) + mass(j)/rho(j)*w(k)
        wi(j) = wi(j) + mass(i)/rho(i)*w(k)
    end do

    !     secondly calculate the rho integration over the space

    do i = 1, ntotal
        call kernel(r, hv, hsml(i), selfdens, hv)
        rho(i) = selfdens*mass(i)
    end do

    !     calculate sph sum for rho:
    do k = 1, niac
        i = pair_i(k)
        j = pair_j(k)
        rho(i) = rho(i) + mass(j)*w(k)
        rho(j) = rho(j) + mass(i)*w(k)
    end do

    !     thirdly, calculate the normalized rho, rho=sum(rho)/sum(w)

    if (nor_density) then
        do i = 1, ntotal
            rho(i) = rho(i)/wi(i)
        end do
    end if

end subroutine sum_density

!> 通过应用连续密度法更新密度的子程序。详见第 4 章中的论述。
!>     subroutine to calculate the density with sph continuity approach.
!>     see equ.(4.34)
!>
!>     ntotal : number of particles                                  [in]
!>     mass   : particle masses                                      [in]
!>     niac   : number of interaction pairs                          [in]
!>     pair_i : list of first partner of interaction pair            [in]
!>     pair_j : list of second partner of interaction pair           [in]
!>     dwdx   : derivation of kernel for all interaction pairs       [in]
!>     vx     : velocities of all particles                          [in]
!>     itype   : type of particles                                   [in]
!>     x      : coordinates of all particles                         [in]
!>     rho    : density                                              [in]
!>     drhodt : density change rate of each particle                [out]

subroutine con_density(ntotal, mass, niac, pair_i, pair_j, dwdx, vx, itype, x, rho, drhodt)

    use sph_kind, only: rk
    use parameter
    implicit none

    !> 在模拟中所使用的粒子总数
    !> number of particles in simulation
    integer, intent(in) :: ntotal
    !> 相互作用对的数目
    integer, intent(in) :: niac
    integer :: pair_i(max_interaction), pair_j(max_interaction), itype(maxn)
    real(rk) :: mass(maxn), dwdx(dim, max_interaction), vx(dim, maxn), x(dim, maxn), rho(maxn), drhodt(maxn)
    integer :: i, j, k, d
    real(rk) :: vcc, dvx(dim)

    do i = 1, ntotal
        drhodt(i) = 0._rk
    end do

    do k = 1, niac

        i = pair_i(k)
        j = pair_j(k)
        do d = 1, dim
            dvx(d) = vx(d, i) - vx(d, j)
        end do
        vcc = dvx(1)*dwdx(1, k)
        do d = 2, dim
            vcc = vcc + dvx(d)*dwdx(d, k)
        end do
        drhodt(i) = drhodt(i) + mass(j)*vcc
        drhodt(j) = drhodt(j) + mass(i)*vcc

    end do

end subroutine con_density


*> \brief \b ZGETRI
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*> \htmlonly
*> Download ZGETRI + dependencies
*>
*> [TGZ]
*>
*> [ZIP]
*>
*> [TXT]
*> \endhtmlonly
*
*  Definition:
*  ===========
*
*       SUBROUTINE ZGETRI( N, A, LDA, IPIV, WORK, LWORK, INFO )
*
*       .. Scalar Arguments ..
*       INTEGER            INFO, LDA, LWORK, N
*       ..
*       .. Array Arguments ..
*       INTEGER            IPIV( * )
*       COMPLEX*16         A( LDA, * ), WORK( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> ZGETRI computes the inverse of a matrix using the LU factorization
*> computed by ZGETRF.
*>
*> This method inverts U and then computes inv(A) by solving the system
*> inv(A)*L = inv(U) for inv(A).
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The order of the matrix A.  N >= 0.
*> \endverbatim
*>
*> \param[in,out] A
*> \verbatim
*>          A is COMPLEX*16 array, dimension (LDA,N)
*>          On entry, the factors L and U from the factorization
*>          A = P*L*U as computed by ZGETRF.
*>          On exit, if INFO = 0, the inverse of the original matrix A.
*> \endverbatim
*>
*> \param[in] LDA
*> \verbatim
*>          LDA is INTEGER
*>          The leading dimension of the array A.  LDA >= max(1,N).
*> \endverbatim
*>
*> \param[in] IPIV
*> \verbatim
*>          IPIV is INTEGER array, dimension (N)
*>          The pivot indices from ZGETRF; for 1<=i<=N, row i of the
*>          matrix was interchanged with row IPIV(i).
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is COMPLEX*16 array, dimension (MAX(1,LWORK))
*>          On exit, if INFO=0, then WORK(1) returns the optimal LWORK.
*> \endverbatim
*>
*> \param[in] LWORK
*> \verbatim
*>          LWORK is INTEGER
*>          The dimension of the array WORK.  LWORK >= max(1,N).
*>          For optimal performance LWORK >= N*NB, where NB is
*>          the optimal blocksize returned by ILAENV.
*>
*>          If LWORK = -1, then a workspace query is assumed; the routine
*>          only calculates the optimal size of the WORK array, returns
*>          this value as the first entry of the WORK array, and no error
*>          message related to LWORK is issued by XERBLA.
*> \endverbatim
*>
*> \param[out] INFO
*> \verbatim
*>          INFO is INTEGER
*>          = 0:  successful exit
*>          < 0:  if INFO = -i, the i-th argument had an illegal value
*>          > 0:  if INFO = i, U(i,i) is exactly zero; the matrix is
*>                singular and its inverse could not be computed.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date December 2016
*
*> \ingroup complex16GEcomputational
*
*  =====================================================================
      SUBROUTINE ZGETRI( N, A, LDA, IPIV, WORK, LWORK, INFO )
*
*  -- LAPACK computational routine (version 3.7.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     December 2016
*
*     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LWORK, N
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      COMPLEX*16         A( LDA, * ), WORK( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      COMPLEX*16         ZERO, ONE
      PARAMETER          ( ZERO = ( 0.0D+0, 0.0D+0 ),
     $                   ONE = ( 1.0D+0, 0.0D+0 ) )
*     ..
*     .. Local Scalars ..
      LOGICAL            LQUERY
      INTEGER            I, IWS, J, JB, JJ, JP, LDWORK, LWKOPT, NB,
     $                   NBMIN, NN
*     ..
*     .. External Functions ..
      INTEGER            ILAENV
      EXTERNAL           ILAENV
*     ..
*     .. External Subroutines ..
      EXTERNAL           XERBLA, ZGEMM, ZGEMV, ZSWAP, ZTRSM, ZTRTRI
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      NB = ILAENV( 1, 'ZGETRI', ' ', N, -1, -1, -1 )
      LWKOPT = N*NB
      WORK( 1 ) = LWKOPT
      LQUERY = ( LWORK.EQ.-1 )
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -3
      ELSE IF( LWORK.LT.MAX( 1, N ) .AND. .NOT.LQUERY ) THEN
         INFO = -6
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'ZGETRI', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 )
     $   RETURN
*
*     Form inv(U).  If INFO > 0 from ZTRTRI, then U is singular,
*     and the inverse is not computed.
*
      CALL ZTRTRI( 'Upper', 'Non-unit', N, A, LDA, INFO )
      IF( INFO.GT.0 )
     $   RETURN
*
      NBMIN = 2
      LDWORK = N
      IF( NB.GT.1 .AND. NB.LT.N ) THEN
         IWS = MAX( LDWORK*NB, 1 )
         IF( LWORK.LT.IWS ) THEN
            NB = LWORK / LDWORK
            NBMIN = MAX( 2, ILAENV( 2, 'ZGETRI', ' ', N, -1, -1, -1 ) )
         END IF
      ELSE
         IWS = N
      END IF
*
*     Solve the equation inv(A)*L = inv(U) for inv(A).
*
      IF( NB.LT.NBMIN .OR. NB.GE.N ) THEN
*
*        Use unblocked code.
*
         DO 20 J = N, 1, -1
*
*           Copy current column of L to WORK and replace with zeros.
*
            DO 10 I = J + 1, N
               WORK( I ) = A( I, J )
               A( I, J ) = ZERO
   10       CONTINUE
*
*           Compute current column of inv(A).
*
            IF( J.LT.N )
     $         CALL ZGEMV( 'No transpose', N, N-J, -ONE, A( 1, J+1 ),
     $                     LDA, WORK( J+1 ), 1, ONE, A( 1, J ), 1 )
   20    CONTINUE
      ELSE
*
*        Use blocked code.
*
         NN = ( ( N-1 ) / NB )*NB + 1
         DO 50 J = NN, 1, -NB
            JB = MIN( NB, N-J+1 )
*
*           Copy current block column of L to WORK and replace with
*           zeros.
*
            DO 40 JJ = J, J + JB - 1
               DO 30 I = JJ + 1, N
                  WORK( I+( JJ-J )*LDWORK ) = A( I, JJ )
                  A( I, JJ ) = ZERO
   30          CONTINUE
   40       CONTINUE
*
*           Compute current block column of inv(A).
*
            IF( J+JB.LE.N )
     $         CALL ZGEMM( 'No transpose', 'No transpose', N, JB,
     $                     N-J-JB+1, -ONE, A( 1, J+JB ), LDA,
     $                     WORK( J+JB ), LDWORK, ONE, A( 1, J ), LDA )
            CALL ZTRSM( 'Right', 'Lower', 'No transpose', 'Unit', N, JB,
     $                  ONE, WORK( J ), LDWORK, A( 1, J ), LDA )
   50    CONTINUE
      END IF
*
*     Apply column interchanges.
*
      DO 60 J = N - 1, 1, -1
         JP = IPIV( J )
         IF( JP.NE.J )
     $      CALL ZSWAP( N, A( 1, J ), 1, A( 1, JP ), 1 )
   60 CONTINUE
*
      WORK( 1 ) = IWS
      RETURN
*
*     End of ZGETRI
*
      END


      program gammabar
c-----------------------------------------------------------------------
c
c                  Average Variogram for a Volume
c                  ******************************
c
c This program calculates the gammabar value from a semivariogram model
c
c
c
c
c
c-----------------------------------------------------------------------
      parameter(MAXNST=4, MDIR=10, DEG2RAD=3.14159265/180.0,
     +          MAXROT=MAXNST+1, EPSLON = 1.0e-20, VERSION=2.905)

      real*8     rotmat(MAXROT,3,3)
      real       aa(MAXNST),c0(1),cc(MAXNST),ang1(MAXNST),ang2(MAXNST),
     +           ang3(MAXNST),anis1(MAXNST),anis2(MAXNST),maxcov
      integer    nst(1),it(MAXNST)
      character  str*40
      logical    testfl
      data       lin/1/
c
c Note VERSION number:
c
      write(*,9999) VERSION
 9999 format(/' GAMMABAR Version: ',f5.3/)
c
c Get the name of the parameter file - try the default name if no input:
c
      write(*,*) 'Which parameter file do you want to use?'
      read (*,'(a20)') str(1:20)
      if(str(1:1).eq.' ') str(1:20) = 'gammabar.par        '
      inquire(file=str(1:20),exist=testfl)
      if(.not.testfl) then
            write(*,*) 'ERROR - the parameter file does not exist,'
            write(*,*) '        check for the file and try again  '
            write(*,*)
            if(str(1:20).eq.'gammabar.par        ') then
                  write(*,*) '        creating a blank parameter file'
                  call makepar
                  write(*,*)
            end if
            stop
      endif
      open(lin,file=str(1:20),status='OLD')
c
c Find Start of Parameters:
c
 1    read(lin,'(a4)',end=98) str(1:4)
      if(str(1:4).ne.'STAR') go to 1
c
c Read Input Parameters:
c

      read(lin,*,err=98) xsiz,ysiz,zsiz
      write(*,*) ' size of block = ',xsiz,ysiz,zsiz

      read(lin,*,err=98) nx,ny,nz
      write(*,*) ' discretization= ',nx,ny,nz

      nx = max(nx,1)
      ny = max(ny,1)
      nz = max(nz,1)

      read(lin,*,err=98) nst(1),c0(1)
      write(*,*) ' nst, c0 = ',nst(1),c0(1)
      if(nst(1).le.0) then
            write(*,9997) nst(1)
 9997       format(' nst must be at least 1, it has been set to ',i4,/,
     +             ' The c or a values can be set to zero')
            stop
      endif
      do i=1,nst(1)
            read(lin,*,err=98) it(i),cc(i),ang1(i),ang2(i),ang3(i)
            read(lin,*,err=98) aa(i),aa1,aa2
            anis1(i) = aa1 / max(aa(i),EPSLON)
            anis2(i) = aa2 / max(aa(i),EPSLON)
            if(it(i).eq.4) then
                  if(aa(i).lt.0.0) stop ' INVALID power variogram'
                  if(aa(i).gt.2.0) stop ' INVALID power variogram'
            end if
            write(*,*) ' it,cc,ang[1,2,3] =  ',it(i),cc(i),
     +                   ang1(i),ang2(i),ang3(i)
            write(*,*) ' a1 a2 a3 = ',aa(i),aa1,aa2
            write(*,*) ' anis 1 2 = ',anis1(i),anis2(i)
      end do
      close(lin)
c
c Get ready to go:
c
      do is=1,nst(1)
            call setrot(ang1(is),ang2(is),ang3(is),anis1(is),anis2(is),
     +                  is,MAXROT,rotmat)
      end do
      call cova3(0.0,0.0,0.0,0.0,0.0,0.0,1,nst,MAXNST,c0,it,cc,aa,
     +                 1,MAXROT,rotmat,cmax,maxcov)
c
c Discretization parameters:
c
      xsz   = xsiz / real(nx)
      xmn   = xsz  / 2.0
      xzero = xsz  * 0.0001
      ysz   = ysiz / real(ny)
      ymn   = ysz  / 2.0
      yzero = ysz  * 0.0001
      zsz   = zsiz / real(nz)
      zmn   = zsz  / 2.0
      zzero = zsz  * 0.0001

      write(*,*)
      write(*,*) 'X direction size ',xsiz,' number ',nx,' offset ',xzero
      write(*,*) 'Y direction size ',ysiz,' number ',ny,' offset ',yzero
      write(*,*) 'Z direction size ',zsiz,' number ',nz,' offset ',zzero
c
c Calculate gammabar:
c
      gammab = 0.0
      do ix=1,nx
      xxi = xmn + real(ix-1)*xsz+xzero
      do iy=1,ny
      yyi = ymn + real(iy-1)*ysz+yzero
      do iz=1,nz
      zzi = zmn + real(iz-1)*zsz+zzero

            do jx=1,nx
            xxj = xmn + real(jx-1)*xsz
            do jy=1,ny
            yyj = ymn + real(jy-1)*ysz
            do jz=1,nz
            zzj = zmn + real(jz-1)*zsz

            call cova3(xxi,yyi,zzi,xxj,yyj,zzj,1,nst,MAXNST,c0,it,
     +                 cc,aa,1,MAXROT,rotmat,cmax,cov)
            gammab = gammab + (maxcov - cov)

            end do
            end do
            end do
      end do
      end do
      end do

      gammab = gammab / (real(nx*ny*nz)**2)

      write(*,*)
      write(*,*) ' Average variogram = ',gammab
c
c Finished:
c
      write(*,9998) VERSION
 9998 format(/' GAMMABAR Version: ',f5.3, ' Finished'/)
      stop
 98   stop 'Error in parameter file somewhere'
      end

      subroutine cova3(x1,y1,z1,x2,y2,z2,ivarg,nst,MAXNST,c0,it,cc,aa,
     +                 irot,MAXROT,rotmat,cmax,cova)
c-----------------------------------------------------------------------
c
c                    Covariance Between Two Points
c                    *****************************
c
c This subroutine calculated the covariance associated with a variogram
c model specified by a nugget effect and nested varigoram structures.
c The anisotropy definition can be different for each nested structure.
c
c
c
c INPUT VARIABLES:
c
c   x1,y1,z1         coordinates of first point
c   x2,y2,z2         coordinates of second point
c   nst(ivarg)       number of nested structures (maximum of 4)
c   ivarg            variogram number (set to 1 unless doing cokriging
c                       or indicator kriging)
c   MAXNST           size of variogram parameter arrays
c   c0(ivarg)        isotropic nugget constant
c   it(i)            type of each nested structure:
c                      1. spherical model of range a;
c                      2. exponential model of parameter a;
c                           i.e. practical range is 3a
c                      3. gaussian model of parameter a;
c                           i.e. practical range is a*sqrt(3)
c                      4. power model of power a (a must be gt. 0  and
c                           lt. 2).  if linear model, a=1,c=slope.
c   cc(i)            multiplicative factor of each nested structure.
c                      (sill-c0) for spherical, exponential,and gaussian
c                      slope for linear model.
c   aa(i)            parameter "a" of each nested structure.
c   irot             index of the rotation matrix for the first nested
c                    structure (the second nested structure will use
c                    irot+1, the third irot+2, and so on)
c   MAXROT           size of rotation matrix arrays
c   rotmat           rotation matrices
c
c
c OUTPUT VARIABLES:
c
c   cmax             maximum covariance
c   cova             covariance between (x1,y1,z1) and (x2,y2,z2)
c
c
c
c EXTERNAL REFERENCES: sqdist    computes anisotropic squared distance
c                      rotmat    computes rotation matrix for distance
c-----------------------------------------------------------------------
      parameter(PI=3.14159265,PMX=1.e10,EPSLON=1.e-10)
      integer   nst(*),it(*)
      real      c0(*),cc(*),aa(*)
      real*8    rotmat(MAXROT,3,3),hsqd,sqdist
c
c Calculate the maximum covariance value (used for zero distances and
c for power model covariance):
c
      istart = 1 + (ivarg-1)*MAXNST
      cmax   = c0(ivarg)
      do is=1,nst(ivarg)
            ist = istart + is - 1
            if(it(ist).eq.4) then
                  cmax = cmax + PMX
            else
                  cmax = cmax + cc(ist)
            endif
      end do
c
c Check for "zero" distance, return with cmax if so:
c
      hsqd = sqdist(x1,y1,z1,x2,y2,z2,irot,MAXROT,rotmat)
      if(real(hsqd).lt.EPSLON) then
            cova = cmax
            return
      endif
c
c Loop over all the structures:
c
      cova = 0.0
      do is=1,nst(ivarg)
            ist = istart + is - 1
c
c Compute the appropriate distance:
c
            if(ist.ne.1) then
                  ir = min((irot+is-1),MAXROT)
                  hsqd=sqdist(x1,y1,z1,x2,y2,z2,ir,MAXROT,rotmat)
            end if
            h = real(dsqrt(hsqd))
c
c Spherical Variogram Model?
c
            if(it(ist).eq.1) then
                  hr = h/aa(ist)
                  if(hr.lt.1.) cova=cova+cc(ist)*(1.-hr*(1.5-.5*hr*hr))
c
c Exponential Variogram Model?
c
            else if(it(ist).eq.2) then
                  cova = cova + cc(ist)*exp(-3.0*h/aa(ist))
c
c Gaussian Variogram Model?
c
            else if(it(ist).eq.3) then
                  cova = cova + cc(ist)*exp(-(3.0*h/aa(ist))
     +                                      *(3.0*h/aa(ist)))
c
c Power Variogram Model?
c
            else if(it(ist).eq.4) then
                  cova = cova + cmax - cc(ist)*(h**aa(ist))
c
c Hole Effect Model?
c
            else if(it(ist).eq.5) then
c                 d = 10.0 * aa(ist)
c                 cova = cova + cc(ist)*exp(-3.0*h/d)*cos(h/aa(ist)*PI)
                  cova = cova + cc(ist)*cos(h/aa(ist)*PI)
            endif
      end do
c
c Finished:
c
      return
      end

      real*8 function sqdist(x1,y1,z1,x2,y2,z2,ind,MAXROT,rotmat)
c-----------------------------------------------------------------------
c
c    Squared Anisotropic Distance Calculation Given Matrix Indicator
c    ***************************************************************
c
c This routine calculates the anisotropic distance between two points
c  given the coordinates of each point and a definition of the
c  anisotropy.
c
c
c INPUT VARIABLES:
c
c   x1,y1,z1         Coordinates of first point
c   x2,y2,z2         Coordinates of second point
c   ind              The rotation matrix to use
c   MAXROT           The maximum number of rotation matrices dimensioned
c   rotmat           The rotation matrices
c
c
c
c OUTPUT VARIABLES:
c
c   sqdist           The squared distance accounting for the anisotropy
c                      and the rotation of coordinates (if any).
c
c
c NO EXTERNAL REFERENCES
c
c
c Author: C. Deutsch                                Date: September 1989
c-----------------------------------------------------------------------
      real*8 rotmat(MAXROT,3,3),cont,dx,dy,dz
c
c Compute component distance vectors and the squared distance:
c
      dx = dble(x1 - x2)
      dy = dble(y1 - y2)
      dz = dble(z1 - z2)
      sqdist = 0.0
      do i=1,3
            cont   = rotmat(ind,i,1) * dx
     +             + rotmat(ind,i,2) * dy
     +             + rotmat(ind,i,3) * dz
            sqdist = sqdist + cont * cont
      end do
      return
      end

      subroutine setrot(ang1,ang2,ang3,anis1,anis2,ind,MAXROT,rotmat)
c-----------------------------------------------------------------------
c
c              Sets up an Anisotropic Rotation Matrix
c              **************************************
c
c Sets up the matrix to transform cartesian coordinates to coordinates
c accounting for angles and anisotropy (see manual for a detailed
c definition):
c
c
c INPUT PARAMETERS:
c
c   ang1             Azimuth angle for principal direction
c   ang2             Dip angle for principal direction
c   ang3             Third rotation angle
c   anis1            First anisotropy ratio
c   anis2            Second anisotropy ratio
c   ind              matrix indicator to initialize
c   MAXROT           maximum number of rotation matrices dimensioned
c   rotmat           rotation matrices
c
c
c NO EXTERNAL REFERENCES
c
c
c Author: C. Deutsch                                Date: September 1989
c-----------------------------------------------------------------------
      parameter(DEG2RAD=3.141592654/180.0,EPSLON=1.e-20)
      real*8    rotmat(MAXROT,3,3),afac1,afac2,sina,sinb,sint,
     +          cosa,cosb,cost
c
c Converts the input angles to three angles which make more
c  mathematical sense:
c
c         alpha   angle between the major axis of anisotropy and the
c                 E-W axis. Note: Counter clockwise is positive.
c         beta    angle between major axis and the horizontal plane.
c                 (The dip of the ellipsoid measured positive down)
c         theta   Angle of rotation of minor axis about the major axis
c                 of the ellipsoid.
c
      if(ang1.ge.0.0.and.ang1.lt.270.0) then
            alpha = (90.0   - ang1) * DEG2RAD
      else
            alpha = (450.0  - ang1) * DEG2RAD
      endif
      beta  = -1.0 * ang2 * DEG2RAD
      theta =        ang3 * DEG2RAD
c
c Get the required sines and cosines:
c
      sina  = dble(sin(alpha))
      sinb  = dble(sin(beta))
      sint  = dble(sin(theta))
      cosa  = dble(cos(alpha))
      cosb  = dble(cos(beta))
      cost  = dble(cos(theta))
c
c Construct the rotation matrix in the required memory:
c
      afac1 = 1.0 / dble(max(anis1,EPSLON))
      afac2 = 1.0 / dble(max(anis2,EPSLON))
      rotmat(ind,1,1) =       (cosb * cosa)
      rotmat(ind,1,2) =       (cosb * sina)
      rotmat(ind,1,3) =       (-sinb)
      rotmat(ind,2,1) = afac1*(-cost*sina + sint*sinb*cosa)
      rotmat(ind,2,2) = afac1*(cost*cosa + sint*sinb*sina)
      rotmat(ind,2,3) = afac1*( sint * cosb)
      rotmat(ind,3,1) = afac2*(sint*sina + cost*sinb*cosa)
      rotmat(ind,3,2) = afac2*(-sint*cosa + cost*sinb*sina)
      rotmat(ind,3,3) = afac2*(cost * cosb)
c
c Return to calling program:
c
      return
      end

      subroutine makepar
c-----------------------------------------------------------------------
c
c                      Write a Parameter File
c                      **********************
c
c
c
c-----------------------------------------------------------------------
      lun = 99
      open(lun,file='gammabar.par')
      write(lun,10)
 10   format('                  Parameters for GAMMABAR',/,
     +       '                  ***********************',/,/,
     +       'START OF PARAMETERS:')

      write(lun,11)
 11   format('160.0  160.0   50.0           ',
     +       '-X,Y,Z size of block')
      write(lun,12)
 12   format(' 21     21     21             ',
     +       '-X,Y,Z discretization')
      write(lun,13)
 13   format('2   0.0                       ',
     +       '-nst, nugget effect')
      write(lun,14)
 14   format('1 0.6    0.0     0.0   0.0    ',
     +       '-  it,cc,ang1,ang2,ang3')
      write(lun,15)
 15   format('      1000.0  3000.0  12.0    ',
     +       '-  a_hmax, a_hmin, a_vert')
      write(lun,16)
 16   format('1 0.4    0.0     0.0   0.0    ',
     +       '-  it,cc,ang1,ang2,ang3')
      write(lun,17)
 17   format('     30000.0  6000.0  50.0    ',
     +       '-  a_hmax, a_hmin, a_vert')

      close(lun)
      return
      end


C$Procedure UDDF ( First derivative of a function, df(x)/dx )

      SUBROUTINE UDDF ( UDFUNC, X, DX, DERIV )

C$ Abstract
C
C     Routine to calculate the first derivative of a caller-specified
C     scalar function using a three-point estimation.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C    None.
C
C$ Keywords
C
C    MATH
C    DERIVATIVE
C
C$ Declarations

      IMPLICIT NONE

      EXTERNAL              UDFUNC

      DOUBLE PRECISION      X
      DOUBLE PRECISION      DX
      DOUBLE PRECISION      DERIV

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     UDFUNC     I   The routine that computes the scalar value
C                    of interest.
C     X          I   Independent variable of UDFUNC.
C     DX         I   Interval from X for derivative calculation.
C     DERIV      O   Approximate derivative of UDFUNC at X.
C
C$ Detailed_Input
C
C     UDFUNC     the routine that returns the value of the scalar
C                quantity function of interest at X. The calling
C                sequence for UDFUNC is:
C
C                   CALL UDFUNC ( X, VALUE )
C
C                where:
C
C                   X       the double precision value of the
C                           independent variable of the function
C                           at which to determine the scalar value.
C
C                   VALUE   the double precision value returned by
C                           UDFUNC at X.
C
C                Functionally:
C
C                   VALUE = UDFUNC ( X )
C
C     X          a scalar double precision value at which to determine
C                the derivative of UDFUNC.
C
C                For many SPICE uses, X will represent ephemeris time,
C                expressed as seconds past J2000 TDB.
C
C     DX         a scalar double precision value representing half the
C                interval in units of X separating the evaluation
C                values of UDFUNC; the evaluations occur at (X + DX)
C                and (X - DX).
C
C                DX may be negative but must be non-zero.
C
C$ Detailed_Output
C
C     DERIV      the scalar double precision approximate value of the
C                first derivative of UDFUNC with respect to X.
C
C                Functionally:
C
C                            d UDFUNC ( x ) |
C                   DERIV =  --             |
C                            dx             |
C                                            X
C
C$ Parameters
C
C    None.
C
C$ Exceptions
C
C     1) A routine in the call tree of this routine signals
C        SPICE(DIVIDEBYZERO) if DX has a value of zero.
C
C$ Files
C
C     If the evaluation of UDFUNC requires SPICE kernel data, the
C     appropriate kernels must be loaded before calling this routine.
C
C        - SPK data: the calling application must load ephemeris data
C          for the targets, observer, and any intermediate objects in
C          a chain connecting the targets and observer for the time
C          used in the evaluation. If aberration corrections are
C          used, the states of target and observer relative to the
C          solar system barycenter must be calculable from the
C          available ephemeris data.
C
C        - If non-inertial reference frames are used, then PCK
C          files, frame kernels, C-kernels, and SCLK kernels may be
C          needed.
C
C     Such kernel data are normally loaded once per program run, NOT
C     every time this routine is called.
C
C$ Particulars
C
C     This routine provides a simple interface to numerically calculate
C     the first derivative of a scalar quantity function, UDFUNC.
C     UDFUNC is expected to be "well behaved" across at the evaluation
C     interval [ X - DX, X + DX ]. This means a linear approximation to
C     the function over the interval is sufficiently accurate to
C     calculate the approximate derivative at X.
C
C     The routine QDERIV performs the differentiation using a three
C     point estimation.
C
C$ Examples
C
C     The numerical results shown for these examples may differ across
C     platforms. The results depend on the SPICE kernels used as
C     input, the compiler and supporting libraries, and the machine
C     specific arithmetic implementation.
C
C     Calculate the time derivative of the light time corresponding to
C     the apparent position of Mercury relative to the Moon at
C     time "JAN 1 2009."
C
C           PROGRAM UDDF_T
C
C           EXTERNAL                 UDFUNC
C
C           DOUBLE PRECISION         ET
C           DOUBLE PRECISION         DT
C           DOUBLE PRECISION         DERIV
C
C     C
C     C     Load leapsecond and SPK kernels. The name of the
C     C     meta kernel file shown here is fictitious; you
C     C     must supply the name of a file available
C     C     on your own computer system.
C     C
C           CALL FURNSH ( 'standard.tm' )
C
C     C
C     C     Use a shift of one second off the epoch of interest.
C     C
C           DT = 1.D0
C
C     C
C     C     Convert the epoch date string to ephemeris seconds.
C     C
C           CALL STR2ET ( 'JAN 1 2009', ET )
C
C     C
C     C     Calculate the derivative of UDFUNC at ET.
C     C
C           CALL UDDF ( UDFUNC, ET, DT, DERIV )
C
C     C
C     C     Output the calculated derivative.
C     C
C           WRITE(*,*) DERIV
C
C           END
C
C     C
C     C     A scalar quantity function that returns the light-time
C     C     between the Moon and Mercury at ET.
C     C
C           SUBROUTINE UDFUNC ( ET, VALUE )
C
C           IMPLICIT NONE
C
C           DOUBLE PRECISION         ET
C           DOUBLE PRECISION         VALUE
C
C           DOUBLE PRECISION         POS  (3)
C           DOUBLE PRECISION         LT
C
C     C
C     C     Evaluate the apparent position of Mercury with respect
C     C     to the Moon at ET.
C     C
C           CALL SPKPOS ( 'MERCURY', ET, 'J2000', 'LT+S', 'MOON',
C          .               POS,   LT )
C
C     C
C     C     Return the light-time value as the scalar quantity.
C     C
C           VALUE = LT
C
C           END
C
C     The program outputs (OS X Intel run):
C
C         -0.00013567094
C
C$ Restrictions
C
C    The function UDFUNC must exist everywhere within [X - DX, X + DX].
C
C$ Literature_References
C
C    See QDERIV header
C
C$ Author_and_Institution
C
C    N.J. Bachman   (JPL)
C    E.D. Wright    (JPL)
C
C$ Version
C
C-   SPICELIB Version 1.0.0  31-MAR-2010 (EDW)
C
C-&

C$ Index_Entries
C
C    first derivative of a user-defined scalar function
C
C-&

C
C     SPICELIB functions
C
      LOGICAL               RETURN
      LOGICAL               FAILED

C
C     Local Variables
C
      INTEGER               N
      DOUBLE PRECISION      DFDX  (1)
      DOUBLE PRECISION      UDVAL (2)

      IF ( RETURN () ) THEN
         RETURN
      END IF

      CALL CHKIN  ( 'UDDF' )

C
C     Apply a three-point estimation of the derivative for
C     UDFUNC at X by evaluating UDFUNC at [X-DX, X+DX].
C
C     The QDERIV call returns a single value in the DFDX array.
C
      N  = 1

C
C     Evaluate the scalar function at the interval boundaries.
C     Check for a FAILED event.
C
      CALL UDFUNC ( X-DX, UDVAL(1) )

      IF (  FAILED() ) THEN
         CALL CHKOUT ( 'UDDF' )
         RETURN
      END IF

      CALL UDFUNC ( X+DX, UDVAL(2) )

      IF (  FAILED() ) THEN
         CALL CHKOUT ( 'UDDF' )
         RETURN
      END IF

C
C     Estimate the derivative at X.
C
      CALL QDERIV ( N, UDVAL(1), UDVAL(2), DX, DFDX )

      IF (  FAILED() ) THEN
         CALL CHKOUT ( 'UDDF' )
         RETURN
      END IF

      DERIV = DFDX(1)

      CALL CHKOUT ( 'UDDF' )
      RETURN

      END


res = SUM(x) / REAL(SIZE(x),prec)


!> K.A.Johannessen et al./Compt. Methods Appl. Mech. Engrg 269 (2014) 471-514 の diagonal structured refinement を再現
program main
  use module_globals
  use module_meshline
  use module_LRspline
  implicit none

  !> global tensor knot
  real(8),allocatable :: tn1(:), tn2(:)

  !> LR-spline
  type(LocallyRefinedSpline),pointer :: lrs_

  !> Mesh Line
  type(MeshLine),pointer :: ml_

  ! local variables
  type(LocallyRefinedSpline),pointer :: lrs
  type(MeshLine),pointer :: ml
  integer :: i, j, k
  real(8) :: pos
  logical :: diag

  ! initialise
  call init_LRspline(lrs_)
  call init_meshline(ml_)

  ! tensor mesh = Bspline の global knotを設定
  ncp(1)=5
  ncp(2)=5
  ncp(0)=(ncp(1)+1)*(ncp(2)+1)
  allocate(tn1(0:ncp(1)+ndeg(1)+1))
  allocate(tn2(0:ncp(2)+ndeg(2)+1))
  tn1=[0.d0,0.d0,0.d0,1.d0,2.d0,3.d0,4.d0,4.d0,4.d0]/4.d0
  tn2=[0.d0,0.d0,0.d0,1.d0,2.d0,3.d0,4.d0,4.d0,4.d0]/4.d0

  ! tensor mesh の local knot を設定 = Bspline の knot
  call set_Bspline(lrs_,tn1,tn2)

  ! 適当なsurfaceを設定
  call set_Greville(lrs_)
  lrs=>lrs_
  do while(associated(lrs))
     lrs%cp(3)=cos(2.d0*pi*lrs%cp(1))*sin(2.d0*pi*lrs%cp(2))
     lrs=>lrs%next
  end do

  ! draw itinial Bspline
  call draw_LRmesh(lrs_,"LRmesh.res")
  call draw_controlpoints(lrs_,"controlpoints.res")
  call draw_surface(lrs_,50,"surface.res")

  do j=1,7 !level 8 まで
     ! set meshlines
     lrs=>lrs_
     do while(associated(lrs))
        diag=true
        do i=0,ndeg(1)+1
           diag=diag.and.(abs(lrs%tn1(i)-lrs%tn2(i)).le.tiny)
        end do
        if(diag) then
           do k=0,ndeg(1)
              pos=(lrs%tn1(k)+lrs%tn1(k+1))*0.5d0
              if((pos.gt.tiny).and.(pos.lt.1.d0-tiny))then
                 call append_meshline(ml_,2,pos,lrs%tn2(0),lrs%tn2(ndeg(2)+1))
              end if
           end do
           do k=0,ndeg(2)
              pos=(lrs%tn2(k)+lrs%tn2(k+1))*0.5d0
              if((pos.gt.tiny).and.(pos.lt.1.d0-tiny))then
                 call append_meshline(ml_,1,pos,lrs%tn1(0),lrs%tn1(ndeg(1)+1))
              end if
           end do
        end if
        lrs=>lrs%next
     end do

     ! draw meshline
     call draw_meshline(ml_,"meshline.res")

     ! refine LR-splines
     ml=>ml_
     do while(associated(ml))
        call refine_LRspline(lrs_,ml_,ml)
        ml=>ml%next
     end do
  end do

  ! draw refined LR-spline
  call draw_LRmesh(lrs_,"LRmesh2.res")
  call draw_controlpoints(lrs_,"controlpoints2.res")
  call draw_surface(lrs_,50,"surface2.res")

  ! finalise
  deallocate(tn1,tn2)
  call uninit_meshline(ml_)
  call uninit_LRspline(lrs_)

end program main


SUBROUTINE add_neighbor_features(pred_last, neigh_voro, probs, features_new, n_atoms, n_neighs, n_probs, n_features)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! Important arrays:
! pred_last:    (n_atoms) array,
!               y_pred values from last layer,
!               previouslly cannot be (n_atoms) as atom ids are not continuous
!               currently should fill the lost values as 0
!
! neigh_voro:   (n_atoms, n_neighs) array,
!               neighbor_list by Voronoi,
!               should deal with the lost atoms due to removing boundaries
!               currently should fill the lost values as 0
!
! probs:        (n_probs) array/list,
!               probs used as prob_thresholds
!
! features_new: (n_atoms, n_features) array,
!               new features returned
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

integer :: n_atoms, n_neighs, n_probs, n_features
integer :: atom, i, j, m, neighs
REAL(8) :: pred_neigh, mean_nn, sum_nn, std_nn, min_nn, max_nn
REAL(8), dimension(n_atoms):: pred_last
integer, dimension(n_atoms, n_neighs):: neigh_voro
REAL(4), dimension(n_probs):: probs
REAL(8), dimension(n_atoms, n_features):: features_new

!f2py intent(in) pred_last, neigh_voro, n_atoms, n_neighs
!f2py intent(in) probs, n_probs
!f2py intent(in) n_features
!f2py intent(in,out) features_new

do atom = 1, n_atoms
    if (neigh_voro(atom, 1) /= 0) then
         mean_nn = 0
         sum_nn = 0
         std_nn = 0
         min_nn = pred_last(neigh_voro(atom, 1))
         max_nn = min_nn
         i = 1
         do while ((i <= n_neighs).and.(neigh_voro(atom, i) /= 0))
             pred_neigh = pred_last(neigh_voro(atom, i))
             sum_nn = sum_nn + pred_neigh
             if (pred_neigh > max_nn) then
               max_nn = pred_neigh
             end if
             if (pred_neigh < min_nn) then
               min_nn = pred_neigh
             end if
             do m = 1, n_probs
                  if (pred_neigh >= probs(m)) then
                  features_new(atom, 5 + m) = features_new(atom, 5 + m) + 1
                  end if
             end do
             i = i + 1
         end do
         neighs = i - 1
         mean_nn = sum_nn / neighs
         features_new(atom, 1) = mean_nn
         features_new(atom, 3) = min_nn
         features_new(atom, 4) = max_nn
         features_new(atom, 5) = sum_nn
         do j = 1, neighs
             std_nn = std_nn + (pred_last(neigh_voro(atom, j)) - mean_nn)**2
         end do
         std_nn = sqrt(std_nn / neighs)
         features_new(atom, 2) = std_nn
    end if
end do
return
END SUBROUTINE


      SUBROUTINE SG03BY( XR, XI, YR, YI, CR, CI, SR, SI, Z )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the parameters for the complex Givens rotation
C
C        (  CR-CI*I   SR-SI*I )   ( XR+XI*I )   ( Z )
C        (                    ) * (         ) = (   ),
C        ( -SR-SI*I   CR+CI*I )   ( YR+YI*I )   ( 0 )
C
C     where CR, CI, SR, SI, XR, XI, YR, YI are real numbers and I is the
C     imaginary unit, I = SQRT(-1). Z is a non-negative real number.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     XR, XI, (input) DOUBLE PRECISION
C     YR, YI  (input) DOUBLE PRECISION
C             The given real scalars XR, XI, YR, YI.
C
C     CR, CI, (output) DOUBLE PRECISION
C     SR, SI, (output) DOUBLE PRECISION
C     Z       (output) DOUBLE PRECISION
C             The computed real scalars CR, CI, SR, SI, Z, defining the
C             complex Givens rotation and Z.
C
C     NUMERICAL ASPECTS
C
C     The subroutine avoids unnecessary overflow.
C
C     FURTHER COMMENTS
C
C     In the interest of speed, this routine does not check the input
C     for errors.
C
C     CONTRIBUTOR
C
C     T. Penzl, Technical University Chemnitz, Germany, Aug. 1998.
C
C     REVISIONS
C
C     Sep. 1998 (V. Sima).
C
C     ******************************************************************
C
C      .. Parameters ..
       DOUBLE PRECISION  ONE, ZERO
       PARAMETER         ( ONE = 1.0D+0, ZERO = 0.0D+0 )
C      .. Scalar Arguments ..
       DOUBLE PRECISION  CI, CR, SI, SR, XI, XR, YI, YR, Z
C      .. Intrinsic Functions ..
       DOUBLE PRECISION  ABS, MAX, SQRT
C      .. Executable Statements ..
C
       Z = MAX( ABS( XR ), ABS( XI ), ABS( YR ), ABS( YI ) )
C
       IF ( Z .EQ. ZERO ) THEN
          CR = ONE
          CI = ZERO
          SR = ZERO
          SI = ZERO
       ELSE
          Z = Z*SQRT( ( XR/Z )**2 + ( XI/Z )**2 +
     $                ( YR/Z )**2 + ( YI/Z )**2 )
          CR = XR/Z
          CI = XI/Z
          SR = YR/Z
          SI = YI/Z
       END IF
C
       RETURN
C
C *** Last line of SG03BY ***
       END


      PROGRAM EHYPOT
      USE, INTRINSIC :: IEEE_ARITHMETIC, ONLY: IEEE_FMA, IEEE_MAX_NUM
      IMPLICIT NONE
      REAL ZERO, ONE
      PARAMETER (ZERO = 0.0, ONE = 1.0)
      COMPLEX C
      REAL CR, CI, A, H
      INTRINSIC ABS, HYPOT, MAX, MIN, SQRT
      WRITE (*,'(A)',ADVANCE='NO') 'C%RE='
      READ (*,*) CR
      WRITE (*,'(A)',ADVANCE='NO') 'C%IM='
      READ (*,*) CI
      C = CMPLX(CR, CI)
      A = ABS(C)
      WRITE (*,1) '          ABS(C)=', A
      H = HYPOT(CR, CI)
      WRITE (*,1) 'HYPOT(C%RE,C%IM)=', H
      CR = ABS(CR)
      CI = ABS(CI)
      A = MAX(CR, CI)
      H = MIN(CR, CI)
      H = IEEE_MAX_NUM(H / A, ZERO)
      H = SQRT(IEEE_FMA(H, H, ONE)) * A
      WRITE (*,1) 'NAIVE(C%RE,C%IM)=', H
 1    FORMAT(A,ES15.9)
      END


module ode1d_other

! General utilities for solving 1D ODEs, not used directly by dftatom. They are
! available here for reuse.

use types, only: dp
use constants, only: pi
use utils, only: stop_error
use ode1d, only: get_midpoints

implicit none
private
public rk4_integrate_inward2, rk4_integrate_inward, rk4_integrate_inward4, &
        adams_extrapolation_inward, adams_interp_inward, &
        integrate_simpson_direct, eulerstep, rk4step, rk4step2, &
        adams_interp_inward_8, adams_interp_outward_8, &
        adams_extrapolation_inward_8, adams_extrapolation_outward_8, &
        adams_extrapolation_inward_6, adams_interp_outward_6, &
        adams_interp_inward_6, adams_extrapolation_outward_6

contains

subroutine rk4_integrate_inward2(R, y0, C1, C1mid, max_val, y1, y2, imin)
! Integrates the following set of equations inwards:
! dy1/dx =      y2
! dy2/dx = C1 * y1
real(dp), intent(in) :: R(:) ! Grid
real(dp), intent(in) :: y0(:) ! Initial condition
! Coefficients C1 at grid points and midpoints:
real(dp), intent(in) :: C1(:), C1mid(:)
! Maximum value (if y1 > max_val, the integration stops)
real(dp), intent(in) :: max_val
! Solution y1 and y2
real(dp), intent(out) :: y1(:), y2(:)
! The integration stops at R(imax)
integer, intent(out) :: imin

integer :: i
integer :: n
real(dp), dimension(size(y0)) :: dym, dyt, yt, dydx, y
real(dp) :: h

n = size(R)
y = y0
y1(n) = y(1)
y2(n) = y(2)
do i = n-1, 1, -1
    ! rk4 step size
    h = R(i)-R(i+1)

    ! Do rk4 step:
    dydx(1) =           y(2)
    dydx(2) = C1(i+1) * y(1)
    yt = y + h/2 * dydx
    dyt(1) =            yt(2)
    dyt(2) = C1mid(i) * yt(1)
    yt = y + h/2 * dyt
    dym(1) =            yt(2)
    dym(2) = C1mid(i) * yt(1)
    yt = y + h * dym
    dym = dyt + dym
    dyt(1) =         yt(2)
    dyt(2) = C1(i) * yt(1)
    y = y + h/6 * (dydx + dyt + 2*dym)

    y1(i) = y(1)
    y2(i) = y(2)
    if (abs(y(1)) >= max_val) then
        imin = i
        return
    end if
end do
imin = 1
end subroutine

real(dp) function adams_extrapolation_outward_6(y, i) result(r)
! Adams extrapolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = +(4277 * y(i  ) &
    -7923  * y(i-1) &
    +9982  * y(i-2) &
    -7298  * y(i-3) &
    +2877  * y(i-4) &
    -475   * y(i-5) &
    ) / 1440
end function

real(dp) function adams_extrapolation_inward_6(y, i) result(r)
! Adams extrapolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(4277 * y(i  ) &
    -7923  * y(i+1) &
    +9982  * y(i+2) &
    -7298  * y(i+3) &
    +2877  * y(i+4) &
    -475   * y(i+5) &
    ) / 1440
end function

real(dp) function adams_interp_outward_6(y, i) result(r)
! Adams interpolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = +(475 * y(i+1) &
    +1427 * y(i  ) &
    -798  * y(i-1) &
    +482  * y(i-2) &
    -173  * y(i-3) &
    +27   * y(i-4) &
    ) / 1440
end function

real(dp) function adams_interp_inward_6(y, i) result(r)
! Adams interpolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(475 * y(i-1) &
    +1427 * y(i  ) &
    -798  * y(i+1) &
    +482  * y(i+2) &
    -173  * y(i+3) &
    +27   * y(i+4) &
    ) / 1440
end function

real(dp) function adams_extrapolation_outward_8(y, i) result(r)
! Adams extrapolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = +(434241  * y(i  ) &
    -1152169  * y(i-1) &
    +2183877  * y(i-2) &
    -2664477  * y(i-3) &
    +2102243  * y(i-4) &
    -1041723  * y(i-5) &
    +295767   * y(i-6) &
    -36799    * y(i-7) &
    ) / 120960
end function

real(dp) function adams_extrapolation_inward_8(y, i) result(r)
! Adams extrapolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(434241  * y(i  ) &
    -1152169  * y(i+1) &
    +2183877  * y(i+2) &
    -2664477  * y(i+3) &
    +2102243  * y(i+4) &
    -1041723  * y(i+5) &
    +295767   * y(i+6) &
    -36799    * y(i+7) &
    ) / 120960
end function

real(dp) function adams_interp_outward_8(y, i) result(r)
! Adams interpolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = +(36799 * y(i+1) &
    +139849 * y(i  ) &
    -121797 * y(i-1) &
    +123133 * y(i-2) &
    -88547  * y(i-3) &
    +41499  * y(i-4) &
    -11351  * y(i-5) &
    +1375   * y(i-6) &
    ) / 120960
end function

real(dp) function adams_interp_inward_8(y, i) result(r)
! Adams interpolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(36799 * y(i-1) &
    +139849 * y(i  ) &
    -121797 * y(i+1) &
    +123133 * y(i+2) &
    -88547  * y(i+3) &
    +41499  * y(i+4) &
    -11351  * y(i+5) &
    +1375   * y(i+6) &
    ) / 120960
end function

real(dp) function adams_interp_inward(y, i) result(r)
! Adams interpolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(9*y(i-1) + 19*y(i) - 5*y(i+1) + y(i+2)) / 24
end function

real(dp) function adams_extrapolation_inward(y, i) result(r)
! Adams extrapolation formula
real(dp), intent(in) :: y(:)
integer, intent(in) :: i
r = -(55*y(i) - 59*y(i+1) + 37*y(i+2) - 9*y(i+3)) / 24
end function

subroutine rk4step(y, x, h, derivs)
!advances the solution of
!y_i'(x)=f_i(x,y_1,y_2,...,y_N)       for i=1,2,...,N; f_i are known
!from x to x+h.
!parameters:
!  y ... y(x) on the input and y(x+h) on the output.
!  derivs(x,y,dydx) .... returns a vector of f_i(x,y1,y2,...)
integer, parameter :: N=2
real(dp), intent(in) :: h, x
real(dp), intent(inout) :: y(N)
interface
    subroutine derivs(x, y, dydx)
        use types
        integer, parameter :: N=2
        real(dp), intent(in) :: x, y(N)
        real(dp), intent(out) :: dydx(N)
    end subroutine
end interface
real(dp) :: dym(N), dyt(N), yt(N), dydx(N)
call derivs(x, y, dydx)
yt = y + h/2 * dydx
call derivs(x+h/2, yt, dyt)
yt = y + h/2 * dyt
call derivs(x+h/2, yt, dym)
yt = y + h * dym
dym = dyt + dym
call derivs(x+h, yt, dyt)
y = y + h/6 * (dydx + dyt + 2*dym)
end subroutine

subroutine rk4step2(y, x, h, derivs)
!advances the solution of
!y_i'(x)=f_i(x,y_1,y_2,...,y_N)       for i=1,2,...,N; f_i are known
!from x to x+h.
!parameters:
!  y ... y(x) on the input and y(x+h) on the output.
!  derivs(x,y,dydx) .... returns a vector of f_i(x,y1,y2,...)
integer, parameter :: N=2
real(dp), intent(in) :: h, x
real(dp), intent(inout) :: y(N)
interface
    subroutine derivs(x, y, x_index, dydx)
        use types
        integer, parameter :: N=2
        integer, intent(in) :: x_index ! 1, 2, 3
        real(dp), intent(in) :: x, y(N)
        real(dp), intent(out) :: dydx(N)
    end subroutine
end interface
real(dp) :: dym(N), dyt(N), yt(N), dydx(N)
call derivs(x, y, 1, dydx)
yt = y + h/2 * dydx
call derivs(x+h/2, yt, 2, dyt)
yt = y + h/2 * dyt
call derivs(x+h/2, yt, 2, dym)
yt = y + h * dym
dym = dyt + dym
call derivs(x+h, yt, 3, dyt)
y = y + h/6 * (dydx + dyt + 2*dym)
end subroutine

subroutine eulerstep(y, x, h, derivs)
integer, parameter :: N=2
real(dp), intent(in) :: h, x
real(dp), intent(inout) :: y(N)
interface
    subroutine derivs(x, y, dydx)
        use types
        integer, parameter :: N=2
        real(dp), intent(in) :: x, y(N)
        real(dp), intent(out) :: dydx(N)
    end subroutine
end interface
real(dp) :: dydx(N)
call derivs(x, y, dydx)
y = y + h * dydx
end subroutine

real(dp) function integrate_simpson_direct(x, f) result(s)
! Integrates the function f(x) using Simpson rule
!
! Returns: the integral
real(dp), intent(in), dimension(:) :: x ! the grid
real(dp), intent(in), dimension(:) :: f ! function defined on the grid x

real(dp) :: dx, f_a, f_mid(size(x)-1), f_b
integer :: i
s = 0
f_mid = get_midpoints(x, f)
do i = 1, size(x)-1
    dx = x(i+1) - x(i)
    f_a = f(i)
    f_b = f(i+1)
    s = s + dx/6 * (f_a + 4*f_mid(i) + f_b)
end do
end function

subroutine rk4_integrate_inward(R, y0, C1, C2, C1mid, C2mid, max_val, &
    y1, y2, imin)
! Integrates the following set of equations inwards:
! dy1/dx =                y2
! dy2/dx = C1 * y1 + C2 * y2
real(dp), intent(in) :: R(:) ! Grid
real(dp), intent(in) :: y0(:) ! Initial condition
! Coefficients C1 and C2 at grid points and midpoints:
real(dp), intent(in) :: C1(:), C2(:), C1mid(:), C2mid(:)
! Maximum value (if y1 > max_val, the integration stops)
real(dp), intent(in) :: max_val
! Solution y1 and y2
real(dp), intent(out) :: y1(:), y2(:)
! The integration stops at R(imax)
integer, intent(out) :: imin

integer :: i
integer :: n
real(dp), dimension(size(y0)) :: dym, dyt, yt, dydx, y
real(dp) :: h

n = size(R)
y = y0
y1(n) = y(1)
y2(n) = y(2)
do i = n-1, 1, -1
    ! rk4 step size
    h = R(i)-R(i+1)

    ! Do rk4 step:
    dydx(1) =                            y(2)
    dydx(2) = C1(i+1) * y(1) + C2(i+1) * y(2)
    yt = y + h/2 * dydx
    dyt(1) =                               yt(2)
    dyt(2) = C1mid(i) * yt(1) + C2mid(i) * yt(2)
    yt = y + h/2 * dyt
    dym(1) =                               yt(2)
    dym(2) = C1mid(i) * yt(1) + C2mid(i) * yt(2)
    yt = y + h * dym
    dym = dyt + dym
    dyt(1) =                         yt(2)
    dyt(2) = C1(i) * yt(1) + C2(i) * yt(2)
    y = y + h/6 * (dydx + dyt + 2*dym)

    y1(i) = y(1)
    y2(i) = y(2)
    if (abs(y(1)) >= max_val) then
        imin = i
        return
    end if
end do
imin = 1
end subroutine

subroutine rk4_integrate_inward4(R, y0, C, Cmid, max_val, y1, y2, imin)
! Integrates the following set of equations inwards:
! dy1/dx = C(:, 1, 1) * y1 + C(:, 1, 2) * y2
! dy2/dx = C(:, 2, 1) * y1 + C(:, 2, 2) * y2
real(dp), intent(in) :: R(:) ! Grid
real(dp), intent(in) :: y0(:) ! Initial condition
! Coefficients C1 and C2 at grid points and midpoints:
real(dp), intent(in) :: C(:, :, :), Cmid(:, :, :)
! Maximum value (if y1 > max_val, the integration stops)
real(dp), intent(in) :: max_val
! Solution y1 and y2
real(dp), intent(out) :: y1(:), y2(:)
! The integration stops at R(imax)
integer, intent(out) :: imin

integer :: i
integer :: n
real(dp), dimension(size(y0)) :: dym, dyt, yt, dydx, y
real(dp) :: h

n = size(R)
y = y0
y1(n) = y(1)
y2(n) = y(2)
do i = n-1, 1, -1
    ! rk4 step size
    h = R(i)-R(i+1)

    ! Do rk4 step:
    dydx(1) = C(i+1, 1, 1) * y(1) + C(i+1, 1, 2) * y(2)
    dydx(2) = C(i+1, 2, 1) * y(1) + C(i+1, 2, 2) * y(2)
    !dydx = matmul(C(i-1, :, :), y)
    yt = y + h/2 * dydx
    dyt(1) = Cmid(i, 1, 1) * yt(1) + Cmid(i, 1, 2) * yt(2)
    dyt(2) = Cmid(i, 2, 1) * yt(1) + Cmid(i, 2, 2) * yt(2)
    !dyt = matmul(Cmid(i-1, :, :), yt)
    yt = y + h/2 * dyt
    dym(1) = Cmid(i, 1, 1) * yt(1) + Cmid(i, 1, 2) * yt(2)
    dym(2) = Cmid(i, 2, 1) * yt(1) + Cmid(i, 2, 2) * yt(2)
    !dym = matmul(Cmid(i-1, :, :), yt)
    yt = y + h * dym
    dym = dyt + dym
    dyt(1) = C(i, 1, 1) * yt(1) + C(i, 1, 2) * yt(2)
    dyt(2) = C(i, 2, 1) * yt(1) + C(i, 2, 2) * yt(2)
    !dyt = matmul(C(i, :, :), yt)
    y = y + h/6 * (dydx + dyt + 2*dym)

    y1(i) = y(1)
    y2(i) = y(2)
    if (abs(y(1)) >= max_val .or. abs(y(2)) >= max_val) then
        imin = i
        return
    end if
end do
imin = 1
end subroutine

end module


module ray

use parameters
use constants
use IC
use numerical_methods
use IO

implicit none

private calculate_ds

public run

contains

subroutine run(alpha,beta,nu,ds,plot)
!Arguments
real(kind=dp),intent(in) :: alpha,beta,nu
real(kind=dp),intent(out) :: ds
integer(kind=4), intent(in) :: plot !plot overrride. if = 1, always plot
!Other
real(kind=dp), dimension(6) :: x
real(kind=dp), dimension(4) :: c
integer(kind=dp) :: counts
real(kind=dp), dimension(Nrows, Ncols) :: output !Array to save to. Column major
integer(kind=dp) :: code

!Set the intial conditions
call initial_conditions(alpha,beta,nu,x,c)

code = 0
counts = 1
output(:,counts) = x

!Integrate numerically
do while (x(1) .GT. Rhor)

    call RKF(x,c)

    if (counts .GT. Ncols) then
    print *, 'Error: counts > Ncols', counts, Ncols
    stop
    endif

    counts = counts + 1
    output(:,counts) = x

    !Exit condition. Intersection search
    if (c(3) .LT. 0.0_dp) then
    call calculate_ds(x,ds)
    code = 2

    exit
    endif

    !Exit condition. !General ray tracing
    if (x(1) .GT. r_obs .and. x(5) .GT. 0.0_dp) then
    code = 1
    Exit
    endif

enddo

!I/O
if (plot .ne. 0) then
call ToFile(output,counts,alpha,beta,nu,c,plot)
endif

end subroutine run

subroutine calculate_ds(v,ds)

real(kind=dp), dimension(6) :: v !input vector
real(kind=dp) :: ds,x,y,z,r,theta,phi,m

!Load data
r = v(1) ; theta = v(2) ; phi = v(3) !+ 2.0_dp * PI
m = sqrt(r**2 + a2)

!Convert to cartesian
x = m*sin(theta)*cos(phi)
y = m*sin(theta)*sin(phi)
z = r*cos(theta)

!Calcuale the square of the difference
ds = (x - xTarget)**2 + (y - yTarget)**2 + (z-zTarget)**2

!ds = (r - rTarget)**2 + (cos(theta) - cos(thetaTarget))**2 + (sin(phi)-sin(phiTarget))**2

end subroutine calculate_ds

subroutine calculate_ds_cartesian(v,ds)

real(kind=dp), dimension(6) :: v !input vector
real(kind=dp) :: ds,x,y,z,r,theta,phi,m

!Load data
r = v(1) ; theta = v(2) ; phi = v(3) !+ 2.0_dp * PI
m = sqrt(r**2 + a2)

!Convert to cartesian
x = m*sin(theta)*cos(phi)
y = m*sin(theta)*sin(phi)
z = r*cos(theta)

!Calcuale the square of the difference
ds = (x - xTarget)**2 + (y - yTarget)**2 + (z-zTarget)**2

end subroutine calculate_ds_cartesian

end module ray


 MODULE GDSWZDCD_MOD
!$$$  MODULE DOCUMENTATION BLOCK
!
! $Revision: 71314 $
!
! MODULE:  GDSWZDCD_MOD  GDS WIZARD MODULE FOR ROTATED EQUIDISTANT
!                        CYLINDRICAL GRIDS (NON "E" STAGGER).
!   PRGMMR: GAYNO     ORG: W/NMC23       DATE: 2015-01-21
!
! ABSTRACT: - CONVERT FROM EARTH TO GRID COORDINATES OR VICE VERSA.
!           - COMPUTE VECTOR ROTATION SINES AND COSINES.
!           - COMPUTE MAP JACOBIANS.
!           - COMPUTE GRID BOX AREA.
!
! PROGRAM HISTORY LOG:
!   2015-01-21  GAYNO   INITIAL VERSION FROM A MERGER OF
!                       ROUTINES GDSWIZCD AND GDSWZDCD.
!
! USAGE:  "USE GDSWZDCD_MOD"  THEN CALL THE PUBLIC DRIVER
!         ROUTINE "GDSWZDCD".
!
! ATTRIBUTES:
!   LANGUAGE: FORTRAN 90
!
!$$$
!
 IMPLICIT NONE

 PRIVATE

 PUBLIC                                 :: GDSWZDCD

 INTEGER,                 PARAMETER     :: KD=SELECTED_REAL_KIND(15,45)

 REAL(KIND=KD),           PARAMETER     :: RERTH=6.3712E6_KD
 REAL(KIND=KD),           PARAMETER     :: PI=3.14159265358979_KD
 REAL(KIND=KD),           PARAMETER     :: DPR=180._KD/PI

 INTEGER                                :: IROT

 REAL(KIND=KD)                          :: CLAT, CLON
 REAL(KIND=KD)                          :: CLAT0, CLATR, DLATS, DLONS
 REAL(KIND=KD)                          :: RLON0, SLAT, SLAT0, SLATR

 CONTAINS

 SUBROUTINE GDSWZDCD(KGDS,IOPT,NPTS,FILL,XPTS,YPTS,RLON,RLAT,NRET, &
                     CROT,SROT,XLON,XLAT,YLON,YLAT,AREA)
!$$$  SUBPROGRAM DOCUMENTATION BLOCK
!
! SUBPROGRAM:  GDSWZDCD   GDS WIZARD FOR ROTATED EQUIDISTANT CYLINDRICAL
!   PRGMMR: GAYNO       ORG: W/NMC23       DATE: 2007-NOV-15
!
! ABSTRACT: THIS SUBPROGRAM DECODES THE GRIB GRID DESCRIPTION SECTION
!           (PASSED IN INTEGER FORM AS DECODED BY SUBPROGRAM W3FI63)
!           AND RETURNS ONE OF THE FOLLOWING:
!             (IOPT=+1) EARTH COORDINATES OF SELECTED GRID COORDINATES
!             (IOPT=-1) GRID COORDINATES OF SELECTED EARTH COORDINATES
!           FOR NON-"E" STAGGERED ROTATED EQUIDISTANT CYLINDRICAL PROJECTIONS.
!           (MASS OR VELOCITY POINTS.)
!           IF THE SELECTED COORDINATES ARE MORE THAN ONE GRIDPOINT
!           BEYOND THE THE EDGES OF THE GRID DOMAIN, THEN THE RELEVANT
!           OUTPUT ELEMENTS ARE SET TO FILL VALUES.
!           THE ACTUAL NUMBER OF VALID POINTS COMPUTED IS RETURNED TOO.
!           OPTIONALLY, THE VECTOR ROTATIONS, THE MAP JACOBIANS AND
!           THE GRID BOX AREAS MAY BE RETURNED AS WELL.  TO COMPUTE
!           THE VECTOR ROTATIONS, THE OPTIONAL ARGUMENTS 'SROT' AND 'CROT'
!           MUST BE PRESENT.  TO COMPUTE THE MAP JACOBIANS, THE
!           OPTIONAL ARGUMENTS 'XLON', 'XLAT', 'YLON', 'YLAT' MUST BE PRESENT.
!           TO COMPUTE THE GRID BOX AREAS, THE OPTIONAL ARGUMENT
!           'AREA' MUST BE PRESENT.
!
! PROGRAM HISTORY LOG:
! 2010-JAN-15  GAYNO     BASED ON ROUTINES GDSWZDCB AND GDSWZDCA
! 2015-JAN-21  GAYNO     MERGER OF GDSWIZCD AND GDSWZDCD.  MAKE
!                        CROT,SORT,XLON,XLAT,YLON,YLAT AND AREA
!                        OPTIONAL ARGUMENTS.  MAKE PART OF A MODULE.
!                        MOVE VECTOR ROTATION, MAP JACOBIAN AND GRID
!                        BOX AREA COMPUTATIONS TO SEPARATE SUBROUTINES.
!
! USAGE:    CALL GDSWZDCD(KGDS,IOPT,NPTS,FILL,XPTS,YPTS,RLON,RLAT,NRET,
!     &                   CROT,SROT,XLON,XLAT,YLON,YLAT,AREA)
!
!   INPUT ARGUMENT LIST:
!     KGDS     - INTEGER (200) GDS PARAMETERS AS DECODED BY W3FI63
!     IOPT     - INTEGER OPTION FLAG
!                (+1 TO COMPUTE EARTH COORDS OF SELECTED GRID COORDS)
!                (-1 TO COMPUTE GRID COORDS OF SELECTED EARTH COORDS)
!     NPTS     - INTEGER MAXIMUM NUMBER OF COORDINATES
!     FILL     - REAL FILL VALUE TO SET INVALID OUTPUT DATA
!                (MUST BE IMPOSSIBLE VALUE; SUGGESTED VALUE: -9999.)
!     XPTS     - REAL (NPTS) GRID X POINT COORDINATES IF IOPT>0
!     YPTS     - REAL (NPTS) GRID Y POINT COORDINATES IF IOPT>0
!     RLON     - REAL (NPTS) EARTH LONGITUDES IN DEGREES E IF IOPT<0
!                (ACCEPTABLE RANGE: -360. TO 360.)
!     RLAT     - REAL (NPTS) EARTH LATITUDES IN DEGREES N IF IOPT<0
!                (ACCEPTABLE RANGE: -90. TO 90.)
!
!   OUTPUT ARGUMENT LIST:
!     XPTS     - REAL (NPTS) GRID X POINT COORDINATES IF IOPT<0
!     YPTS     - REAL (NPTS) GRID Y POINT COORDINATES IF IOPT<0
!     RLON     - REAL (NPTS) EARTH LONGITUDES IN DEGREES E IF IOPT>0
!     RLAT     - REAL (NPTS) EARTH LATITUDES IN DEGREES N IF IOPT>0
!     NRET     - INTEGER NUMBER OF VALID POINTS COMPUTED
!     CROT     - REAL, OPTIONAL (NPTS) CLOCKWISE VECTOR ROTATION COSINES
!     SROT     - REAL, OPTIONAL (NPTS) CLOCKWISE VECTOR ROTATION SINES
!                (UGRID=CROT*UEARTH-SROT*VEARTH;
!                 VGRID=SROT*UEARTH+CROT*VEARTH)
!     XLON     - REAL, OPTIONAL (NPTS) DX/DLON IN 1/DEGREES
!     XLAT     - REAL, OPTIONAL (NPTS) DX/DLAT IN 1/DEGREES
!     YLON     - REAL, OPTIONAL (NPTS) DY/DLON IN 1/DEGREES
!     YLAT     - REAL, OPTIONAL (NPTS) DY/DLAT IN 1/DEGREES
!     AREA     - REAL, OPTIONAL (NPTS) AREA WEIGHTS IN M**2
!
! ATTRIBUTES:
!   LANGUAGE: FORTRAN 90
!
!$$$
 IMPLICIT NONE
!
 INTEGER,                 INTENT(IN   ) :: IOPT,KGDS(200),NPTS
 INTEGER,                 INTENT(  OUT) :: NRET
!
 REAL,                    INTENT(IN   ) :: FILL
 REAL,                    INTENT(INOUT) :: RLON(NPTS),RLAT(NPTS)
 REAL,                    INTENT(INOUT) :: XPTS(NPTS),YPTS(NPTS)
 REAL,  OPTIONAL,         INTENT(  OUT) :: CROT(NPTS),SROT(NPTS)
 REAL,  OPTIONAL,         INTENT(  OUT) :: XLON(NPTS),XLAT(NPTS)
 REAL,  OPTIONAL,         INTENT(  OUT) :: YLON(NPTS),YLAT(NPTS),AREA(NPTS)
!
 INTEGER                                :: IM,JM,ISCAN,JSCAN,N
!
 LOGICAL                                :: LROT, LMAP, LAREA
!
 REAL(KIND=KD)                          :: HI,HJ,HS,HS2
 REAL(KIND=KD)                          :: RLAT1,RLON1,RLAT0,RLAT2,RLON2
 REAL(KIND=KD)                          :: SLAT1,CLAT1
 REAL(KIND=KD)                          :: SLAT2,CLAT2,CLON2
 REAL(KIND=KD)                          :: CLON1,CLONR
 REAL(KIND=KD)                          :: RLATR,RLONR
 REAL(KIND=KD)                          :: WBD,SBD,NBD,EBD
 REAL                                   :: XMIN,XMAX,YMIN,YMAX
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 IF(PRESENT(CROT)) CROT=FILL
 IF(PRESENT(SROT)) SROT=FILL
 IF(PRESENT(XLON)) XLON=FILL
 IF(PRESENT(XLAT)) XLAT=FILL
 IF(PRESENT(YLON)) YLON=FILL
 IF(PRESENT(YLAT)) YLAT=FILL
 IF(PRESENT(AREA)) AREA=FILL
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 IF(KGDS(1).EQ.205) THEN
   RLAT1=KGDS(4)*1.E-3_KD
   RLON1=KGDS(5)*1.E-3_KD
   RLAT0=KGDS(7)*1.E-3_KD
   RLON0=KGDS(8)*1.E-3_KD
   RLAT2=KGDS(12)*1.E-3_KD
   RLON2=KGDS(13)*1.E-3_KD
   IROT=MOD(KGDS(6)/8,2)
   IM=KGDS(2)
   JM=KGDS(3)
   ISCAN=MOD(KGDS(11)/128,2)
   JSCAN=MOD(KGDS(11)/64,2)
   HI=(-1.)**ISCAN
   HJ=(-1.)**(1-JSCAN)
   SLAT1=SIN(RLAT1/DPR)
   CLAT1=COS(RLAT1/DPR)
   SLAT0=SIN(RLAT0/DPR)
   CLAT0=COS(RLAT0/DPR)
   HS=SIGN(1._KD,MOD(RLON1-RLON0+180+3600,360._KD)-180)
   CLON1=COS((RLON1-RLON0)/DPR)
   SLATR=CLAT0*SLAT1-SLAT0*CLAT1*CLON1
   CLATR=SQRT(1-SLATR**2)
   CLONR=(CLAT0*CLAT1*CLON1+SLAT0*SLAT1)/CLATR
   RLATR=DPR*ASIN(SLATR)
   RLONR=HS*DPR*ACOS(CLONR)
   WBD=RLONR
   SBD=RLATR
   SLAT2=SIN(RLAT2/DPR)
   CLAT2=COS(RLAT2/DPR)
   HS2=SIGN(1._KD,MOD(RLON2-RLON0+180+3600,360._KD)-180)
   CLON2=COS((RLON2-RLON0)/DPR)
   SLATR=CLAT0*SLAT2-SLAT0*CLAT2*CLON2
   CLATR=SQRT(1-SLATR**2)
   CLONR=(CLAT0*CLAT2*CLON2+SLAT0*SLAT2)/CLATR
   NBD=DPR*ASIN(SLATR)
   EBD=HS2*DPR*ACOS(CLONR)
   DLATS=(NBD-SBD)/FLOAT(JM-1)
   DLONS=(EBD-WBD)/FLOAT(IM-1)
   XMIN=0
   XMAX=IM+1
   YMIN=0
   YMAX=JM+1
   NRET=0
   IF(PRESENT(CROT).AND.PRESENT(SROT))THEN
     LROT=.TRUE.
   ELSE
     LROT=.FALSE.
   ENDIF
   IF(PRESENT(XLON).AND.PRESENT(XLAT).AND.PRESENT(YLON).AND.PRESENT(YLAT))THEN
     LMAP=.TRUE.
   ELSE
     LMAP=.FALSE.
   ENDIF
   IF(PRESENT(AREA))THEN
     LAREA=.TRUE.
   ELSE
     LAREA=.FALSE.
   ENDIF
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
!  TRANSLATE GRID COORDINATES TO EARTH COORDINATES
   IF(IOPT.EQ.0.OR.IOPT.EQ.1) THEN
     DO N=1,NPTS
       IF(XPTS(N).GE.XMIN.AND.XPTS(N).LE.XMAX.AND. &
          YPTS(N).GE.YMIN.AND.YPTS(N).LE.YMAX) THEN
         RLONR=WBD+(XPTS(N)-1._KD)*DLONS
         RLATR=SBD+(YPTS(N)-1._KD)*DLATS
         IF(RLONR <= 0._KD) THEN
           HS=-HI
         ELSE
           HS=HI
         ENDIF
         CLONR=COS(RLONR/DPR)
         SLATR=SIN(RLATR/DPR)
         CLATR=COS(RLATR/DPR)
         SLAT=CLAT0*SLATR+SLAT0*CLATR*CLONR
         IF(SLAT.LE.-1) THEN
           CLAT=0.
           CLON=COS(RLON0/DPR)
           RLON(N)=0
           RLAT(N)=-90
         ELSEIF(SLAT.GE.1) THEN
           CLAT=0.
           CLON=COS(RLON0/DPR)
           RLON(N)=0
           RLAT(N)=90
         ELSE
           CLAT=SQRT(1-SLAT**2)
           CLON=(CLAT0*CLATR*CLONR-SLAT0*SLATR)/CLAT
           CLON=MIN(MAX(CLON,-1._KD),1._KD)
           RLON(N)=MOD(RLON0+HS*DPR*ACOS(CLON)+3600,360._KD)
           RLAT(N)=DPR*ASIN(SLAT)
         ENDIF
         NRET=NRET+1
         IF(LROT) CALL GDSWZDCD_VECT_ROT(RLON(N), CROT(N), SROT(N))
         IF(LMAP) CALL GDSWZDCD_MAP_JACOB(FILL, RLON(N), &
                                          XLON(N), XLAT(N), YLON(N), YLAT(N))
         IF(LAREA) CALL GDSWZDCD_GRID_AREA(FILL, AREA(N))
       ELSE
         RLON(N)=FILL
         RLAT(N)=FILL
       ENDIF
     ENDDO
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
!  TRANSLATE EARTH COORDINATES TO GRID COORDINATES
   ELSEIF(IOPT.EQ.-1) THEN
     DO N=1,NPTS
       IF(ABS(RLON(N)).LE.360.AND.ABS(RLAT(N)).LE.90) THEN
         HS=SIGN(1._KD,MOD(RLON(N)-RLON0+180+3600,360._KD)-180)
         CLON=COS((RLON(N)-RLON0)/DPR)
         SLAT=SIN(RLAT(N)/DPR)
         CLAT=COS(RLAT(N)/DPR)
         SLATR=CLAT0*SLAT-SLAT0*CLAT*CLON
         IF(SLATR.LE.-1) THEN
           CLATR=0.
           RLONR=0
           RLATR=-90
         ELSEIF(SLATR.GE.1) THEN
           CLATR=0.
           RLONR=0
           RLATR=90
         ELSE
           CLATR=SQRT(1-SLATR**2)
           CLONR=(CLAT0*CLAT*CLON+SLAT0*SLAT)/CLATR
           CLONR=MIN(MAX(CLONR,-1._KD),1._KD)
           RLONR=HS*DPR*ACOS(CLONR)
           RLATR=DPR*ASIN(SLATR)
         ENDIF
         XPTS(N)=(RLONR-WBD)/DLONS+1._KD
         YPTS(N)=(RLATR-SBD)/DLATS+1._KD
         IF(XPTS(N).GE.XMIN.AND.XPTS(N).LE.XMAX.AND. &
            YPTS(N).GE.YMIN.AND.YPTS(N).LE.YMAX) THEN
           NRET=NRET+1
           IF(LROT) CALL GDSWZDCD_VECT_ROT(RLON(N), CROT(N), SROT(N))
           IF(LMAP) CALL GDSWZDCD_MAP_JACOB(FILL, RLON(N), &
                                            XLON(N), XLAT(N), YLON(N), YLAT(N))
           IF(LAREA) CALL GDSWZDCD_GRID_AREA(FILL, AREA(N))
         ELSE
           XPTS(N)=FILL
           YPTS(N)=FILL
         ENDIF
       ELSE
         XPTS(N)=FILL
         YPTS(N)=FILL
       ENDIF
     ENDDO
   ENDIF
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
!  PROJECTION UNRECOGNIZED
 ELSE
   IF(IOPT.GE.0) THEN
     DO N=1,NPTS
       RLON(N)=FILL
       RLAT(N)=FILL
     ENDDO
   ENDIF
   IF(IOPT.LE.0) THEN
     DO N=1,NPTS
       XPTS(N)=FILL
       YPTS(N)=FILL
     ENDDO
   ENDIF
 ENDIF
! - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 END SUBROUTINE GDSWZDCD
!
 SUBROUTINE GDSWZDCD_VECT_ROT(RLON, CROT, SROT)
!$$$  SUBPROGRAM DOCUMENTATION BLOCK
!
! SUBPROGRAM:  GDSWZDCD_VECT_ROT   VECTOR ROTATION FIELDS FOR
!                                  ROTATED EQUIDISTANT CYLINDRICAL
!                                  GRIDS - NON "E" STAGGER.
!
!   PRGMMR: GAYNO     ORG: W/NMC23       DATE: 2015-01-21
!
! ABSTRACT: THIS SUBPROGRAM COMPUTES THE VECTOR ROTATION SINES AND
!           COSINES FOR A ROTATED EQUIDISTANT CYLINDRICAL GRID -
!           NON "E" STAGGER.
!
! PROGRAM HISTORY LOG:
! 2015-01-21  GAYNO    INITIAL VERSION
!
! USAGE:    CALL GDSWZDCD_VECT_ROT(RLON, CROT, SROT)
!
!   INPUT ARGUMENT LIST:
!     RLON     - LONGITUDE IN DEGREES (REAL)
!
!   OUTPUT ARGUMENT LIST:
!     CROT     - CLOCKWISE VECTOR ROTATION COSINES (REAL)
!     SROT     - CLOCKWISE VECTOR ROTATION SINES (REAL)
!                (UGRID=CROT*UEARTH-SROT*VEARTH;
!                 VGRID=SROT*UEARTH+CROT*VEARTH)
!
! ATTRIBUTES:
!   LANGUAGE: FORTRAN 90
!
!$$$
!
 IMPLICIT NONE

 REAL         ,    INTENT(IN   ) :: RLON
 REAL         ,    INTENT(  OUT) :: CROT, SROT

 REAL(KIND=KD)                   :: SLON

 IF(IROT.EQ.1) THEN
   IF(CLATR.LE.0) THEN
     CROT=-SIGN(1._KD,SLATR*SLAT0)
     SROT=0.
   ELSE
     SLON=SIN((RLON-RLON0)/DPR)
     CROT=(CLAT0*CLAT+SLAT0*SLAT*CLON)/CLATR
     SROT=SLAT0*SLON/CLATR
   ENDIF
 ELSE
   CROT=1.
   SROT=0.
 ENDIF

 END SUBROUTINE GDSWZDCD_VECT_ROT
!
 SUBROUTINE GDSWZDCD_MAP_JACOB(FILL, RLON, &
                               XLON, XLAT, YLON, YLAT)
!$$$  SUBPROGRAM DOCUMENTATION BLOCK
!
! SUBPROGRAM:  GDSWZDCD_MAP_JACOB  MAP JACOBIANS FOR
!                                  ROTATED EQUIDISTANT CYLINDRICAL
!                                  GRIDS - NON "E" STAGGER.
!
!   PRGMMR: GAYNO     ORG: W/NMC23       DATE: 2015-01-21
!
! ABSTRACT: THIS SUBPROGRAM COMPUTES THE MAP JACOBIANS FOR
!           A ROTATED EQUIDISTANT CYLINDRICAL GRID -
!           NON "E" STAGGER.
!
! PROGRAM HISTORY LOG:
! 2015-01-21  GAYNO    INITIAL VERSION
!
! USAGE:  CALL GDSWZDCD_MAP_JACOB(FILL,RLON,XLON,XLAT,YLON,YLAT)
!
!   INPUT ARGUMENT LIST:
!     FILL     - FILL VALUE FOR UNDEFINED POINTS (REAL)
!     RLON     - LONGITUDE IN DEGREES (REAL)
!
!   OUTPUT ARGUMENT LIST:
!     XLON     - DX/DLON IN 1/DEGREES (REAL)
!     XLAT     - DX/DLAT IN 1/DEGREES (REAL)
!     YLON     - DY/DLON IN 1/DEGREES (REAL)
!     YLAT     - DY/DLAT IN 1/DEGREES (REAL)
!
! ATTRIBUTES:
!   LANGUAGE: FORTRAN 90
!
!$$$
!
 IMPLICIT NONE

 REAL         ,    INTENT(IN   ) :: FILL, RLON
 REAL         ,    INTENT(  OUT) :: XLON, XLAT, YLON, YLAT

 REAL(KIND=KD)                   :: SLON, TERM1, TERM2

 IF(CLATR.LE.0._KD) THEN
   XLON=FILL
   XLAT=FILL
   YLON=FILL
   YLAT=FILL
 ELSE
   SLON=SIN((RLON-RLON0)/DPR)
   TERM1=(CLAT0*CLAT+SLAT0*SLAT*CLON)/CLATR
   TERM2=SLAT0*SLON/CLATR
   XLON=TERM1*CLAT/(DLONS*CLATR)
   XLAT=-TERM2/(DLONS*CLATR)
   YLON=TERM2*CLAT/DLATS
   YLAT=TERM1/DLATS
 ENDIF

 END SUBROUTINE GDSWZDCD_MAP_JACOB
!
 SUBROUTINE GDSWZDCD_GRID_AREA(FILL, AREA)
!$$$  SUBPROGRAM DOCUMENTATION BLOCK
!
! SUBPROGRAM:  GDSWZDCD_GRID_AREA  GRID BOX AREA FOR
!                                  ROTATED EQUIDISTANT CYLINDRICAL
!                                  GRIDS - NON "E" STAGGER.
!
!   PRGMMR: GAYNO     ORG: W/NMC23       DATE: 2015-01-21
!
! ABSTRACT: THIS SUBPROGRAM COMPUTES THE GRID BOX AREA FOR
!           A ROTATED EQUIDISTANT CYLINDRICAL GRID -
!           NON "E" STAGGER.
!
! PROGRAM HISTORY LOG:
! 2015-01-21  GAYNO    INITIAL VERSION
!
! USAGE:  CALL GDSWZDCD_GRID_AREA(FILL,AREA)
!
!   INPUT ARGUMENT LIST:
!     FILL     - FILL VALUE FOR UNDEFINED POINTS (REAL)
!
!   OUTPUT ARGUMENT LIST:
!     AREA     - AREA WEIGHTS IN M**2 (REAL)
!
! ATTRIBUTES:
!   LANGUAGE: FORTRAN 90
!
!$$$
!
 IMPLICIT NONE

 REAL,             INTENT(IN   ) :: FILL
 REAL,             INTENT(  OUT) :: AREA

 IF(CLATR.LE.0._KD) THEN
   AREA=FILL
 ELSE
   AREA=2._KD*(RERTH**2)*CLATR*(DLONS/DPR)*SIN(0.5_KD*DLATS/DPR)
 ENDIF

 END SUBROUTINE GDSWZDCD_GRID_AREA

 END MODULE GDSWZDCD_MOD


      subroutine bndsol (mode,g,mdg,nb,ip,ir,x,n,rnorm,ier)
c     c.l.lawson and r.j.hanson, jet propulsion laboratory, 1973 jun 12
c     to appear in 'solving least squares problems', prentice-hall, 1974
c	   sequential solution of a banded least squares problem..
c	   solution phase.   for the accumulation phase use bndacc.
c
c     mode = 1	   solve r*x=y	 where r and y are in the g( ) array
c		   and x will be stored in the x( ) array.
c	     2	   solve (r**t)*x=y   where r is in g( ),
c		   y is initially in x( ), and x replaces y in x( ),
c	     3	   solve r*x=y	 where r is in g( ).
c		   y is initially in x( ), and x replaces y in x( ).
c
c     the second subscript of g( ) must be dimensioned at least
c     nb+1 in the calling program.
      dimension g(mdg,1),x(n)
      zero=0.
      ier = 0
c
      rnorm=zero
      go to (10,90,50), mode
c				    ********************* mode = 1
c				    algc step 26
   10	   do 20 j=1,n
   20	   x(j)=g(j,nb+1)
      rsq=zero
      np1=n+1
      irm1=ir-1
      if (np1.gt.irm1) go to 40
	   do 30 j=np1,irm1
   30	   rsq=rsq+g(j,nb+1)**2
      rnorm=sqrt(rsq)
   40 continue
c				    ********************* mode = 3
c				    alg. step 27
   50	   do 80 ii=1,n
	   i=n+1-ii
c				    alg. step 28
	   s=zero
	   l=max0(0,i-ip)
c				    alg. step 29
	   if (i.eq.n) go to 70
c				    alg. step 30
	   ie=min0(n+1-i,nb)
		do 60 j=2,ie
		jg=j+l
		ix=i-1+j
   60		s=s+g(i,jg)*x(ix)
c				    alg. step 31
   70	   if (g(i,l+1)) 80,130,80
   80	   x(i)=(x(i)-s)/g(i,l+1)
c				    alg. step 32
      return
c				    ********************* mode = 2
   90	   do 120 j=1,n
	   s=zero
	   if (j.eq.1) go to 110
	   i1=max0(1,j-nb+1)
	   i2=j-1
		do 100 i=i1,i2
		l=j-i+1+max0(0,i-ip)
  100		s=s+x(i)*g(i,l)
  110	   l=max0(0,j-ip)
	   if (g(j,l+1)) 120,130,120
  120	   x(j)=(x(j)-s)/g(j,l+1)
      return
c
c     error return: zero diagonal term in bndsol
  130 ier = 1
      return
      end


************************************************************************
* Inverse computes the inverse of a matrix.
* Input:
*   A: n-by-n matrix A
*   n: dimension of A
* Output:
*   A: mangled LU decomposition of A
*   Ainv: inverse of A
*   perm: permutation vector

      subroutine pvXInverse(A, Ainv, n, perm)
      implicit none
      integer n, perm(n)
      Double Complex A(n,n), Ainv(n,n)

      integer i, j

      call XLUDecomp(A, n, perm)
      do i = 1, n
        do j = 1, n
          Ainv(j,i) = dcmplx(0d0,0d0)
        enddo
        Ainv(i,i) = dcmplx(1d0,0d0)
        call XLUBackSubst(A, n, perm, Ainv(1,i))
      enddo
      end


!
!     CalculiX - A 3-dimensional finite element program
!              Copyright (C) 1998-2015 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.
!
      subroutine effectivemodalmass(neq,nactdof,mi,adb,aub,jq,
     &  irow,nev,z,co,nk)
!
!     calculates the effective modal mass for frequency
!     calculations
!
      implicit none
!
      integer i,j,k,nk,mi(*),nactdof(0:mi(2),*),neq(*),jq(*),irow(*),
     &  nev
!
      real*8 x(neq(2)),y(neq(2)),adb(*),aub(*),z(*),part(nev,6),
     &  toteffmass(6),effmodmass(nev,6),toteffmodmass(6),co(3,*)
!
!     translations in x, y and z
!
      do j=1,3
         do i=1,neq(2)
            x(i)=0.d0
         enddo
         do i=1,nk
            if(nactdof(j,i).gt.0) x(nactdof(j,i))=1.d0
         enddo
!
!        {y}=[M].{x}
!
         call op(neq(2),x,y,adb,aub,jq,irow)
!
!        participation factors {phi_k}^T.{y}
!
         do k=1,nev
            call multvec(neq(2),z((k-1)*neq(2)+1),y,part(k,j))
         enddo
!
!        total effective mass   {x}^T.{y}
!
         call multvec(neq(2),x,y,toteffmass(j))
!
!        effective modal mass and its total
!
         toteffmodmass(j)=0.d0
         do k=1,nev
            effmodmass(k,j)=(part(k,j)**2)
            toteffmodmass(j)=toteffmodmass(j)+effmodmass(k,j)
         enddo
      enddo
!
!     rotations about x, y and z
!
      do j=4,6
         do i=1,neq(2)
            x(i)=0.d0
         enddo
         do i=1,nk
            if(j.eq.4) then
               if(nactdof(2,i).gt.0) then
                  x(nactdof(2,i))=-co(3,i)
               endif
               if(nactdof(3,i).gt.0) then
                  x(nactdof(3,i))=co(2,i)
               endif
            elseif(j.eq.5) then
               if(nactdof(3,i).gt.0) then
                  x(nactdof(3,i))=-co(1,i)
               endif
               if(nactdof(1,i).gt.0) then
                  x(nactdof(1,i))=co(3,i)
               endif
            elseif(j.eq.6) then
               if(nactdof(1,i).gt.0) then
                  x(nactdof(1,i))=-co(2,i)
               endif
               if(nactdof(2,i).gt.0) then
                  x(nactdof(2,i))=co(1,i)
               endif
            endif
         enddo
!
!        {y}=[M].{x}
!
         call op(neq(2),x,y,adb,aub,jq,irow)
!
!        participation factors {phi_k}^T.{y}
!
         do k=1,nev
            call multvec(neq(2),z((k-1)*neq(2)+1),y,part(k,j))
         enddo
!
!        total effective mass   {x}^T.{y}
!
         call multvec(neq(2),x,y,toteffmass(j))
!
!        effective modal mass and its total
!
         toteffmodmass(j)=0.d0
         do k=1,nev
            effmodmass(k,j)=(part(k,j)**2)
            toteffmodmass(j)=toteffmodmass(j)+effmodmass(k,j)
         enddo
      enddo
!
!     writing the participation factors into the .dat-file
!
      write(5,*)
      write(5,*) '    P A R T I C I P A T I O N   F A C T O R S'
      write(5,*)
      write(5,100)
 100  format(   'MODE NO.   X-COMPONENT     Y-COMPONENT     Z-COMPONENT
     &    X-ROTATION      Y-ROTATION      Z-ROTATION')
      write(5,*)
      do k=1,nev
         write(5,'(i7,6(2x,e14.7))') k,(part(k,j),j=1,6)
      enddo
!
!     writing the effective mass into the .dat-file
!
      write(5,*)
      write(5,*) '    E F F E C T I V E   M O D A L   M A S S'
      write(5,*)
      write(5,100)
      write(5,*)
      do k=1,nev
         write(5,'(i7,6(2x,e14.7))') k,(effmodmass(k,j),j=1,6)
      enddo
      write(5,'(a7,6(2x,e14.7))') 'TOTAL  ',(toteffmodmass(j),j=1,6)
!
!     writing the total effective mass into the .dat-file
!
      write(5,*)
      write(5,*) '    T O T A L   E F F E C T I V E   M A S S'
      write(5,*)
      write(5,100)
      write(5,*)
      write(5,'(a7,6(2x,e14.7))') '       ',(toteffmass(j),j=1,6)
      write(5,*)
!
      return
      end