module module_dyn

contains

subroutine dyn(j,k,dx,g,eta,dt,dy,un,u,wet,v,vn,h,etan)
      integer(4), parameter :: ny=500
      integer(4), parameter :: nx=500
! local parameters
      integer :: j  ! Intent Ignore
      integer :: k  ! Intent Ignore
      real, intent(In) :: dx
      real, intent(In) :: g
      real, dimension(0:ny+1,0:nx+1) :: eta  ! Intent Ignore
      real, intent(In) :: dt
      real, intent(In) :: dy
      real, dimension(0:ny+1,0:nx+1), intent(InOut) :: un
      real, dimension(0:ny+1,0:nx+1), intent(In) :: u
      integer, dimension(0:ny+1,0:nx+1), intent(In) :: wet
      real, dimension(0:ny+1,0:nx+1), intent(In) :: v
      real, dimension(0:ny+1,0:nx+1), intent(InOut) :: vn
      real, dimension(0:ny+1,0:nx+1), intent(In) :: h
      real, dimension(0:ny+1,0:nx+1) :: etan  ! Intent Ignore
real, dimension(0:ny+1,0:nx+1) :: du
real, dimension(0:ny+1,0:nx+1) :: dv
real :: uu
real :: vv
real :: duu
real :: dvv
real :: hue
real :: huw
real :: hwp
real :: hwn
real :: hen
real :: hep
real :: hvn
real :: hvs
real :: hsp
real :: hsn
real :: hnn
real :: hnp
do j = 1,ny
do k = 1,nx
  du(j,k) = -dt*g*(eta(j,k+1)-eta(j,k))/dx
  dv(j,k) = -dt*g*(eta(j+1,k)-eta(j,k))/dy
  !debug
  !write(90,*)"j = ",j," k = ",k," du = ",du(j,k)," dv = ",dv(j,k)
end do
end do
do j = 1,ny
do k = 1,nx
! prediction for u
un(j,k) = 0.0
uu = u(j,k)
duu = du(j,k)
if(wet(j,k)==1) then
  if((wet(j,k+1)==1).or.(duu>0.0)) un(j,k) = uu+duu
else
  if((wet(j,k+1)==1).and.(duu<0.0)) un(j,k) = uu+duu
end if
! prediction for v
vv = v(j,k)
dvv = dv(j,k)
vn(j,k) = 0.0
if(wet(j,k)==1) then
  if((wet(j+1,k)==1).or.(dvv>0.0)) vn(j,k) = vv+dvv
else
  if((wet(j+1,k)==1).and.(dvv<0.0)) vn(j,k) = vv+dvv
end if
!debug
!write(90,*)"j=",j," k=",k," un=",un(j,k)," vn=",vn(j,k), "wet(j,k)=", wet(j,k), " wet(j+1,k)=", wet(j+1,k), " uu=", uu, " duu=", duu, " vv=", vv, " dvv=", dvv
end do
end do
! sea level predictor
do j = 1,ny
do k = 1,nx
  hep = 0.5*(un(j,k)+abs(un(j,k)))*h(j,k)
  hen = 0.5*(un(j,k)-abs(un(j,k)))*h(j,k+1)
  hue = hep+hen
  hwp = 0.5*(un(j,k-1)+abs(un(j,k-1)))*h(j,k-1)
  hwn = 0.5*(un(j,k-1)-abs(un(j,k-1)))*h(j,k)
  huw = hwp+hwn
  hnp = 0.5*(vn(j,k)+abs(vn(j,k)))*h(j,k)
  hnn = 0.5*(vn(j,k)-abs(vn(j,k)))*h(j+1,k)
  hvn = hnp+hnn
  hsp = 0.5*(vn(j-1,k)+abs(vn(j-1,k)))*h(j-1,k)
  hsn = 0.5*(vn(j-1,k)-abs(vn(j-1,k)))*h(j,k)
  hvs = hsp+hsn
  etan(j,k) = eta(j,k)-dt*(hue-huw)/dx-dt*(hvn-hvs)/dy
#ifdef DBG
  !debug
  write(90,*)"j=",j," k=",k," etan=",etan(j,k)," eta=",eta(j,k)
#endif
end do
end do
end subroutine dyn

end module module_dyn


      DOUBLE PRECISION FUNCTION ZTHZ(H,IH,Z,IZ,N,I1,I2)
C
C***********************************************************************
C                                                                      *
C                                                                      *
C     Copyright:        Eduardo Casas Renteria                         *
C                       Cecilia Pola Mendez                            *
C                                                                      *
C       Departamento de Matematicas,Estadistica y Computacion          *
C       -----------------------------------------------------          *
C                     UNIVERSIDAD DE CANTABRIA                         *
C                     ------------------------                         *
C                          FEBRERO 1988                                *
C                                                                      *
C***********************************************************************
C
C     OBJETIVO:
C        Este subprograma calcula el elemento  (i1,i2)  del producto
C        matricial  ZTHZ.
C
C     LISTA DE LLAMADA:
C
C        H     Matriz  de  dimension  (IH,N)  que  contiene, en  sus  N
C              primeras filas, la matriz  H.  Es suficiente  suministrar
C              la parte triangular inferior. El resto de la matriz no se
C              utiliza.
C
C        IH    Primera dimension de la matriz  H. IH >= N.
C
C        Z     Matriz de dimension  (IZ,*) que contiene la matriz  Z.
C
C        IZ    Primera dimension de la matriz  Z. IZ >= N.
C
C        N     Numero de filas de  Z, numero de filas y columnas de  H.
C
C      I1,I2   (I1,I2) indica el elemento que se desea calcular.
C
C        Este subprograma trabaja en doble precision via una sentencia
C     "implicit":
C                Implicit double precision (a-h,o-z)
C
C     SUBPROGRAMAS AUXILIARES: ddot
C     FUNCIONES FORTRAN INTRINSECAS: mod
C
C
      implicit double precision(a-h,o-z)
      dimension h(ih,*),z(iz,*)
      zthz=0
      do 10 j=1,n
         jj=j+1
         s=ddot(j,h(j,1),ih,z(1,i2),1)
         s=s+ddot(n-j,h(jj,j),1,z(jj,i2),1)
         zthz=zthz+s*z(j,i1)
10    continue
      end


      SUBROUTINE DRDVAL (X, Y, Z, RD, IERR)
c Copyright (c) 1996 California Institute of Technology, Pasadena, CA.
c ALL RIGHTS RESERVED.
c Based on Government Sponsored Research NAS7-03001.
c>>   2001-07-16 DRDVAL Krogh  Change -1.0 to -1.d0.
C>>   1998-10-29 DRDVAL Krogh  Moved external statement up for mangle.
C>>   1995-11-17 DRDVAL Krogh  Converted SFTRAN to Fortran 77.
c>>   1994-10-19 DRDVAL Krogh  Changes to use M77CON
c>>   1994-08-31 DRDVAL WV Snyder JPL use 2-arg min and max for C conv.
C>>   1991-10-31 DRDVAL WV Snyder JPL Incorporate changes from Carlson
C>>   1990-11-27 DRDVAL WV Snyder JPL Convert Carlson's code for MATH77
C
C     COMPUTE THE INCOMPLETE ELLIPTIC INTEGRAL OF THE SECOND KIND
C
C     RD(X,Y,Z) = INTEGRAL FROM ZERO TO INFINITY OF
C
C                           -1/2     -1/2     -3/2
C                 (3/2)(T+X)    (T+Y)    (T+Z)    DT,
C
C     WHERE X AND Y ARE NONNEGATIVE, X + Y IS POSITIVE, AND Z IS
C     POSITIVE.  IF X OR Y IS ZERO, THE INTEGRAL IS COMPLETE.
C     THE DUPLICATION THEOREM IS ITERATED UNTIL THE VARIABLES ARE
C     NEARLY EQUAL, AND THE FUNCTION IS THEN EXPANDED IN TAYLOR
C     SERIES TO FIFTH ORDER.
C     REFERENCE: B. C. CARLSON AND E. M. NOTIS, ALGORITHMS FOR
C     INCOMPLETE ELLIPTIC INTEGRALS, ACM TRANSACTIONS ON MATHEMA-
C     TICAL SOFTWARE, 7 (1981), 398-403; B. C. CARLSON, COMPUTING
C     ELLIPTIC INTEGRALS BY DUPLICATION, NUMER. MATH. 33 (1979),
C     1-16.
C     AUTHORS: B. C. CARLSON AND ELAINE M. NOTIS, AMES LABORATORY-
C     DOE, IOWA STATE UNIVERSITY, AMES, IA 50011, AND R. L. PEXTON,
C     LAWRENCE LIVERMORE NATIONAL LABORATORY, LIVERMORE, CA 94550.
C     AUG. 1, 1979, REVISED JAN. 15, 1987.
C
C     CHECK VALUE: RD(0,2,1) = 1.79721 03521 03388 31115 98837
C     CHECK: RD(X,Y,Z) + RD(Y,Z,X) + RD(Z,X,Y)
C     = 3 / DSQRT(X * Y * Z), WHERE X, Y, AND Z ARE POSITIVE.
C
      DOUBLE PRECISION X, Y, Z, RD
      INTEGER IERR
C
C     INPUT ...
C
C     X, Y, AND Z ARE THE VARIABLES IN THE INTEGRAL RD(X,Y,Z).
C
C     OUTPUT ...
C
C     RD IS THE VALUE OF THE INCOMPLETE ELLIPTIC INTEGRAL.
C
C     IERR IS THE RETURN ERROR CODE.
C          IERR = 0  FOR NORMAL COMPLETION OF THE SUBROUTINE.
C          IERR = 1  X, Y, OR Z IS NEGATIVE.
C          IERR = 2  X+Y OR Z IS TOO SMALL.
C          IERR = 3  X, Y, OR Z IS TOO LARGE.
C
C ----------------------------------------------------------------------
C--D replaces "?": ?ERM1, ?ERV1, ?RDVAL
C ----------------------------------------------------------------------
      EXTERNAL D1MACH
      DOUBLE PRECISION D1MACH
      DOUBLE PRECISION C1,C2,C3,C4,EA,EB,EC,ED,EF,EPSLON,EPWR,ERRTOL
      DOUBLE PRECISION LAMDA,LOLIM,MU,POWER4,SIGMA,S1,S2,UPLIM,XN,XNDEV
      DOUBLE PRECISION XNROOT,YN,YNDEV,YNROOT,ZN,ZNDEV,ZNROOT
      PARAMETER (C1 = 3.0d0 / 14.0d0)
      PARAMETER (C2 = 1.0d0 / 6.0d0)
      PARAMETER (C3 = 9.0d0 / 22.0d0)
      PARAMETER (C4 = 3.0d0 / 26.0d0)
      PARAMETER (EPWR = -2.0d0 / 3.0d0)
C ----------------------------------------------------------------------
C
C     MACHINE DEPENDENT PARAMETERS ...
C
C     ERRTOL IS SET TO THE DESIRED ERROR TOLERANCE.
C     RELATIVE ERROR DUE TO TRUNCATION IS LESS THAN
C     3 * ERRTOL ** 6 / (1 - ERRTOL) ** 3/2.
C
C     LOLIM AND UPLIM DETERMINE THE RANGE OF VALID ARGUMENTS.
C     LOLIM IS NOT LESS THAN 2 / (MACHINE MAXIMUM) ** (2/3).
C     UPLIM IS NOT GREATER THAN (0.1 * ERRTOL / MACHINE
C     MINIMUM) ** (2/3).
C
      SAVE LOLIM, UPLIM, ERRTOL
      DATA LOLIM /-1.0d0/
C ----------------------------------------------------------------------
C          WARNING. CHANGES IN THE PROGRAM MAY IMPROVE SPEED AT THE
C          EXPENSE OF ROBUSTNESS.
C ----------------------------------------------------------------------
C
      RD = 0.0
      IF (MIN(X,MIN(Y,Z)) .LT. 0.0d0) THEN
         IERR = 1
         CALL DERM1 ('DRDVAL',1,0,'One of X, Y or Z is negative','X',x,
     1     ',')
         CALL DERV1 ('Y',Y,',')
         CALL DERV1 ('Z',Z,'.')
         RETURN
      END IF
      IF (LOLIM .LT. 0.0) THEN
         ERRTOL = (.28d0 * D1MACH(4)) ** (1.0d0/6.0d0)
         LOLIM = 2.0001d0 * D1MACH(2) ** EPWR
         UPLIM = (10.0d0 * D1MACH(1) / ERRTOL) ** EPWR
      END IF
      IF (MIN(X+Y,Z) .LT. LOLIM) THEN
         IERR = 2
         CALL DERM1 ('DRDVAL',2,0,'MIN(X+Y,Z) < LOLIM','X',x,
     1     ',')
         CALL DERV1 ('Y',Y,',')
         CALL DERV1 ('Z',Z,',')
         CALL DERV1 ('LOLIM',LOLIM,'.')
         RETURN
      END IF
      IF (MAX(X,MAX(Y,Z)) .GT. UPLIM) THEN
         IERR = 3
         CALL DERM1 ('DRDVAL',3,0,'One of X, Y or Z > UPLIM','X',x,
     1     ',')
         CALL DERV1 ('Y',Y,',')
         CALL DERV1 ('Z',Z,',')
         CALL DERV1 ('UPLIM',UPLIM,'.')
         RETURN
      END IF
C
      IERR = 0
      XN = X
      YN = Y
      ZN = Z
      SIGMA = 0.0d0
      POWER4 = 1.0d0
C
   20 CONTINUE
         MU = (XN + YN + 3.0d0 * ZN) / 5.0d0
         XNDEV = (MU - XN) / MU
         YNDEV = (MU - YN) / MU
         ZNDEV = (MU - ZN) / MU
         EPSLON = MAX(ABS(XNDEV),MAX(ABS(YNDEV),ABS(ZNDEV)))
         IF (EPSLON .LT. ERRTOL) THEN
            EA = XNDEV * YNDEV
            EB = ZNDEV * ZNDEV
            EC = EA - EB
            ED = EA - 6.0d0 * EB
            EF = ED + EC + EC
            EB = C4 * ZNDEV
            S1 = ED * (0.25d0 * C3 * ED - 1.5d0 * EB * EF - C1)
            S2 = ZNDEV * (C2 * EF + ZNDEV * (EB * EA - C3 * EC))
            RD = 3.0d0 * SIGMA + POWER4 * (1.0d0+S1+S2) / (MU*SQRT(MU))
            RETURN
         END IF
         XNROOT = SQRT(XN)
         YNROOT = SQRT(YN)
         ZNROOT = SQRT(ZN)
         LAMDA = XNROOT * (YNROOT + ZNROOT) + YNROOT * ZNROOT
         SIGMA = SIGMA + POWER4 / (ZNROOT * (ZN + LAMDA))
         POWER4 = POWER4 * 0.25d0
         XN = (XN + LAMDA) * 0.25d0
         YN = (YN + LAMDA) * 0.25d0
         ZN = (ZN + LAMDA) * 0.25d0
      GO TO 20
      END


!
! Copyright 2020-2022 Fluid Numerics LLC
! Author : Joseph Schoonover (joe@fluidnumerics.com)
! Support : support@fluidnumerics.com
!
! //////////////////////////////////////////////////////////////////////////////////////////////// !
MODULE SELF_DG

  USE SELF_Metadata
  USE SELF_MPI
  USE SELF_Mesh
  USE SELF_MappedData
  USE SELF_HDF5
  USE HDF5

  TYPE,PUBLIC :: DG2D
    TYPE(MappedScalar2D),PUBLIC :: solution
    TYPE(Scalar2D),PUBLIC :: plotSolution
    TYPE(MappedVector2D),PUBLIC :: solutionGradient
    TYPE(MappedVector2D),PUBLIC :: flux
    TYPE(MappedScalar2D),PUBLIC :: source
    TYPE(MappedScalar2D),PUBLIC :: fluxDivergence
    TYPE(MappedScalar2D),PUBLIC :: dSdt
    TYPE(MPILayer),PUBLIC :: decomp
    TYPE(Mesh2D),PUBLIC :: mesh
    TYPE(SEMQuad),PUBLIC :: geometry
    TYPE(Metadata),ALLOCATABLE,PUBLIC :: solutionMetaData(:)

    ! Work arrays
    ! Can't be private to be accessible by type extensions
    ! TO DO : Really need to figure out the reuse business for the initializer...
    TYPE(MappedScalar2D) :: workScalar
    TYPE(MappedVector2D) :: workVector
    TYPE(MappedVector2D) :: compFlux
    TYPE(MappedTensor2D) :: workTensor

  CONTAINS

    PROCEDURE,PUBLIC :: Init => Init_DG2D
    PROCEDURE,PUBLIC :: Free => Free_DG2D

    PROCEDURE,PUBLIC :: UpdateHost => UpdateHost_DG2D
    PROCEDURE,PUBLIC :: UpdateDevice => UpdateDevice_DG2D

!    PROCEDURE,PUBLIC :: FluxMethod => FluxMethod_DG2D
    PROCEDURE,PUBLIC :: CalculateSolutionGradient => CalculateSolutionGradient_DG2D
    PROCEDURE,PUBLIC :: CalculateFluxDivergence => CalculateFluxDivergence_DG2D
    PROCEDURE,PUBLIC :: CalculateDSDt => CalculateDSDt_DG2D

    PROCEDURE,PUBLIC :: Read => Read_DG2D
    PROCEDURE,PUBLIC :: Write => Write_DG2D

  END TYPE DG2D

  TYPE,PUBLIC :: DG3D
    TYPE(MappedScalar3D),PUBLIC :: solution
    TYPE(Scalar3D),PUBLIC :: plotSolution
    TYPE(MappedVector3D),PUBLIC :: solutionGradient
    TYPE(MappedVector3D),PUBLIC :: flux
    TYPE(MappedScalar3D),PUBLIC :: source
    TYPE(MappedScalar3D),PUBLIC :: fluxDivergence
    TYPE(MappedScalar3D),PUBLIC :: dSdt
    TYPE(MPILayer),PUBLIC :: decomp
    TYPE(Mesh3D),PUBLIC :: mesh
    TYPE(SEMHex),PUBLIC :: geometry
    TYPE(Metadata),ALLOCATABLE,PUBLIC :: solutionMetaData(:)

    ! Work arrays
    TYPE(MappedScalar3D) :: workScalar
    TYPE(MappedVector3D) :: workVector
    TYPE(MappedVector3D) :: compFlux
    TYPE(MappedTensor3D) :: workTensor

  CONTAINS

    PROCEDURE,PUBLIC :: Init => Init_DG3D
    PROCEDURE,PUBLIC :: Free => Free_DG3D

    PROCEDURE,PUBLIC :: UpdateHost => UpdateHost_DG3D
    PROCEDURE,PUBLIC :: UpdateDevice => UpdateDevice_DG3D

    PROCEDURE,PUBLIC :: CalculateSolutionGradient => CalculateSolutionGradient_DG3D
    PROCEDURE,PUBLIC :: CalculateFluxDivergence => CalculateFluxDivergence_DG3D
    PROCEDURE,PUBLIC :: CalculateDSDt => CalculateDSDt_DG3D

    PROCEDURE,PUBLIC :: Read => Read_DG3D
    PROCEDURE,PUBLIC :: Write => Write_DG3D

  END TYPE DG3D

  INTEGER,PARAMETER :: SELF_DG_BASSIREBAY = 100

  INTERFACE
    SUBROUTINE CalculateDSDt_DG2D_gpu_wrapper(fluxDivergence, source, dSdt, N, nVar, nEl) &
      bind(c,name="CalculateDSDt_DG2D_gpu_wrapper")
      USE iso_c_binding
      IMPLICIT NONE
      TYPE(c_ptr) :: fluxDivergence, source, dSdt
      INTEGER(C_INT),VALUE :: N,nVar,nEl
    END SUBROUTINE CalculateDSDt_DG2D_gpu_wrapper
  END INTERFACE

  INTERFACE
    SUBROUTINE CalculateDSDt_DG3D_gpu_wrapper(fluxDivergence, source, dSdt, N, nVar, nEl) &
      bind(c,name="CalculateDSDt_DG3D_gpu_wrapper")
      USE iso_c_binding
      IMPLICIT NONE
      TYPE(c_ptr) :: fluxDivergence, source, dSdt
      INTEGER(C_INT),VALUE :: N,nVar,nEl
    END SUBROUTINE CalculateDSDt_DG3D_gpu_wrapper
  END INTERFACE

CONTAINS

  SUBROUTINE Init_DG2D(this,interp,nvar,enableMPI,spec)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(out) :: this
    INTEGER,INTENT(in) :: cqType
    INTEGER,INTENT(in) :: tqType
    INTEGER,INTENT(in) :: cqDegree
    INTEGER,INTENT(in) :: tqDegree
    INTEGER,INTENT(in) :: nvar
    LOGICAL,INTENT(in) :: enableMPI
    TYPE(MeshSpec),INTENT(in) :: spec

    CALL this % decomp % Init(enableMPI)

    ! Load Mesh
    CALL this % mesh % Load(spec,this % decomp)

    CALL this % decomp % SetMaxMsg(this % mesh % nUniqueSides)

    CALL this % decomp % setElemToRank(this % mesh % nElem)

    ! Create geometry from mesh
    CALL this % geometry % GenerateFromMesh(this % mesh,cqType,tqType,cqDegree,tqDegree)

    CALL this % solution % Init(interp,nVar,this % mesh % nElem)
    CALL this % plotSolution % Init(tqDegree,tqType,tqDegree,tqType,nVar,this % mesh % nElem)
    CALL this % dSdt % Init(interp,nVar,this % mesh % nElem)
    CALL this % solutionGradient % Init(interp,nVar,this % mesh % nElem)
    CALL this % flux % Init(interp,nVar,this % mesh % nElem)
    CALL this % source % Init(interp,nVar,this % mesh % nElem)
    CALL this % fluxDivergence % Init(interp,nVar,this % mesh % nElem)

    CALL this % workScalar % Init(interp,nVar,this % mesh % nElem)
    CALL this % workVector % Init(interp,nVar,this % mesh % nElem)
    CALL this % workTensor % Init(interp,nVar,this % mesh % nElem)
    CALL this % compFlux % Init(interp,nVar,this % mesh % nElem)

    ALLOCATE (this % solutionMetaData(1:nvar))

  END SUBROUTINE Init_DG2D

  SUBROUTINE Free_DG2D(this)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this

    CALL this % mesh % Free()
    CALL this % geometry % Free()
    CALL this % solution % Free()
    CALL this % plotSolution % Free()
    CALL this % dSdt % Free()
    CALL this % solutionGradient % Free()
    CALL this % flux % Free()
    CALL this % source % Free()
    CALL this % fluxDivergence % Free()
    CALL this % workScalar % Free()
    CALL this % workVector % Free()
    CALL this % workTensor % Free()
    CALL this % compFlux % Free()
    DEALLOCATE (this % solutionMetaData)

  END SUBROUTINE Free_DG2D

  SUBROUTINE UpdateHost_DG2D(this)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this

    CALL this % mesh % UpdateHost()
    CALL this % geometry % UpdateHost()
    CALL this % solution % UpdateHost()
    CALL this % plotSolution % UpdateHost()
    CALL this % dSdt % UpdateHost()
    CALL this % solutionGradient % UpdateHost()
    CALL this % flux % UpdateHost()
    CALL this % source % UpdateHost()
    CALL this % fluxDivergence % UpdateHost()
    CALL this % workScalar % UpdateHost()
    CALL this % workVector % UpdateHost()
    CALL this % workTensor % UpdateHost()
    CALL this % compFlux % UpdateHost()

  END SUBROUTINE UpdateHost_DG2D

  SUBROUTINE UpdateDevice_DG2D(this)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this

    CALL this % mesh % UpdateDevice()
    CALL this % geometry % UpdateDevice()
    CALL this % plotSolution % UpdateDevice()
    CALL this % dSdt % UpdateDevice()
    CALL this % solutionGradient % UpdateDevice()
    CALL this % flux % UpdateDevice()
    CALL this % source % UpdateDevice()
    CALL this % fluxDivergence % UpdateDevice()
    CALL this % workScalar % UpdateDevice()
    CALL this % workVector % UpdateDevice()
    CALL this % workTensor % UpdateDevice()
    CALL this % compFlux % UpdateDevice()

  END SUBROUTINE UpdateDevice_DG2D

  SUBROUTINE CalculateSolutionGradient_DG2D(this,gpuAccel)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel

    CALL this % solution % BassiRebaySides(gpuAccel)

    CALL this % solution % Gradient(this % workTensor, &
                                    this % geometry, &
                                    this % solutionGradient, &
                                    selfWeakDGForm,gpuAccel)

  END SUBROUTINE CalculateSolutionGradient_DG2D

  SUBROUTINE CalculateFluxDivergence_DG2D(this,gpuAccel)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel

    CALL this % flux % Divergence(this % geometry, &
                                  this % fluxDivergence, &
                                  selfWeakDGForm,gpuAccel)

  END SUBROUTINE CalculateFluxDivergence_DG2D

  SUBROUTINE CalculateDSDt_DG2D(this,gpuAccel)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel
    ! Local
    INTEGER :: i, j, k, iVar, iEl

    IF( gpuAccel )THEN

      CALL CalculateDSDt_DG2D_gpu_wrapper( this % fluxDivergence % interior % deviceData, &
                                      this % source % interior % deviceData, &
                                      this % dSdt % interior % deviceData, &
                                      this % solution % interp % N, &
                                      this % solution % nVar, &
                                      this % solution % nElem )

    ELSE

      DO iEl = 1, this % solution % nElem
        DO iVar = 1, this % solution % nVar
          DO j = 0, this % solution % interp % N
            DO i = 0, this % solution % interp % N

              this % dSdt % interior % hostData(i,j,iVar,iEl) = &
                      this % source % interior % hostData(i,j,iVar,iEl) -&
                      this % fluxDivergence % interior % hostData(i,j,iVar,iEl)

            ENDDO
          ENDDO
        ENDDO
      ENDDO

    ENDIF

  END SUBROUTINE CalculateDSDt_DG2D

  SUBROUTINE Write_DG2D(this,fileName)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(in) :: this
    CHARACTER(*),INTENT(in) :: fileName
    ! Local
    INTEGER(HID_T) :: fileId
    INTEGER(HID_T) :: solOffset(1:4)
    INTEGER(HID_T) :: xOffset(1:5)
    INTEGER(HID_T) :: bOffset(1:4)
    INTEGER(HID_T) :: bxOffset(1:5)
    INTEGER(HID_T) :: solGlobalDims(1:4)
    INTEGER(HID_T) :: xGlobalDims(1:5)
    INTEGER(HID_T) :: bGlobalDims(1:4)
    INTEGER(HID_T) :: bxGlobalDims(1:5)
    INTEGER :: firstElem

    IF (this % decomp % mpiEnabled) THEN

      CALL Open_HDF5(fileName,H5F_ACC_TRUNC_F,fileId,this % decomp % mpiComm)

      firstElem = this % decomp % offsetElem % hostData(this % decomp % rankId)
      solOffset(1:4) = (/0,0,1,firstElem/)
      solGlobalDims(1:4) = (/this % solution % interp % N, &
                             this % solution % interp % N, &
                             this % solution % nVar, &
                             this % decomp % nElem/)

      xOffset(1:5) = (/1,0,0,1,firstElem/)
      xGlobalDims(1:5) = (/2, &
                           this % solution % interp % N, &
                           this % solution % interp % N, &
                           this % solution % nVar, &
                           this % decomp % nElem/)

      ! Offsets and dimensions for element boundary data
      bOffset(1:4) = (/0,1,1,firstElem/)
      bGlobalDims(1:4) = (/this % solution % interp % N, &
                           this % solution % nVar, &
                           4,&
                           this % decomp % nElem/)

      bxOffset(1:5) = (/1,0,1,1,firstElem/)
      bxGlobalDims(1:5) = (/2,&
                           this % solution % interp % N, &
                           this % solution % nVar, &
                           4,&
                           this % decomp % nElem/)

      CALL CreateGroup_HDF5(fileId,'/quadrature')

      IF( this % decomp % rankId == 0 )THEN
        CALL WriteArray_HDF5(fileId,'/quadrature/xi', &
                             this % solution % interp % controlPoints)

        CALL WriteArray_HDF5(fileId,'/quadrature/weights', &
                             this % solution % interp % qWeights)

        CALL WriteArray_HDF5(fileId,'/quadrature/dgmatrix', &
                             this % solution % interp % dgMatrix)

        CALL WriteArray_HDF5(fileId,'/quadrature/dmatrix', &
                             this % solution % interp % dMatrix)
      ENDIF

      CALL CreateGroup_HDF5(fileId,'/state')

      CALL CreateGroup_HDF5(fileId,'/state/interior')

      CALL CreateGroup_HDF5(fileId,'/state/boundary')

      CALL CreateGroup_HDF5(fileId,'/mesh')

      CALL CreateGroup_HDF5(fileId,'/mesh/interior')

      CALL CreateGroup_HDF5(fileId,'/mesh/boundary')

      CALL WriteArray_HDF5(fileId,'/state/interior/solution', &
                           this % solution % interior,solOffset,solGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solution', &
                           this % solution % boundary,bOffset,bGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/fluxDivergence', &
                           this % fluxDivergence % interior,solOffset,solGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/flux', &
                           this % flux % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/flux', &
                           this % flux % boundary,bxOffset,bxGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/solutionGradient', &
                           this % solutionGradient % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solutionGradient', &
                           this % solutionGradient % boundary,bxOffset,bxGlobalDims)

      CALL WriteArray_HDF5(fileId,'/mesh/interior/x', &
                           this % geometry % x % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/mesh/boundary/x', &
                           this % geometry % x % boundary,bxOffset,bxGlobalDims)

      CALL Close_HDF5(fileId)

    ELSE

      CALL Open_HDF5(fileName,H5F_ACC_TRUNC_F,fileId)

      CALL CreateGroup_HDF5(fileId,'/quadrature')

      CALL WriteArray_HDF5(fileId,'/quadrature/xi', &
                           this % solution % interp % controlPoints)

      CALL WriteArray_HDF5(fileId,'/quadrature/weights', &
                           this % solution % interp % qWeights)

      CALL WriteArray_HDF5(fileId,'/quadrature/dgmatrix', &
                           this % solution % interp % dgMatrix)

      CALL WriteArray_HDF5(fileId,'/quadrature/dmatrix', &
                           this % solution % interp % dMatrix)

      CALL CreateGroup_HDF5(fileId,'/state')

      CALL CreateGroup_HDF5(fileId,'/state/interior')

      CALL CreateGroup_HDF5(fileId,'/state/boundary')

      CALL CreateGroup_HDF5(fileId,'/mesh')

      CALL CreateGroup_HDF5(fileId,'/mesh/interior')

      CALL CreateGroup_HDF5(fileId,'/mesh/boundary')

      CALL WriteArray_HDF5(fileId,'/state/interior/solution',this % solution % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solution',this % solution % boundary)

      CALL WriteArray_HDF5(fileId,'/state/interior/fluxDivergence',this % fluxDivergence % interior)

      CALL WriteArray_HDF5(fileId,'/state/interior/flux',this % flux % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/flux',this % flux % boundary)

      CALL WriteArray_HDF5(fileId,'/state/interior/solutionGradient',this % solutionGradient % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solutionGradient',this % solutionGradient % boundary)

      CALL WriteArray_HDF5(fileId,'/mesh/interior/x',this % geometry % x % interior)

      CALL WriteArray_HDF5(fileId,'/mesh/boundary/x',this % geometry % x % boundary)

      CALL Close_HDF5(fileId)

    END IF

  END SUBROUTINE Write_DG2D

  SUBROUTINE Read_DG2D(this,fileName)
    IMPLICIT NONE
    CLASS(DG2D),INTENT(inout) :: this
    CHARACTER(*),INTENT(in) :: fileName
    ! Local
    INTEGER(HID_T) :: fileId
    INTEGER(HID_T) :: solOffset(1:4)
    INTEGER :: firstElem
    INTEGER :: N

    IF (this % decomp % mpiEnabled) THEN
      CALL Open_HDF5(fileName,H5F_ACC_RDWR_F,fileId, &
                     this % decomp % mpiComm)
    ELSE
      CALL Open_HDF5(fileName,H5F_ACC_RDWR_F,fileId)
    END IF

    CALL ReadAttribute_HDF5(fileId,'N',N)

    IF (this % solution % interp % N /= N) THEN
      STOP 'Error : Solution polynomial degree does not match input file'
    END IF

    IF (this % decomp % mpiEnabled) THEN
      firstElem = this % decomp % offsetElem % hostData(this % decomp % rankId) + 1
      solOffset(1:4) = (/0,0,1,firstElem/)
      CALL ReadArray_HDF5(fileId,'/state/interior/solution', &
                          this % solution % interior,solOffset)
    ELSE
      CALL ReadArray_HDF5(fileId,'/state/interior/solution',this % solution % interior)
    END IF

    CALL Close_HDF5(fileId)

  END SUBROUTINE Read_DG2D

  SUBROUTINE Init_DG3D(this,cqType,tqType,cqDegree,tqDegree,nvar,enableMPI,spec)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(out) :: this
    INTEGER,INTENT(in) :: cqType
    INTEGER,INTENT(in) :: tqType
    INTEGER,INTENT(in) :: cqDegree
    INTEGER,INTENT(in) :: tqDegree
    INTEGER,INTENT(in) :: nvar
    LOGICAL,INTENT(in) :: enableMPI
    TYPE(MeshSpec),INTENT(in) :: spec

    CALL this % decomp % Init(enableMPI)

    ! Load Mesh
    CALL this % mesh % Load(spec,this % decomp)

    CALL this % decomp % SetMaxMsg(this % mesh % nUniqueSides)

    ! Create geometry from mesh
    CALL this % geometry % GenerateFromMesh(this % mesh,cqType,tqType,cqDegree,tqDegree)

    CALL this % solution % Init(interp,nVar,this % mesh % nElem)
    CALL this % dSdt % Init(interp,nVar,this % mesh % nElem)
    CALL this % solutionGradient % Init(interp,nVar,this % mesh % nElem)
    CALL this % flux % Init(interp,nVar,this % mesh % nElem)
    CALL this % source % Init(interp,nVar,this % mesh % nElem)
    CALL this % fluxDivergence % Init(interp,nVar,this % mesh % nElem)

    CALL this % workScalar % Init(interp,nVar,this % mesh % nElem)
    CALL this % workVector % Init(interp,nVar,this % mesh % nElem)
    CALL this % workTensor % Init(interp,nVar,this % mesh % nElem)
    CALL this % compFlux % Init(interp,nVar,this % mesh % nElem)

    ALLOCATE (this % solutionMetaData(1:nvar))

  END SUBROUTINE Init_DG3D

  SUBROUTINE Free_DG3D(this)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this

    CALL this % mesh % Free()
    CALL this % geometry % Free()
    CALL this % solution % Free()
    CALL this % dSdt % Free()
    CALL this % solutionGradient % Free()
    CALL this % flux % Free()
    CALL this % source % Free()
    CALL this % fluxDivergence % Free()
    CALL this % workScalar % Free()
    CALL this % workVector % Free()
    CALL this % workTensor % Free()
    CALL this % compFlux % Free()
    DEALLOCATE (this % solutionMetaData)

  END SUBROUTINE Free_DG3D

  SUBROUTINE UpdateHost_DG3D(this)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this

    CALL this % mesh % UpdateHost()
    CALL this % geometry % UpdateHost()
    CALL this % solution % UpdateHost()
    CALL this % dSdt % UpdateHost()
    CALL this % solutionGradient % UpdateHost()
    CALL this % flux % UpdateHost()
    CALL this % source % UpdateHost()
    CALL this % fluxDivergence % UpdateHost()
    CALL this % workScalar % UpdateHost()
    CALL this % workVector % UpdateHost()
    CALL this % workTensor % UpdateHost()
    CALL this % compFlux % UpdateHost()

  END SUBROUTINE UpdateHost_DG3D

  SUBROUTINE UpdateDevice_DG3D(this)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this

    CALL this % mesh % UpdateDevice()
    CALL this % geometry % UpdateDevice()
    CALL this % solution % UpdateDevice()
    CALL this % dSdt % UpdateDevice()
    CALL this % solutionGradient % UpdateDevice()
    CALL this % flux % UpdateDevice()
    CALL this % source % UpdateDevice()
    CALL this % fluxDivergence % UpdateDevice()
    CALL this % workScalar % UpdateDevice()
    CALL this % workVector % UpdateDevice()
    CALL this % workTensor % UpdateDevice()
    CALL this % compFlux % UpdateDevice()

  END SUBROUTINE UpdateDevice_DG3D

  SUBROUTINE CalculateSolutionGradient_DG3D(this,gpuAccel)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel

    CALL this % solution % BassiRebaySides(gpuAccel)

    CALL this % solution % Gradient(this % workTensor, &
                                    this % geometry, &
                                    this % solutionGradient, &
                                    selfWeakDGForm,gpuAccel)

  END SUBROUTINE CalculateSolutionGradient_DG3D

  SUBROUTINE CalculateFluxDivergence_DG3D(this,gpuAccel)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel

    CALL this % flux % Divergence(this % geometry, &
                                  this % fluxDivergence, &
                                  selfWeakDGForm,gpuAccel)

  END SUBROUTINE CalculateFluxDivergence_DG3D

  SUBROUTINE CalculateDSDt_DG3D(this,gpuAccel)
    !! Adds the flux convergence and source terms together and assigns to dSdt
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this
    LOGICAL,INTENT(in) :: gpuAccel
    ! Local
    INTEGER :: i, j, k, iVar, iEl

    IF( gpuAccel )THEN

      CALL CalculateDSDt_DG3D_gpu_wrapper( this % fluxDivergence % interior % deviceData, &
                                      this % source % interior % deviceData, &
                                      this % dSdt % interior % deviceData, &
                                      this % solution % interp % N, &
                                      this % solution % nVar, &
                                      this % solution % nElem )

    ELSE

      DO iEl = 1, this % solution % nElem
        DO iVar = 1, this % solution % nVar
          DO k = 0, this % solution % interp % N
            DO j = 0, this % solution % interp % N
              DO i = 0, this % solution % interp % N

                this % dSdt % interior % hostData(i,j,k,iVar,iEl) = &
                        this % source % interior % hostData(i,j,k,iVar,iEl) -&
                        this % fluxDivergence % interior % hostData(i,j,k,iVar,iEl)

              ENDDO
            ENDDO
          ENDDO
        ENDDO
      ENDDO

    ENDIF

  END SUBROUTINE CalculateDSDt_DG3D

  SUBROUTINE Write_DG3D(this,fileName)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(in) :: this
    CHARACTER(*),INTENT(in) :: fileName
    ! Local
    INTEGER(HID_T) :: fileId
    INTEGER(HID_T) :: solOffset(1:5)
    INTEGER(HID_T) :: xOffset(1:6)
    INTEGER(HID_T) :: bOffset(1:5)
    INTEGER(HID_T) :: bxOffset(1:6)
    INTEGER(HID_T) :: solGlobalDims(1:5)
    INTEGER(HID_T) :: xGlobalDims(1:6)
    INTEGER(HID_T) :: bGlobalDims(1:5)
    INTEGER(HID_T) :: bxGlobalDims(1:6)
    INTEGER :: firstElem

    IF (this % decomp % mpiEnabled) THEN

      CALL Open_HDF5(fileName,H5F_ACC_TRUNC_F,fileId, &
                     this % decomp % mpiComm)

      firstElem = this % decomp % offsetElem % hostData(this % decomp % rankId)
      solOffset(1:5) = (/0,0,0,1,firstElem/)
      solGlobalDims(1:5) = (/this % solution % interp % N, &
                             this % solution % interp % N, &
                             this % solution % interp % N, &
                             this % solution % nVar, &
                             this % decomp % nElem/)
      xOffset(1:6) = (/1,0,0,0,1,firstElem/)
      xGlobalDims(1:6) = (/3, &
                           this % solution % interp % N, &
                           this % solution % interp % N, &
                           this % solution % interp % N, &
                           this % solution % nVar, &
                           this % decomp % nElem/)

      ! Offsets and dimensions for element boundary data
      bOffset(1:5) = (/0,0,1,1,firstElem/)
      bGlobalDims(1:5) = (/this % solution % interp % N, &
                           this % solution % interp % N, &
                           this % solution % nVar, &
                           6,&
                           this % decomp % nElem/)

      bxOffset(1:6) = (/1,0,0,1,1,firstElem/)
      bxGlobalDims(1:6) = (/3,&
                           this % solution % interp % N, &
                           this % solution % interp % N, &
                           this % solution % nVar, &
                           6,&
                           this % decomp % nElem/)

      CALL CreateGroup_HDF5(fileId,'/quadrature')

      IF( this % decomp % rankId == 0 )THEN
        CALL WriteArray_HDF5(fileId,'/quadrature/xi', &
                             this % solution % interp % controlPoints)

        CALL WriteArray_HDF5(fileId,'/quadrature/weights', &
                             this % solution % interp % qWeights)

        CALL WriteArray_HDF5(fileId,'/quadrature/dgmatrix', &
                             this % solution % interp % dgMatrix)

        CALL WriteArray_HDF5(fileId,'/quadrature/dmatrix', &
                             this % solution % interp % dMatrix)
      ENDIF

      CALL CreateGroup_HDF5(fileId,'/state')

      CALL CreateGroup_HDF5(fileId,'/state/interior')

      CALL CreateGroup_HDF5(fileId,'/state/boundary')

      CALL CreateGroup_HDF5(fileId,'/mesh')

      CALL CreateGroup_HDF5(fileId,'/mesh/interior')

      CALL CreateGroup_HDF5(fileId,'/mesh/boundary')

      CALL WriteArray_HDF5(fileId,'/state/interior/solution', &
                           this % solution % interior,solOffset,solGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solution', &
                           this % solution % boundary,bOffset,bGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/fluxDivergence', &
                           this % fluxDivergence % interior,solOffset,solGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/flux', &
                           this % flux % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/flux', &
                           this % flux % boundary,bxOffset,bxGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/interior/solutionGradient', &
                           this % solutionGradient % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solutionGradient', &
                           this % solutionGradient % boundary,bxOffset,bxGlobalDims)

      CALL WriteArray_HDF5(fileId,'/mesh/interior/x', &
                           this % geometry % x % interior,xOffset,xGlobalDims)

      CALL WriteArray_HDF5(fileId,'/mesh/boundary/x', &
                           this % geometry % x % boundary,bxOffset,bxGlobalDims)

      CALL Close_HDF5(fileId)

    ELSE

      CALL Open_HDF5(fileName,H5F_ACC_TRUNC_F,fileId)

      CALL CreateGroup_HDF5(fileId,'/quadrature')

      CALL WriteArray_HDF5(fileId,'/quadrature/xi', &
                           this % solution % interp % controlPoints)

      CALL WriteArray_HDF5(fileId,'/quadrature/weights', &
                           this % solution % interp % qWeights)

      CALL WriteArray_HDF5(fileId,'/quadrature/dgmatrix', &
                           this % solution % interp % dgMatrix)

      CALL WriteArray_HDF5(fileId,'/quadrature/dmatrix', &
                           this % solution % interp % dMatrix)

      CALL CreateGroup_HDF5(fileId,'/state')

      CALL CreateGroup_HDF5(fileId,'/state/interior')

      CALL CreateGroup_HDF5(fileId,'/state/boundary')

      CALL CreateGroup_HDF5(fileId,'/mesh')

      CALL CreateGroup_HDF5(fileId,'/mesh/interior')

      CALL CreateGroup_HDF5(fileId,'/mesh/boundary')

      CALL WriteArray_HDF5(fileId,'/state/interior/solution',this % solution % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solution',this % solution % boundary)

      CALL WriteArray_HDF5(fileId,'/state/interior/fluxDivergence',this % fluxDivergence % interior)

      CALL WriteArray_HDF5(fileId,'/state/interior/flux',this % flux % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/flux',this % flux % boundary)

      CALL WriteArray_HDF5(fileId,'/state/interior/solutionGradient',this % solutionGradient % interior)

      CALL WriteArray_HDF5(fileId,'/state/boundary/solutionGradient',this % solutionGradient % boundary)

      CALL WriteArray_HDF5(fileId,'/mesh/interior/x',this % geometry % x % interior)

      CALL WriteArray_HDF5(fileId,'/mesh/boundary/x',this % geometry % x % boundary)

      CALL Close_HDF5(fileId)

    END IF

  END SUBROUTINE Write_DG3D

  SUBROUTINE Read_DG3D(this,fileName)
    IMPLICIT NONE
    CLASS(DG3D),INTENT(inout) :: this
    CHARACTER(*),INTENT(in) :: fileName
    ! Local
    INTEGER(HID_T) :: fileId
    INTEGER(HID_T) :: solOffset(1:5)
    INTEGER :: firstElem
    INTEGER :: N

    IF (this % decomp % mpiEnabled) THEN
      CALL Open_HDF5(fileName,H5F_ACC_RDWR_F,fileId, &
                     this % decomp % mpiComm)
    ELSE
      CALL Open_HDF5(fileName,H5F_ACC_RDWR_F,fileId)
    END IF

    CALL ReadAttribute_HDF5(fileId,'N',N)

    IF (this % solution % interp % N /= N) THEN
      STOP 'Error : Solution polynomial degree does not match input file'
    END IF

    IF (this % decomp % mpiEnabled) THEN
      firstElem = this % decomp % offsetElem % hostData(this % decomp % rankId) + 1
      solOffset(1:5) = (/0,0,0,1,firstElem/)
      CALL ReadArray_HDF5(fileId,'/state/interior/solution',this % solution % interior,solOffset)
    ELSE
      CALL ReadArray_HDF5(fileId,'/state/interior/solution',this % solution % interior)
    END IF

    CALL Close_HDF5(fileId)

  END SUBROUTINE Read_DG3D

END MODULE SELF_DG


program construction

    implicit none
    real   , dimension(2,3) :: tsource
    real   , dimension(2,3) :: fsource
    real   , dimension(2,3) :: result
    logical, dimension(2,3) :: mask
    integer                 :: m(6)
    integer                 :: n(4)
    integer                 :: r1(2)
    integer                 :: r2(4)

    interface
        subroutine write_array (a)
            real :: a(:,:)
        end subroutine write_array

        subroutine write_l_array (a)
            logical :: a(:,:)
        end subroutine write_l_array
    end interface

    ! Merge two arrays
    tsource = reshape((/  35,  23,  18,  28,  26,  39 /), (/ 2, 3 /))
    fsource = reshape((/ -35, -23, -18, -28, -26, -39 /), (/ 2, 3 /))
    mask    = reshape((/.true.,.false.,.false.,.true.,.false.,.false./),(/2,3/))

    result = merge(tsource, fsource, mask)
    call write_array(tsource)
    call write_array(fsource)
    call write_l_array(mask)
    call write_array(result)

    ! Pack arrays: gathering nonzero elements
    m  = (/ 1, 0, 0, 0, 5, 0 /)
    r1 = pack(m, m /= 0)
    write(*, fmt="(6(I0, ' '))") r1

    ! Pack arrays: gathering nonzero elements and appending from vector
    n = (/ 1, 0, 0, 2 /)
    r2 = pack(n, n /= 0, (/0, 0, 3, 4/))
    write(*, FMT="(4(I0, ' '))") r2

end program construction

subroutine write_array(a)
    implicit none
    real :: a(:,:)
    integer :: i, j

    do i = lbound(a,1), ubound(a,1)
        write(*,*) (a(i, j), j = lbound(a,2), ubound(a,2))
    end do
    return

end subroutine write_array

subroutine write_l_array(a)
    implicit none
    logical :: a(:,:)
    integer :: i, j

    do i = lbound(a,1), ubound(a,1)
        write(*,*) (a(i, j), j = lbound(a,2), ubound(a,2))
    end do
    return

end subroutine write_l_array


      DOUBLE PRECISION FUNCTION PPND16 (P, IFAULT)
C
C      ALGORITHM AS241  APPL. STATIST. (1988) VOL. 37, NO. 3
C
C      Produces the normal deviate Z corresponding to a given lower
C      tail area of P; Z is accurate to about 1 part in 10**16.
C
C      The hash sums below are the sums of the mantissas of the
C      coefficients.   They are included for use in checking
C      transcription.
C
      DOUBLE PRECISION ZERO, ONE, HALF, SPLIT1, SPLIT2, CONST1,
     *           CONST2, A0, A1, A2, A3, A4, A5, A6, A7,
     *           B1, B2, B3, B4, B5, B6, B7,
     *           C0, C1, C2, C3, C4, C5, C6, C7,
     *           D1, D2, D3, D4, D5, D6, D7,
     *           E0, E1, E2, E3, E4, E5, E6, E7,
     *           F1, F2, F3, F4, F5, F6, F7, P, Q, R
      PARAMETER (ZERO = 0.D0, ONE = 1.D0, HALF = 0.5D0,
     *            SPLIT1 = 0.425D0, SPLIT2 = 5.D0,
     *            CONST1 = 0.180625D0, CONST2 = 1.6D0)
C
C      Coefficients for P close to 0.5
C
      PARAMETER (A0 = 3.3871328727963666080D0,
     *           A1 = 1.3314166789178437745D+2,
     *           A2 = 1.9715909503065514427D+3,
     *           A3 = 1.3731693765509461125D+4,
     *           A4 = 4.5921953931549871457D+4,
     *           A5 = 6.7265770927008700853D+4,
     *           A6 = 3.3430575583588128105D+4,
     *           A7 = 2.5090809287301226727D+3,
     *           B1 = 4.2313330701600911252D+1,
     *           B2 = 6.8718700749205790830D+2,
     *           B3 = 5.3941960214247511077D+3,
     *           B4 = 2.1213794301586595867D+4,
     *           B5 = 3.9307895800092710610D+4,
     *           B6 = 2.8729085735721942674D+4,
     *           B7 = 5.2264952788528545610D+3)
C      HASH SUM AB    55.88319 28806 14901 4439
C
C      Coefficients for P not close to 0, 0.5 or 1.
C
      PARAMETER (C0 = 1.42343711074968357734D0,
     *          C1 = 4.63033784615654529590D0,
     *          C2 = 5.76949722146069140550D0,
     *          C3 = 3.64784832476320460504D0,
     *          C4 = 1.27045825245236838258D0,
     *          C5 = 2.41780725177450611770D-1,
     *          C6=2.27238449892691845833D-2,
     *          C7 = 7.74545014278341407640D-4,
     *          D1 = 2.05319162663775882187D0,
     *          D2 = 1.67638483018380384940D0,
     *          D3 = 6.89767334985100004550D-1,
     *          D4 = 1.48103976427480074590D-1,
     *          D5 = 1.51986665636164571966D-2,
     *          D6 = 5.47593808499534494600D-4,
     *          D7 = 1.05075007164441684324D-9)
C      HASH SUM       49.33206 50330 16102 89036
C
C      Coefficients for P near 0 or 1.
C
      PARAMETER (E0 = 6.65790464350110377720D0,
     *          E1 = 5.46378491116411436990D0,
     *          E2 = 1.78482653991729133580D0,
     *          E3 = 2.96560571828504891230D-1,
     *          E4 = 2.65321895265761230930D-2,
     *          E5 = 1.24266094738807843860D-3,
     *          E6 = 2.71155556874348757815D-5,
     *          E7 = 2.01033439929228813265D-7,
     *          F1 = 5.99832206555887937690D-1,
     *          F2 = 1.36929880922735805310D-1,
     *          F3 = 1.48753612908506148525D-2,
     *          F4 = 7.86869131145613259100D-4,
     *          F5 = 1.84631831751005468180D-5,
     *          F6 = 1.42151175831644588870D-7,
     *          F7 = 2.04426310338993978564D-15)
C      HASH SUM EF    47.52583 31754 92896 71629
C
      IFAULT = 0
      Q = P - HALF
      IF (ABS(Q) .LE. SPLIT1) THEN
        R = CONST1 - Q * Q
        PPND16 = Q * (((((((A7 * R + A6) * R + A5) * R + A4) * R + A3)
     *             * R + A2) * R + A1) * R + A0) /
     *             (((((((B7 * R + B6) * R + B5) * R + B4) * R + B3)
     *             * R + B2) * R + B1) * R + ONE)
        RETURN
      ELSE
        IF (Q .LT. ZERO) THEN
          R = P
        ELSE
          R = ONE - P
        END IF
        IF (R .LE. ZERO) THEN
          IFAULT = 1
          PPND16 = ZERO
          RETURN
        END IF
        R = SQRT(-LOG(R))
        IF (R .LE. SPLIT2) THEN
          R = R - CONST2
          PPND16 = (((((((C7 * R + C6) * R + C5) * R + C4) * R + C3)
     *              * R + C2) * R + C1) * R + C0) /
     *              (((((((D7 * R + D6) * R + D5) * R + D4) * R + D3)
     *              * R + D2) * R + D1) * R + ONE)
        ELSE
          R = R - SPLIT2
          PPND16 = (((((((E7 * R + E6) * R + E5) * R + E4) * R + E3)
     *             * R + E2) * R + E1) * R + E0) /
     *             (((((((F7 * R + F6) * R + F5) * R + F4) * R + F3)
     *             * R + F2) * R + F1) * R + ONE)
        END IF
        IF (Q .LT. ZERO) PPND16 = - PPND16
        RETURN
      END IF
      END

      SUBROUTINE CALERF(ARG,RESULT,JINT)
C------------------------------------------------------------------
C
C This packet evaluates  erf(x),  erfc(x),  and  exp(x*x)*erfc(x)
C   for a real argument  x.  It contains three FUNCTION type
C   subprograms: ERF, ERFC, and ERFCX (or DERF, DERFC, and DERFCX),
C   and one SUBROUTINE type subprogram, CALERF.  The calling
C   statements for the primary entries are:
C
C                   Y=ERF(X)     (or   Y=DERF(X)),
C
C                   Y=ERFC(X)    (or   Y=DERFC(X)),
C   and
C                   Y=ERFCX(X)   (or   Y=DERFCX(X)).
C
C   The routine  CALERF  is intended for internal packet use only,
C   all computations within the packet being concentrated in this
C   routine.  The function subprograms invoke  CALERF  with the
C   statement
C
C          CALL CALERF(ARG,RESULT,JINT)
C
C   where the parameter usage is as follows
C
C      Function                     Parameters for CALERF
C       call              ARG                  Result          JINT
C
C     ERF(ARG)      ANY REAL ARGUMENT         ERF(ARG)          0
C     ERFC(ARG)     ABS(ARG) .LT. XBIG        ERFC(ARG)         1
C     ERFCX(ARG)    XNEG .LT. ARG .LT. XMAX   ERFCX(ARG)        2
C
C   The main computation evaluates near-minimax approximations
C   from "Rational Chebyshev approximations for the error function"
C   by W. J. Cody, Math. Comp., 1969, PP. 631-638.  This
C   transportable program uses rational functions that theoretically
C   approximate  erf(x)  and  erfc(x)  to at least 18 significant
C   decimal digits.  The accuracy achieved depends on the arithmetic
C   system, the compiler, the intrinsic functions, and proper
C   selection of the machine-dependent constants.
C
C*******************************************************************
C*******************************************************************
C
C Explanation of machine-dependent constants
C
C   XMIN   = the smallest positive floating-point number.
C   XINF   = the largest positive finite floating-point number.
C   XNEG   = the largest negative argument acceptable to ERFCX;
C            the negative of the solution to the equation
C            2*exp(x*x) = XINF.
C   XSMALL = argument below which erf(x) may be represented by
C            2*x/sqrt(pi)  and above which  x*x  will not underflow.
C            A conservative value is the largest machine number X
C            such that   1.0 + X = 1.0   to machine precision.
C   XBIG   = largest argument acceptable to ERFC;  solution to
C            the equation:  W(x) * (1-0.5/x**2) = XMIN,  where
C            W(x) = exp(-x*x)/[x*sqrt(pi)].
C   XHUGE  = argument above which  1.0 - 1/(2*x*x) = 1.0  to
C            machine precision.  A conservative value is
C            1/[2*sqrt(XSMALL)]
C   XMAX   = largest acceptable argument to ERFCX; the minimum
C            of XINF and 1/[sqrt(pi)*XMIN].
C
C   Approximate values for some important machines are:
C
C                          XMIN       XINF        XNEG     XSMALL
C
C    C 7600      (S.P.)  3.13E-294   1.26E+322   -27.220  7.11E-15
C  CRAY-1        (S.P.)  4.58E-2467  5.45E+2465  -75.345  7.11E-15
C  IEEE (IBM/XT,
C    SUN, etc.)  (S.P.)  1.18E-38    3.40E+38     -9.382  5.96E-8
C  IEEE (IBM/XT,
C    SUN, etc.)  (D.P.)  2.23D-308   1.79D+308   -26.628  1.11D-16
C  IBM 195       (D.P.)  5.40D-79    7.23E+75    -13.190  1.39D-17
C  UNIVAC 1108   (D.P.)  2.78D-309   8.98D+307   -26.615  1.73D-18
C  VAX D-Format  (D.P.)  2.94D-39    1.70D+38     -9.345  1.39D-17
C  VAX G-Format  (D.P.)  5.56D-309   8.98D+307   -26.615  1.11D-16
C
C
C                          XBIG       XHUGE       XMAX
C
C    C 7600      (S.P.)  25.922      8.39E+6     1.80X+293
C  CRAY-1        (S.P.)  75.326      8.39E+6     5.45E+2465
C  IEEE (IBM/XT,
C    SUN, etc.)  (S.P.)   9.194      2.90E+3     4.79E+37
C  IEEE (IBM/XT,
C    SUN, etc.)  (D.P.)  26.543      6.71D+7     2.53D+307
C  IBM 195       (D.P.)  13.306      1.90D+8     7.23E+75
C  UNIVAC 1108   (D.P.)  26.582      5.37D+8     8.98D+307
C  VAX D-Format  (D.P.)   9.269      1.90D+8     1.70D+38
C  VAX G-Format  (D.P.)  26.569      6.71D+7     8.98D+307
C
C*******************************************************************
C*******************************************************************
C
C Error returns
C
C  The program returns  ERFC = 0      for  ARG .GE. XBIG;
C
C                       ERFCX = XINF  for  ARG .LT. XNEG;
C      and
C                       ERFCX = 0     for  ARG .GE. XMAX.
C
C
C Intrinsic functions required are:
C
C     ABS, AINT, EXP
C
C
C  Author: W. J. Cody
C          Mathematics and Computer Science Division
C          Argonne National Laboratory
C          Argonne, IL 60439
C
C  Latest modification: March 19, 1990
C
C------------------------------------------------------------------
      INTEGER I,JINT
CS    REAL
      DOUBLE PRECISION
     1     A,ARG,B,C,D,DEL,FOUR,HALF,P,ONE,Q,RESULT,SIXTEN,SQRPI,
     2     TWO,THRESH,X,XBIG,XDEN,XHUGE,XINF,XMAX,XNEG,XNUM,XSMALL,
     3     Y,YSQ,ZERO
      DIMENSION A(5),B(4),C(9),D(8),P(6),Q(5)
C------------------------------------------------------------------
C  Mathematical constants
C------------------------------------------------------------------
CS    DATA FOUR,ONE,HALF,TWO,ZERO/4.0E0,1.0E0,0.5E0,2.0E0,0.0E0/,
CS   1     SQRPI/5.6418958354775628695E-1/,THRESH/0.46875E0/,
CS   2     SIXTEN/16.0E0/
      DATA FOUR,ONE,HALF,TWO,ZERO/4.0D0,1.0D0,0.5D0,2.0D0,0.0D0/,
     1     SQRPI/5.6418958354775628695D-1/,THRESH/0.46875D0/,
     2     SIXTEN/16.0D0/
C------------------------------------------------------------------
C  Machine-dependent constants
C------------------------------------------------------------------
CS    DATA XINF,XNEG,XSMALL/3.40E+38,-9.382E0,5.96E-8/,
CS   1     XBIG,XHUGE,XMAX/9.194E0,2.90E3,4.79E37/
      DATA XINF,XNEG,XSMALL/1.79D308,-26.628D0,1.11D-16/,
     1     XBIG,XHUGE,XMAX/26.543D0,6.71D7,2.53D307/
C------------------------------------------------------------------
C  Coefficients for approximation to  erf  in first interval
C------------------------------------------------------------------
CS    DATA A/3.16112374387056560E00,1.13864154151050156E02,
CS   1       3.77485237685302021E02,3.20937758913846947E03,
CS   2       1.85777706184603153E-1/
CS    DATA B/2.36012909523441209E01,2.44024637934444173E02,
CS   1       1.28261652607737228E03,2.84423683343917062E03/
      DATA A/3.16112374387056560D00,1.13864154151050156D02,
     1       3.77485237685302021D02,3.20937758913846947D03,
     2       1.85777706184603153D-1/
      DATA B/2.36012909523441209D01,2.44024637934444173D02,
     1       1.28261652607737228D03,2.84423683343917062D03/
C------------------------------------------------------------------
C  Coefficients for approximation to  erfc  in second interval
C------------------------------------------------------------------
CS    DATA C/5.64188496988670089E-1,8.88314979438837594E0,
CS   1       6.61191906371416295E01,2.98635138197400131E02,
CS   2       8.81952221241769090E02,1.71204761263407058E03,
CS   3       2.05107837782607147E03,1.23033935479799725E03,
CS   4       2.15311535474403846E-8/
CS    DATA D/1.57449261107098347E01,1.17693950891312499E02,
CS   1       5.37181101862009858E02,1.62138957456669019E03,
CS   2       3.29079923573345963E03,4.36261909014324716E03,
CS   3       3.43936767414372164E03,1.23033935480374942E03/
      DATA C/5.64188496988670089D-1,8.88314979438837594D0,
     1       6.61191906371416295D01,2.98635138197400131D02,
     2       8.81952221241769090D02,1.71204761263407058D03,
     3       2.05107837782607147D03,1.23033935479799725D03,
     4       2.15311535474403846D-8/
      DATA D/1.57449261107098347D01,1.17693950891312499D02,
     1       5.37181101862009858D02,1.62138957456669019D03,
     2       3.29079923573345963D03,4.36261909014324716D03,
     3       3.43936767414372164D03,1.23033935480374942D03/
C------------------------------------------------------------------
C  Coefficients for approximation to  erfc  in third interval
C------------------------------------------------------------------
CS    DATA P/3.05326634961232344E-1,3.60344899949804439E-1,
CS   1       1.25781726111229246E-1,1.60837851487422766E-2,
CS   2       6.58749161529837803E-4,1.63153871373020978E-2/
CS    DATA Q/2.56852019228982242E00,1.87295284992346047E00,
CS   1       5.27905102951428412E-1,6.05183413124413191E-2,
CS   2       2.33520497626869185E-3/
      DATA P/3.05326634961232344D-1,3.60344899949804439D-1,
     1       1.25781726111229246D-1,1.60837851487422766D-2,
     2       6.58749161529837803D-4,1.63153871373020978D-2/
      DATA Q/2.56852019228982242D00,1.87295284992346047D00,
     1       5.27905102951428412D-1,6.05183413124413191D-2,
     2       2.33520497626869185D-3/
C------------------------------------------------------------------
      X = ARG
      Y = ABS(X)
      IF (Y .LE. THRESH) THEN
C------------------------------------------------------------------
C  Evaluate  erf  for  |X| <= 0.46875
C------------------------------------------------------------------
            YSQ = ZERO
            IF (Y .GT. XSMALL) YSQ = Y * Y
            XNUM = A(5)*YSQ
            XDEN = YSQ
            DO 20 I = 1, 3
               XNUM = (XNUM + A(I)) * YSQ
               XDEN = (XDEN + B(I)) * YSQ
   20       CONTINUE
            RESULT = X * (XNUM + A(4)) / (XDEN + B(4))
            IF (JINT .NE. 0) RESULT = ONE - RESULT
            IF (JINT .EQ. 2) RESULT = EXP(YSQ) * RESULT
            GO TO 800
C------------------------------------------------------------------
C  Evaluate  erfc  for 0.46875 <= |X| <= 4.0
C------------------------------------------------------------------
         ELSE IF (Y .LE. FOUR) THEN
            XNUM = C(9)*Y
            XDEN = Y
            DO 120 I = 1, 7
               XNUM = (XNUM + C(I)) * Y
               XDEN = (XDEN + D(I)) * Y
  120       CONTINUE
            RESULT = (XNUM + C(8)) / (XDEN + D(8))
            IF (JINT .NE. 2) THEN
               YSQ = AINT(Y*SIXTEN)/SIXTEN
               DEL = (Y-YSQ)*(Y+YSQ)
               RESULT = EXP(-YSQ*YSQ) * EXP(-DEL) * RESULT
            END IF
C------------------------------------------------------------------
C  Evaluate  erfc  for |X| > 4.0
C------------------------------------------------------------------
         ELSE
            RESULT = ZERO
            IF (Y .GE. XBIG) THEN
               IF ((JINT .NE. 2) .OR. (Y .GE. XMAX)) GO TO 300
               IF (Y .GE. XHUGE) THEN
                  RESULT = SQRPI / Y
                  GO TO 300
               END IF
            END IF
            YSQ = ONE / (Y * Y)
            XNUM = P(6)*YSQ
            XDEN = YSQ
            DO 240 I = 1, 4
               XNUM = (XNUM + P(I)) * YSQ
               XDEN = (XDEN + Q(I)) * YSQ
  240       CONTINUE
            RESULT = YSQ *(XNUM + P(5)) / (XDEN + Q(5))
            RESULT = (SQRPI -  RESULT) / Y
            IF (JINT .NE. 2) THEN
               YSQ = AINT(Y*SIXTEN)/SIXTEN
               DEL = (Y-YSQ)*(Y+YSQ)
               RESULT = EXP(-YSQ*YSQ) * EXP(-DEL) * RESULT
            END IF
      END IF
C------------------------------------------------------------------
C  Fix up for negative argument, erf, etc.
C------------------------------------------------------------------
  300 IF (JINT .EQ. 0) THEN
            RESULT = (HALF - RESULT) + HALF
            IF (X .LT. ZERO) RESULT = -RESULT
         ELSE IF (JINT .EQ. 1) THEN
            IF (X .LT. ZERO) RESULT = TWO - RESULT
         ELSE
            IF (X .LT. ZERO) THEN
               IF (X .LT. XNEG) THEN
                     RESULT = XINF
                  ELSE
                     YSQ = AINT(X*SIXTEN)/SIXTEN
                     DEL = (X-YSQ)*(X+YSQ)
                     Y = EXP(YSQ*YSQ) * EXP(DEL)
                     RESULT = (Y+Y) - RESULT
               END IF
            END IF
      END IF
  800 RETURN
C---------- Last card of CALERF ----------
      END
CS    REAL FUNCTION ERF(X)
      DOUBLE PRECISION FUNCTION DERF(X)
C--------------------------------------------------------------------
C
C This subprogram computes approximate values for erf(x).
C   (see comments heading CALERF).
C
C   Author/date: W. J. Cody, January 8, 1985
C
C--------------------------------------------------------------------
      INTEGER JINT
CS    REAL             X, RESULT
      DOUBLE PRECISION X, RESULT
C------------------------------------------------------------------
      JINT = 0
      CALL CALERF(X,RESULT,JINT)
CS    ERF = RESULT
      DERF = RESULT
      RETURN
C---------- Last card of DERF ----------
      END
CS    REAL FUNCTION ERFC(X)
      DOUBLE PRECISION FUNCTION DERFC(X)
C--------------------------------------------------------------------
C
C This subprogram computes approximate values for erfc(x).
C   (see comments heading CALERF).
C
C   Author/date: W. J. Cody, January 8, 1985
C
C--------------------------------------------------------------------
      INTEGER JINT
CS    REAL             X, RESULT
      DOUBLE PRECISION X, RESULT
C------------------------------------------------------------------
      JINT = 1
      CALL CALERF(X,RESULT,JINT)
CS    ERFC = RESULT
      DERFC = RESULT
      RETURN
C---------- Last card of DERFC ----------
      END
CS    REAL FUNCTION ERFCX(X)
      DOUBLE PRECISION FUNCTION DERFCX(X)
C------------------------------------------------------------------
C
C This subprogram computes approximate values for exp(x*x) * erfc(x).
C   (see comments heading CALERF).
C
C   Author/date: W. J. Cody, March 30, 1987
C
C------------------------------------------------------------------
      INTEGER JINT
CS    REAL             X, RESULT
      DOUBLE PRECISION X, RESULT
C------------------------------------------------------------------
      JINT = 2
      CALL CALERF(X,RESULT,JINT)
CS    ERFCX = RESULT
      DERFCX = RESULT
      RETURN
C---------- Last card of DERFCX ----------
      END


c this file contains the following subroutines:
c
c readharmfile: reads a file with the harmonic expansion of the potential and density.
c all the relevant stuff ends up in a common block that only needs to be seen by
c the routines in this file (I think!). This routine must be called first.
c
c dens(r,z): density at point (r,z)
c
c densrpsi(r,psi): density as a function of r and potential psi.
c
c pot(r,z): potential at point (r,z)
c
c force(r,z,fr,fz): radial and z  force at (r,z)
c all the rest is (slightly nobbled in places) numerical recipes routines.
c

c
c calculate the force by interpolating the derived radial functions
c
c      subroutine force(lm,s,z,fs,fz,fs1,fz1)
      subroutine force(s,z,fs,fz,pot)
      parameter (pi=3.1415926535)
      parameter (pc0=0.282094792, pc2=0.630783131, pc4=0.846284375)
c constants cn are sqrt((2*l+1)/(4*pi))
      parameter (pc6=1.017107236, pc8=1.163106623)

      dimension pc(20), p(20), dp(20)
      common /potconstants/ apot(20,0:20000), frad(20,0:20000),
     +     dr, nr, lmax, potcor
      common /legendre/ plcon(0:40)
      common /gparameters/ a,b,c,v0,q,psi00,
     +                      psiouta, rho1a, sigbulge,
     +                      grmdisk, grdisk, gzdisk, goutdisk, gdrtrunc,
     +                      potcor1
      common /moreconstants/ v02, v03, rdisk2, diskconst, bulgea
      common /diskpars/ sigr0, disksr, nrdisk

      common /flags/ idiskflag, ibulgeflag, ihaloflag

c      write(*,*) 'Force entered with s,z=',s,z
c      write(*,*) 'lmax =',lmax
c      write(*,*) 'apot starts as:'
c      do ll=1,3
c         write(*,*) (apot(ll,i),i=1,10)
c      enddo
      r=sqrt(s*s+z*z)
      ihi=int(r/dr)+1
      if (ihi.lt.1) ihi=1
      if (ihi.gt.nr) ihi=nr
      r1=dr*(ihi-1)
      r2=dr*ihi
      redge = nr*dr
      t=(r-r1)/(r2-r1)
      ihim1 = ihi - 1
      tm1 = 1.0 - t
      if (r.eq.0.) then
         fs = 0.0
         fz = 0.0
      else
         costheta=z/r
         ct2 = costheta*costheta
         sintheta=s/r

         do l=0,lmax,2
            pc(l/2+1) = sqrt((2.0*l + 1)/(4.0*pi))
            p(l/2+1) = plgndr1(l,costheta)
			if( costheta .eq. 1.0 ) then
               dp(l/2+1) = 0.0
            else
               st2 = 1.0 - costheta*costheta
               dp(l/2+1) = l*(plgndr1(l-1, costheta) - costheta*p(l/2+1))/st2
            endif
         enddo
         do i=1,lmax/2+1
            p(i) = p(i)*pc(i)
            dp(i) = dp(i)*pc(i)
         enddo

         if( r .le. redge ) then
             fr = 0.0
             do i=1,lmax/2+1
                fr = fr + p(i)*(t*frad(i,ihi) + tm1*frad(i,ihim1))
             enddo
             fth = 0.0
             do i=2,lmax/2+1
                fth = fth - sintheta*dp(i)*(t*apot(i,ihi) + tm1*apot(i,ihim1))
             enddo
             pot = 0.0
             do i=1,lmax/2+1
                pot = pot + p(i)*(t*apot(i,ihi) + tm1*apot(i,ihim1))
             enddo
         else
! expressions below for fr and pot not correct if apot shifted!!
             fr = 0.0
             do i=1,lmax/2+1
                 l = 2*(i-1)
                 fr = fr - (l+1)*p(i)*apot(i,nr)/redge*(redge/r)**(l+2)
             enddo
             fth = 0.0
             do i=2,lmax/2+1
                 l = 2*(i-1)
                 fth = fth - sintheta*dp(i)*apot(i,nr)*(redge/r)**(l+1)
	     enddo
             pot = 0.0
             do i=1,lmax/2+1
                l = 2*(i-1)
                pot = pot + p(i)*apot(i,nr)*(redge/r)**(l+1)
             enddo
         endif
         if( idiskflag .eq. 1.OR.idiskflag.EQ.3 ) then
             pot = pot + appdiskpot(s,z)
         endif

      if( idiskflag .eq. 2 .or. idiskflag .eq. 3) then
       pot= pot + thickdiskpot(s,z)
!       fr=fr+thickdiskforce(r)
! fth (or rather fs and fz directly) tbd, see below
      endif

      if(ihaloflag.eq.2) then
       pot= pot + fixedpot(r)
       fr=fr-fixedforce(r)
! fixed force negative, fr positive
! fixed halo is spherical so no fth
      endif

         fs = -(sintheta*fr + costheta/r*fth)
         fz = -(costheta*fr - sintheta/r*fth)

!         write(*,*) 'Force before disk correction ',fr,fth
         if( idiskflag .eq. 1 .OR. idiskflag.EQ.3) then
            call appdiskforce(s,z,fsad,fzad)
c            write(*,*) 'Force of disk correction ',fsad,fzad
            fs = fs + fsad
            fz = fz + fzad
         endif

         if( idiskflag .eq. 2 .OR. idiskflag.EQ.3) then
!            print*,fs,fz,thickdiskrforce(s,z)
            fs = fs + thickdiskrforce(s,z)
            fz = fz + thickdiskzforce(s,z)
         endif
      endif
      return
      end


      SUBROUTINE MB03CZ( A, LDA, B, LDB, D, LDD, CO1, SI1, CO2, SI2,
     $                   CO3, SI3 )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute unitary matrices Q1, Q2, and Q3 for a complex 2-by-2
C     regular pencil aAB - bD, with A, B, D upper triangular, such that
C     Q3' A Q2, Q2' B Q1, Q3' D Q1 are still upper triangular, but the
C     eigenvalues are in reversed order. The matrices Q1, Q2, and Q3 are
C     represented by
C
C          (  CO1  SI1  )       (  CO2  SI2  )       (  CO3  SI3  )
C     Q1 = (            ), Q2 = (            ), Q3 = (            ).
C          ( -SI1' CO1  )       ( -SI2' CO2  )       ( -SI3' CO3  )
C
C     The notation M' denotes the conjugate transpose of the matrix M.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     A       (input) COMPLEX*16 array, dimension (LDA, 2)
C             On entry, the leading 2-by-2 upper triangular part of
C             this array must contain the matrix A of the pencil.
C             The (2,1) entry is not referenced.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= 2.
C
C     B       (input) COMPLEX*16 array, dimension (LDB, 2)
C             On entry, the leading 2-by-2 upper triangular part of
C             this array must contain the matrix B of the pencil.
C             The (2,1) entry is not referenced.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= 2.
C
C     D       (input) COMPLEX*16 array, dimension (LDD, 2)
C             On entry, the leading 2-by-2 upper triangular part of
C             this array must contain the matrix D of the pencil.
C             The (2,1) entry is not referenced.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= 2.
C
C     CO1     (output) DOUBLE PRECISION
C             The upper left element of the unitary matrix Q1.
C
C     SI1     (output) COMPLEX*16
C             The upper right element of the unitary matrix Q1.
C
C     CO2     (output) DOUBLE PRECISION
C             The upper left element of the unitary matrix Q2.
C
C     SI2     (output) COMPLEX*16
C             The upper right element of the unitary matrix Q2.
C
C     CO3     (output) DOUBLE PRECISION
C             The upper left element of the unitary matrix Q3.
C
C     SI3     (output) COMPLEX*16
C             The upper right element of the unitary matrix Q3.
C
C     METHOD
C
C     The algorithm uses unitary transformations as described on page 37
C     in [1].
C
C     REFERENCES
C
C     [1] Benner, P., Byers, R., Mehrmann, V. and Xu, H.
C         Numerical Computation of Deflating Subspaces of Embedded
C         Hamiltonian Pencils.
C         Tech. Rep. SFB393/99-15, Technical University Chemnitz,
C         Germany, June 1999.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically backward stable.
C
C     CONTRIBUTOR
C
C     Matthias Voigt, Fakultaet fuer Mathematik, Technische Universitaet
C     Chemnitz, April 21, 2009.
C
C     REVISIONS
C
C     V. Sima, Aug. 2009 (SLICOT version of the routine ZBTFEX).
C     V. Sima, Nov. 2009, Nov. 2010, Dec. 2010.
C     M. Voigt, Jan. 2012.
C
C     KEYWORDS
C
C     Eigenvalue exchange, matrix pencil, upper triangular matrix.
C
C     ******************************************************************
C
C     .. Scalar Arguments ..
      INTEGER            LDA, LDB, LDD
      DOUBLE PRECISION   CO1, CO2, CO3
      COMPLEX*16         SI1, SI2, SI3
C
C     .. Array Arguments ..
      COMPLEX*16         A( LDA, * ), B( LDB, * ), D( LDD, * )
C
C     .. Local Scalars ..
      COMPLEX*16         F, G, TMP
C
C     .. External Subroutines ..
      EXTERNAL           ZLARTG
C
C     .. Executable Statements ..
C
C     For efficiency, the input arguments are not tested.
C
C     Computations.
C
      G = A( 1, 1 )*B( 1, 1 )*D( 2, 2 ) - A( 2, 2 )*B( 2, 2 )*D( 1, 1 )
      F = ( A( 1, 1 )*B( 1, 2 ) + A( 1, 2 )*B( 2, 2 ) )*D( 2, 2 ) -
     $      A( 2, 2 )*B( 2, 2 )*D( 1, 2 )
      CALL ZLARTG( F, G, CO1, SI1, TMP )
      F = ( A( 1, 2 )*D( 2, 2 ) - A( 2, 2 )*D( 1, 2 ) )*B( 1, 1 ) +
     $      A( 2, 2 )*D( 1, 1 )*B( 1, 2 )
      CALL ZLARTG( F, G, CO2, SI2, TMP )
      F = ( B( 1, 2 )*D( 1, 1 ) - B( 1, 1 )*D( 1, 2 ) )*A( 1, 1 ) +
     $      A( 1, 2 )*B( 2, 2 )*D( 1, 1 )
      CALL ZLARTG( F, G, CO3, SI3, TMP )
C
      RETURN
C *** Last line of MB03CZ ***
      END


C++******************************************************************
C Program SHELLS_PLOT
C To plot shell distributions generated by Peter Quinn's programs
C
C JLP Version 23-09-87
C--******************************************************************
	PROGRAM SHELLS_PLOT
	PARAMETER (IDIM1=600,KCUR=20)
	REAL*4 INPUT(IDIM1,5)
	REAL*4 XPLOT(IDIM1,KCUR),YPLOT(IDIM1,KCUR)
	INTEGER NPTS(KCUR)
	CHARACTER NCHAR(KCUR)*4,PCOLOR(KCUR)*30
	CHARACTER CHAR1*30,CHAR2*30,TITLE*40,PLOTDEV*32
	CHARACTER ANS*1,NAME*40,BUFF*80
	LOGICAL REAL_DATA

10	FORMAT(A)

	PRINT 21
21	FORMAT(' PROGRAM SHELL_PLOT',/,
     1	' VERSION OF 01-02-88')

C Enter the columns to plot :
	PRINT 26
26	FORMAT(' TYPE OF DIAGRAM : 1= ln(dn/d1) vs ln(n-phi+tau-1) ',
     1	/,' OR                  2= ln(dn/d1) vs n  ?')
	READ(5,*) ICHOICE
	 IF(ICHOICE.EQ.1)THEN
	  CHAR1=' ln(n-phi+tau-1)'
	  CHAR2='ln(dn/d1)'
	  ICOLX=5
	  ICOLY=4
	 ELSE
	  CHAR1=' n'
	  CHAR2='ln(dn/d1)'
	  ICOLX=1
	  ICOLY=4
	 ENDIF

C Initialize the number of curves :
	KCU=0

C****************** LOOP ON THE INPUT FILES *****************
C Open the input file :
22	KCU=KCU+1
	PRINT *,' INPUT FILE ?'
	READ(5,10) NAME
	OPEN(1,FILE=NAME,STATUS='OLD',ERR=22)

C Read the header :
	READ(1,10)BUFF
	PRINT *,BUFF
	READ(1,10)BUFF
	PRINT *,BUFF
	READ(1,10)BUFF
	PRINT *,BUFF

C Read the file :
	 DO I=1,1000
	  READ(1,*,END=999) (INPUT(I,K),K=1,5)
	 END DO
999	CLOSE(1)
	NPTS(KCU)=I-1
	PRINT *,NPTS(KCU),' POINTS ENTERED'

C Check if it is real data or not :
	PRINT *,' DO THEY RESULT FROM ACTUAL MEASUREMENTS',
     1	' OF A SHELL GALAXY (N)?'
	READ(5,10) ANS
	REAL_DATA=(ANS.EQ.'Y'.OR.ANS.EQ.'y')

C Load the curve (normalization with the last value) :
	IF(REAL_DATA)THEN
	    PRINT *,' ENTER TAU AND PHI :'
	    READ(5,*) TAU,PHI
	  IF(ICHOICE.EQ.1)THEN
	     DO I=1,NPTS(KCU)
	       XPLOT(I,KCU)=ALOG(INPUT(I,1)+TAU-PHI)
	       YPLOT(I,KCU)=INPUT(I,ICOLY)-INPUT(NPTS(KCU),ICOLY)
	     END DO
	  ELSE
	     DO I=1,NPTS(KCU)
	       XPLOT(I,KCU)=INPUT(I,1)+TAU
	       YPLOT(I,KCU)=INPUT(I,ICOLY)-INPUT(NPTS(KCU),ICOLY)
	     END DO
	  ENDIF
	ELSE
	    DO I=1,NPTS(KCU)
	      XPLOT(I,KCU)=INPUT(I,ICOLX)
	      YPLOT(I,KCU)=INPUT(I,ICOLY)-INPUT(NPTS(KCU),ICOLY)
	    END DO
	ENDIF

	PRINT *,' 2,3 TRIANGLE  4,5 + X  6,7 SQUARES 8,9  O '
	PRINT *,' SYMBOL : 42=CROSSES, 82=CIRCLES, ... ?'
	READ(5,10) NCHAR(KCU)
        PCOLOR(KCU)='Default'

	PRINT *,' DO YOU WANT TO DISPLAY ANOTHER CURVE ? (N)'
	READ(5,10) ANS
	IF(ANS.EQ.'Y'.OR.ANS.EQ.'y') GOTO 22

C********************* DISPLAYING THE CURVES *****************
C Plot the curves :
	  PRINT *,' OUTPUT DEVICE : FILE, TEKTRO, CIFER_T5,...'
	  READ(5,10) PLOTDEV
	  PRINT *,' TITLE ?'
	  READ(5,10) TITLE

	  CALL NEWPLOT(XPLOT,YPLOT,NPTS,IDIM1,KCU,
     1	CHAR1,CHAR2,TITLE,NCHAR,PCOLOR,PLOTDEV,' ',' ')

	END


INTEGER(kind = INT16), DIMENSION(0:7), PARAMETER                                :: const_factorial_INT16 = (/ &
                                                                                1_INT16, &
                                                                                1_INT16, &
                                                                                2_INT16, &
                                                                                6_INT16, &
                                                                                24_INT16, &
                                                                                120_INT16, &
                                                                                720_INT16, &
                                                                                5040_INT16  &
                                                                                /)