CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C                                                                     C
C  TWO ROUTINES TO FILL THE ARRAYS INCLUDING ALL THE SCALAR INTEGRALS C
C     FOR THE LEPTOQUARK PAIR HADROPRODUCTION                         C
C                                                                     C
C     SCALAR_ARRAY_B(MASSIN,SK3B0A,SK3B0B,SK3B0C,SK3B0D,SK3B0P)       C
C     SCALAR_ARRAY_C(MASSIN,SK3C0A,SK3C0B,SK3C0C,SK3D0)               C
C                                                                     C
C                                                                     C
C    INPUT  : MASSIN(1:2,1:6)                                         C
C                                                                     C
C             CONTAINING IN THE FIRST ROW  : S,T,U,T1,U1              C
C                               SECOND ROW : STOP-1 MASS SQUARED      C
C                                            [STOP-2 MASS SQUARED]    C
C                                            [SQUARK MASS SQUARED]    C
C                                            [GLUINO MASS SQUARED]    C
C                                            TOP MASS SQUARED         C
C                                            MASS SCALE SQUARED       C
C                                                                     C
C                                                                     C
C    OUTPUT : SK3B0A(1:5)         B_0(K1,M,M)                         C
C             SK3B0B(1:6)         B_0(K1+K2,M,M)                      C
C             SK3B0C(1:2)         B_0(P1,M1,M2)                       C
C             SK3B0D(1:2,1:2)     B_0(K1+P1,M1,M2)                    C
C             SK3B0P(1:3)         B_0'(K1/P1,M1,M2)                   C
C                                                                     C
C             SK3C0A(1:8)         C_0(K1,K2,M1,M2,M3)                 C
C             SK3C0B(1:4)         C_0(P1,P2,M1,M2,M3)                 C
C             SK3C0C(1:5,1:2)     C_0(K1,P1,M1,M2,M3)                 C
C             SK3D0(1:7,1:2)      D_0(........)                       C
C                                                                     C
C   N.B. : THE INTEGRAL MEASURE IS D^NQ/(I*Pi^2)                      C
C          ALL VARIABLES REAL*8, ONLY THE REAL PARTS OF THE INTEGRALS C
C                                                                     C
C                                                                     C
C    NECESSARY SUBROUTINES :  XSPENCE.F                               C
C                             XWOPOINT_2.F                            C
C                             XTHREEPOINT_2.F                         C
C                             XDENNER_2.F                             C
C                                                                     C
C    CALLED FUNCTIONS : B_FIN                                         C
C                       BP_FIN                                        C
C                       C_FIN                                         C
C                       D_FIN                                         C
C                       LIR2,LIC2                                     C
C                                                                     C
C                                                                     C
C    LAST MODIFIED : 26.03.97                                         C
C                                                                     C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC

c ---------------------------------------------------------------------
      subroutine SCALAR_ARRAY_LQ_B(m2,sba,sbb,sbc,sbd,sbp)

      implicit none

      integer n1,n2
      real*8 m2(1:2,1:6)
      real*8 sba(1:5),sbb(1:6),sbc(1:2),sbd(1:2,1:2)
      real*8 sbp(1:3)

      real*8 m1(1:2,1:6)
      real*8 B02,BP02

      m2(1,6) = 1.D-16

      do n1=1,5
         sba(n1) = 1.D40
      end do

      do n1=1,6
         sbb(n1) = 1.D40
      end do

      do n1=1,2
         sbc(n1) = 1.D40
      end do

      do n1=1,2
         do n2=1,2
            sbd(n1,n2) = 1.D40
         end do
      end do

      do n1=1,3
         sbp(n1) = 1.D40
      end do

      do n1=1,6
         m1(2,n1) = sqrt(abs(m2(2,n1)))
      end do

c      sba(1) = B02(0.D0,m1(2,4),m1(2,4),m2(2,6))
      sba(2) = B02(0.D0,m1(2,5),m1(2,5),m2(2,6))
c      sba(3) = B02(0.D0,m1(2,3),m1(2,4),m2(2,6))
c      sba(4) = B02(0.D0,m1(2,3),m1(2,3),m2(2,6))
c      sba(5) = B02(0.D0,m1(2,2),m1(2,2),m2(2,6))

      sbb(1) = B02(m2(1,1),0.D0,0.D0,m2(2,6))
      sbb(2) = B02(m2(1,1),m1(2,1),m1(2,1),m2(2,6))
c      sbb(3) = B02(m2(1,1),m1(2,4),m1(2,4),m2(2,6))
      sbb(4) = B02(m2(1,1),m1(2,5),m1(2,5),m2(2,6))
c      sbb(5) = B02(m2(1,1),m1(2,3),m1(2,3),m2(2,6))
c      sbb(6) = B02(m2(1,1),m1(2,2),m1(2,2),m2(2,6))

      sbc(1) = B02(m2(2,1),m1(2,1),0.D0,m2(2,6))
c      sbc(2) = B02(m2(2,1),m1(2,4),m1(2,5),m2(2,6))

      sbd(1,1) = B02(m2(1,2),m1(2,1),0.D0,m2(2,6))
c      sbd(2,1) = B02(m2(1,2),m1(2,4),m1(2,5),m2(2,6))

      sbd(1,2) = B02(m2(1,3),m1(2,1),0.D0,m2(2,6))
c      sbd(2,2) = B02(m2(1,3),m1(2,4),m1(2,5),m2(2,6))

      sbp(1) = BP02(m2(2,1),m1(2,1),0.D0,m2(2,6))
c      sbp(2) = BP02(m2(2,1),m1(2,4),m1(2,5),m2(2,6))
c      sbp(3) = BP02(0.D0,m1(2,3),m1(2,4),m2(2,6))

      return
      end

c ---------------------------------------------------------------------
      subroutine SCALAR_ARRAY_LQ_C(m2,sca,scb,scc,sd)

      implicit none

      integer n1,n2
      real*8 m2(1:2,1:6), s
      real*8 sca(1:8),scb(1:4),scc(1:5,1:2)
      real*8 sd(1:7,1:2)

      real*8 Lir2
c      real*8 C_fin_w,C_fin_d,D_fin,eps
      real*8 zeta2,xw1,xx1
      complex*16 cw1,cw5,cx1,cx5
      complex*16 CSPEN
c      complex*16 Lic2,cw2,cw3,cw4,cx2,cx3,cx4

c               real part of the spence function included in D04
      Lir2(s) = real( CSPEN(dcmplx(s)) )
c      Lic2(s) =       CSPEN(dcmplx(s))

c      eps = 1.D-4

      do n1=1,8
         sca(n1) = 1.D40
      end do

      do n1=1,4
         scb(n1) = 1.D40
      end do

      do n1=1,5
         do n2=1,2
            scc(n1,n2) = 1.D40
         end do
      end do

      do n1=1,7
         do n2=1,2
            sd(n1,n2) = 1.D40
         end do
      end do

      zeta2 = ( 4.D0*atan(1.D0) )**2 /6.D0
      xw1 = sqrt(1.D0-4.D0*m2(2,1)/m2(1,1))
      xx1 = ( 1.D0 - xw1 )/( 1.D0 + xw1 )

      cw1 = sqrt( dcmplx( 1.D0-4.D0*m2(2,1)/m2(1,1)) )
      cx1 = ( 1.D0 - cw1 )/( 1.D0 + cw1 )
c      cw2 = sqrt( dcmplx( 1.D0-4.D0*m2(2,2)/m2(1,1)) )
c      cx2 = ( 1.D0 - cw2 )/( 1.D0 + cw2 )
c      cw3 = sqrt( dcmplx( 1.D0-4.D0*m2(2,3)/m2(1,1)) )
c      cx3 = ( 1.D0 - cw3 )/( 1.D0 + cw3 )
c      cw4 = sqrt( dcmplx( 1.D0-4.D0*m2(2,4)/m2(1,1)) )
c      cx4 = ( 1.D0 - cw4 )/( 1.D0 + cw4 )
      cw5 = sqrt( dcmplx( 1.D0-4.D0*m2(2,5)/m2(1,1)) )
      cx5 = ( 1.D0 - cw5 )/( 1.D0 + cw5 )

      scc(1,1) = (  log(-m2(1,4)/m2(2,1))**2
     &            + Lir2(m2(1,2)/m2(2,1))
     &            + zeta2/4.D0              )/m2(1,4)
      scc(1,2) = (  log(-m2(1,5)/m2(2,1))**2
     &            + Lir2(m2(1,3)/m2(2,1))
     &            + zeta2/4.D0              )/m2(1,5)

c   the three point functions with two vanishing external masses
c   according to roland resp. wim
      sca(1) = (  log(m2(1,1)/m2(2,1))**2 /2.D0
     &          - 7.D0/2.D0*zeta2                )/m2(1,1)
      sca(2) = real( log(-cx1)**2 ) /(2.D0*m2(1,1))
c      sca(3) = real( log(-cx4)**2 ) /(2.D0*m2(1,1))
      sca(4) = real( log(-cx5)**2 ) /(2.D0*m2(1,1))
c      sca(5) = real(  Lic2(1.D0 + m2(2,4)*cx3/m2(2,3))
c     &               + Lic2(1.D0 + m2(2,4)/cx3/m2(2,3))
c     &               - 2.D0*Lir2(1.D0 - m2(2,4)/m2(2,3))
c     &               + log(-cx3)**2                   )/m2(1,1)
c      sca(6) = real(  Lic2(1.D0 + m2(2,3)*cx4/m2(2,4))
c     &               + Lic2(1.D0 + m2(2,3)/cx4/m2(2,4))
c     &               - 2.D0*Lir2(1.D0 - m2(2,3)/m2(2,4))
c     &               + log(-cx4)**2                   )/m2(1,1)
c      sca(7) = real( log(-cx3)**2 ) /(2.D0*m2(1,1))
c      sca(8) = real( log(-cx2)**2 ) /(2.D0*m2(1,1))

      scb(1) = (  - 2.D0*log(xx1)*log(1.D0-xx1)
     &            - 2.D0*Lir2(xx1)
     &            + log(xx1)**2/2.D0
     &            - 4.D0*zeta2                  )/m2(1,1)/xw1
      scb(2) = (  2.D0 * Lir2(-xx1)
     &          + log(xx1)**2 /2.D0
     &          + zeta2              )/(m2(1,1)*xw1)
c      scb(3) = C_fin_w(m2(1,1),m2(2,1),m2(2,1),m2(2,4),m2(2,5),m2(2,4))
c      scb(4) = C_fin_w(m2(1,1),m2(2,1),m2(2,1),m2(2,5),m2(2,4),m2(2,5))

c      scc(2,1) = C_fin_d(m2(1,2),m2(2,1),eps,m2(2,5),m2(2,4),m2(2,3))
      scc(3,1) = ( - Lir2(m2(1,2)/m2(2,1)) + zeta2 )/m2(1,4)
c      scc(4,1) = C_fin_d(m2(1,2),m2(2,1),eps,m2(2,5),m2(2,4),m2(2,4))
c      scc(5,1) = C_fin_d(m2(1,2),m2(2,1),eps,m2(2,4),m2(2,5),m2(2,5))

c      scc(2,2) = C_fin_d(m2(1,3),m2(2,1),eps,m2(2,5),m2(2,4),m2(2,3))
      scc(3,2) = ( - Lir2(m2(1,3)/m2(2,1)) + zeta2 )/m2(1,5)
c      scc(4,2) = C_fin_d(m2(1,3),m2(2,1),eps,m2(2,5),m2(2,4),m2(2,4))
c      scc(5,2) = C_fin_d(m2(1,3),m2(2,1),eps,m2(2,4),m2(2,5),m2(2,5))

c   the special four point function according to prd40
      sd(1,1) = (  2.D0*log(m2(1,1)/m2(2,1))*log(-m2(1,4)/m2(2,1))
     &           - 4.D0*zeta2        )/(m2(1,1)*m2(1,4))
      sd(2,1) = (- 2.D0*log(xx1)*log(1.D0-xx1)
     &           + 2.D0*log(xx1)*log(1.D0+xx1)
     &           - 2.D0*log(xx1)*log(-m2(1,4)/m2(2,1))
     &           - 2.D0*Lir2(xx1)
     &           + 2.D0*Lir2(-xx1)
     &           - 3.D0*zeta2        )/(m2(1,1)*m2(1,4)*xw1)
      sd(3,1) = (  2.D0*log(-m2(1,4)/m2(2,1))*log(-m2(1,5)/m2(2,1))
     &           - 7.D0*zeta2/2.D0     )/(m2(1,4)*m2(1,5))

      sd(1,2) = (  2.D0*log(m2(1,1)/m2(2,1))*log(-m2(1,5)/m2(2,1))
     &           - 4.D0*zeta2        )/(m2(1,1)*m2(1,5))
      sd(2,2) = (- 2.D0*log(xx1)*log(1.D0-xx1)
     &           + 2.D0*log(xx1)*log(1.D0+xx1)
     &           - 2.D0*log(xx1)*log(-m2(1,5)/m2(2,1))
     &           - 2.D0*Lir2(xx1)
     &           + 2.D0*Lir2(-xx1)
     &           - 3.D0*zeta2        )/(m2(1,1)*m2(1,5)*xw1)
      sd(3,2) = (  2.D0*log(-m2(1,5)/m2(2,1))*log(-m2(1,4)/m2(2,1))
     &           - 7.D0*zeta2/2.D0     )/(m2(1,5)*m2(1,4))

c   the general four point functions
c      sd(4,1) = D_fin(m2(2,1),eps,eps,m2(2,1),m2(1,2),m2(1,1),
c     &                m2(2,4),m2(2,5),m2(2,5),m2(2,5))
c      sd(5,1) = D_fin(eps,eps,m2(2,1),m2(2,1),m2(1,1),m2(1,2),
c     &                m2(2,4),m2(2,4),m2(2,4),m2(2,5))
c      sd(6,1) = D_fin(eps,m2(2,1),eps,m2(2,1),m2(1,3),m2(1,2),
c     &                m2(2,5),m2(2,5),m2(2,4),m2(2,4))
c      sd(7,1) = D_fin(eps,eps,m2(2,1),m2(2,1),m2(1,1),m2(1,2),
c     &                m2(2,4),m2(2,3),m2(2,4),m2(2,5))

c      sd(4,2) = D_fin(m2(2,1),eps,eps,m2(2,1),m2(1,3),m2(1,1),
c     &                m2(2,4),m2(2,5),m2(2,5),m2(2,5))
c      sd(5,2) = D_fin(eps,eps,m2(2,1),m2(2,1),m2(1,1),m2(1,3),
c     &                m2(2,4),m2(2,4),m2(2,4),m2(2,5))
c      sd(6,2) = D_fin(eps,m2(2,1),eps,m2(2,1),m2(1,2),m2(1,3),
c     &                m2(2,5),m2(2,5),m2(2,4),m2(2,4))
c      sd(7,2) = D_fin(eps,eps,m2(2,1),m2(2,1),m2(1,1),m2(1,3),
c     &                m2(2,4),m2(2,3),m2(2,4),m2(2,5))

      return
      end

c$$$cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c$$$c                                                              c
c$$$c all the other routines :                                     c
c$$$c                                                              c
c$$$c      B_fin(s,xm1**2,xm2**2,xmu**2)                           c
c$$$c      Bp_fin(s,xm1**2,xm2**2,xmu**2)                          c
c$$$c      C_fin(p0**2,p1**2,p2**2,m0**2,m1**2,m2**2)              c
c$$$c      D_fin(p1**2,p2**2,p3**2,p4,p12,p23,m12,m22,m32,m42)     c
c$$$c      Lic2(x),Lir2(x)                                         c
c$$$c                                                              c
c$$$cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c$$$      function B_fin(s,m12,m22,mu2)
c$$$
c$$$      implicit none
c$$$
c$$$      real*8 B_fin,s,m12,m22,mu2,m1,m2,B02
c$$$
c$$$      m1 = sqrt(m12)
c$$$      m2 = sqrt(m22)
c$$$
c$$$      B_fin = B02(s,m1,m2,mu2)
c$$$
c$$$      return
c$$$      end
c$$$
c$$$c---------------------------------------------------------------
c$$$      function Bp_fin(s,m12,m22,mu2)
c$$$
c$$$      implicit none
c$$$
c$$$      real*8 Bp_fin,s,m12,m22,mu2,m1,m2,Bp02
c$$$
c$$$      m1 = sqrt(m12)
c$$$      m2 = sqrt(m22)
c$$$
c$$$      Bp_fin = BP02(s,m1,m2,mu2)
c$$$
c$$$      return
c$$$      end


c
c
c     ###################################################
c     ##  COPYRIGHT (C)  2000  by  Jay William Ponder  ##
c     ##              All Rights Reserved              ##
c     ###################################################
c
c     #################################################################
c     ##                                                             ##
c     ##  program spectrum  --  power spectrum from autocorrelation  ##
c     ##                                                             ##
c     #################################################################
c
c
c     "spectrum" computes a power spectrum over a wavelength range
c     from the velocity autocorrelation as a function of time
c
c
      program spectrum
      implicit none
      include 'files.i'
      include 'iounit.i'
      include 'math.i'
      include 'units.i'
      integer maxvel,maxfreq
      parameter (maxvel=100000)
      parameter (maxfreq=5000)
      integer i,k,nsamp
      integer ivel,nvel
      integer freeunit
      real*8 factor,aver
      real*8 norm,step
      real*8 freq,time
      real*8 vel(0:maxvel)
      real*8 intense(maxfreq)
      logical exist,done
      character*120 velfile
      character*120 record
      character*120 string
c
c
c     perform the standard initialization functions
c
      call initial
c
c     try to get a filename from the command line arguments
c
      call nextarg (velfile,exist)
      if (exist) then
         call basefile (velfile)
         call suffix (velfile,'vel','old')
         inquire (file=velfile,exist=exist)
      end if
c
c     ask for the velocity autocorrelation data filename
c
      do while (.not. exist)
         write (iout,10)
   10    format (/,' Enter Name of Velocity Autocorrelation',
     &              ' File :  ',$)
         read (input,20)  velfile
   20    format (a120)
         call basefile (velfile)
         call suffix (velfile,'vel','old')
         inquire (file=velfile,exist=exist)
      end do
c
c     get the time step between autocorrelation data points
c
      step = -1.0d0
      call nextarg (string,exist)
      if (exist)  read (string,*,err=30,end=30)  step
   30 continue
      if (step .le. 0.0d0) then
         write (iout,40)
   40    format (/,' Enter Time Step for Autocorrelation Data',
     &              ' [1.0 fs] :  ',$)
         read (input,50)  step
   50    format (f20.0)
      end if
      if (step .le. 0.0d0)  step = 1.0d0
      step = 0.001d0 * step
c
c     open the velocity autocorrelation data file
c
      ivel = freeunit ()
      open (unit=ivel,file=velfile,status='old')
      rewind (unit=ivel)
c
c     read through file headers to the start of the data
c
      done = .false.
      do while (.not. done)
         read (ivel,60)  record
   60    format (a120)
         if (record(4:13) .eq. 'Separation') then
            done = .true.
            read (ivel,70)
   70       format ()
         end if
      end do
c
c     read the velocity autocorrelation as a function of time
c
      do i = 1, maxvel
         read (ivel,80,err=90,end=90)  record
   80    format (a120)
         read (record,*)  k,nsamp,aver,norm
         nvel = k
         vel(k) = norm
      end do
   90 continue
c
c     compute the power spectrum via discrete Fourier transform
c
      factor = 2.0d0 * pi * lightspd
      do i = 1, maxfreq
         freq = factor * dble(i)
         intense(i) = 0.0d0
         do k = 0, nvel
            time = step * dble(k)
            intense(i) = intense(i) + vel(k)*cos(freq*time)
         end do
         intense(i) = 1000.0d0 * step * intense(i)
      end do
c
c     print the power spectrum intensity at each wavelength
c
      write (iout,100)
  100 format (/,' Power Spectrum from Velocity Autocorrelation :',
     &        //,4x,'Frequency (cm-1)',10x,'Intensity',/)
      do i = 1, maxfreq
         write (iout,110)  dble(i),intense(i)
  110    format (3x,f12.2,8x,f16.6)
      end do
      end


module fcn_isentropic_vortex
#include <messenger.h>
    use mod_kinds,      only: rk,ik
    use mod_constants,  only: ZERO, HALF, ONE, TWO, THREE, FIVE, EIGHT, PI
    use type_point,     only: point_t
    use type_point_ad,  only: point_ad_t
    use type_function,  only: function_t
    use DNAD_D
    implicit none
    private

    !>  This function defines an analytical solution to the 2D Euler equations.
    !!  It may be used to provide initial conditions for an unsteady simulation,
    !!  as well as to compute numerical errors.
    !!
    !!  This solution is implemented as in Fidkowski 2016
    !!
    !!  @author Eric Wolf
    !!  @date   2/15/2017
    !!
    !-------------------------------------------------------------------------------
    type, extends(function_t), public :: isentropic_vortex_f

        integer(ik) :: ivar

    contains
        procedure   :: init
        procedure   :: compute
    end type isentropic_vortex_f
    !********************************************************************************

contains

    !>
    !!
    !!  @author Nathan A. Wukie
    !!  @date   2/2/2016
    !!
    !-------------------------------------------------------------------------
    subroutine init(self)
        class(isentropic_vortex_f),  intent(inout)  :: self

        ! Set function name
        call self%set_name("isentropic_vortex")

        call self%add_option('ivar', 1._rk)

    end subroutine init
    !*************************************************************************

    !>
    !!
    !!  @author Nathan A. Wukie
    !!  @date   2/2/2016
    !!
    !!
    !-----------------------------------------------------------------------------------------
    impure elemental function compute(self,time,coord) result(val)
        class(isentropic_vortex_f),     intent(inout)  :: self
        real(rk),                       intent(in)  :: time
        type(point_ad_t),               intent(in)  :: coord

        type(AD_D)  :: x, y, z, r, &
                       u, v, w, &
                       rho, p, drho, dp, theta, du, dv, val

        integer(ik) :: ivar
        real(rk)    :: gam, uinf, vinf, dumax, b

        call chidg_signal(FATAL,"isentropic_vortex: needs reimplemented for differentiated coordinates.")

!        x     = coord%c1_
!        y     = coord%c2_
!        z     = coord%c3_
!        r     = sqrt(x*x + y*y)
!!        theta = atan2(y,x)
!
!
!
!        !
!        ! Base parameters
!        !
!        gam = 1.4_rk
!        uinf = 0.5_rk
!        vinf = 0._rk
!        dumax = 0.5_rk*uinf
!        b = 0.2_rk
!
!
!        !
!        ! Perturbations
!        !
!        drho = (ONE - HALF*(gam - ONE)*dumax*dumax*exp(ONE - (r*r/(b*b))))**(ONE/(gam-ONE))
!        dp   = (ONE/gam)*drho**(gam)
!        du   = -(dumax/b)*r*exp(HALF*(ONE - (r*r/(b*b)))) * sin(theta)
!        dv   =  (dumax/b)*r*exp(HALF*(ONE - (r*r/(b*b)))) * cos(theta)
!
!
!        !
!        ! Total quantities
!        !
!        rho = drho
!        p   = dp
!        u   = uinf + du
!        v   = vinf + dv
!        w   = ZERO
!
!
!
!
!        ivar = nint(self%get_option_value('ivar'))
!        select case (ivar)
!            ! RHO
!            case (1)
!                val = rho
!
!            ! RHO-U
!            case (2)
!                val = rho * u
!
!            ! RHO-V
!            case (3)
!                val = rho * v
!
!            ! RHO-W
!            case (4)
!                val = rho * w
!
!            ! RHO-E
!            case (5)
!                val = p/(gam-ONE)  +  HALF*rho*(u*u + v*v + w*w)
!
!        end select

    end function compute
    !**********************************************************************************

end module fcn_isentropic_vortex


SUBROUTINE idomats(g,nsm)

!-------------------------------------------------------------
!
!  Integral discrete ordinates matrices
!
!   Control the construction of jmat, kmat, jpsi, kpsi
!   Matrices nned to be made only once, then idot.f90 will
!    use them to solve the system of equations
!
!-------------------------------------------------------------

USE invar
USE solvar
IMPLICIT NONE
INTEGER, INTENT(IN) :: g, nsm
INTEGER :: i, j, l, info, m, tt, t, ieq
INTEGER, DIMENSION(:), ALLOCATABLE :: piv
REAL*8 :: symm, one, zero
REAL*8, DIMENSION(:), ALLOCATABLE :: q, wrk, symmvec
REAL*8, DIMENSION(:,:), ALLOCATABLE :: tmat

! Initialize the matrices
kmat = 0.0
jpsi = 0.0
kpsi = 0.0

! Set one and zero
one = 1.0
zero = 0.0

! Allocate the iteration Jacobian
ALLOCATE(jmat(nsm,nsm))
jmat = 0.0

! Allocate the temp arrays
ALLOCATE(q(nsm))

! Allocate idos-dependent arrays
ALLOCATE(piv(nsm), wrk(nsm))

IF (sym == 1 .OR. idos == 1) THEN
   ALLOCATE(symmvec(nsm))
END IF

! Can now start the single sweep for constructing all matrices
CALL matsweep(g)

! Begin constructing the RHS
DO i = 1, nx
   DO l = 0, anord
      ieq = (i-1)*sord + 1 + l
      src(ieq) = sm(ieq,g)/sigs(mat(i),l,g,g)
   END DO
END DO

! Multiply the source vector by the Jacobi iteration matrix
IF (tpose == 1) THEN
   DO j = 1, nsm
      q(j) = DOT_PRODUCT(jmat(:,j),src)
   END DO
ELSE
   q = MATMUL(jmat,src)
END IF

! Form the LHS matrix
jmat = -jmat
DO t = 1, nsm
   jmat(t,t) = 1.0 + jmat(t,t)
END DO

IF (sym == 1 .OR. idos == 1) THEN
   IF (tpose == 1) THEN
      ! Symmetrize jmat and put q into sv
      DO i = 1, nx
         m = mat(i)
         DO l = 0, anord
            ieq = (i-1)*sord + 1 + l
            symm = sigs(m,l,g,g)*dx(i)*(2.0*l+1.0)
            IF (MOD(l,2) == 1) symm = -1.0*symm        ! Odd order has the negative mult.
            symmvec(ieq) = symm
            DO tt = 1, nsm
               jmat(tt,ieq) = symm*jmat(tt,ieq)
            END DO
         END DO
      END DO
   ELSE
      DO i = 1, nx
         m = mat(i)
         DO l = 0, anord
            ieq = (i-1)*sord + 1 + l
            symm = sigs(m,l,g,g)*dx(i)*(2.0*l+1.0)
            IF (MOD(l,2) == 1) symm = -1.0*symm        ! Odd order has the negative mult.
            symmvec(ieq) = symm
            DO tt = 1, nsm
               jmat(ieq,tt) = symm*jmat(ieq,tt)
            END DO
         END DO
      END DO
   END IF
END IF

! Other operations depend on the IDO Solver value: 0-Direct, 1-CG
! If idos=0, need to invert the current jmat and multiply q and kmat
IF (idos == 0) THEN
   IF (sym == 0) THEN
      ! Invert the new LHS matrix
      CALL dgetrf(nsm,nsm,jmat,nsm,piv,info)
      IF (info /= 0) THEN
         WRITE (8,'(//,1X,A)') "ERROR: jmat either has illegal value or is singular, cannot factor."
         STOP
      END IF
      CALL dgetri(nsm,jmat,nsm,piv,wrk,nsm,info)
      IF (info /= 0) THEN
         WRITE (8,'(//,1X,A)') "ERROR: jmat either has illegal value or is singular, cannot invert."
         STOP
      END IF

      ! Save the matrix vector product of the inverted matrix and the modified source
      ! Then save the product of the inverted matrix and kmat
      IF (tpose == 0) THEN
         sv = MATMUL(jmat,q)
         ALLOCATE(tmat(nsm,apo))
         DO t = 1, 2
            CALL DGEMM('n','n',nsm,apo,nsm,one,jmat,nsm,kmat(:,:,t),nsm,zero,tmat,nsm)
            kmat(:,:,t) = tmat
         END DO
      ELSE
         DO j = 1, nsm
            sv(j) = DOT_PRODUCT(jmat(:,j),q)
         END DO
         ! kmat*jmat
         ALLOCATE(tmat(apo,nsm))
         DO t = 1, 2
            CALL DGEMM('n','n',apo,nsm,nsm,one,kmat(:,:,t),apo,jmat,nsm,zero,tmat,apo)
            kmat(:,:,t) = tmat
         END DO
      END IF

      ! Now have the five needed values: src, sv, kmat, jpsi, kpsi
      DEALLOCATE(jmat,tmat)
      DEALLOCATE(piv,q,wrk)
!---------------------------------------------------------------
! Symmetric direct
   ELSE
      ! Invert the new LHS matrix with symmetric routine.
      ! Symmetric INDEFINITE routines used. Not positive definite system.
      CALL dsytrf('U',nsm,jmat,nsm,piv,wrk,nsm,info)
      IF (info /= 0) THEN
         WRITE (8,'(//,1X,A)') "ERROR: jmat either has illegal value or is singular, cannot factor."
         STOP
      END IF
      CALL dsytri('U',nsm,jmat,nsm,piv,wrk,info)
      IF (info /= 0) THEN
         WRITE (8,'(//,1X,A)') "ERROR: jmat either has illegal value or is singular, cannot invert."
         STOP
      END IF

      ! Symmetric inverter only returns triangular portion of invert, copy rest
      DO j = 1, nsm
         DO i = 1, j
            jmat(j,i) = jmat(i,j)
         END DO
      END DO

      ! Multiply q by the symmvec and set sv
      ! Then multiply kmat's by symmvec
      ! Lastly reset kmat as jmat*kmat
      q = symmvec*q
      IF (tpose == 0) THEN
         sv = MATMUL(jmat,q)
         DO t = 1, 2
            DO i = 1, nsm
               kmat(i,:,t) = symmvec(i)*kmat(i,:,t)
            END DO
         END DO
         ALLOCATE(tmat(nsm,apo))
         DO t = 1, 2
            CALL DGEMM('n','n',nsm,apo,nsm,one,jmat,nsm,kmat(:,:,t),nsm,zero,tmat,nsm)
            kmat(:,:,t) = tmat
         END DO
      ELSE
         DO j = 1, nsm
            sv(j) = DOT_PRODUCT(jmat(:,j),q)
         END DO
         DO t = 1, 2
            DO j = 1, nsm
               kmat(:,j,t) = symmvec(j)*kmat(:,j,t)
            END DO
         END DO
         ! Aided a bit by the symmetry of jmat, use that feature.
         ! tmat comes out as the normal kmat shape instead of transposed
         ALLOCATE(tmat(nsm,apo))
         DO t = 1, 2
            CALL DGEMM('n','t',nsm,apo,nsm,one,jmat,nsm,kmat(:,:,t),apo,zero,tmat,nsm)
            kmat(:,:,t) = TRANSPOSE(tmat)
         END DO
      END IF

      ! Now have the five needed values: src, sv, kmat, jpsi, kpsi
      DEALLOCATE(jmat,tmat,q,symmvec,piv,wrk)
   END IF
!------------------------------------------------------
! CG
ELSE
   ! Reset kmat with the symmvec
   IF (tpose == 1) THEN
      DO t = 1, 2
         DO j = 1, nsm
            kmat(:,j,t) = symmvec(j)*kmat(:,j,t)
         END DO
      END DO
   ELSE
      DO t = 1, 2
         DO i = 1, nsm
            kmat(i,:,t) = symmvec(i)*kmat(i,:,t)
         END DO
      END DO
   END IF

   ! Set the sv vector
   sv = symmvec*q

   ! Now have the six needed values: src, sv, jmat, kmat, jpsi, kpsi
   DEALLOCATE(q,symmvec)
END IF

RETURN
END SUBROUTINE idomats


program upacs_convention
  implicit none
  type block
     integer in,jn,kn
     double precision,pointer::dq(:,:,:,:)
     double precision,pointer::inv_vol(:,:,:)
  end type block
  type face
     double precision,pointer:: flux(:)
  end type face
  type(block) blk
  type(face) cface(1024,1024,1024)
  integer n,i,j,k,im,jm,km
  integer:: bdtv_nFlowVar=1024
  integer,dimension(3) :: idelta
  double precision,pointer ::dq(:,:,:,:),inv_vol(:,:,:)
  double precision,pointer :: flux(:,:,:,:)
  integer :: in,jn,kn

  blk%in = 1024
  blk%jn = 1024
  blk%kn = 1024
  allocate(blk%dq(1024,1024,1024,1024))
  allocate(blk%inv_vol(1024,1024,1024))
  do k=1,1024
     do j=1,1024
        do i=1,1024
           allocate(cface(i,j,k)%flux(1024))
           do n=1,1024
              cface(i,j,k)%flux(n) = n
              blk%dq(i,j,k,n) = n
              blk%inv_vol(i,j,k) = i*j*k
           end do
        end do
     end do
  end do

  dq=>blk%dq
  inv_vol=>blk%inv_vol
  in = blk%in
  jn = blk%jn
  kn = blk%kn
  do k = 1, kn
     do j = 1, jn
        do i = 1, in
           flux( :, i, j, k) = cface(i, j, k)%flux(:)
        end do
     end do
  end do
  do n=1,bdtv_nFlowVar
     do k=1,kn
        do j=1,jn
           do i=1,in
              im=i-idelta(1)
              jm=j-idelta(2)
              km=k-idelta(3)
              dq(i,j,k,n) = dq(i,j,k,n) - inv_vol(i,j,k) &
                   * ( flux(n,i,j,k) - flux(n,im,jm,km) )
           end do
        end do
     end do
  end do
end program upacs_convention


!>
!! @file p_main.f90
!! @brief Contains program p_main
!! @author S. Bryngelson, K. Schimdmayer, V. Coralic, J. Meng, K. Maeda, T. Colonius
!! @version 1.0
!! @date JUNE 06 2019

!> @brief This program takes care of setting up the initial condition and
!!              grid data for the multicomponent flow code.
PROGRAM p_main

    ! Dependencies =============================================================
    USE m_derived_types         !< Definitions of the derived types

    USE m_global_parameters     !< Global parameters for the code

    USE m_mpi_proxy             !< Message passing interface (MPI) module proxy

    USE m_variables_conversion  !< Subroutines to change the state variables from
                                !! one form to another

    USE m_start_up              !< Procedures to read in and check consistency of
                                !! the user provided inputs and data

    USE m_grid                  !< Procedures to generate (non-)uniform grids

    USE m_initial_condition     !< Procedures to generate initial condition

    USE m_data_output           !< Procedures to write the grid data and the
                                !! conservative variables to files
    ! ==========================================================================

    IMPLICIT NONE

    ! Initialization of the MPI environment
    CALL s_mpi_initialize()

    ! Rank 0 processor assigns default values to user inputs prior to reading
    ! those in from the input file. Next, the user inputs are read in and their
    ! consistency is checked. The detection of any inconsistencies automatically
    ! leads to the termination of the pre-process.
    IF (proc_rank == 0) THEN
        CALL s_assign_default_values_to_user_inputs()
        CALL s_read_input_file()
        CALL s_check_input_file()
    END IF

    ! Broadcasting the user inputs to all of the processors and performing the
    ! parallel computational domain decomposition. Neither procedure has to be
    ! carried out if pre-process is in fact not truly executed in parallel.
    CALL s_mpi_bcast_user_inputs()
    CALL s_initialize_parallel_io()
    CALL s_mpi_decompose_computational_domain()

    ! Computation of parameters, allocation procedures, and/or any other tasks
    ! needed to properly setup the modules
    CALL s_initialize_global_parameters_module()
    CALL s_initialize_data_output_module()
    CALL s_initialize_variables_conversion_module()
    CALL s_initialize_start_up_module()
    CALL s_initialize_grid_module()
    CALL s_initialize_initial_condition_module()

    ! Associate pointers for serial or parallel I/O
    IF (parallel_io .NEQV. .TRUE.) THEN
        s_generate_grid => s_generate_serial_grid
        s_read_grid_data_files => s_read_serial_grid_data_files
        s_read_ic_data_files => s_read_serial_ic_data_files
        s_write_data_files => s_write_serial_data_files
    ELSE
        s_generate_grid => s_generate_parallel_grid
        s_read_grid_data_files => s_read_parallel_grid_data_files
        s_read_ic_data_files => s_read_parallel_ic_data_files
        s_write_data_files => s_write_parallel_data_files
    END IF

    ! Setting up the grid and the initial condition. If the grid is read in from
    ! preexisting grid data files, it is checked for consistency. If the grid is
    ! not read in, it is generated from scratch according to the inputs provided
    ! by the user. The initial condition may also be read in. It in turn is not
    ! checked for consistency since it WILL further be edited by the pre-process
    ! and also because it may be incomplete at the time it is read in. Finally,
    ! when the grid and initial condition are completely setup, they are written
    ! to their respective data files.

    ! Setting up grid and initial condition

    IF(old_grid) THEN
        CALL s_read_grid_data_files(dflt_int)
        CALL s_check_grid_data_files()
    ELSE
        IF (parallel_io .NEQV. .TRUE.) THEN
                CALL s_generate_grid(dflt_int)
        ELSE
            IF (proc_rank == 0) CALL s_generate_grid(dflt_int)
            CALL s_mpi_barrier()
            CALL s_read_grid_data_files(dflt_int)
            CALL s_check_grid_data_files()
        END IF
    END IF

    IF(old_ic) CALL s_read_ic_data_files(q_cons_vf)

    CALL s_generate_initial_condition()

    CALL s_write_data_files(q_cons_vf)

    ! Disassociate pointers for serial and parallel I/O
    s_generate_grid => NULL()
    s_read_grid_data_files => NULL()
    s_read_ic_data_files => NULL()
    s_write_data_files => NULL()

    ! Deallocation procedures for the modules
    CALL s_finalize_initial_condition_module()
    CALL s_finalize_grid_module()
    CALL s_finalize_start_up_module()
    CALL s_finalize_variables_conversion_module()
    CALL s_finalize_data_output_module()
    CALL s_finalize_global_parameters_module()

    ! Finalization of the MPI environment
    CALL s_mpi_finalize()

END PROGRAM p_main


      MODULE fsub_I
      INTERFACE
!...Generated by Pacific-Sierra Research 77to90  4.4G  10:47:13  03/09/06
      SUBROUTINE fsub (N, X, FVAL)
      USE vast_kind_param,ONLY: DOUBLE
      integer, INTENT(IN) :: N
      real(DOUBLE), INTENT(IN) :: X
      real(DOUBLE), INTENT(OUT) :: FVAL
      END SUBROUTINE
      END INTERFACE
      END MODULE


!===== パラメータを決めるモジュール ====

module prm_matrix

	implicit none

	integer, parameter :: ki = 5400, ks = 30, kt = 30, ku = 20, kv = 20, nd = 100
	integer, parameter :: kis = 4000, kif = 4000
	integer, parameter :: kfg = 4000, kmp = 4000, kfo = 4000, kio = 4000
	integer, parameter :: kfe = (ks+3) * (kt+3), kmr = (ku+3) * (kv+3)
	integer, parameter :: kmpa = kmp+1, kdata = kmp + kio
	integer, parameter :: kdata2 = kmp + ki
	integer, parameter :: ms = ks*nd, mt = kt*nd, ndd = nd*nd

end module prm_matrix

! ki 断層面を決める節点の数


! { dg-do run }
! Test the fix for PR57522, in which the associate name had a
! 'span' of an INTEGER rather than that of 'mytype'.
!
! Contributed by A Briolat  <alan.briolat@gmail.com>
!
program test_associate
  type mytype
    integer :: a = 1, b = 2
  end type
  type(mytype) :: t(4), u(2,2)
  integer :: c(4)
  t%a = [0, 1, 2, 3]
  t%b = [4, 5, 6, 7]
  associate (a => t%a)
! Test 'a' is OK on lhs and/or rhs of assignments
    c = a - 1
    if (any (c .ne. [-1,0,1,2])) STOP 1
    a = a + 1
    if (any (a .ne. [1,2,3,4])) STOP 2
    a = t%b
    if (any (a .ne. t%b)) STOP 3
! Test 'a' is OK as an actual argument
    c = foo(a)
    if (any (c .ne. t%b + 10)) STOP 4
  end associate
! Make sure that the fix works for multi-dimensional arrays...
  associate (a => u%a)
    if (any (a .ne. reshape ([1,1,1,1],[2,2]))) STOP 5
  end associate
! ...and sections
  associate (a => t(2:3)%b)
    if (any (a .ne. [5,6])) STOP 6
  end associate
contains
  function foo(arg) result(res)
    integer :: arg(4), res(4)
    res = arg + 10
  end function
end program


subroutine nsc_nscbc_density_ex(e,dtinv,accph,accvh,gpden,gpmom,gpene,gpden0,gpmom0,gpene0,grden,grmom,grene,incre)
!-----------------------------------------------------------------------
!****f* Nscomp/nsc_nscbc
! NAME
!    nsc_nscbc
! DESCRIPTION
!    This routine computes a non-reflecting boundary condition
!    for subsonic flows, based on the NSCBC approach.
!    Higher order derivatives (stress and heat flux) are neglected.
!    It recalculates the essential boundary conditions.
!    Transverse, and pressure terms are neglected (commented).
! USED BY
!    nsc_Endbouope
!***
!-----------------------------------------------------------------------
   use typre
   use Mod_Element
   implicit none
   class(FiniteElement) :: e
   real(rp) :: dtinv,accph,accvh
   real(rp) :: gpden,gpmom(e%ndime),gpene,gpden0,gpmom0(e%ndime),gpene0
   real(rp) :: grden(e%ndime),grmom(e%ndime,e%ndime),grene(e%ndime)
   real(rp),    intent(inout) :: incre(e%ndime+2)

   integer(ip)                :: idime
   integer(ip)                :: inodb,jnode,inode,jnodb

   real(rp) :: cgamma,aux_k,gptem,aux_k0
   real(rp) :: lomom(e%ndime),auxlogrm(e%ndime,e%ndime),lomom0(e%ndime)
   real(rp) :: logrd(e%ndime),logrm(e%ndime,e%ndime),logre(e%ndime)
   real(rp) :: logrv(e%ndime,e%ndime),logrp(e%ndime)
   real(rp) :: gppre,lospd,dens,veloc(e%ndime),cartvel(e%ndime),gppre0,gptem0
   real(rp) :: L1,L2,L5,d(2)

   cgamma = accph/accvh
   aux_k = dot_product(gpmom,gpmom)/(gpden*gpden)
   aux_k0 = dot_product(gpmom0,gpmom0)/(gpden0*gpden0)

   !baloc is (t1,t2,n)
   !therefore (ndime) is the normal component.
   !(1) is a transversal for 2d and (2) transversal for 3d
   !local density derivative
   logrd = matmul(grden,e%baloc)

   !local momentum and derivatives
   lomom = matmul(gpmom,e%baloc)
   lomom0 = matmul(gpmom0,e%baloc)
   ! d_l m_l = T_lc (d_c m_c) T_cl
   auxlogrm = matmul(transpose(grmom),e%baloc)
   logrm = matmul(transpose(auxlogrm),e%baloc)

   !local energy derivative
   logre = matmul(grene,e%baloc)

   !Calulate local pressure and gradient
   gppre = (cgamma-1.0_rp)*(gpene-gpden*aux_k/2.0_rp)
   gppre0 = (cgamma-1.0_rp)*(gpene0-gpden0*aux_k0/2.0_rp)

   !d_j(p)=(gamma-1)*(dj(etot) - m/rho dj(m) + m·m/rho·rho dj(rho))
   logrp = 0.0_rp
   logrp = logrp + (cgamma-1.0_rp) * (logre + aux_k*logrd)
   logrp = logrp - (cgamma-1.0_rp) * matmul(lomom,logrm)/gpden

   !Calulate local temperature
   gptem = gppre/(gpden*(accph-accvh))
   gptem0 = gppre0/(gpden0*(accph-accvh))

   !Calculate speed of sound
   lospd = sqrt(cgamma*gppre/gpden)

   !Calculate local velocity derivatives
   !dj(ui) = dj(mi)/rho - ui/rho (dj rho)
   do idime=1,e%ndime
      logrv(idime,1:e%ndime) = logrm(idime,1:e%ndime)/gpden -&
                               lomom(idime)*logrd(1:e%ndime)/(gpden*gpden)
   end do

   !Calculate amplitude of outgoing characteristic wave (L1) in terms of interior points
   !Subsonic outgoing wave lambda_1 = (+) soundspeed (-) velocity
   !           <---|      (+n) boundary
   !           <---|----  (+c)
   !               |
   !           ----|--->  (-u1)
   L1 = (lospd-lomom(e%ndime)/gpden)*(-gpden*lospd*logrv(e%ndime,e%ndime)+logrp(e%ndime))
   !L5 in terms of L1 and velocity derivative
   L5 = L1 - 2*lospd*(lomom(e%ndime)-gpden*lomom0(e%ndime)/gpden0)*dtinv

   !Calculate vector of derivatives
   d = 0.0_rp
   d(2) = (L1+L5)/2.0_rp
   L2 = (cgamma-1.0_rp)*d(2)+ gpden*lospd*lospd*(1.0_rp-gptem0/gptem)*dtinv
   d(1) = (L2+d(2))/(lospd*lospd)

   !Density equation
   dens = 0.0_rp
   dens = dens + d(1)

   ! Compute prescribed traction
   incre(1)           = dens

end subroutine nsc_nscbc_density_ex


subroutine ray_xyz(xzt,yzt,zzt, xst,yst, alfa,iiips, hmax)
real dray(5000),hray(5000),tray(5000)
real dray1(5000),hray1(5000)
real dray2(5000),hray2(5000)

common /ray_path/npray,xray(5000),yray(5000),zray(5000)
common/pi/pi,per
common/ray/npath,dray,hray,tray
common/grid/zgrmax,dzlay,dsmin

dx=xzt-xst
dy=yzt-yst
dshor=sqrt(dx*dx+dy*dy)

if(dshor.lt.10.) then
	dist=sqrt(dshor*dshor + zzt*zzt)
	npp=dist/dsmin
	ds=dist/npp
	do i=1,npp+1
		xray(i)=xzt+(i-1)*ds*((xst-xzt)/dist)
		yray(i)=yzt+(i-1)*ds*((yst-yzt)/dist)
		zray(i)=zzt+(i-1)*ds*((0. -zzt)/dist)
	end do
	npray=npp+1
	return
end if

vzt=vrefmod(zzt,iiips)

sina=sin(alfa*per)
cosa=cos(alfa*per)

cosb=(dx)/dshor
sinb=(dy)/dshor

zst=0
rzt=6371.-zzt
px=rzt*sina/vzt

zbot=zgrmax
if(alfa.ge.90) zbot=zzt

!write(*,*)' alfa=',alfa
!write(*,*)' px=',px,' zst=',zst,' zbot=',zbot
!write(*,*)' dshor=',dshor

call halftrace(px,zst,zbot,iiips,dzlay,zgrmax, time,dist1,hmax)
if(hmax.gt.zbot) hmax=zbot
!do i=1,npath
	!write(*,*)' dray=',dray(i),' hray=',hray(i)
!end do

if(npath.eq.0) then
	dist=sqrt(dshor*dshor + zzt*zzt)
	npp=dist/dsmin
	ds=dist/npp
	do i=1,npp+1
		xray(i)=xzt+(i-1)*ds*((xst-xzt)/dist)
		yray(i)=yzt+(i-1)*ds*((yst-yzt)/dist)
		zray(i)=zzt+(i-1)*ds*((0. -zzt)/dist)
	end do
	npray=npp+1
	return
end if
!write(*,*)' dist=',dist1,' time=',time,' hmax=',hmax

np=1
dray1(np)=0
hray1(np)=zst

do ii=1,npath
	np=np+1
	dray1(np)=dray(ii)
	hray1(np)=hray(ii)
	!write(*,*)np,dray1(np),hray1(np)
end do
!pause
npath1=np
dist=dray(npath)

if(alfa.ge.90) then
	np=1
	dray(np)=0
	hray(np)=zzt
	do i=npath1,1,-1
		d1=dray1(i)
		h1=hray1(i)
		if(d1.ge.dshor) cycle
		if(abs(h1-zzt).lt.0.00001) cycle
		np=np+1
		dray(np)=dshor-d1
		hray(np)=h1
!				write(*,*)np,dray(np),hray(np)
	end do
	npath=np
	goto 991
end if

call halftrace(px,zzt,zbot,iiips,dzlay,zgrmax, time,dist2,hmax)

!write(*,*)' dist=',dist2,' time=',time,' hmax=',hmax

np=1
dray2(np)=0
hray2(np)=zzt

do ii=1,npath
	np=np+1
	dray2(np)=dray(ii)
	hray2(np)=hray(ii)
	!write(*,*)np,dray2(np),hray2(np)
end do

npath2=np

i2=1
h2=hray2(i2)
d2=dray2(i2)
hlast2=hray2(1)
do i1=1,npath1
	h1=hray1(i1)
	d1=dray1(i1)
	if(h1.gt.h2) then
		i2=i2+1
		h2=hray2(i2)
		d2=dray2(i2)
	end if
	if(d1+d2.gt.dshor) exit
end do
i2=i2-1
!write(*,*)' i2=',i2
np=0
do i=1,i2
	np=np+1
	dray(np)=dray2(i)
	hray(np)=hray2(i)
	!write(*,*)dray2(i),hray2(i)
end do
i1=i1-1
!write(*,*)' i1=',i1
do i=i1,1,-1
	np=np+1
	dray(np)=dshor-dray1(i)
	hray(np)=hray1(i)
	!write(*,*)dshor-dray1(i),hray1(i)
end do
!		do i=1,np
!			write(*,*)dray(i),hray(i)
!		end do
!		write(*,*)' dshor=',dshor
npath=np
!		pause

991		continue

np=1
dray2(1)=dray(1)
hray2(1)=hray(1)
do ii=2,npath
	d1=dray2(np)
	z1=hray2(np)
	d2=dray(ii)
	z2=hray(ii)
	ds=sqrt((d2-d1)*(d2-d1)+(z2-z1)*(z2-z1))
	if(ds.lt.dsmin.and.ii.ne.npath.and.z2.lt.zgrmax) cycle
	ns=ds/dsmin
	!write(*,*)' ds=',ds,' ns=',ns
	if(ns.eq.0)ns=1
	dz=(z2-z1)/ns
	dd=(d2-d1)/ns
	if(z1.gt.zgrmax-10.and.z2.gt.zgrmax-10.and.ns.ne.1) then
!			write(*,*)' z1=',z1,' d1=',d1
!			write(*,*)' z2=',z2,' d2=',d2,' ns=',ns
		np=np+1
		dray2(np)=d2
		hray2(np)=z2
		cycle
	end if

	do iii=1,ns
		np=np+1
		dray2(np)=d1+iii*dd
		hray2(np)=z1+iii*dz
	end do
end do
npath2=np

!write(*,*)' npath2=',npath2
!write(*,*)' ztr:',xzt,yzt,zzt
np=0
do ii=1,npath2
	!write(*,*)ii,dray2(ii),hray2(ii)
	x=xzt-dray2(ii)*cosb
	y=yzt-dray2(ii)*sinb
	z=hray2(ii)
	np=np+1
	xray(np)=x
	yray(np)=y
	zray(np)=z
	!write(*,*)x,y,z
end do
npray=np
!write(*,*)' stat:',xst,yst,zst

return
end


      subroutine tsteee(trans,cod,beam,srot,al,phib,dx,dy,theta,enarad,
     $     apsi1,apsi2,fb1,fb2,mfring,fringe,next)
      use ffs_flag
      use tmacro
      use bendib, only:rbh,rbl,tbendal
      use temw, only:tsetr0,tmulbs
      use kradlib, only:tradke
      use mathfun
      implicit none
      real*8 epslon,a3,a5,a7,a9,a11,a13,a15
      parameter (epslon=1.d-6)
      parameter (a3=1.d0/6.d0,a5=3.d0/40.d0,a7=5.d0/112.d0,
     1           a9=35.d0/1152.d0,a11=63.d0/2816.d0,
     1           a13=231.d0/13312.d0,a15=143.d0/10240.d0)
      integer*4 mfring,nrad,ndiv,n,n1,n2
      real*8 trans(6,12),cod(6),beam(42),srot(3,9),
     $     al,phib,dx,dy,theta,f1r,f2r,
     $     fb1,fb2,rhob,rbc,alc,phic,alx,alr,
     $     dxfr1,dyfr1,
     $     dxfr2,dyfr2,
     $     b,aln,phin,pr,pxi,pyi,rhoe,s,dpz1,
     $     pz1,dpx,pxf,d,w,u,spz,spx,phsq,dl,dpz2,pz2,
     $     dyfra1,dyfra2,apsi1,apsi2,tanp1,tanp2
      real*8 trans1(6,6)
      logical*4 enarad,fringe,next,prev,krad
      if(al .eq. 0.d0)then
        call tthine(trans,cod,beam,srot,2,al,-phib,dx,dy,theta,
     $       .false.)
        return
      elseif(phib .eq. 0.d0)then
        call tdrife(trans,cod,beam,srot,al,
     $         0.d0,0.d0,0.d0,0.d0,.true.,.false.,irad)
        return
      endif
      call tchge(trans,cod,beam,srot,
     $     dx,dy,theta,0.d0,0.d0,.true.)
      krad=enarad .and. al .ne. 0.d0
      if(krad)then
        call tsetr0(trans(:,1:6),cod(1:6),0.d0,0.d0)
      endif
      rhob=al/phib
      prev=bradprev .ne. 0.d0
      rbc=1.d0
      f1r=0.d0
      f2r=0.d0
      if(fb1 .ne. 0.d0 .and. (mfring .gt. 0 .or. mfring .eq. -1))then
        dxfr1=-fb1**2/rhob/24.d0
        dyfr1=fb1/rhob**2/6.d0
        if(fringe)then
          dyfra1=4.d0*dyfr1/fb1**2
        else
          dyfra1=0.d0
        endif
        call tblfre(trans,cod,beam,dxfr1,dyfr1,dyfra1)
        f1r=.5d0*fb1
        rbc=rbc-f1r/al
        n1=-1
      else
        n1=1
      endif
      if(fb2 .ne. 0.d0 .and.
     $       (mfring .gt. 0 .or. mfring .eq. -2))then
        f2r=0.5d0*fb2
        rbc=rbc-f2r/al
        n2=2
      else
        n2=0
      endif
      alc=al*rbc
      phic=phib*rbc
      if(enarad)then
        tanp1=tan(apsi1)
        tanp2=tan(apsi2)
        b=-brhoz/rhob
        nrad=int(abs(alc/epsrad*crad*(h0*b)**2))
        ndiv=1+max(int(nrad*emidiv*emidib),
     1       int(abs(phib*h0*anrad)/epsrad/1.d6*emidiv*emidib))
c        write(*,*)'tsteee ',ndiv,nrad,phib,b,epsrad,crad
      else
c     begin initialize for preventing compiler warning
        tanp1=0.d0
        tanp2=0.d0
c     end   initialize for preventing compiler warning
        ndiv=1
      endif
      aln=alc/ndiv
      phin=phic/ndiv
      if(mfring .ne. -2)then
c        call tbfrie(trans,cod,beam,-rhob,0.d0,.true.)
        call tbedge(trans,cod,beam,al,-phib,apsi1,.true.)
      endif
      call tinitr(trans1)
      do 100 n=n1,ndiv+n2
        call tbendal(n,ndiv,f1r,f2r,aln,alx,alr)
        pr=1.d0+cod(6)
        pxi=cod(2)/pr
        pyi=cod(4)/pr
        rhoe=rhob*pr
        s=pxi**2+pyi**2
        dpz1=-s/(1.d0+sqrtl(1.d0-s))
        pz1=1.d0+dpz1
        dpx=alx/rhoe
        pxf=pxi+dpx
        s=pxf**2+pyi**2
        dpz2=-s/(1.d0+sqrtl(1.d0-s))
        pz2=1.d0+dpz2
        phsq=pxi**2+pz1**2
        d=pxf*pz1+pxi*pz2
        if(d .eq. 0.d0 .or. pxi*pxf .lt. 0.d0)then
          u=(dpx+pxf*dpz1-pxi*dpz2)/phsq
          w=(pxf*dpz1-pxi*dpz2+dpx*pyi**2)/phsq
        else
          u=dpx*(pxf+pxi)/d
          w=-dpx*(pxf*dpz1+pxi*dpz2)/d
        endif
        s=u**2
        if(s .gt. 2.d-2)then
          dl=rhoe*((asin(u)/u-1.d0)*u+w)
        else
          if(s .gt. 2.d-4)then
            dl=rhoe*(s*(a3+s*(a5+s*(a7+s*(a9+s*(a11+s*(a13+s*a15))))))
     $           *u+w)
          else
            dl=rhoe*(s*(a3+s*(a5+s*(a7+s*a9)))*u+w)
          endif
        endif
        spz=pz2+pz1
        spx=pxf+pxi
        phsq=(1.d0-pyi)*(1.d0+pyi)
        trans1(1,2)=(2.d0*spz+spx*(pxf/pz2+pxi/pz1))/spz**2*alx/pr
        trans1(1,6)=-spx/pz1/pz2/spz*alx/pr
        trans1(1,4)=-pyi*trans1(1,6)
        trans1(5,2)=trans1(1,6)
        trans1(5,6)= rhob*u/pz1/pz2
        trans1(5,4)=-pyi*trans1(5,6)
        trans1(3,2)=trans1(1,4)
        trans1(3,4)=(alx+dl)/pr-pyi*trans1(5,4)
        trans1(3,6)=trans1(5,4)
        trans1(5,6)=trans1(5,6)-(alx+dl)/pr+h0/h1emit**3*alx
        call tmultr5(trans,trans1,irad)
        call tmulbs(beam ,trans1,.true.)
        cod(1)=cod(1)+alx*spx/spz
        cod(2)=pxf*pr
        cod(3)=cod(3)+pyi*(alx+dl)
        cod(5)=cod(5)-(dl+dvemit*alx)
        if(n .ne. ndiv+n2 .and. krad)then
          call tradke(trans,cod,beam,srot,alr,0.d0,0.d0)
        endif
100   continue
      if(.not. next)then
        bradprev=0.d0
      endif
      if(mfring .ne. -1)then
c        call tbfrie(trans,cod,beam, rhob,0.d0,.false.)
        call tbedge(trans,cod,beam,al,-phib,apsi2,.false.)
      endif
      if(f2r .ne. 0.d0)then
        dxfr2=fb2**2/rhob/24.d0
        dyfr2=fb2/rhob**2/6.d0
        if(fringe)then
          dyfra2=4.d0*dyfr2/fb2**2
        else
          dyfra2=0.d0
        endif
        call tblfre(trans,cod,beam,dxfr2,dyfr2,dyfra2)
      endif
      if(krad)then
        call tradke(trans,cod,beam,srot,alr,0.d0,0.d0)
      endif
      call tchge(trans,cod,beam,srot,
     $     -dx,-dy,-theta,0.d0,0.d0,.false.)
      return
      end


c ----------------------------------------------------------------------

c     this subroutine computes the local energy and potential energy
c     of a configuration.

c ----------------------------------------------------------------------

      subroutine local(q, vloc)

      implicit double precision (a-h, o-z)

      include 'sizes.h'

      include 'qsats.h'

      common /bincom/ bin, binvrs, r2min

c --- alpha is the exponential parameter in psi:

c     psi = N * exp(-alpha*(r-r0)**2) * Jastrow

c --- bb is the exponential parameter in Jastrow:

c     ln Jastrow(ij) = -0.5 * (bb/rij)**5

      dimension q(NATOM3), dlng(NATOM3), d2lng(NATOM3)

      do i=1, NATOM3
         dlng(i)=0.0d0
         d2lng(i)=0.0d0
      end do

      do i=1, NATOMS

         xx=q(3*i-2)
         yy=q(3*i-1)
         zz=q(3*i)

         dlng(3*i-2)=dlng(3*i-2)-2.0d0*aa*xx
         dlng(3*i-1)=dlng(3*i-1)-2.0d0*aa*yy
         dlng(3*i)  =dlng(3*i)  -2.0d0*aa*zz

         d2lng(3*i-2)=d2lng(3*i-2)-2.0d0*aa
         d2lng(3*i-1)=d2lng(3*i-1)-2.0d0*aa
         d2lng(3*i)  =d2lng(3*i)  -2.0d0*aa

      end do

c --- loop over all interacting pairs.

      vloc=0.0d0
      tloc=0.0d0

      do n=1, nvpair

         i=ivpair(1, n)
         j=ivpair(2, n)

         dx=-((q(3*j-2))+vpvec(1, n)+(-q(3*i-2)))
         dy=-((q(3*j-1))+vpvec(2, n)+(-q(3*i-1)))
         dz=-((q(3*j))  +vpvec(3, n)+(-q(3*i))  )

         r2=dx*dx+dy*dy+dz*dz

         ibin=int((r2-r2min)*binvrs)+1

         if (ibin.gt.0) then
            dr=(r2-r2min)-bin*dble(ibin-1)
            vloc=vloc+v(1, ibin)+v(2, ibin)*dr
         else
            vloc=vloc+v(1, 1)
         end if

         br2=bb*bb/r2

         br5=br2*br2*sqrt(br2)

         br52=br5/r2

         dlng(3*i-2)=dlng(3*i-2)+2.5d0*br52*dx
         dlng(3*i-1)=dlng(3*i-1)+2.5d0*br52*dy
         dlng(3*i)  =dlng(3*i)  +2.5d0*br52*dz

         d2lng(3*i-2)=d2lng(3*i-2)+2.5d0*br52*
     *                             (1.0d0-7.0d0*dx**2/r2)
         d2lng(3*i-1)=d2lng(3*i-1)+2.5d0*br52*
     *                             (1.0d0-7.0d0*dy**2/r2)
         d2lng(3*i)  =d2lng(3*i)  +2.5d0*br52*
     *                             (1.0d0-7.0d0*dz**2/r2)

      end do

c --- now sum up the kinetic energy components.

      do i=1, NATOM3
         tloc=tloc+d2lng(i)+dlng(i)**2
      end do

c --- account for mass factor and for double-counting of pairs.

      tloc=-0.5d0*tloc/amass
      vloc=0.5d0*vloc

      return
      end


      SUBROUTINE SB10TD( N, M, NP, NCON, NMEAS, D, LDD, TU, LDTU, TY,
     $                   LDTY, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK,
     $                   RCOND, TOL, IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrices of the H2 optimal discrete-time controller
C
C              | AK | BK |
C          K = |----|----|,
C              | CK | DK |
C
C     from the matrices of the controller for the normalized system,
C     as determined by the SLICOT Library routine SB10SD.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     NCON    (input) INTEGER
C             The number of control inputs (M2).  M >= NCON >= 0.
C             NP-NMEAS >= NCON.
C
C     NMEAS   (input) INTEGER
C             The number of measurements (NP2).  NP >= NMEAS >= 0.
C             M-NCON >= NMEAS.
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading NP-by-M part of this array must contain the
C             system input/output matrix D. Only the trailing
C             NMEAS-by-NCON submatrix D22 is used.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     TU      (input) DOUBLE PRECISION array, dimension (LDTU,M2)
C             The leading M2-by-M2 part of this array must contain the
C             control transformation matrix TU, as obtained by the
C             SLICOT Library routine SB10PD.
C
C     LDTU    INTEGER
C             The leading dimension of the array TU.  LDTU >= max(1,M2).
C
C     TY      (input) DOUBLE PRECISION array, dimension (LDTY,NP2)
C             The leading NP2-by-NP2 part of this array must contain the
C             measurement transformation matrix TY, as obtained by the
C             SLICOT Library routine SB10PD.
C
C     LDTY    INTEGER
C             The leading dimension of the array TY.
C             LDTY >= max(1,NP2).
C
C     AK      (input/output) DOUBLE PRECISION array, dimension (LDAK,N)
C             On entry, the leading N-by-N part of this array must
C             contain controller state matrix for the normalized system
C             as obtained by the SLICOT Library routine SB10SD.
C             On exit, the leading N-by-N part of this array contains
C             controller state matrix AK.
C
C     LDAK    INTEGER
C             The leading dimension of the array AK.  LDAK >= max(1,N).
C
C     BK      (input/output) DOUBLE PRECISION array, dimension
C             (LDBK,NMEAS)
C             On entry, the leading N-by-NMEAS part of this array must
C             contain controller input matrix for the normalized system
C             as obtained by the SLICOT Library routine SB10SD.
C             On exit, the leading N-by-NMEAS part of this array
C             contains controller input matrix BK.
C
C     LDBK    INTEGER
C             The leading dimension of the array BK.  LDBK >= max(1,N).
C
C     CK      (input/output) DOUBLE PRECISION array, dimension (LDCK,N)
C             On entry, the leading NCON-by-N part of this array must
C             contain controller output matrix for the normalized
C             system as obtained by the SLICOT Library routine SB10SD.
C             On exit, the leading NCON-by-N part of this array contains
C             controller output matrix CK.
C
C     LDCK    INTEGER
C             The leading dimension of the array CK.
C             LDCK >= max(1,NCON).
C
C     DK      (input/output) DOUBLE PRECISION array, dimension
C             (LDDK,NMEAS)
C             On entry, the leading NCON-by-NMEAS part of this array
C             must contain controller matrix DK for the normalized
C             system as obtained by the SLICOT Library routine SB10SD.
C             On exit, the leading NCON-by-NMEAS part of this array
C             contains controller input/output matrix DK.
C
C     LDDK    INTEGER
C             The leading dimension of the array DK.
C             LDDK >= max(1,NCON).
C
C     RCOND   (output) DOUBLE PRECISION
C             RCOND contains an estimate of the reciprocal condition
C             number of the matrix Im2 + DKHAT*D22 which must be
C             inverted in the computation of the controller.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             Tolerance used in determining the nonsingularity of the
C             matrix which must be inverted. If TOL <= 0, then a default
C             value equal to sqrt(EPS) is used, where EPS is the
C             relative machine precision.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*M2)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= max(N*M2,N*NP2,M2*NP2,M2*M2+4*M2).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if the matrix Im2 + DKHAT*D22 is singular, or the
C                   estimated condition number is larger than or equal
C                   to 1/TOL.
C
C     METHOD
C
C     The routine implements the formulas given in [1].
C
C     REFERENCES
C
C     [1] Zhou, K., Doyle, J.C., and Glover, K.
C         Robust and Optimal Control.
C         Prentice-Hall, Upper Saddle River, NJ, 1996.
C
C     [2] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
C         Fortran 77 routines for Hinf and H2 design of linear
C         discrete-time control systems.
C         Report 99-8, Department of Engineering, Leicester University,
C         April 1999.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of the result depends on the condition numbers of the
C     input and output transformations and of the matrix Im2 +
C     DKHAT*D22.
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, April 1999.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 1999,
C     Jan. 2000.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, H2 optimal control, LQG, LQR, optimal
C     regulator, robust control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER            INFO, LDAK, LDBK, LDCK, LDD, LDDK, LDTU, LDTY,
     $                   LDWORK, M, N, NCON, NMEAS, NP
      DOUBLE PRECISION   RCOND, TOL
C     ..
C     .. Array Arguments ..
      INTEGER            IWORK( * )
      DOUBLE PRECISION   AK( LDAK, * ), BK( LDBK, * ), CK( LDCK, * ),
     $                   D( LDD, * ), DK( LDDK, * ), DWORK( * ),
     $                   TU( LDTU, * ), TY( LDTY, * )
C     ..
C     .. Local Scalars ..
      INTEGER            INFO2, IWRK, M1, M2, MINWRK, NP1, NP2
      DOUBLE PRECISION   ANORM, TOLL
C     ..
C     .. External Functions
      DOUBLE PRECISION   DLAMCH, DLANGE
      EXTERNAL           DLAMCH, DLANGE
C     ..
C     .. External Subroutines ..
      EXTERNAL           DGECON, DGEMM, DGETRF, DGETRS, DLACPY, DLASET,
     $                   XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          MAX, SQRT
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      M1  = M - NCON
      M2  = NCON
      NP1 = NP - NMEAS
      NP2 = NMEAS
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
         INFO = -4
      ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
         INFO = -5
      ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
         INFO = -7
      ELSE IF( LDTU.LT.MAX( 1, M2 ) ) THEN
         INFO = -9
      ELSE IF( LDTY.LT.MAX( 1, NP2 ) ) THEN
         INFO = -11
      ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
         INFO = -13
      ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
         INFO = -15
      ELSE IF( LDCK.LT.MAX( 1, M2 ) ) THEN
         INFO = -17
      ELSE IF( LDDK.LT.MAX( 1, M2 ) ) THEN
         INFO = -19
      ELSE
C
C        Compute workspace.
C
         MINWRK = MAX ( N*M2, N*NP2, M2*NP2, M2*( M2 + 4 ) )
         IF( LDWORK.LT.MINWRK )
     $      INFO = -24
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10TD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
     $    .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
         RCOND = ONE
         RETURN
      END IF
C
      TOLL = TOL
      IF( TOLL.LE.ZERO ) THEN
C
C        Set the default value of the tolerance for nonsingularity test.
C
         TOLL = SQRT( DLAMCH( 'Epsilon' )  )
      END IF
C
C     Find BKHAT .
C
      CALL DGEMM( 'N', 'N', N, NP2, NP2, ONE, BK, LDBK, TY, LDTY, ZERO,
     $            DWORK, N )
      CALL DLACPY ('Full', N, NP2, DWORK, N, BK, LDBK )
C
C     Find CKHAT .
C
      CALL DGEMM( 'N', 'N', M2, N, M2, ONE, TU, LDTU, CK, LDCK, ZERO,
     $            DWORK, M2 )
      CALL DLACPY ('Full', M2, N, DWORK, M2, CK, LDCK )
C
C     Compute DKHAT .
C
      CALL DGEMM( 'N', 'N', M2, NP2, M2, ONE, TU, LDTU, DK, LDDK, ZERO,
     $            DWORK, M2 )
      CALL DGEMM( 'N', 'N', M2, NP2, NP2, ONE, DWORK, M2, TY, LDTY,
     $            ZERO, DK, LDDK )
C
C     Compute Im2 + DKHAT*D22 .
C
      IWRK = M2*M2 + 1
      CALL DLASET( 'Full', M2, M2, ZERO, ONE, DWORK, M2 )
      CALL DGEMM( 'N', 'N', M2, M2, NP2, ONE, DK, LDDK,
     $            D( NP1+1, M1+1 ), LDD, ONE, DWORK, M2 )
      ANORM = DLANGE( '1', M2, M2, DWORK, M2, DWORK( IWRK ) )
      CALL DGETRF( M2, M2, DWORK, M2, IWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = 1
         RETURN
      END IF
      CALL DGECON( '1', M2, DWORK, M2, ANORM, RCOND, DWORK( IWRK ),
     $             IWORK( M2+1 ), INFO2 )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND.LT.TOLL ) THEN
         INFO = 1
         RETURN
      END IF
C
C     Compute CK .
C
      CALL DGETRS( 'N', M2, N, DWORK, M2, IWORK, CK, LDCK, INFO2 )
C
C     Compute DK .
C
      CALL DGETRS( 'N', M2, NP2, DWORK, M2, IWORK, DK, LDDK, INFO2 )
C
C     Compute AK .
C
      CALL DGEMM( 'N', 'N', N, M2, NP2, ONE, BK, LDBK, D( NP1+1, M1+1 ),
     $            LDD, ZERO, DWORK, N )
      CALL DGEMM( 'N', 'N', N, N, M2, -ONE, DWORK, N, CK, LDCK, ONE, AK,
     $            LDAK )
C
C     Compute BK .
C
      CALL DGEMM( 'N', 'N', N, NP2, M2, -ONE, DWORK, N, DK, LDDK,
     $            ONE, BK, LDBK )
      RETURN
C *** Last line of SB10TD ***
      END


!
!   VnVFRRetJ.f90
!
!
!
!   Created by Kurt Mitman on 14/10/14.
!

DOUBLE PRECISION FUNCTION FnVFRRetJ(bsav)

USE Params
USE Globals
USE funcs
USE Procedures

IMPLICIT NONE

DOUBLE PRECISION, INTENT(IN) :: bsav
DOUBLE PRECISION             :: cons
DOUBLE PRECISION             :: vals(2)
INTEGER                      :: inds(2)

cons=gliq-FnQb(bsav,0.0d0)*bsav

if(cons .LT. cmin) then
    FnVFRRetJ = 1.d10+10.0d0*(cmin-cons)

else
    FnVFRRetJ = -1.0d0*(urent(cons,GridHR(gih),gij)+ubequest(bsav))
endif

END FUNCTION FnVFRRetJ


MODULE RiemannProblemInitializationModule

  USE KindModule, ONLY: &
    DP
  USE ProgramHeaderModule, ONLY: &
    ProgramName, &
    nX, nNodesX
  USE UtilitiesModule, ONLY: &
    NodeNumberX
  USE MeshModule, ONLY: &
    MeshX, &
    NodeCoordinate
  USE GeometryFieldsModule, ONLY: &
    uGF, nGF
  USE FluidFieldsModule, ONLY: &
    uCF, nCF, &
    uPF, nPF, iPF_D, iPF_V1, iPF_V2, iPF_V3, iPF_E, iPF_Ne, &
    uAF, nAF, iAF_P, iAF_T, iAF_S, iAF_Ye, iAF_E, iAF_Gm, iAF_Cs
  USE EquationOfStateModule, ONLY: &
    ComputeInternalEnergyDensityFromPressure, &
    ComputeAuxiliary_Fluid
  USE EulerEquationsUtilitiesModule_GR, ONLY: &
    ComputeConserved

  IMPLICIT NONE
  PRIVATE

  PUBLIC :: InitializeRiemannProblem1D

CONTAINS

  SUBROUTINE InitializeRiemannProblem1D &
               ( D_L, V_L, P_L, D_R, V_R, P_R, X_D_Option )

    REAL(DP),               INTENT(in) :: D_L, P_L, D_R, P_R
    REAL(DP), DIMENSION(3), INTENT(in) :: V_L, V_R
    REAL(DP),               INTENT(in), OPTIONAL :: X_D_Option

    INTEGER  :: iX1, iX2, iX3
    INTEGER  :: iNodeX1, iNodeX2, iNodeX3, iNode
    REAL(DP) :: X_D, X1

    X_D = 0.5_DP
    IF( PRESENT( X_D_Option ) ) &
      X_D = X_D_Option

    WRITE(*,*)
    WRITE(*,'(A2,A6,A)') &
      '', 'INFO: ', TRIM( ProgramName )
    WRITE(*,*)
    WRITE(*,'(A7,A6,ES10.3E2)') &
      '', 'X_D = ', X_D
    WRITE(*,*)
    WRITE(*,'(A7,A6,ES10.3E2,A24,A6,ES10.3E2)') &
      '', 'D_L = ', D_L, '', 'D_R = ', D_R
    WRITE(*,'(A7,A6,3ES10.3E2,A4,A6,3ES10.3E2)') &
      '', 'V_L = ', V_L, '', 'V_R = ', V_R
    WRITE(*,'(A7,A6,ES10.3E2,A24,A6,ES10.3E2)') &
      '', 'P_L = ', P_L, '', 'P_R = ', P_R

    DO iX3 = 1, nX(3)
      DO iX2 = 1, nX(2)
        DO iX1 = 1, nX(1)

          DO iNodeX3 = 1, nNodesX(3)
            DO iNodeX2 = 1, nNodesX(2)
              DO iNodeX1 = 1, nNodesX(1)

                X1 = NodeCoordinate( MeshX(1), iX1, iNodeX1 )

                iNode = NodeNumberX( iNodeX1, iNodeX2, iNodeX3 )

                IF( X1 <= X_D )THEN

                  ! -- Left State --

                  uPF(iNode,iX1,iX2,iX3,iPF_D)  = D_L
                  uPF(iNode,iX1,iX2,iX3,iPF_V1) = V_L(1)
                  uPF(iNode,iX1,iX2,iX3,iPF_V2) = V_L(2)
                  uPF(iNode,iX1,iX2,iX3,iPF_V3) = V_L(3)
                  uAF(iNode,iX1,iX2,iX3,iAF_P)  = P_L

                ELSE

                  ! -- Right State --

                  uPF(iNode,iX1,iX2,iX3,iPF_D)  = D_R
                  uPF(iNode,iX1,iX2,iX3,iPF_V1) = V_R(1)
                  uPF(iNode,iX1,iX2,iX3,iPF_V2) = V_R(2)
                  uPF(iNode,iX1,iX2,iX3,iPF_V3) = V_R(3)
                  uAF(iNode,iX1,iX2,iX3,iAF_P)  = P_R

                END IF

              END DO
            END DO
          END DO

          CALL ComputeInternalEnergyDensityFromPressure &
                 ( uPF(:,iX1,iX2,iX3,iPF_D),  uAF(:,iX1,iX2,iX3,iAF_P), &
                   uAF(:,iX1,iX2,iX3,iAF_Ye), uPF(:,iX1,iX2,iX3,iPF_E) )

          CALL ComputeAuxiliary_Fluid &
                 ( uPF(:,iX1,iX2,iX3,iPF_D ), uPF(:,iX1,iX2,iX3,iPF_E ), &
                   uPF(:,iX1,iX2,iX3,iPF_Ne), uAF(:,iX1,iX2,iX3,iAF_P ), &
                   uAF(:,iX1,iX2,iX3,iAF_T ), uAF(:,iX1,iX2,iX3,iAF_Ye), &
                   uAF(:,iX1,iX2,iX3,iAF_S ), uAF(:,iX1,iX2,iX3,iAF_E ), &
                   uAF(:,iX1,iX2,iX3,iAF_Gm), uAF(:,iX1,iX2,iX3,iAF_Cs) )

          CALL ComputeConserved &
                 ( uPF(:,iX1,iX2,iX3,1:nPF), uAF(:,iX1,iX2,iX3,1:nAF), &
                   uGF(:,iX1,iX2,iX3,1:nGF), uCF(:,iX1,iX2,iX3,1:nCF) )

        END DO
      END DO
    END DO

  END SUBROUTINE InitializeRiemannProblem1D

END MODULE RiemannProblemInitializationModule


SUBROUTINE CUNGTR_F95( A, TAU, UPLO, INFO )
!
!  -- LAPACK95 interface driver routine (version 3.0) --
!     UNI-C, Denmark; Univ. of Tennessee, USA; NAG Ltd., UK
!     September, 2000
!
!  .. USE STATEMENTS ..
   USE LA_PRECISION, ONLY: WP => SP
   USE LA_AUXMOD, ONLY: ERINFO, LSAME
   USE F77_LAPACK, ONLY: UNGTR_F77 => LA_UNGTR, ILAENV_F77 => LA_ILAENV
!  .. IMPLICIT STATEMENT ..
   IMPLICIT NONE
!  .. CHARACTER ARGUMENTS ..
   CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: UPLO
!  .. SCALAR ARGUMENTS ..
   INTEGER, INTENT(OUT), OPTIONAL :: INFO
!  .. ARRAY ARGUMENTS ..
   COMPLEX(WP), INTENT(IN) :: TAU(:)
   COMPLEX(WP), INTENT(INOUT) :: A(:,:)
!-----------------------------------------------------------------
!
! Purpose
! =======
!
! LA_ORGTR / LA_UNGTR generates a real orthogonal / complex unitary
! matrix Q which is defined as the product of elementary reflectors,
! as returned by LA_SYTRD / LA_HETRD:
!
! if UPLO = 'U', Q = H(n-1) . . . H(2) H(1),
!
! if UPLO = 'L', Q = H(1) H(2) . . . H(n-1).
!
! =======
!
!    SUBROUTINE LA_ORGTR / LA_UNGTR( A, TAU, UPLO, INFO )
!    .. Scalar Arguments ..
!       CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: UPLO
!       INTEGER, INTENT(OUT), OPTIONAL :: INFO
!    .. Array Arguments ..
!       <type>(<wp>), INTENT(IN) :: TAU(:)
!       <type>(<wp>), INTENT(INOUT) :: A(:,:)
!    where
!       <type> ::= REAL | COMPLEX
!       <wp>   ::= KIND(1.0) | KIND(1.0D0)
!
! Defaults
! ========
!
! 1. If UPLO is not present then UPLO = 'U' is assumed.
!
! Arguments
! =========
!
! A       (input/output) either REAL or COMPLEX square array,
!         shape (:,:), size(A,1) == size(A,2) >= 0.
!         On entry, the vectors which define the elementary
!            reflectors, as returned by LA_SYTRD or LA_HETRD.
!         On exit the orthogonal or unitary matrix Q.
!
! TAU     (input) either REAL or COMPLEX array,
!         shape (:), size(TAU) == size(A,1)-1.
!         TAU(i) must contain the scalar factor of the elementary
!         reflector H(i), as returned by LA_SYTRD or LA_HETRD.
!
! UPLO    Optional, (input) CHARACTER*1
!         If UPLO is present then:
!            = 'U':  Upper triangle of A is stored
!            = 'L':  Lower triangle of A is stored
!         otherwise UPLO = 'U' is assumed.
!
! INFO    Optional, (output) INTEGER
!         If INFO is present:
!            = 0: successful exit
!            < 0: if INFO = -i, the i-th argument had an illegal value
!         If INFO is not present and an error occurs, then the program
!            is terminated with an error message.
!
! ----------------------------------
!  .. LOCAL PARAMETERS ..
   CHARACTER(LEN=8), PARAMETER :: SRNAME = 'LA_UNGTR'
   CHARACTER(LEN=5), PARAMETER :: BSNAM  = 'CUNGQ'
!  .. LOCAL SCALARS ..
   CHARACTER(LEN=1) :: LUPLO
   CHARACTER(LEN=6) :: BSNAME
   INTEGER :: LINFO, LWORK, NB, ISTAT, ISTAT1, N, LD
!  .. LOCAL ARRAYS ..
   COMPLEX(WP), POINTER :: WORK(:)
!  .. INTRINSIC FUNCTIONS ..
   INTRINSIC MAX, PRESENT, SIZE
!  .. EXECUTABLE STATEMENTS ..
   LINFO = 0; N = SIZE(A,1); LD = MAX(1,N); ISTAT = 0
   IF( PRESENT(UPLO) ) THEN; LUPLO = UPLO; ELSE; LUPLO = 'U'; END IF
!  .. TEST THE ARGUMENTS
   IF( SIZE( A, 2 ) /= N .OR. N < 0 )THEN; LINFO = -1
   ELSE IF( SIZE( TAU ) /= N-1 )THEN; LINFO = -2
   ELSE IF( .NOT.LSAME(LUPLO,'U') .AND. .NOT.LSAME(LUPLO,'L') )THEN; LINFO = -3
   ELSE IF( N > 0 )THEN
!     .. DETERMINE THE WORKSPACE
      IF( LSAME(LUPLO,'U') )THEN; BSNAME = BSNAM // 'L'
      ELSE; BSNAME = BSNAM // 'R'; ENDIF
      NB = ILAENV_F77( 1, BSNAME, ' ', N-1, N-1, N-1, -1 )
      IF( NB < 1 .OR. NB >= N ) NB = 1
      LWORK = MAX( 1, (N-1)*NB ); ALLOCATE(WORK(LWORK), STAT=ISTAT)
      IF( ISTAT /= 0 )THEN; DEALLOCATE(WORK, STAT=ISTAT)
         LWORK = MAX( 1, N-1 ); ALLOCATE(WORK(LWORK), STAT=ISTAT)
         IF( ISTAT == 0 ) CALL ERINFO( -200, SRNAME, LINFO )
      ENDIF
      IF( ISTAT == 0 )THEN
!     .. CALL LAPACK77 ROUTINE
         CALL UNGTR_F77( LUPLO, N, A, LD, TAU, WORK, LWORK, LINFO )
      ELSE; LINFO = -100; ENDIF
      DEALLOCATE(WORK, STAT=ISTAT1)
   ENDIF
   CALL ERINFO(LINFO, SRNAME, INFO, ISTAT)
END SUBROUTINE CUNGTR_F95


C FILE: FINEGRID.F
      SUBROUTINE FINEGRID(dRf,drC,dRfsize,FineGridFact,dZZf,
     &                    MapIndex,MapFact,CellIndex)
C
C     CALCULATE THE NEW GRID
C
      INTEGER FineGridFact,kkinit,Nr, dRfsize
      REAL*4 dZZf(dRfsize*FineGridFact)
      REAL*4 dRf(dRfsize)
      REAL*4 drC(dRfsize+1)
      REAL*4 Zf(dRfsize+1)
      REAL*4 Zc(dRfsize)
      REAL*4 ZZc(dRfsize*FineGridFact)
      REAL*4 ZZf(dRfsize*FineGridFact+1)
      INTEGER MapIndex(dRfsize*FineGridFact)
      INTEGER CellIndex(dRfsize*FineGridFact)
      REAL*4 MapFact(dRfsize*FineGridFact)
      INTEGER NZZ
Cf2py intent(in) dRf,dRfsize,FineGridFact
Cf2py intent(out) dZZf, MapIndex,MapFact
Cf2py intent(out) CellIndex
Cf2py depend(dRfsize*FineGridFact) dZZf
Cf2py depend(dRfsize*FineGridFact) MapFact
Cf2py depend(dRfsize*FineGridFact) CellIndex
      kkinit = 1.
      Nr=dRfsize
      DO k=1,Nr
        DO kk=kkinit,kkinit+FineGridFact-1
          dZZf(kk) = dRf(k) / FineGridFact
        ENDDO
        kkinit = kkinit + FineGridFact
      ENDDO

C     find the depths
      Zf(1) = 0.
      Zc(1) = drC(1)
      DO k=2,Nr
        Zf(k) = Zf(k-1) + drF(k-1)
        Zc(k) = Zc(k-1) + drC(k)
      ENDDO
      Zf(Nr+1) = Zf(Nr) + drF(Nr)

      NZZ = dRfsize*FineGridFact

      ZZf(1) = 0.
      ZZc(1) = 0.5  * dZZf(1)

      DO kk=2,NZZ+1
            ZZf(kk) = ZZf(kk-1) + dZZf(kk-1)
            ZZc(kk-1) = 0.5 * (ZZf(kk) + ZZf(kk-1))
      ENDDO

C     create the interpolating mapping arrays
      k = 1
      DO kk=1,NZZ
C       see if ZZc point is less than the top Zc point
        IF ( ZZc(kk) .LT. Zc(1) ) THEN
          MapIndex(kk) = 1
          MapFact(kk) = 1.
C       see if ZZc point is greater than the bottom Zc point
        ELSE IF ( (ZZc(kk) .GE. Zc(Nr)) .OR. (k .EQ. Nr) ) THEN
          MapIndex(kk) = Nr - 1
          MapFact(kk) = 0.
C       Otherwise we are somewhere in between and need to do interpolation)
        ELSE IF ( (ZZc(kk) .GE. Zc(k))
     &   .AND. (ZZc(kk) .LT. Zc(Nr)) ) THEN
C         Find the proper k value
          DO WHILE (ZZc(kk) .GE. Zc(k+1))
            k = k + 1
          ENDDO
C         If the loop stopped, that means Zc(k) <= ZZc(kk) < ZZc(k+1)
          MapIndex(kk) = k
          MapFact(kk) = 1.0  - (( ZZc(kk) - Zc(k) ) / drC(k+1))
        ELSE
C       This means there was a problem
C          WRITE(msgBuf,'(A,I4,A,I4,A,1E14.6,A,2E14.6)')
C     &     'S/R LAYERS_INIT_FIXED: kk=', kk, ', k=', k,
C     &     ', ZZc(kk)=', ZZc(kk),' , Zc(k)=',Zc(k)
C          CALL PRINT_ERROR( msgBuf, myThid )
C          STOP 'ABNORMAL END: S/R LAYERS_INIT_FIXED'
        ENDIF
        IF ( ZZc(kk).LT.Zf(MapIndex(kk)+1) ) THEN
          CellIndex(kk) = MapIndex(kk)
        ELSE
          CellIndex(kk) = MapIndex(kk)+1
        ENDIF
      ENDDO

      END
C END FILE FINEGRID.F


module input_m

	use globvar,		only: ntotal,nvirt,ntotal_loc,nhalo_loc,nvirt_loc,parts,scale_k,maxnloc
	use globvar_para,	only: procid,numprocs,bounds_glob
	use mpi
	use param,			only: dim,irho,dxo,f,hsml,mp,np,op,pp
	use error_msg_m,	only: error_msg
	use output_m,		only: write_ini_config

	public:: input,virt_part

contains

	!==============================================================================================================================
	subroutine input(generate)
	! Generates initial physical particle configuration.
	! 2 cases: return only number of particles retrieved, or generating the particles

		implicit none
		logical,intent(in):: generate
		integer:: i,j,k,d,n,n_loc,n_loc_i,n_start,n_done
		real(f):: xi,yi
		real(f),parameter:: dx = dxo,dy = dxo, xl = 25d0, yl = 25d0

		select case (generate)

			case (.false.)

				ntotal = mp*np

			case (.true.)

				! how many particles to generate per process
				n_loc_i = ceiling(dble(ntotal)/numprocs)
				if (procid.eq.numprocs-1) then
					n_loc = ntotal - (numprocs-1)*n_loc_i
				else
					n_loc = n_loc_i
				end if
				n_start = procid*n_loc_i + 1
				n_done = n_start + n_loc_i - 1

				! stopping program if array bounds are exceeded
				if ( (procid.eq.0) .and. (n_loc.gt.maxnloc) ) call error_msg(1,1)

				! intitial setup
				n = 0
				ntotal_loc = 0
				do i = 1,mp
					do j = 1,np
						n = n + 1 ! tracking total number of particles generated
						! Only generating particles assigned to process
						if ( (n.ge.n_start) .and. (n.le.n_done) ) then
							ntotal_loc = ntotal_loc + 1
							parts(ntotal_loc)%indglob = n
							parts(ntotal_loc)%indloc = ntotal_loc
							parts(ntotal_loc)%x(1) = (i-0.5d0)*dx
							parts(ntotal_loc)%x(2) = (j-0.5d0)*dy
							parts(ntotal_loc)%vx(:) = 0d0
							parts(ntotal_loc)%itype = 1
							parts(ntotal_loc)%rho = irho
							parts(ntotal_loc)%p = 0d0
						end if
					end do
				end do

				call write_ini_config

		end select

	end subroutine input

	!==============================================================================================================================
	subroutine virt_part(generate)
	! Generates the virtual particle configuration. Can change over time or remain static
	! 2 cases: return only number of particles retrieved, or generating the particles

		implicit none
		integer:: i,k,d,n
		real(f):: xi,yi,xmin_loc,ymin_loc,xmax_loc,ymax_loc
		logical,intent(in):: generate
		real(f),parameter:: dx = dxo, dy = dxo, xmin = 0d0, ymin = 0d0, xmax = 75d0, ymax = 40d0

		select case (generate)

			case (.false.)

				nvirt = 2*op + 2*pp

			case (.true.)

				xmin_loc = bounds_glob(1,procid+1) - scale_k*hsml
				ymin_loc = bounds_glob(2,procid+1) - scale_k*hsml
				xmax_loc = bounds_glob(3,procid+1) + scale_k*hsml
				ymax_loc = bounds_glob(4,procid+1) + scale_k*hsml

				nvirt_loc = 0
				n = ntotal ! counter used to track particle indices

				! lower boundary
				do i = 1,op
					n = n + 1
					xi = xmin + (i-0.5d0)*dx
					yi = ymin - 0.5d0*dy
					if ( (xi.ge.xmin_loc) .and. (yi.ge.ymin_loc) .and. (xi.le.xmax_loc) .and. (yi.le.ymax_loc) ) then
						nvirt_loc = nvirt_loc + 1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indglob = n
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indloc = ntotal_loc+nhalo_loc+nvirt_loc
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%itype = -1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(1) = xi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(2) = yi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%vx(:) = 0d0
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%rho = irho
					end if
				end do

				! upper boundary
				do i = 1,op
					n = n + 1
					xi = xmin + (i-0.5d0)*dx
					yi = ymax - 1.5d0*dy
					if ( (xi.ge.xmin_loc) .and. (yi.ge.ymin_loc) .and. (xi.le.xmax_loc) .and. (yi.le.ymax_loc) ) then
						nvirt_loc = nvirt_loc + 1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indglob = n
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indloc = ntotal_loc+nhalo_loc+nvirt_loc
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%itype = -1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(1) = xi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(2) = yi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%vx(:) = 0d0
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%rho = irho
					end if
				end do

				! left boundary
				do i = 1,pp
					n = n + 1
					xi = xmin - 0.5d0*dx
					yi = ymin + (i-1.5d0)*dy
					if ( (xi.ge.xmin_loc) .and. (yi.ge.ymin_loc) .and. (xi.le.xmax_loc) .and. (yi.le.ymax_loc) ) then
						nvirt_loc = nvirt_loc + 1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indglob = n
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indloc = ntotal_loc+nhalo_loc+nvirt_loc
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%itype = -1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(1) = xi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(2) = yi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%vx(:) = 0d0
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%rho = irho
					end if
				end do

				! right boundary
				do i = 1,pp
					n = n + 1
					xi = xmax + 0.5d0*dx
					yi = ymin + (i-1.5d0)*dy
					if ( (xi.ge.xmin_loc) .and. (yi.ge.ymin_loc) .and. (xi.le.xmax_loc) .and. (yi.le.ymax_loc) ) then
						nvirt_loc = nvirt_loc + 1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indglob = n
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%indloc = ntotal_loc+nhalo_loc+nvirt_loc
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%itype = -1
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(1) = xi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%x(2) = yi
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%vx(:) = 0d0
						parts(ntotal_loc+nhalo_loc+nvirt_loc)%rho = irho
					end if
				end do

		end select

	end subroutine virt_part

end module input_m


module intrp_module

  use bl_types

  implicit none

  integer, parameter, private :: d = dp_t

  real(d), parameter :: dirichlet(5,7) = &
       reshape((/ &
                2.0_d,  -1.0_d,        0.0_d,        0.0_d,       0.0_d,        0.0_d, 0.0_d, &
          8.0_d/3.0_d,  -2.0_d,  1.0_d/3.0_d,        0.0_d,       0.0_d,        0.0_d, 0.0_d, &
         16.0_d/5.0_d,  -3.0_d,        1.0_d, -1.0_d/5.0_d,       0.0_d,        0.0_d, 0.0_d, &
       128.0_d/35.0_d,  -4.0_d,        2.0_d, -4.0_d/5.0_d, 1.0_d/7.0_d,        0.0_d, 0.0_d, &
       256.0_d/63.0_d,  -5.0_d, 10.0_d/3.0_d,       -2.0_d, 5.0_d/7.0_d, -1.0_d/9.0_d, 0.0_d  &
       /), (/5,7/))

  real(d), parameter :: neumann(5,7) = &
       reshape((/ &
                 -1.0_d,           1.0_d,             0.0_d,            0.0_d,           0.0_d,            0.0_d, 0.0_d, &
                 -1.0_d,           1.0_d,             0.0_d,            0.0_d,           0.0_d,            0.0_d, 0.0_d, &
         -24.0_d/23.0_d,   21.0_d/23.0_d,      3.0_d/23.0_d,    -1.0_d/23.0_d,           0.0_d,            0.0_d, 0.0_d, &
         -12.0_d/11.0_d,   17.0_d/22.0_d,      9.0_d/22.0_d,    -5.0_d/22.0_d,    1.0_d/22.0_d,            0.0_d, 0.0_d, &
       -640.0_d/563.0_d, 335.0_d/563.0_d, 1430.0_d/1689.0_d, -370.0_d/563.0_d, 145.0_d/563.0_d, -71.0_d/1689.0_d, 0.0_d  &
       /), (/5,7/))

  real(d), parameter :: corn(5,6) = &
       reshape((/ &
       1.0_d,   0.0_d,  0.0_d,  0.0_d, 0.0_d, 0.0_d, &
       2.0_d,  -1.0_d,  0.0_d,  0.0_d, 0.0_d, 0.0_d, &
       3.0_d,  -3.0_d,  1.0_d,  0.0_d, 0.0_d, 0.0_d, &
       4.0_d,  -6.0_d,  4.0_d, -1.0_d, 0.0_d, 0.0_d, &
       5.0_d, -10.0_d, 10.0_d, -5.0_d, 1.0_d, 0.0_d  &
       /), (/5,6/))

  type bc_rec
     integer :: dim = 0
     integer, pointer :: ord(:) => Null()
     real(dp_t), pointer :: cf(:,:) => Null()
     logical, pointer :: m_xl(:) => Null()
     logical, pointer :: m_xh(:) => Null()
     logical, pointer :: m_yl(:) => Null()
     logical, pointer :: m_yh(:) => Null()
  end type bc_rec

contains

  subroutine bc_interp_2d(ff, lo, bcr, &
       v_xl, v_xh, &
       v_yl, v_yh  &
       )

    integer, intent(in) :: lo(:)
    real(dp_t), intent(inout) :: ff(lo(1)-1:,lo(2)-1:)
    real(dp_t), intent(in) :: v_xl(lo(2):)
    real(dp_t), intent(in) :: v_xh(lo(2):)
    real(dp_t), intent(in) :: v_yl(lo(2):)
    real(dp_t), intent(in) :: v_yh(lo(2):)
    type(bc_rec), intent(in) :: bcr
    integer :: n, i, j
    integer :: hi(size(lo))

    hi(1) = ubound(ff,1)-1; hi(2) = ubound(ff,2)-1

    ! X Face
    do j = lo(2), hi(2)
       if ( bcr%m_xl(j) ) then
          ff(lo(1)-1, j) = v_xl(j)*bcr%cf(1,0) + sum(ff(lo(1):lo(1)+bcr%ord(1):+1,j)*bcr%cf(1,1:bcr%ord(1)))
       end if
       if ( bcr%m_xh(j) ) then
          ff(hi(1)+1, j) = v_xh(j)*bcr%cf(1,0) + sum(ff(hi(1):hi(1)-bcr%ord(1)+1:-1,j)*bcr%cf(1,1:bcr%ord(1)))
       end if
    end do

    ! Y Face
    do i = lo(1), hi(1)
       if ( bcr%m_yl(i) ) then
          ff(i,lo(2)-1) = v_yl(i)*bcr%cf(2,0) + sum(ff(i,lo(2):lo(2)+bcr%ord(2):+1)*bcr%cf(2,1:bcr%ord(2)))
       end if
       if ( bcr%m_yh(i) ) then
          ff(i,hi(2)+1) = v_yh(i)*bcr%cf(2,0) + sum(ff(i,hi(2):hi(2)-bcr%ord(2)+1:-1)*bcr%cf(2,1:bcr%ord(2)))
       end if
    end do

  end subroutine bc_interp_2d

end module intrp_module


! DART software - Copyright UCAR. This open source software is provided
! by UCAR, "as is", without charge, subject to all terms of use at
! http://www.image.ucar.edu/DAReS/DART/DART_download
!
! DART $Id$

program test_quad_reg_interp

! example of how to use the quad interpolate code on a fully regular grid.

!>@todo FIXME include the state struct or fold this into a version of model_mod_check

! Modules that are absolutely required for use are listed
use        types_mod, only : r8, i8, MISSING_R8, deg2rad, rad2deg
use    utilities_mod, only : error_handler, initialize_utilities, finalize_utilities
use   random_seq_mod, only : init_random_seq, random_seq_type, &
                             random_uniform, random_gaussian

use quad_utils_mod, only : quad_interp_handle, init_quad_interp, finalize_quad_interp, set_quad_coords,       &
                           quad_lon_lat_locate, quad_lon_lat_evaluate, GRID_QUAD_FULLY_REGULAR,               &
                           GRID_QUAD_IRREG_SPACED_REGULAR, GRID_QUAD_FULLY_IRREGULAR, GRID_QUAD_UNKNOWN_TYPE, &
                           QUAD_LOCATED_UNKNOWN, QUAD_LOCATED_CELL_CENTERS, QUAD_LOCATED_LON_EDGES,           &
                           QUAD_LOCATED_LAT_EDGES, QUAD_LOCATED_CELL_CORNERS

implicit none

integer :: debug = 0

type(quad_interp_handle) :: h

! data grid.  these are the values we will interpolate from.

! data grid size
! (we compute delta lon, delta lat based on these vals)
integer, parameter :: ndx = 9
integer, parameter :: ndy = 5

! extents of the data grid (these mimic a regional model's grid)
real(r8) :: data_start_lon = 100.0_r8
real(r8) :: data_end_lon   = 150.5_r8
real(r8) :: data_start_lat = -11.4_r8
real(r8) :: data_end_lat   =  34.1_r8

! these aren't needed for the interpolation, but they're written
! out for ease of plotting the results
real(r8) :: grid_lons(ndx)
real(r8) :: grid_lats(ndy)

! data values on the grid
real(r8) :: grid_data(ndx, ndy) = MISSING_R8
integer  :: data_choice = 0   ! see code for selection values

! percent of data values that should be marked 'missing data'
!real(r8) :: miss_percent =   0.0_r8    ! none
 real(r8) :: miss_percent =   3.0_r8    ! 3%
!real(r8) :: miss_percent = 100.0_r8    ! all

! interpolation test grid.  we construct a different grid
! and call the interpolation code on each corner of this
! other grid.  called 'sampling grid' to differentiate it
! from the 'data grid'.  usually much denser so we can look
! for discontinuties or errors in the interp code.

! sampling grid size
integer, parameter :: nsx = 210
integer, parameter :: nsy = 150

! locations of sampling grid
real(r8) :: sample_lons(nsx, nsy) = MISSING_R8
real(r8) :: sample_lats(nsx, nsy) = MISSING_R8

! extents of the sampling grid
real(r8) :: sample_start_lon = 110.0_r8
real(r8) :: sample_end_lon   = 140.0_r8
real(r8) :: sample_start_lat = -20.0_r8
real(r8) :: sample_end_lat   =  30.0_r8

! angle to rotate sampling grid in degrees
! positive is counterclockwise; will rotate
! around lower left grid point (start lon/lat).
!real(r8) :: angle =  10.0_r8
!real(r8) :: angle =  45.0_r8
 real(r8) :: angle = -65.0_r8
!real(r8) :: angle =  30.0_r8
!real(r8) :: angle =  90.0_r8
!real(r8) :: angle = -30.0_r8
!real(r8) :: angle = -10.0_r8
!real(r8) :: angle =   0.0_r8

! deform grid by this fraction of the deltas
!real(r8) :: lon_def = 0.25_r8
!real(r8) :: lat_def = 0.25_r8
 real(r8) :: lon_def = 0.01_r8
 real(r8) :: lat_def = 0.01_r8
!real(r8) :: lon_def = 0.0_r8
!real(r8) :: lat_def = 0.0_r8

! where interpolated values are stored on reg grid
real(r8) :: interp_data(nsx, nsy) = MISSING_R8

type(random_seq_type) :: ran

integer  :: i, j
real(r8) :: data_del_lon, data_del_lat, sample_del_lon, sample_del_lat
integer  :: lon_indices(4), lat_indices(4)
real(r8) :: lon_fract, lat_fract
integer  :: istatus
real(r8) :: invals(4), outval
integer  :: iunit_orig, iunit_interp

call initialize_utilities('test_quad_reg_interp')
call init_random_seq(ran)

! "data grid" corners and data vals

data_del_lon = (data_end_lon - data_start_lon) / ndx
data_del_lat = (data_end_lat - data_start_lat) / ndy

do i=1, ndx
   do j=1, ndy
      ! generate the data values on the corners.  pick one:
      select case (data_choice)
      case (1)
         ! increasing monotonically
         grid_data(i, j) = (j-1)*ndx + i
      case (2)
         ! constant by row
         grid_data(i, j) = j
      case (3)
         ! constant by column
         grid_data(i, j) = i
      case (4)
         ! based on lon only
         grid_data(i, j) = data_start_lon + (data_del_lon * (i-1))
      case (5)
         ! based on lat only
         grid_data(i, j) = data_start_lat + (data_del_lat * (j-1))
      case (6)
         ! random between (0-10)
         grid_data(i, j) = random_uniform(ran) * 10.0_r8
      case default
         ! gaussian with mean 0 and stddev 1
         grid_data(i, j) = random_gaussian(ran, 0.0_r8, 1.0_r8)
      end select

      if (miss_percent > 0.0_r8) then
        if (random_uniform(ran) * 100.0_r8 < miss_percent) grid_data(i, j) = MISSING_R8
      endif

   enddo
enddo

sample_del_lon = (sample_end_lon - sample_start_lon) / nsx
sample_del_lat = (sample_end_lat - sample_start_lat) / nsy

! "sampled grid" spacing along each axis
do i=1, nsx
   do j=1, nsy
      sample_lons(i, j) = sample_start_lon + (i-1)*sample_del_lon + deform(sample_del_lon, lon_def, ran)
      sample_lats(i, j) = sample_start_lat + (j-1)*sample_del_lat + deform(sample_del_lat, lat_def, ran)

      ! generate locations of the corners of all the quads
      if (angle /= 0.0_r8) &
         call rotate(sample_lons(i, j), sample_lats(i, j), angle, sample_start_lon, sample_start_lat)
enddo
enddo

! end of data setup - now call interp routines

call init_quad_interp(GRID_QUAD_FULLY_REGULAR, ndx, ndy, QUAD_LOCATED_CELL_CENTERS, .false., .false., .false., h)
call set_quad_coords(h, data_start_lon, data_del_lon, data_start_lat, data_del_lat)

do i=1, nsx
   do j=1, nsy

      !>this interface now returns an array of 4 index combinations
      !>so the calling code can do loops from 1 to 4 instead of making
      !>combinations of lat/lon bot/top in the right order for eval.

      call quad_lon_lat_locate(h, sample_lons(i,j), sample_lats(i,j), lon_indices, lat_indices, &
                               lon_fract, lat_fract, istatus)
      if (istatus /= 0) then
         !print *, 'location outside of grid: ', sample_lons(i,j), sample_lats(i,j)
         interp_data(i, j) = MISSING_R8
         cycle
      endif
      if(debug > 0)print *, i, j, lon_indices, lat_indices, lon_fract, lat_fract, sample_lons(i,j), sample_lats(i,j)

      ! get values of data at lon/lat bot/top indices, counterclockwise around quad
      invals(1) = grid_data(lon_indices(1), lat_indices(1))
      invals(2) = grid_data(lon_indices(2), lat_indices(2))
      invals(3) = grid_data(lon_indices(3), lat_indices(3))
      invals(4) = grid_data(lon_indices(4), lat_indices(4))

      ! where does this go?  inside quad_lon_lat_evaluate() or here?
      if (any(invals == MISSING_R8)) then
         interp_data(i, j) = MISSING_R8
         cycle
      endif

      call quad_lon_lat_evaluate(h, lon_fract, lat_fract, invals, outval, istatus)

      if (istatus == 0) then
         interp_data(i, j) = outval
      else
         interp_data(i, j) = MISSING_R8
      endif

   enddo
enddo

! this program doesn't currently have any missing locations - but i'll test that next.
if (debug > 0) print *, 'number of missing values in  input data: ', count(grid_data(:,:) == MISSING_R8)
if (debug > 0) print *, 'number of missing values in output data: ', count(interp_data(:,:) == MISSING_R8)

! generate these only for output for plotting the results
! these aren't needed for the interpolation.
do i=1,ndx
   grid_lons(i) = data_start_lon + (data_del_lon * (i-1))
enddo
do j=1,ndy
   grid_lats(j) = data_start_lat + (data_del_lat * (j-1))
enddo

call writeit_1d('data_lons_1d_reg_test.txt', ndx, grid_lons)
call writeit_1d('data_lats_1d_reg_test.txt', ndy, grid_lats)
call writeit_2d('data_data_2d_reg_test.txt', ndx, ndy, grid_data)

call writeit_2d('sample_lons_2d_reg_test.txt', nsx, nsy, sample_lons)
call writeit_2d('sample_lats_2d_reg_test.txt', nsx, nsy, sample_lats)
call writeit_2d('sample_data_2d_reg_test.txt', nsx, nsy, interp_data)

call finalize_quad_interp(h)
if (debug > 0) print *, 'closed files and finalized interp handle'

call finalize_utilities('test_quad_reg_interp')

contains

!------------------------------------------------------------
! rotate vector a counterclockwise by angle theta, relative
! to the given origin point.

subroutine rotate(x, y, theta, x0, y0)
 real(r8), intent(inout) :: x, y
 real(r8), intent(in)    :: theta
 real(r8), intent(in)    :: x0, y0

real(r8) :: a(2), b(2)
real(r8) :: r(2,2)
real(r8) :: rads

a(1) = x - x0
a(2) = y - y0

rads = theta * deg2rad

r(1,1) = cos(rads)
r(1,2) = sin(rads)
r(2,1) = sin(-rads)
r(2,2) = cos(rads)

b(1) = r(1,1)*a(1) + r(1,2)*a(2)
b(2) = r(2,1)*a(1) + r(2,2)*a(2)

x = b(1) + x0
y = b(2) + y0

end subroutine rotate

!------------------------------------------------------------
! compute +/- a random value based on a width and percentage
! of that width

function deform(width, fraction, seq)

use random_seq_mod

 real(r8), intent(in) :: width
 real(r8), intent(in) :: fraction
 type(random_seq_type), intent(inout) :: seq
 real(r8)             :: deform

real(r8) :: val

! random val between -1 and 1
val = (random_uniform(seq) * 2.0_r8) - 1.0_r8

deform = val * width * fraction

end function deform

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

subroutine writeit_1d(fname, nx, dataarray)

use    utilities_mod

 character(len=*), intent(in) :: fname
 integer, intent(in) :: nx
 real(r8), intent(in) :: dataarray(:)

integer :: i, j, iunit

iunit = open_file(fname, action='write')

do i=1, nx
   write(iunit, *) dataarray(i)
enddo

call close_file(iunit)

end subroutine writeit_1d

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

subroutine writeit_2d(fname, nx, ny, dataarray)

use    utilities_mod

 character(len=*), intent(in) :: fname
 integer, intent(in) :: nx, ny
 real(r8), intent(in) :: dataarray(nx, ny)

integer :: i, j, iunit

iunit = open_file(fname, action='write')

do i=1, nx
   do j=1, ny
      write(iunit, *) dataarray(i, j)
   enddo
enddo

call close_file(iunit)

end subroutine writeit_2d

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

end program test_quad_reg_interp

! <next few lines under version control, do not edit>
! $URL$
! $Id$
! $Revision$
! $Date$