*DECK BKISR
      SUBROUTINE BKISR (X, N, SUM, IERR)
C***BEGIN PROLOGUE  BKISR
C***SUBSIDIARY
C***PURPOSE  Subsidiary to BSKIN
C***LIBRARY   SLATEC
C***TYPE      SINGLE PRECISION (BKISR-S, DBKISR-D)
C***AUTHOR  Amos, D. E., (SNLA)
C***DESCRIPTION
C
C     BKISR computes repeated integrals of the K0 Bessel function
C     by the series for N=0,1, and 2.
C
C***SEE ALSO  BSKIN
C***ROUTINES CALLED  PSIXN, R1MACH
C***REVISION HISTORY  (YYMMDD)
C   820601  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900328  Added TYPE section.  (WRB)
C   910722  Updated AUTHOR section.  (ALS)
C***END PROLOGUE  BKISR
      INTEGER I, IERR, K, KK, KKN, K1, N, NP
      REAL AK, ATOL, BK, C, FK, FN, HX, HXS, POL, PR, SUM, TKP, TOL,
     * TRM, X, XLN
      REAL PSIXN, R1MACH
      DIMENSION C(2)
      SAVE C
C
      DATA C(1), C(2) /1.57079632679489662E+00,1.0E0/
C***FIRST EXECUTABLE STATEMENT  BKISR
      IERR=0
      TOL = MAX(R1MACH(4),1.0E-18)
      IF (X.LT.TOL) GO TO 50
      PR = 1.0E0
      POL = 0.0E0
      IF (N.EQ.0) GO TO 20
      DO 10 I=1,N
        POL = -POL*X + C(I)
        PR = PR*X/I
   10 CONTINUE
   20 CONTINUE
      HX = X*0.5E0
      HXS = HX*HX
      XLN = LOG(HX)
      NP = N + 1
      TKP = 3.0E0
      FK = 2.0E0
      FN = N
      BK = 4.0E0
      AK = 2.0E0/((FN+1.0E0)*(FN+2.0E0))
      SUM = AK*(PSIXN(N+3)-PSIXN(3)+PSIXN(2)-XLN)
      ATOL = SUM*TOL*0.75E0
      DO 30 K=2,20
        AK = AK*(HXS/BK)*((TKP+1.0E0)/(TKP+FN+1.0E0))*(TKP/(TKP+FN))
        K1 = K + 1
        KK = K1 + K
        KKN = KK + N
        TRM = (PSIXN(K1)+PSIXN(KKN)-PSIXN(KK)-XLN)*AK
        SUM = SUM + TRM
        IF (ABS(TRM).LE.ATOL) GO TO 40
        TKP = TKP + 2.0E0
        BK = BK + TKP
        FK = FK + 1.0E0
   30 CONTINUE
      GO TO 80
   40 CONTINUE
      SUM = (SUM*HXS+PSIXN(NP)-XLN)*PR
      IF (N.EQ.1) SUM = -SUM
      SUM = POL + SUM
      RETURN
C-----------------------------------------------------------------------
C     SMALL X CASE, X.LT.WORD TOLERANCE
C-----------------------------------------------------------------------
   50 CONTINUE
      IF (N.GT.0) GO TO 60
      HX = X*0.5E0
      SUM = PSIXN(1) - LOG(HX)
      RETURN
   60 CONTINUE
      SUM = C(N)
      RETURN
   80 CONTINUE
      IERR=2
      RETURN
      END


!-----------------------------------------------------------------------
!
! Header with common subroutine
!
      include 't320-basis-f.h'
!
! Header with QFunctions
!
      include 't510-operator-f.h'
!-----------------------------------------------------------------------
      program test
      implicit none
      include 'ceed/fortran.h'

      integer ceed,err,i
      integer stridesu(3)
      integer erestrictx,erestrictu,erestrictui
      integer bx,bu
      integer qf_setup,qf_mass
      integer op_setup,op_mass
      integer qdata,x,u,v
      integer nelem,p,q,d
      integer val,row,col,offset
      parameter(nelem=12)
      parameter(p=6)
      parameter(q=4)
      parameter(d=2)
      integer ndofs,nqpts,nx,ny
      parameter(nx=3)
      parameter(ny=2)
      parameter(ndofs=(nx*2+1)*(ny*2+1))
      parameter(nqpts=nelem*q)
      integer indx(nelem*p)
      real*8 arrx(d*ndofs)
      integer*8 voffset,xoffset

      real*8 qref(d*q)
      real*8 qweight(q)
      real*8 interp(p*q)
      real*8 grad(d*p*q)
      real*8 total

      real*8 hv(ndofs)

      character arg*32

      external setup,mass

      call getarg(1,arg)

      call ceedinit(trim(arg)//char(0),ceed,err)

      do i=0,ndofs-1
        arrx(i+1)=mod(i,(nx*2+1))
        arrx(i+1)=arrx(i+1)*(1.d0/(nx*2.d0))
        val=(i/(nx*2+1))
        arrx(i+1+ndofs)=val*(1.d0/(ny*2.d0))
      enddo
      do i=0,5
        col=mod(i,nx)
        row=i/nx
        offset=col*2+row*(nx*2+1)*2

        indx(i*2*p+1)=2+offset
        indx(i*2*p+2)=9+offset
        indx(i*2*p+3)=16+offset
        indx(i*2*p+4)=1+offset
        indx(i*2*p+5)=8+offset
        indx(i*2*p+6)=0+offset

        indx(i*2*p+7)=14+offset
        indx(i*2*p+8)=7+offset
        indx(i*2*p+9)=0+offset
        indx(i*2*p+10)=15+offset
        indx(i*2*p+11)=8+offset
        indx(i*2*p+12)=16+offset
      enddo

      call ceedelemrestrictioncreate(ceed,nelem,p,d,ndofs,d*ndofs,&
     & ceed_mem_host,ceed_use_pointer,indx,erestrictx,err)

      call ceedelemrestrictioncreate(ceed,nelem,p,1,1,ndofs,ceed_mem_host,&
     & ceed_use_pointer,indx,erestrictu,err)
      stridesu=[1,q,q]
      call ceedelemrestrictioncreatestrided(ceed,nelem,q,1,nqpts,stridesu,&
     & erestrictui,err)

      call buildmats(qref,qweight,interp,grad)
      call ceedbasiscreateh1(ceed,ceed_triangle,d,p,q,interp,grad,qref,qweight,&
     & bx,err)
      call buildmats(qref,qweight,interp,grad)
      call ceedbasiscreateh1(ceed,ceed_triangle,1,p,q,interp,grad,qref,qweight,&
     & bu,err)

      call ceedqfunctioncreateinterior(ceed,1,setup,&
     &SOURCE_DIR&
     &//'t510-operator.h:setup'//char(0),qf_setup,err)
      call ceedqfunctionaddinput(qf_setup,'_weight',1,ceed_eval_weight,err)
      call ceedqfunctionaddinput(qf_setup,'x',d*d,ceed_eval_grad,err)
      call ceedqfunctionaddoutput(qf_setup,'rho',1,ceed_eval_none,err)

      call ceedqfunctioncreateinterior(ceed,1,mass,&
     &SOURCE_DIR&
     &//'t510-operator.h:mass'//char(0),qf_mass,err)
      call ceedqfunctionaddinput(qf_mass,'rho',1,ceed_eval_none,err)
      call ceedqfunctionaddinput(qf_mass,'u',1,ceed_eval_interp,err)
      call ceedqfunctionaddoutput(qf_mass,'v',1,ceed_eval_interp,err)

      call ceedoperatorcreate(ceed,qf_setup,ceed_qfunction_none,&
     & ceed_qfunction_none,op_setup,err)
      call ceedoperatorcreate(ceed,qf_mass,ceed_qfunction_none,&
     & ceed_qfunction_none,op_mass,err)

      call ceedvectorcreate(ceed,d*ndofs,x,err)
      xoffset=0
      call ceedvectorsetarray(x,ceed_mem_host,ceed_use_pointer,arrx,xoffset,err)
      call ceedvectorcreate(ceed,nqpts,qdata,err)

      call ceedoperatorsetfield(op_setup,'_weight',ceed_elemrestriction_none,&
     & bx,ceed_vector_none,err)
      call ceedoperatorsetfield(op_setup,'x',erestrictx,bx,&
     & ceed_vector_active,err)
      call ceedoperatorsetfield(op_setup,'rho',erestrictui,&
     & ceed_basis_collocated,ceed_vector_active,err)
      call ceedoperatorsetfield(op_mass,'rho',erestrictui,&
     & ceed_basis_collocated,qdata,err)
      call ceedoperatorsetfield(op_mass,'u',erestrictu,bu,&
     & ceed_vector_active,err)
      call ceedoperatorsetfield(op_mass,'v',erestrictu,bu,&
     & ceed_vector_active,err)

      call ceedoperatorapply(op_setup,x,qdata,ceed_request_immediate,err)

      call ceedvectorcreate(ceed,ndofs,u,err)
      call ceedvectorsetvalue(u,1.d0,err)
      call ceedvectorcreate(ceed,ndofs,v,err)
      call ceedoperatorapply(op_mass,u,v,ceed_request_immediate,err)

      call ceedvectorgetarrayread(v,ceed_mem_host,hv,voffset,err)
      total=0.
      do i=1,ndofs
        total=total+hv(voffset+i)
      enddo
      if (abs(total-1.)>1.0d-10) then
! LCOV_EXCL_START
        write(*,*) 'Computed Area: ',total,' != True Area: 1.0'
! LCOV_EXCL_STOP
      endif
      call ceedvectorrestorearrayread(v,hv,voffset,err)

      call ceedvectordestroy(qdata,err)
      call ceedvectordestroy(x,err)
      call ceedvectordestroy(u,err)
      call ceedvectordestroy(v,err)
      call ceedoperatordestroy(op_mass,err)
      call ceedoperatordestroy(op_setup,err)
      call ceedqfunctiondestroy(qf_mass,err)
      call ceedqfunctiondestroy(qf_setup,err)
      call ceedbasisdestroy(bu,err)
      call ceedbasisdestroy(bx,err)
      call ceedelemrestrictiondestroy(erestrictu,err)
      call ceedelemrestrictiondestroy(erestrictx,err)
      call ceedelemrestrictiondestroy(erestrictui,err)
      call ceeddestroy(ceed,err)
      end
!-----------------------------------------------------------------------


c     # ------------------------------------------------------------
c     # Compute cell-centered velocity fields
c     #
c     # Define cell-centered velocity field using basis functions
c     #
c     #      u = u1*t1 + u2*t2   (div u might not be zero)
c     #
c     # NOTE: All arguments to routines here should be in computational
c     # coordinates (x,y) in [0,1]x[0,1].  Mapping from brick domains
c     # to [0,1]x[0,1] should be done from calling routines, as should
c     # any mappings that convert from an orthogonal coordinate system
c     # to a non-orthogonal system.
c     # ------------------------------------------------------------

      subroutine annulus_velocity_derivs(x,y,t, u,vcart,derivs,flag)
      implicit none

      double precision x, y, t, u(2), vcart(3), derivs(4)
      integer flag

      double precision pi, pi2
      common /compi/ pi, pi2

      double precision revs_per_s, cart_speed, amplitude, freq
      common /velocity_comm/ revs_per_s, cart_speed, amplitude, freq

      integer example
      common /example_comm/ example

      double precision xp,yp,zp, ravg, xc, d, tfinal, A
      double precision r, theta, w, nc

c     # Set non-zeros derivs only
      if (example .eq. 0) then
         flag = 0
c        # Rigid body rotation
         u(1) = revs_per_s
         u(2) = 0
         derivs(1) = 0
         derivs(2) = 0
         derivs(3) = 0
         derivs(4) = 0
      else
          flag = 1
          derivs(1) = 0
          derivs(2) = 0
          derivs(3) = 0
          if (example .eq. 1) then
              vcart(1) = cart_speed
              vcart(2) = 0
          elseif (example .eq. 2) then
              vcart(1) = 0
              vcart(2) = cart_speed
          elseif (example .eq. 3) then
             call map_comp2annulus(x,y,theta,r)
             w = 0.5
             vcart(1) = -(1-w)*pi2*r*sin(theta)
             vcart(2) = (1+w)*pi2*r*cos(theta)
             nc = sqrt(vcart(1)**2 + vcart(2)**2)

             vcart(1) = -vcart(1)/nc
             vcart(2) = -vcart(2)/nc
         elseif (example .eq. 4) then
              A = amplitude
              tfinal = 0.25
              vcart(1) = cart_speed
              vcart(2) = pi2*A*cos(freq*pi2*t/tfinal)/tfinal;
          elseif (example .eq. 5) then
              A = amplitude
              tfinal = 0.25
              vcart(1) = cart_speed*pi*sin(pi*t/tfinal)/2.d0
              vcart(2) = 0
          endif
          vcart(3) = 0
      endif

      end

c     # ----------------------------------------------------------------
c     #                       Public interface
c     # ----------------------------------------------------------------

c     # ---------------------------------------------
c     # Called from setaux
c     #
c     #    -- used to compute velocity at faces
c     # ---------------------------------------------
      subroutine velocity_components_cart(x,y,t,vcart)
      implicit none

      double precision x,y,t, u(2), vcart(3), uderivs(4)
      double precision t1(3), t2(3)
      integer flag, k

      call annulus_velocity_derivs(x,y,t, u,vcart,uderivs,flag)

      if (flag .eq. 0) then
c         # Velocity components are given in spherical components
c         # and must be converted to Cartesian
          call map_covariant_basis(x, y, t1,t2)

          do k = 1,3
              vcart(k) = u(1)*t1(k) + u(2)*t2(k)
          enddo
      endif

      end

c     # ------------------------------------------------------------
c     # Called from map_divergence
c     #
c     #    -- Needed to define ODE system to get exact solution
c     # ------------------------------------------------------------
      subroutine velocity_derivs(x,y,t, u, vcart, derivs, flag)
      implicit none

      double precision x,y,t, u(2), vcart(3), derivs(4)
      double precision t1(3), t2(3), t1n2, t2n2, map_dot
      double precision t1inv(3), t2inv(3)
      integer flag

      call annulus_velocity_derivs(x,y,t, u,vcart,derivs,flag)

      if (flag .eq. 1) then
c         # Velocity components are given in Cartesian components
          call map_covariant_basis(x, y, t1,t2)
          t1n2 = map_dot(t1,t1)
          t2n2 = map_dot(t2,t2)
          u(1) = map_dot(vcart,t1)/t1n2
          u(2) = map_dot(vcart,t2)/t2n2

c          call map_contravariant_basis(x, y, t1inv,t2inv)
c          # Convert Cartesian derivatives to u(1),u(2) derivatives
c          #
c          #   grad v1 = (dv1/dx)*t1c + (dv1/dy)*t2c + (dv1/dz)*t3c
c          #   grad v2 = (dv2/dx)*t1c + (dv2/dy)*t2c + (dv2/dz)*t3c
c          # .....
c          # Don't think this is needed ...

      endif

      end

c     # ------------------------------------------------------------
c     # Called from qexact
c     #
c     #  -- components relative to basis are needed.
c     # ------------------------------------------------------------
      subroutine velocity_components(x,y,t,u)
      implicit none

      double precision x,y,t, u(2), vcart(3), derivs(4)
      integer flag

      call velocity_derivs(x,y,t, u,vcart,derivs,flag)

      end


      subroutine dbdqr(n, D, E, c1, c2)
      implicit none
      integer n
      double precision D(*),E(*),c1,c2

      integer i
      double precision cs,sn,r

      if (n.lt.2) return
      do i=1,n-1
         call dlartg(d(i),e(i),cs,sn,r)
         d(i) = r
         e(i) = sn*d(i+1)
         d(i+1) = cs*d(i+1)
      enddo
      call dlartg(d(n),e(n),cs,sn,r)
      d(n) = r
      e(n) = 0.0
      c1 = sn
      c2 = cs
      end


MODULE korc_spatial_distribution
  !! @note Module with subroutines for generating the initial spatial distribution
  !! of the different partciles' species in the simulation. @endnote
  USE korc_types
  USE korc_constants
  USE korc_HDF5
  USE korc_hpc
  use korc_fields
  use korc_profiles
  use korc_rnd_numbers
  use korc_random
  use korc_hammersley_generator
  use korc_avalanche
  use korc_experimental_pdf

  IMPLICIT NONE

  REAL(rp), PRIVATE, PARAMETER :: minmax_buffer_size = 10.0_rp

  PUBLIC :: intitial_spatial_distribution
  PRIVATE :: uniform,&
       disk,&
       torus,&
       elliptic_torus,&
       exponential_elliptic_torus,&
       gaussian_elliptic_torus,&
       exponential_torus,&
       gaussian_torus,&
       fzero,&
       MH_gaussian_elliptic_torus,&
       indicator,&
       PSI_ROT,&
       Spong_3D,&
       Spong_2D, &
       MH_psi

CONTAINS

subroutine uniform(spp)
  !! @note Initializing to zero the particles' position when
  !! simulating a 'UNIFORM' plasma. @endnote
  !! Even though in a simulation of a uniform plasma the particles'
  !! position is not advanced, we initialize their position to zero.
  !! @todo Modify KORC for not allocating the particles' position
  !! spp%vars%X and to do not use it along the simulation.
  TYPE(SPECIES), INTENT(INOUT) :: spp
    !! An instance of the derived type SPECIES containing all the
    !! parameters and simulation variables of the different
    !!species in the simulation.

  spp%vars%X = 0.0_rp
end subroutine uniform

subroutine disk(params,spp)
  !! @note Subrotuine for generating a uniform disk/ring as the
  !! initial spatial condition of a given species of particles
  !! in the simulation. @endnote
  !! This uniform disk/ring distribution is generated using the
  !! Inverse Transform Sampling method. In this case, the (toroidal)
  !! radial distribution function of the particles is:
  !!
  !! $$f(r) = \left\{ \begin{array}{ll} 0 & r<r_{min} \\
  !! \frac{1}{2\pi^2(r_{max}^2-r_{min}^2)R_0}
  !! & r_{min}<r<r_{max} \\ 0 & r>r_{max} \end{array} \right.,$$
  !!
  !! where \(r_{min}\) and \(r_{max}\) are the inner and outer
  !! radius of the uniform ring distribution, and \(R_0\) is the
  !! cylindrical radial position of the center of the disk/ring distribution.
  !! This distribution is so that \(\int_0^{2\pi}\int_{r_{min}}^{r_{max}} f(r)
  !! J(r,\theta) drd\theta = 1 \), where \(\theta\) is the poloidal angle,
  !! and \(J(r,\theta)=r(R_0 + r\cos\theta)\) is the Jacobian of the
  !! transformation of Cartesian coordinates to toroidal coordinates.
  !! Notice that in the case of a disk \(r_{min}=0\). As a convention,
  !! this spatial distribution will be generated on the \(xz\)-plane.
  !! Using the Inverse Transform Sampling method we sample \(f(r)\),
  !! and obtain the radial position of the particles as \(r = \sqrt{(r_{max}^2
  !! - r_{min}^2)U + r_{min}^2}\), where \(U\) is a uniform deviate in \([0,1]\).
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
    !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
    !! An instance of the derived type SPECIES containing all
    !! the parameters and simulation variables of the different
    !! species in the simulation.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r
    !! Radial position of the particles \(r\).
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
    !! Uniform deviates in the range \([0,2\pi]\) representing
    !! the uniform poloidal angle \(\theta\) distribution of the particles.

  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)

  theta = 2.0_rp*C_PI*theta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  r = SQRT((spp%r_outter**2 - spp%r_inner**2)*r + spp%r_inner**2)
  spp%vars%X(:,1) = ( spp%Ro + r*COS(theta) )*COS(spp%PHIo)
  spp%vars%X(:,2) = ( spp%Ro + r*COS(theta) )*SIN(spp%PHIo)
  spp%vars%X(:,3) = spp%Zo + r*SIN(theta)

  DEALLOCATE(theta)
  DEALLOCATE(r)
end subroutine disk

subroutine torus(params,spp)
  !! @note Subrotuine for generating a uniform torus/torus
  !! shell as the initial spatial condition of a given species
  !! of particles in the simulation.@endnote
  !! This distribution is generated using the Inverse Transform
  !! Sampling method. This distribution follows the same radial
  !! distribution of a uniform disk/ring distribution, see the
  !! documentation of the [[disk]] subroutine.
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
    !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
    !! An instance of the derived type SPECIES
    !! containing all the parameters and simulation variables of the
    !! different species in the simulation.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r
    !! Radial position of the particles \(r\).
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
  !! Uniform deviates in the range \([0,2\pi]\)
  !! representing the uniform poloidal angle \(\theta\)
  !! distribution of the particles.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: zeta
  !! Uniform deviates in the range \([0,2\pi]\) representing
  !! the uniform toroidal angle \(\zeta\) distribution of the particles.
  INTEGER,DIMENSION(33) :: seed=(/1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1/)

  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( zeta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  if (.not.params%SameRandSeed) then
     call init_random_seed()
  else
     call random_seed(put=seed)
  end if
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  if (.not.params%SameRandSeed) then
     call init_random_seed()
  else
     call random_seed(put=seed)
  end if
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  if (.not.params%SameRandSeed) then
     call init_random_seed()
  else
     call random_seed(put=seed)
  end if
  call RANDOM_NUMBER(r)

  r = SQRT((spp%r_outter**2 - spp%r_inner**2)*r + spp%r_inner**2)
  spp%vars%X(:,1) = ( spp%Ro + r*COS(theta) )*SIN(zeta)
  spp%vars%X(:,2) = ( spp%Ro + r*COS(theta) )*COS(zeta)
  spp%vars%X(:,3) = spp%Zo + r*SIN(theta)

  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)

end subroutine torus

subroutine elliptic_torus(params,spp)
  !! @note Subroutine for generating a uniform elliptic torus as the initial
  !! spatial condition of a given particle species in the simulation. @endnote
  !! An initial spatial distribution following the uniform distribution of
  !! [[torus]] is modified through a shear transformation and a rotation to
  !! generate a uniform spatial distribution on tori with elliptic cross sections.
  !! First, we obtain the uniform spatial distribution in a torus of minor radius
  !! \(r_0\), see [[torus]]. Then, we perform a shear transformation that changes
  !! the cross section of the torus from circular to a tilted ellipse. In
  !! cylindrical coordinates this shear transformation is given by:
  !!
  !! $$R' = R + \alpha Z,$$
  !! $$Z' = Z,$$
  !!
  !! where \(\alpha\) is the shear factor of the transformation.
  !! Here, \(R\) and \(Z\) are the radial and vertical position of the particles
  !! uniformly distributed in a circular torus, \(R'\) and \(Z'\) are their
  !! new positions when following a uniform distribution in a torus with
  !! elliptic circular cross section. The center of the ellipse is
  !! \(R_0' = R_0 + \alpha Z_0\), and \(Z_0 = Z_0\), where \(R_0\) and \(Z_0\)
  !! is the center of the initial circular torus. The major and minor semi-axes
  !! of the tilted ellipse cross section is:
  !!
  !! $$a' = \left[ - \frac{2r_0^2}{\alpha \sqrt{\alpha^2 + 4} - (2+\alpha^2)}
  !!  \right]^{1/2},$$
  !! $$b' = \left[ \frac{2r_0^2}{\alpha \sqrt{\alpha^2 + 4} + (2+\alpha^2)}
  !!  \right]^{1/2}.$$
  !!
  !! Finally, we rotate the ellipse cross section anticlockwise along
  !! \((R_0',Z_0')\) by \(\Theta = \cot^{-1}(\alpha/2)/2\), so the major semi-axis is
  !! parallel to the \(Z\)-axis.
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
    !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
    !! An instance of the derived type SPECIES containing all the parameters
    !! and simulation variables of the different species in the simulation.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: rotation_angle
    !! This is the angle \(\Theta\) in the equations above.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
    !! Uniform deviates in the range \([0,2\pi]\) representing the uniform
    !! poloidal angle \(\theta\) distribution of the particles.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r
    !! Radial position of the particles \(r\).
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: zeta
    !! Uniform deviates in the range \([0,2\pi]\) representing
    !! the uniform toroidal angle \(\zeta\) distribution of the particles.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X
    !! Auxiliary vector used in the coordinate transformations.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y
    !! Auxiliary vector used in the coordinate transformations.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X1
    !! Auxiliary vector used in the coordinate transformations.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y1
    !! Auxiliary vector used in the coordinate transformations.

  ALLOCATE(X1(spp%ppp))
  ALLOCATE(Y1(spp%ppp))
  ALLOCATE(X(spp%ppp))
  ALLOCATE(Y(spp%ppp))
  ALLOCATE(rotation_angle(spp%ppp))
  ALLOCATE(theta(spp%ppp))
  ALLOCATE(zeta(spp%ppp))
  ALLOCATE(r(spp%ppp))

  ! Initial condition of uniformly distributed particles on a disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  call init_random_seed()
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  r = SQRT((spp%r_outter**2 - spp%r_inner**2)*r + spp%r_inner**2)

  Y = r*SIN(theta)
  X = r*COS(theta) + spp%shear_factor*Y

  !> @todo Modify this approximation.
  rotation_angle = 0.5_rp*C_PI - ATAN(1.0_rp,1.0_rp + spp%shear_factor);

  X1 = X*COS(rotation_angle) - Y*SIN(rotation_angle) + spp%Ro
  Y1 = X*SIN(rotation_angle) + Y*COS(rotation_angle) + spp%Zo

  spp%vars%X(:,1) = X1*SIN(zeta)
  spp%vars%X(:,2) = X1*COS(zeta)
  spp%vars%X(:,3) = Y1

  DEALLOCATE(X1)
  DEALLOCATE(Y1)
  DEALLOCATE(X)
  DEALLOCATE(Y)
  DEALLOCATE(rotation_angle)
  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)
end subroutine elliptic_torus

!> @note Function used to find the zeros of \(f(r)\) of \ref
!! korc_spatial_distribution.exponential_torus. @endnote
!!
!! @param f Value of function.
!! @param r Guess value of radial position of the particles.
!! @param a Minor radius of the toroidal distribution \(r_0\)
!! @param ko Decay rate of radial distribution, see \(f(r)\) of \ref korc_spatial_distribution.exponential_torus.
!! @param P Deviate of a random uniform distribution in the interval \([0,1]\).
FUNCTION fzero(r,a,ko,P) RESULT(f)
	REAL(rp) 				:: f
	REAL(rp), INTENT(IN) 	:: r
	REAL(rp), INTENT(IN) 	:: a
	REAL(rp), INTENT(IN) 	:: ko
	REAL(rp), INTENT(IN) 	:: P

	f = EXP(-ko*r)*(1.0_rp + r*ko) + ( 1.0_rp - EXP(-ko*a)*(1.0_rp + a*ko) )*P - 1.0_rp
END FUNCTION fzero

!> @brief Subroutine that generates a exponentially decaying radial distribution of particles in a circular cross-section torus of
!! major and minor radi \(R_0\) and \(r_0\), respectively.
!! @details We generate this exponentially decaying radial distribution \(f(r)\) following the same approach as in
!! \ref korc_spatial_distribution.disk, but this time, the radial distribution is given by:
!!
!!
!! $$f(r) = \left\{ \begin{array}{ll} \frac{k_0^2}{4\pi^2 R_0}\frac{\exp{\left( -k_0 r \right)}}{1 - \exp{\left( -k_0r_0\right)}\left[ 1 + k_0 r_0 \right]} &\
!! r<r_0 \\ 0 & r>r_0 \end{array} \right.$$
!!
!!
!! The radial position of the particles \(r\) is obtained using the Inverse Trasnform Sampling method, finding \(r\) numerically
!! through the Newton-Raphson method. First, we calculate the particles' radial distribution in a disk centered at \((R,Z) = (0,0)\).
!! Then, we transfor to a new set of coordinates where the disk is centered at \((R,Z) = (R_0,Z_0)\). Finally, we generate the
!! toroidal distribution by givin each particle a toroidal angle \(\zeta\) which follows a uniform distribution in the interval
!! \([0,2\pi]\).
!!
!! @param[in] params Core KORC simulation parameters.
!! @param[in,out] spp An instance of the derived type SPECIES containing all the parameters and simulation variables of the different
!! species in the simulation.
!! @param fl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param relerr Tolerance used to determine when to stop iterating the Newton-Raphson method for finding \(r\).
!! @param r Radial position of the particles \(r\).
!! @param theta Uniform deviates in the range \([0,2\pi]\) representing the uniform poloidal angle \(\theta\) distribution of the particles.
!! @param zeta Uniform deviates in the range \([0,2\pi]\) representing the uniform toroidal angle \(\zeta\) distribution of the particles.
!! @param pp Particle iterator.
subroutine exponential_torus(params,spp)
  TYPE(KORC_PARAMS), INTENT(IN) 		:: params
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
  REAL(rp) 				:: fl
  REAL(rp) 				:: fr
  REAL(rp) 				:: fm
  REAL(rp) 				:: rl
  REAL(rp) 				:: rr
  REAL(rp) 				:: rm
  REAL(rp) 				:: relerr
  REAL(rp), DIMENSION(:), ALLOCATABLE :: r
  REAL(rp), DIMENSION(:), ALLOCATABLE :: theta
  REAL(rp), DIMENSION(:), ALLOCATABLE :: zeta
  INTEGER 				:: pp

  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( zeta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a
  ! disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  call init_random_seed()
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  ! Newton-Raphson applied here for finding the radial distribution
  do pp=1_idef,spp%ppp
     rl = 0.0_rp
     rr = spp%r_outter

     fl = fzero(rl,spp%r_outter,spp%falloff_rate,r(pp))
     fr = fzero(rr,spp%r_outter,spp%falloff_rate,r(pp))
     if (fl.GT.korc_zero) then
        relerr = 100*ABS(fl - fr)/fl
     else
        relerr = 100*ABS(fl - fr)/fr
     end if

     do while(relerr.GT.1.0_rp)
        rm = 0.5_rp*(rr - rl) + rl
        fm = fzero(rm,spp%r_outter,spp%falloff_rate,r(pp))

        if (SIGN(1.0_rp,fm).EQ.SIGN(1.0_rp,fr)) then
           rr = rm
        else
           rl = rm
        end if

        fl = fzero(rl,spp%r_outter,spp%falloff_rate,r(pp))
        fr = fzero(rr,spp%r_outter,spp%falloff_rate,r(pp))

        if (fl.GT.korc_zero) then
           relerr = 100*ABS(fl - fr)/fl
        else
           relerr = 100*ABS(fl - fr)/fr
        end if
     end do
     r(pp) = rm
  end do

  spp%vars%X(:,1) = ( spp%Ro + r*COS(theta) )*SIN(zeta)
  spp%vars%X(:,2) = ( spp%Ro + r*COS(theta) )*COS(zeta)
  spp%vars%X(:,3) = spp%Zo + r*SIN(theta)

  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)
end subroutine exponential_torus

!> @brief Subroutine that generates an exponentially decaying radial distribution in an elliptic torus as the initial spatial
!! condition of a given particle species in the simulation.
!! @details As a first step, we generate an exponentially decaying radial distribution in a circular cross-section torus as in
!! \ref korc_spatial_distribution.exponential_torus. Then we transform this spatial distribution to a one in an torus with an
!! elliptic cross section, this following the same approach as in \ref korc_spatial_distribution.elliptic_torus.
!!
!! @param[in] params Core KORC simulation parameters.
!! @param[in,out] spp An instance of the derived type SPECIES containing all the parameters and simulation variables of the different
!! species in the simulation.
!! @param fl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param relerr Tolerance used to determine when to stop iterating the Newton-Raphson method for finding \(r\).
!! @param rotation_angle This is the angle \(\Theta\) in \ref korc_spatial_distribution.elliptic_torus.
!! @param r Radial position of the particles \(r\).
!! @param theta Uniform deviates in the range \([0,2\pi]\) representing the uniform poloidal angle \(\theta\) distribution of the particles.
!! @param zeta Uniform deviates in the range \([0,2\pi]\) representing the uniform toroidal angle \(\zeta\) distribution of the particles.
!! @param X Auxiliary vector used in the coordinate transformations.
!! @param Y Auxiliary vector used in the coordinate transformations.
!! @param X1 Auxiliary vector used in the coordinate transformations.
!! @param Y1 Auxiliary vector used in the coordinate transformations.
!! @param pp Particle iterator.
subroutine exponential_elliptic_torus(params,spp)
  TYPE(KORC_PARAMS), INTENT(IN) 		:: params
  TYPE(SPECIES), INTENT(INOUT) 			:: spp
  REAL(rp) 					:: fl
  REAL(rp) 					:: fr
  REAL(rp) 					:: fm
  REAL(rp) 					:: rl
  REAL(rp) 					:: rr
  REAL(rp) 					:: rm
  REAL(rp) 					:: relerr
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: rotation_angle
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: zeta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X1
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y1
  INTEGER 				:: pp

  ALLOCATE(X1(spp%ppp))
  ALLOCATE(Y1(spp%ppp))
  ALLOCATE(X(spp%ppp))
  ALLOCATE(Y(spp%ppp))
  ALLOCATE( rotation_angle(spp%ppp) )
  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( zeta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a
  ! disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  call init_random_seed()
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  do pp=1_idef,spp%ppp
     rl = 0.0_rp
     rr = spp%r_outter

     fl = fzero(rl,spp%r_outter,spp%falloff_rate,r(pp))
     fr = fzero(rr,spp%r_outter,spp%falloff_rate,r(pp))
     if (fl.GT.korc_zero) then
        relerr = 100*ABS(fl - fr)/fl
     else
        relerr = 100*ABS(fl - fr)/fr
     end if

     do while(relerr.GT.1.0_rp)
        rm = 0.5_rp*(rr - rl) + rl
        fm = fzero(rm,spp%r_outter,spp%falloff_rate,r(pp))

        if (SIGN(1.0_rp,fm).EQ.SIGN(1.0_rp,fr)) then
           rr = rm
        else
           rl = rm
        end if

        fl = fzero(rl,spp%r_outter,spp%falloff_rate,r(pp))
        fr = fzero(rr,spp%r_outter,spp%falloff_rate,r(pp))

        if (fl.GT.korc_zero) then
           relerr = 100*ABS(fl - fr)/fl
        else
           relerr = 100*ABS(fl - fr)/fr
        end if
     end do
     r(pp) = rm
  end do

  Y = r*SIN(theta)
  X = r*COS(theta) + spp%shear_factor*Y

  rotation_angle = 0.5_rp*C_PI - ATAN(1.0_rp,1.0_rp + spp%shear_factor);

  X1 = X*COS(rotation_angle) - Y*SIN(rotation_angle) + spp%Ro
  Y1 = X*SIN(rotation_angle) + Y*COS(rotation_angle) + spp%Zo

  spp%vars%X(:,1) = X1*SIN(zeta)
  spp%vars%X(:,2) = X1*COS(zeta)
  spp%vars%X(:,3) = Y1

  DEALLOCATE(X1)
  DEALLOCATE(Y1)
  DEALLOCATE(X)
  DEALLOCATE(Y)
  DEALLOCATE(rotation_angle)
  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)
end subroutine exponential_elliptic_torus

!> @brief Subroutine that generates a Gaussian radial distribution in an elliptic torus as the initial spatial
!! condition of a given particle species in the simulation.
!! @details As a first step, we generate an Gaussian radial distribution in a circular cross-section torus as in
!! \ref korc_spatial_distribution.gaussian_torus. Then we transform this spatial distribution to a one in an torus with an
!! elliptic cross section, this following the same approach as in \ref korc_spatial_distribution.elliptic_torus.
!!
!! @param[in] params Core KORC simulation parameters.
!! @param[in,out] spp An instance of the derived type SPECIES containing all the parameters and simulation variables of the different
!! species in the simulation.
!! @param rotation_angle This is the angle \(\Theta\) in \ref korc_spatial_distribution.elliptic_torus.
!! @param r Radial position of the particles \(r\).
!! @param theta Uniform deviates in the range \([0,2\pi]\) representing the uniform poloidal angle \(\theta\) distribution of the particles.
!! @param zeta Uniform deviates in the range \([0,2\pi]\) representing the uniform toroidal angle \(\zeta\) distribution of the particles.
!! @param X Auxiliary vector used in the coordinate transformations.
!! @param Y Auxiliary vector used in the coordinate transformations.
!! @param X1 Auxiliary vector used in the coordinate transformations.
!! @param Y1 Auxiliary vector used in the coordinate transformations.
!! @param sigma Standard deviation \(\sigma\) of the radial distribution function.
!! @param pp Particle iterator.
subroutine gaussian_elliptic_torus(params,spp)
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: rotation_angle
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: zeta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: X1
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Y1
  REAL(rp) 				:: sigma
  INTEGER 				:: pp

  ALLOCATE(X1(spp%ppp))
  ALLOCATE(Y1(spp%ppp))
  ALLOCATE(X(spp%ppp))
  ALLOCATE(Y(spp%ppp))
  ALLOCATE( rotation_angle(spp%ppp) )
  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( zeta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a
  ! disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  call init_random_seed()
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  sigma = 1.0_rp/SQRT(2.0_rp*(spp%falloff_rate/params%cpp%length))
  sigma = sigma/params%cpp%length

  r = sigma*SQRT(-2.0_rp*LOG(1.0_rp - (1.0_rp - &
       EXP(-0.5_rp*spp%r_outter**2/sigma**2))*r))
!  spp%vars%X(:,1) = ( spp%Ro + r*COS(theta) )*SIN(zeta)
!  spp%vars%X(:,2) = ( spp%Ro + r*COS(theta) )*COS(zeta)
!  spp%vars%X(:,3) = spp%Zo + r*SIN(theta)

  Y = r*SIN(theta)
  X = r*COS(theta) + spp%shear_factor*Y

  rotation_angle = 0.5_rp*C_PI - ATAN(1.0_rp,1.0_rp + spp%shear_factor);

  X1 = X*COS(rotation_angle) - Y*SIN(rotation_angle) + spp%Ro
  Y1 = X*SIN(rotation_angle) + Y*COS(rotation_angle) + spp%Zo

  spp%vars%X(:,1) = X1*SIN(zeta)
  spp%vars%X(:,2) = X1*COS(zeta)
  spp%vars%X(:,3) = Y1

  DEALLOCATE(X1)
  DEALLOCATE(Y1)
  DEALLOCATE(X)
  DEALLOCATE(Y)
  DEALLOCATE(rotation_angle)
  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)
end subroutine gaussian_elliptic_torus

FUNCTION PSI_ROT(R,R0,sigR,Z,Z0,sigZ,theta)
  REAL(rp), INTENT(IN) 	:: R
  !! R-coordinate of MH sampled location
  REAL(rp), INTENT(IN) 	:: R0
  !! R-coordinate of center of 2D Gaussian
  REAL(rp), INTENT(IN) 	:: sigR
  !! Variance of first dimension of 2D Gaussian
  REAL(rp), INTENT(IN) 	:: Z
  !! Z-coordinate of MH sampled location
  REAL(rp), INTENT(IN) 	:: Z0
  !! Z-coordinate of center of 2D Gaussian
  REAL(rp), INTENT(IN) 	:: sigZ
  !! Variance of second dimension of 2D Gaussian
  REAL(rp), INTENT(IN) 	:: theta
  !! Angle of counter-clockwise rotation (in radians), of 2D Gaussian
  !! distribution relative to R,Z
  REAL(rp) 		:: PSI_ROT
  !! Argument of exponential comprising 2D Gaussian distribution

  PSI_ROT=(R-R0)**2*((cos(theta))**2/(2*sigR**2)+(sin(theta))**2/(2*sigZ**2))+ &
       2*(R-R0)*(Z-Z0)*cos(theta)*sin(theta)*(1/(2*sigR**2)-1/(2*sigZ**2))+ &
       (Z-Z0)**2*((sin(theta))**2/(2*sigR**2)+(cos(theta))**2/(2*sigZ**2))

END FUNCTION PSI_ROT

FUNCTION indicator(psi,psi_max)
  REAL(rp), INTENT(IN)  :: psi
  REAL(rp), INTENT(IN)  :: psi_max
  REAL(rp)              :: indicator

  IF (psi.LT.psi_max) THEN
     indicator=1
  ELSE
     indicator=0
  END IF

END FUNCTION indicator

FUNCTION random_norm(mean,sigma)
	REAL(rp), INTENT(IN) :: mean
	REAL(rp), INTENT(IN) :: sigma
	REAL(rp)             :: random_norm
	REAL(rp)             :: rand1, rand2

	call RANDOM_NUMBER(rand1)
	call RANDOM_NUMBER(rand2)

	random_norm = mean+sigma*SQRT(-2.0_rp*LOG(rand1))*COS(2.0_rp*C_PI*rand2);
END FUNCTION random_norm

subroutine MH_gaussian_elliptic_torus(params,spp)
  !! @note Subroutine that generates a 2D Gaussian distribution in an
  !! elliptic torus as the initial spatial condition of a given particle
  !! species in the simulation. @endnote
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
  !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
  !! An instance of the derived type SPECIES containing all the parameters
  !! and simulation variables of the different species in the simulation.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: R_samples
  !!
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Z_samples
  !!
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: ZETA_samples
  !!
  REAL(rp) 				:: psi_max_buff
  !!
  REAL(rp) 				:: theta_rad
  !!
  REAL(rp) 				:: R_buffer
  !!
  REAL(rp) 				:: Z_buffer
  !!
  REAL(rp) 				:: R_test
  !!
  REAL(rp) 				:: Z_test
  !!
  REAL(rp) 				:: psi0
  !!
  REAL(rp) 				:: psi1
  !!
  REAL(rp) 				:: rand_unif
  !!
  REAL(rp) 				:: ratio
  !!
  INTEGER				:: nsamples
  !!
  INTEGER 				:: ii
  !! Particle iterator.
  INTEGER 				:: mpierr
  LOGICAL :: accepted

  nsamples = spp%ppp*params%mpi_params%nmpi

  psi_max_buff = spp%psi_max*1.1_rp

  theta_rad=C_PI*spp%theta_gauss/180.0_rp

  if (params%mpi_params%rank.EQ.0_idef) then
     ALLOCATE(R_samples(nsamples))
     ! Number of samples to distribute among all MPI processes
     ALLOCATE(Z_samples(nsamples))
     ! Number of samples to distribute among all MPI processes
     ALLOCATE(ZETA_samples(nsamples))
     ! Number of samples to distribute among all MPI processes

     ! Transient !

     R_buffer = spp%Ro
     Z_buffer = spp%Zo

     ii=2_idef
     do while (ii .LE. 1000_idef)
        R_test = R_buffer + random_norm(0.0_rp,spp%sigmaR)
        Z_test = Z_buffer + random_norm(0.0_rp,spp%sigmaZ)

        psi0=PSI_ROT(R_buffer,spp%Ro,spp%sigmaR,Z_buffer,spp%Zo, &
             spp%sigmaZ,theta_rad)
        psi1=PSI_ROT(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo,spp%sigmaZ,theta_rad)

        ratio = indicator(psi1,spp%psi_max)*R_test*EXP(-psi1)/(R_buffer*EXP(-psi0))

        if (ratio .GE. 1.0_rp) then
           R_buffer = R_test
           Z_buffer = Z_test
           ii = ii + 1_idef
        else
           call RANDOM_NUMBER(rand_unif)
           if (rand_unif .LT. ratio) then
              R_buffer = R_test
              Z_buffer = Z_test
              ii = ii + 1_idef
           end if
        end if
     end do
     ! Transient !

     ii=1_idef
     do while (ii .LE. nsamples)

        R_test = R_buffer + random_norm(0.0_rp,spp%sigmaR)
        Z_test = Z_buffer + random_norm(0.0_rp,spp%sigmaZ)

        psi0=PSI_ROT(R_buffer,spp%Ro,spp%sigmaR,Z_buffer,spp%Zo, &
             spp%sigmaZ,theta_rad)
        psi1=PSI_ROT(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo,spp%sigmaZ,theta_rad)

        ratio = indicator(psi1,psi_max_buff)*R_test*EXP(-psi1)/(R_buffer*EXP(-psi0))

        accepted=.false.
        if (ratio .GE. 1.0_rp) then
           accepted=.true.
           R_buffer = R_test
           Z_buffer = Z_test
        else
           call RANDOM_NUMBER(rand_unif)
           if (rand_unif .LT. ratio) then
              accepted=.true.
              R_buffer = R_test
              Z_buffer = Z_test
           end if
        end if

        IF (INT(indicator(psi1,spp%psi_max)).EQ.1.and.accepted) THEN
           R_samples(ii) = R_buffer
           Z_samples(ii) = Z_buffer
!           call RANDOM_NUMBER(rand_unif)
           ZETA_samples(ii) = 2.0_rp*C_PI!!*rand_unif
           ii = ii + 1_idef
        END IF

     end do

  end if

  CALL MPI_SCATTER(R_samples*sin(ZETA_samples),spp%ppp,MPI_REAL8, &
       spp%vars%X(:,1),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(R_samples*cos(ZETA_samples),spp%ppp,MPI_REAL8, &
       spp%vars%X(:,2),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(Z_samples,spp%ppp,MPI_REAL8,spp%vars%X(:,3), &
       spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)

  call MPI_BARRIER(MPI_COMM_WORLD,mpierr)

  if (params%mpi_params%rank.EQ.0_idef) then
     DEALLOCATE(R_samples)
     DEALLOCATE(Z_samples)
     DEALLOCATE(ZETA_samples)
  end if

end subroutine MH_gaussian_elliptic_torus

!> @brief Subroutine that generates a Gaussian radial distribution of particles in a circular cross-section torus of
!! major and minor radi \(R_0\) and \(r_0\), respectively.
!! @details We generate this exponentially decaying radial distribution \(f(r)\) following the same approach as in
!! \ref korc_spatial_distribution.disk, but this time, the radial distribution is given by:
!!
!!
!! $$f(r) = \left\{ \begin{array}{ll} \frac{1}{4\pi^2 \sigma^2 R_0}\frac{\exp{\left( -\frac{r^2}{2\sigma^2} \right)}}{1 - \exp{\left( -\frac{r_0^2}{2\sigma^2} \right)}} &\
!! r<r_0 \\ 0 & r>r_0 \end{array} \right. $$
!!
!!
!! The radial position of the particles \(r\) is obtained using the Inverse Trasnform Sampling method, finding \(r\) numerically
!! through the Newton-Raphson method. First, we calculate the particles' radial distribution in a disk centered at \((R,Z) = (0,0)\).
!! Then, we transfor to a new set of coordinates where the disk is centered at \((R,Z) = (R_0,Z_0)\). Finally, we generate the
!! toroidal distribution by givin each particle a toroidal angle \(\zeta\) which follows a uniform distribution in the interval
!! \([0,2\pi]\).
!!
!! @param[in] params Core KORC simulation parameters.
!! @param[in,out] spp An instance of the derived type SPECIES containing all the parameters and simulation variables of the different
!! species in the simulation.
!! @param fl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param fm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rl Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rr Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param rm Variable used in the Newton-Raphson method for finding the radial position of each particle.
!! @param relerr Tolerance used to determine when to stop iterating the Newton-Raphson method for finding \(r\).
!! @param r Radial position of the particles \(r\).
!! @param theta Uniform deviates in the range \([0,2\pi]\) representing the uniform poloidal angle \(\theta\) distribution of the particles.
!! @param zeta Uniform deviates in the range \([0,2\pi]\) representing the uniform toroidal angle \(\zeta\) distribution of the particles.
!! @param pp Particle iterator.
subroutine gaussian_torus(params,spp)
  TYPE(KORC_PARAMS), INTENT(IN) 			:: params
  TYPE(SPECIES), INTENT(INOUT) 			:: spp
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: theta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: zeta
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: r ! temporary vars
  REAL(rp) 				:: sigma

  ALLOCATE( theta(spp%ppp) )
  ALLOCATE( zeta(spp%ppp) )
  ALLOCATE( r(spp%ppp) )

  ! Initial condition of uniformly distributed particles on a disk in the xz-plane
  ! A unique velocity direction
  call init_u_random(10986546_8)

  call init_random_seed()
  call RANDOM_NUMBER(theta)
  theta = 2.0_rp*C_PI*theta

  call init_random_seed()
  call RANDOM_NUMBER(zeta)
  zeta = 2.0_rp*C_PI*zeta

  ! Uniform distribution on a disk at a fixed azimuthal theta
  call init_random_seed()
  call RANDOM_NUMBER(r)

  sigma = 1.0_rp/SQRT(2.0_rp*(spp%falloff_rate/params%cpp%length))
  sigma = sigma/params%cpp%length

  r = sigma*SQRT(-2.0_rp*LOG(1.0_rp - (1.0_rp - &
       EXP(-0.5_rp*spp%r_outter**2/sigma**2))*r))
  spp%vars%X(:,1) = ( spp%Ro + r*COS(theta) )*SIN(zeta)
  spp%vars%X(:,2) = ( spp%Ro + r*COS(theta) )*COS(zeta)
  spp%vars%X(:,3) = spp%Zo + r*SIN(theta)

  DEALLOCATE(theta)
  DEALLOCATE(zeta)
  DEALLOCATE(r)
end subroutine gaussian_torus

function Spong_2D(R0,b,w,dlam,R,Z,T)
  REAL(rp), INTENT(IN) :: R0
  REAL(rp), INTENT(IN) :: b
  REAL(rp), INTENT(IN) :: w
  REAL(rp), INTENT(IN) :: dlam
  REAL(rp), INTENT(IN) :: R
  REAL(rp), INTENT(IN) :: Z
  REAL(rp), INTENT(IN) :: T

  Real(rp) :: rm
  Real(rp) :: lam

  REAL(rp) :: Spong_2D

  rm=sqrt((R-R0)**2+Z**2)
  lam=(sin(deg2rad(T)))**2

  Spong_2D=(1-tanh((rm-b)/w))/(1-tanh(-b/w))*exp(-(lam/dlam)**2)

end function Spong_2D

subroutine Spong_3D(params,spp)
  !! @note Subroutine that generates a 2D Gaussian distribution in an
  !! elliptic torus as the initial spatial condition of a given particle
  !! species in the simulation. @endnote
  TYPE(KORC_PARAMS), INTENT(IN) 	:: params
  !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
  !! An instance of the derived type SPECIES containing all the parameters
  !! and simulation variables of the different species in the simulation.

  !! An instance of the KORC derived type FIELDS.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: R_samples
  !! Major radial location of all samples
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: PHI_samples
  !! Azimuithal angle of all samples
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Z_samples
  !! Vertical location of all samples

  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: T_samples
  !! Pitch angle of all samples

  REAL(rp) 				:: psi_max_buff
  !! Value of buffer above desired maximum argument of 2D Gaussian spatial
  !! profile
  REAL(rp) 				:: minmax
  !! Temporary variable used for setting buffers

  !! Minimum domain for momentum sampling including buffer
  REAL(rp) 				:: max_pitch_angle
  !! Maximum domain for pitch angle sampling including buffer
  REAL(rp) 				:: min_pitch_angle
  !! Minimum domain for pitch angle sampling including buffer
  REAL(rp) 				:: theta_rad
  !! Angle of rotation of 2D Gaussian spatial distribution in radians
  REAL(rp) 				:: R_buffer
  !! Previous sample of R location
  REAL(rp) 				:: Z_buffer
  !! Previous sample of Z location

  REAL(rp) 				:: T_buffer
  !! Previous sample of pitch angle
  REAL(rp) 				:: R_test
  !! Present sample of R location
  REAL(rp) 				:: Z_test
  !! Present sample of Z location

  REAL(rp) 				:: T_test
  !! Present sample of pitch angle
  REAL(rp) 				:: psi0
  !! Previous value of 2D Gaussian argument based on R_buffer, Z_buffer
  REAL(rp) 				:: psi1
  !! Present value of 2D Gaussian argument based on R_test, Z_test
  REAL(rp) 				:: f0
  !! Evaluation of Avalanche distribution with previous sample
  REAL(rp) 				:: f1
  !! Evaluation of Avalanche distribution with present sample
  REAL(rp) 				:: rand_unif
  !! Uniform random variable [0,1]
  REAL(rp) 				:: ratio
  !! MH selection criteria
  INTEGER				:: nsamples
  !! Total number of samples to be distributed over all mpi processes
  INTEGER 				:: ii
  !! Sample iterator.
  INTEGER 				:: mpierr
  !! mpi error indicator

  nsamples = spp%ppp*params%mpi_params%nmpi

  psi_max_buff = spp%psi_max*1.1_rp

  theta_rad=C_PI*spp%theta_gauss/180.0_rp

  ! buffer at minimum pitch angle boundary
  if (spp%etao_lims(1).GE.korc_zero) then
     do ii=1_idef,INT(minmax_buffer_size,idef)
        minmax = spp%etao_lims(1) - REAL(ii,rp)* &
             (spp%etao_lims(2)-spp%etao_lims(1))/100_rp
        if (minmax.GT.0.0_rp) then
           min_pitch_angle = minmax
        end if
     end do
  else
     min_pitch_angle = spp%etao_lims(1)
  end if

  ! buffer at maximum pitch angle boundary
  do ii=1_idef,INT(minmax_buffer_size,idef)
     minmax = spp%etao_lims(2) + REAL(ii,rp)* &
          (spp%etao_lims(2)-spp%etao_lims(1))/100_rp
     if (minmax.LE.180.0_rp) then
        max_pitch_angle = minmax
     end if
  end do

  if (params%mpi_params%rank.EQ.0_idef) then
     ALLOCATE(R_samples(nsamples))
     ! Number of samples to distribute among all MPI processes
     ALLOCATE(PHI_samples(nsamples))
     ! Number of samples to distribute among all MPI processes
     ALLOCATE(Z_samples(nsamples))
     ! Number of samples to distribute among all MPI processes
     ALLOCATE(T_samples(nsamples))
     ! Number of samples to distribute among all MPI processes

     ! Transient !

     R_buffer = spp%Ro
     Z_buffer = spp%Zo

     call RANDOM_NUMBER(rand_unif)
     T_buffer = min_pitch_angle + (max_pitch_angle  &
          - min_pitch_angle)*rand_unif

!     write(output_unit_write,'("length norm: ",E17.10)') params%cpp%length

     ii=1_idef
     do while (ii .LE. 1000_idef)

!        write(output_unit_write,'("burn:",I15)') ii

        R_test = R_buffer + random_norm(0.0_rp,spp%dR)
        Z_test = Z_buffer + random_norm(0.0_rp,spp%dZ)
        T_test = T_buffer + random_norm(0.0_rp,spp%dth)

        ! Test that pitch angle and momentum are within chosen boundary
        do while ((T_test .GT. spp%etao_lims(2)).OR. &
             (T_test .LT. spp%etao_lims(1)))
           T_test = T_buffer + random_norm(0.0_rp,spp%dth)
        end do

        ! initialize 2D gaussian argument and distribution function, or
        ! copy from previous sample
        if (ii==1) then
           psi0=PSI_ROT(R_buffer,spp%Ro,spp%sigmaR,Z_buffer,spp%Zo, &
                spp%sigmaZ,theta_rad)

           f0=Spong_2D(spp%Ro,spp%Spong_b,spp%Spong_w,spp%Spong_dlam, &
                R_buffer,Z_buffer,T_buffer)
        else
           psi0=psi1
           f0=f1
        end if

        psi1=PSI_ROT(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo, &
             spp%sigmaZ,theta_rad)

        f1=Spong_2D(spp%Ro,spp%Spong_b,spp%Spong_w,spp%Spong_dlam, &
             R_test,Z_test,T_test)

!        write(output_unit_write,'("psi0: ",E17.10)') psi0
!        write(output_unit_write,'("psi1: ",E17.10)') psi1

!        write(output_unit_write,'("f0: ",E17.10)') f0
!        write(output_unit_write,'("f1: ",E17.10)') f1

        ! Calculate acceptance ratio for MH algorithm. fRE function
        ! incorporates p^2 factor of spherical coordinate Jacobian
        ! for velocity phase space, factors of sin(pitch angle) for velocity
        ! phase space and cylindrical coordinate Jacobian R for spatial
        ! phase space incorporated here.
        ratio = indicator(psi1,spp%psi_max)*R_test*EXP(-psi1)*f1/ &
             (R_buffer*EXP(-psi0)*f0)

        if (ratio .GE. 1.0_rp) then
           R_buffer = R_test
           Z_buffer = Z_test
           T_buffer = T_test
           ii = ii + 1_idef
        else
           call RANDOM_NUMBER(rand_unif)
           if (rand_unif .LT. ratio) then
              R_buffer = R_test
              Z_buffer = Z_test
              T_buffer = T_test
              ii = ii + 1_idef
           end if
        end if
     end do
     ! Transient !

     ii=1_idef
     do while (ii .LE. nsamples)

!        write(output_unit_write,'("sample:",I15)') ii

        if (modulo(ii,10000).eq.0) then
           write(output_unit_write,'("Sample: ",I10)') ii
        end if

        R_test = R_buffer + random_norm(0.0_rp,spp%dR)
        Z_test = Z_buffer + random_norm(0.0_rp,spp%dZ)
        T_test = T_buffer + random_norm(0.0_rp,spp%dth)

        ! Selection boundary is set with buffer region
        do while ((T_test .GT. max_pitch_angle).OR. &
             (T_test .LT. min_pitch_angle))
           if (T_test.lt.0) then
              T_test=abs(T_test)
              exit
           end if
           T_test = T_buffer + random_norm(0.0_rp,spp%dth)
        end do

        psi0=psi1
        f0=f1

        psi1=PSI_ROT(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo, &
             spp%sigmaZ,theta_rad)

        f1=Spong_2D(spp%Ro,spp%Spong_b,spp%Spong_w,spp%Spong_dlam, &
             R_test,Z_test,T_test)

        ratio = indicator(psi1,psi_max_buff)*R_test*EXP(-psi1)*f1/ &
             (R_buffer*EXP(-psi0)*f0)

        if (ratio .GE. 1.0_rp) then
           R_buffer = R_test
           Z_buffer = Z_test
           T_buffer = T_test
        else
           call RANDOM_NUMBER(rand_unif)
           if (rand_unif .LT. ratio) then
              R_buffer = R_test
              Z_buffer = Z_test
              T_buffer = T_test
           end if
        end if

        ! Only accept sample if it is within desired boundary, but
        ! add to MC above if within buffer. This helps make the boundary
        ! more defined.
        IF ((INT(indicator(psi1,spp%psi_max)).EQ.1).AND. &
             (T_buffer.LE.spp%etao_lims(2)).AND. &
             (T_buffer.GE.spp%etao_lims(1))) THEN
           R_samples(ii) = R_buffer
           Z_samples(ii) = Z_buffer
           T_samples(ii) = T_buffer
           ! Sample phi location uniformly
           call RANDOM_NUMBER(rand_unif)
           PHI_samples(ii) = 2.0_rp*C_PI*rand_unif
           ii = ii + 1_idef
        END IF

     end do

!  if (minval(R_samples(:)).lt.1._rp/params%cpp%length) stop 'error with sample'
!  write(output_unit_write,'("R_sample: ",E17.10)') R_samples(:)*params%cpp%length

  end if

  CALL MPI_SCATTER(R_samples*cos(PHI_samples),spp%ppp,MPI_REAL8, &
       spp%vars%X(:,1),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(R_samples*sin(PHI_samples),spp%ppp,MPI_REAL8, &
       spp%vars%X(:,2),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(Z_samples,spp%ppp,MPI_REAL8, &
       spp%vars%X(:,3),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
!  CALL MPI_SCATTER(T_samples,spp%ppp,MPI_REAL8, &
!       spp%vars%eta,spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)

  call MPI_BARRIER(MPI_COMM_WORLD,mpierr)

!  write(output_unit_write,'("X_X: ",E17.10)') spp%vars%X(:,1)*params%cpp%length

  ! gamma is kept for each particle, not the momentum

  if (params%orbit_model(1:2).eq.'GC') call cart_to_cyl(spp%vars%X,spp%vars%Y)

!  write(output_unit_write,'("Y_R: ",E17.10)') spp%vars%Y(:,1)*params%cpp%length

!  if (minval(spp%vars%Y(:,1)).lt.1._rp/params%cpp%length) stop 'error with avalanche'

  if (params%mpi_params%rank.EQ.0_idef) then
     DEALLOCATE(R_samples)
     DEALLOCATE(Z_samples)
     DEALLOCATE(PHI_samples)
     DEALLOCATE(T_samples)
  end if

end subroutine Spong_3D

subroutine MH_psi(params,spp,F)
  !! @note Subroutine that generates a 2D Gaussian distribution in an
  !! elliptic torus as the initial spatial condition of a given particle
  !! species in the simulation. @endnote
  TYPE(KORC_PARAMS), INTENT(INOUT) 	:: params
  !! Core KORC simulation parameters.
  TYPE(SPECIES), INTENT(INOUT) 		:: spp
  !! An instance of the derived type SPECIES containing all the parameters
  !! and simulation variables of the different species in the simulation.
  TYPE(FIELDS), INTENT(IN)                                   :: F
  !! An instance of the KORC derived type FIELDS.
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: R_samples,X_samples,Y_samples
  !! Major radial location of all samples
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: PHI_samples
  !! Azimuithal angle of all samples
  REAL(rp), DIMENSION(:), ALLOCATABLE 	:: Z_samples
  !! Vertical location of all samples

  REAL(rp) 				:: min_R,max_R
  REAL(rp) 				:: min_Z,max_Z

  REAL(rp) 				:: R_buffer
  !! Previous sample of R location
  REAL(rp) 				:: Z_buffer
  !! Previous sample of Z location

  REAL(rp) 				:: R_test
  !! Present sample of R location
  REAL(rp) 				:: Z_test
  !! Present sample of Z location

  REAL(rp) 				:: psi_max,psi_max_buff
  REAL(rp)  :: PSIp_lim,PSIP0,PSIN,PSIN0,PSIN1,sigma,psi0,psi1

  REAL(rp) 				:: rand_unif
  !! Uniform random variable [0,1]
  REAL(rp) 				:: ratio
  !! MH selection criteria
  INTEGER				:: nsamples
  !! Total number of samples to be distributed over all mpi processes
  INTEGER 				:: ii
  !! Sample iterator.
  INTEGER 				:: mpierr
  !! mpi error indicator

  LOGICAL :: accepted
  INTEGER,DIMENSION(33) :: seed=(/1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1/)

  if (params%mpi_params%rank.EQ.0_idef) then
     write(output_unit_write,*) '*** START SAMPLING ***'
  end if

  nsamples = spp%ppp*params%mpi_params%nmpi

  params%GC_coords=.TRUE.
  PSIp_lim=F%PSIp_lim

  if (params%field_model.eq.'M3D_C1') then
     min_R=params%rmin/params%cpp%length
     max_R=params%rmax/params%cpp%length
     min_Z=params%Zmin/params%cpp%length
     max_Z=params%Zmax/params%cpp%length

     PSIp0=F%PSIp_0
     psi_max = spp%psi_max
     psi_max_buff = spp%psi_max
  else
     min_R=minval(F%X%R)
     max_R=maxval(F%X%R)
     min_Z=minval(F%X%Z)
     max_Z=maxval(F%X%Z)

     PSIp0=F%PSIP_min
     psi_max = spp%psi_max
     psi_max_buff = spp%psi_max*2._rp
  end if

  sigma=spp%sigmaR*params%cpp%length

  !write(output_unit_write,*) min_R,max_R
  !write(output_unit_write,*) min_Z,max_Z

  ALLOCATE(R_samples(nsamples))
  ALLOCATE(X_samples(nsamples))
  ALLOCATE(Y_samples(nsamples))
  ! Number of samples to distribute among all MPI processes
  ALLOCATE(PHI_samples(nsamples))
  ! Number of samples to distribute among all MPI processes
  ALLOCATE(Z_samples(nsamples))
  ! Number of samples to distribute among all MPI processes

  if (params%mpi_params%rank.EQ.0_idef) then
        ! Transient !

     R_buffer = spp%Ro
     Z_buffer = spp%Zo

     if (.not.params%SameRandSeed) then
        call init_random_seed()
     else
        call random_seed(put=seed)
     end if

     write(output_unit_write,'("Begin burn: ",I10)')
     accepted=.false.
     ii=1_idef
     do while (ii .LE. 1000_idef)

        if (modulo(ii,100).eq.0) then
           write(output_unit_write,'("Burn: ",I10)') ii
        end if

        !R_test = R_buffer + random_norm(0.0_rp,spp%dR)
        !R_test = R_buffer + get_random_mkl_N(0.0_rp,spp%dR)
        R_test = R_buffer + get_random_N()*spp%dR

        !Z_test = Z_buffer + random_norm(0.0_rp,spp%dZ)
        !Z_test = Z_buffer + get_random_mkl_N(0.0_rp,spp%dZ)
        Z_test = Z_buffer + get_random_N()*spp%dZ

        do while ((R_test.GT.max_R).OR.(R_test .LT. min_R))
           !eta_test = eta_buffer + random_norm(0.0_rp,spp%dth)
           !eta_test = eta_buffer + get_random_mkl_N(0.0_rp,spp%dth)
           R_test = R_buffer + get_random_N()*spp%dR
        end do

        do while ((Z_test.GT.max_Z).OR.(Z_test .LT. min_Z))
           !eta_test = eta_buffer + random_norm(0.0_rp,spp%dth)
           !eta_test = eta_buffer + get_random_mkl_N(0.0_rp,spp%dth)
           Z_test = Z_buffer + get_random_N()*spp%dZ
        end do

        ! initialize 2D gaussian argument and distribution function, or
        ! copy from previous sample
        if (ii==1) then

           spp%vars%Y(1,1)=R_buffer
           spp%vars%Y(1,2)=0
           spp%vars%Y(1,3)=Z_buffer

           !write(6,*) 'R',R_buffer
           !write(6,*) 'Z',Z_buffer

           if (params%field_model.eq.'M3D_C1') then
              call get_m3d_c1_vector_potential(spp%vars,F,params)
           else
              call get_fields(params,spp%vars,F)
           end if
           spp%vars%flagCon=1_is

           !write(6,*) 'may have crashed'

           !write(6,*) 'R',R_buffer
           !write(6,*) 'Z',Z_buffer
           !write(6,*) 'PSIlim',PSIp_lim
           !write(6,*) 'PSI0',PSIp0
           !write(output_unit_write,*) 'PSI1',psi1
           !write(6,*) 'PSI0',psi0
           !write(output_unit_write,*) 'PSIN1',PSIN1
           !write(6,*) 'PSIN0',PSIN0

           psi0=spp%vars%PSI_P(1)
           PSIN0=(psi0-PSIp0)/(PSIp_lim-PSIp0)

        end if

        if (accepted) then
           PSIN0=PSIN1
        end if

!        psi1=PSI_ROT_exp(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo, &
        !             spp%sigmaZ,theta_rad)
        spp%vars%Y(1,1)=R_test
        spp%vars%Y(1,2)=0._rp
        spp%vars%Y(1,3)=Z_test

        if (params%field_model.eq.'M3D_C1') then
           call get_m3d_c1_vector_potential(spp%vars,F,params)
        else
           call get_fields(params,spp%vars,F)
        end if
        spp%vars%flagCon=1_is

        psi1=spp%vars%PSI_P(1)

        !write(output_unit_write,*) 'PSIlim',PSIp_lim
        !write(output_unit_write,*) 'PSI0',PSIp0
        !write(output_unit_write,*) 'PSI',psi1

        PSIN1=(psi1-PSIp0)/(PSIp_lim-PSIp0)

        !write(output_unit_write,*) 'R',R_test
        !write(output_unit_write,*) 'Z',Z_test
        !write(output_unit_write,*) 'PSIlim',PSIp_lim
        !write(output_unit_write,*) 'PSI0',PSIp0
        !write(output_unit_write,*) 'PSI1',psi1
        !write(output_unit_write,*) 'PSI0',psi0
        !write(output_unit_write,*) 'PSIN',PSIN1
        !write(output_unit_write,*) 'PSIN0',PSIN0

        ! Calculate acceptance ratio for MH algorithm. fRE function
        ! incorporates p^2 factor of spherical coordinate Jacobian
        ! for velocity phase space, factors of sin(pitch angle) for velocity
        ! phase space and cylindrical coordinate Jacobian R for spatial
        ! phase space incorporated here.

        ratio = indicator(PSIN1,psi_max)* &
             R_test*EXP(-PSIN1/sigma)/ &
             (R_buffer*EXP(-PSIN0/sigma))

!        ratio = f1*sin(deg2rad(eta_test))/(f0*sin(deg2rad(eta_buffer)))

        accepted=.false.
        if (ratio .GE. 1.0_rp) then
           accepted=.true.
           R_buffer = R_test
           Z_buffer = Z_test
           ii = ii + 1_idef

           !write(output_unit_write,*) 'PSIN',PSIN1
        else
!           call RANDOM_NUMBER(rand_unif)
!           if (rand_unif .LT. ratio) then
           !if (get_random_mkl_U() .LT. ratio) then
           if (get_random_U() .LT. ratio) then
              accepted=.true.
              R_buffer = R_test
              Z_buffer = Z_test
              ii = ii + 1_idef

              !write(output_unit_write,*) 'PSIN',PSIN1
           end if
        end if
     end do
     ! Transient !

     write(output_unit_write,'("Begin sample: ",I10)')
     ii=1_idef
     do while (ii .LE. nsamples)

!        write(output_unit_write,'("sample:",I15)') ii

       if (modulo(ii,nsamples/10).eq.0) then
           write(output_unit_write,'("Sample: ",I10)') ii
        end if

        !R_test = R_buffer + random_norm(0.0_rp,spp%dR)
        !R_test = R_buffer + get_random_mkl_N(0.0_rp,spp%dR)
        R_test = R_buffer + get_random_N()*spp%dR

        !Z_test = Z_buffer + random_norm(0.0_rp,spp%dZ)
        !Z_test = Z_buffer + get_random_mkl_N(0.0_rp,spp%dZ)
        Z_test = Z_buffer + get_random_N()*spp%dZ

        do while ((R_test.GT.max_R).OR.(R_test .LT. min_R))
           !eta_test = eta_buffer + random_norm(0.0_rp,spp%dth)
           !eta_test = eta_buffer + get_random_mkl_N(0.0_rp,spp%dth)
           R_test = R_buffer + get_random_N()*spp%dR
        end do

        do while ((Z_test.GT.max_Z).OR.(Z_test .LT. min_Z))
           !eta_test = eta_buffer + random_norm(0.0_rp,spp%dth)
           !eta_test = eta_buffer + get_random_mkl_N(0.0_rp,spp%dth)
           Z_test = Z_buffer + get_random_N()*spp%dZ
        end do

        if (accepted) then
           PSIN0=PSIN1
        end if

!        psi1=PSI_ROT_exp(R_test,spp%Ro,spp%sigmaR,Z_test,spp%Zo, &
!             spp%sigmaZ,theta_rad)
        spp%vars%Y(1,1)=R_test
        spp%vars%Y(1,2)=0
        spp%vars%Y(1,3)=Z_test

        if (params%field_model.eq.'M3D_C1') then
           call get_m3d_c1_vector_potential(spp%vars,F,params)
        else
           call get_fields(params,spp%vars,F)
        end if
        spp%vars%flagCon=1_is

        psi1=spp%vars%PSI_P(1)

        !write(output_unit_write,*) 'PSIlim',PSIp_lim
        !write(output_unit_write,*) 'PSI0',PSIp0
        !write(output_unit_write,*) 'PSI',psi1

        PSIN1=(psi1-PSIp0)/(PSIp_lim-PSIp0)

        !write(output_unit_write,*) 'R',R_test
        !write(output_unit_write,*) 'Z',Z_test
        !write(output_unit_write,*) 'PSIlim',PSIp_lim
        !write(output_unit_write,*) 'PSI0',PSIp0
        !write(output_unit_write,*) 'PSI1',psi1
        !write(output_unit_write,*) 'PSI0',psi0
        !write(output_unit_write,*) 'PSIN',PSIN1
        !write(output_unit_write,*) 'PSIN0',PSIN0

        ratio = indicator(PSIN1,psi_max_buff)* &
             R_test*EXP(-PSIN1/sigma)/ &
             (R_buffer*EXP(-PSIN0/sigma))

!        ratio = f1*sin(deg2rad(eta_test))/(f0*sin(deg2rad(eta_buffer)))

        accepted=.false.
        if (ratio .GE. 1.0_rp) then
           accepted=.true.
           R_buffer = R_test
           Z_buffer = Z_test
        else
           !call RANDOM_NUMBER(rand_unif)
           !if (rand_unif .LT. ratio) then
           !if (get_random_mkl_U() .LT. ratio) then
           if (get_random_U() .LT. ratio) then
              accepted=.true.
              R_buffer = R_test
              Z_buffer = Z_test
           end if
        end if

!        write(output_unit_write,'("R: ",E17.10)') R_buffer
!        write(output_unit_write,'("Z: ",E17.10)') Z_buffer

        ! Only accept sample if it is within desired boundary, but
        ! add to MC above if within buffer. This helps make the boundary
        ! more defined.
        IF ((INT(indicator(PSIN1,psi_max)).EQ.1).AND. &
             ACCEPTED) THEN
           R_samples(ii) = R_buffer
           Z_samples(ii) = Z_buffer

           !write(output_unit_write,*) 'PSIN',PSIN1

!           write(output_unit_write,*) 'RS',R_buffer

           ! Sample phi location uniformly
           !call RANDOM_NUMBER(rand_unif)
           !PHI_samples(ii) = 2.0_rp*C_PI*rand_unif
           !PHI_samples(ii) = 2.0_rp*C_PI*get_random_mkl_U()
           PHI_samples(ii) = 2.0_rp*C_PI*get_random_U()
           ii = ii + 1_idef
        END IF

     end do

!  if (minval(R_samples(:)).lt.1._rp/params%cpp%length) stop 'error with sample'
!  write(output_unit_write,'("R_sample: ",E17.10)') R_samples(:)*params%cpp%length

     X_samples=R_samples*cos(PHI_samples)
     Y_samples=R_samples*sin(PHI_samples)

!     write(output_unit_write,*) 'R_samples',R_samples
!     write(output_unit_write,*) 'PHI_samples',PHI_samples
!     write(output_unit_write,*) 'Z_samples',Z_samples
!     write(output_unit_write,*) 'G_samples',G_samples
!     write(output_unit_write,*) 'eta_samples',eta_samples

  end if

  params%GC_coords=.FALSE.

  call MPI_BARRIER(MPI_COMM_WORLD,mpierr)

  CALL MPI_SCATTER(X_samples,spp%ppp,MPI_REAL8, &
       spp%vars%X(:,1),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(Y_samples,spp%ppp,MPI_REAL8, &
       spp%vars%X(:,2),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)
  CALL MPI_SCATTER(Z_samples,spp%ppp,MPI_REAL8, &
       spp%vars%X(:,3),spp%ppp,MPI_REAL8,0,MPI_COMM_WORLD,mpierr)

  call MPI_BARRIER(MPI_COMM_WORLD,mpierr)

  if (params%orbit_model(1:2).eq.'GC') call cart_to_cyl(spp%vars%X,spp%vars%Y)

  DEALLOCATE(R_samples)
  DEALLOCATE(X_samples)
  DEALLOCATE(Y_samples)
  DEALLOCATE(Z_samples)
  DEALLOCATE(PHI_samples)

end subroutine MH_psi

subroutine intitial_spatial_distribution(params,spp,P,F)
  !! @note Subroutine that contains calls to the different subroutines
  !! for initializing the simulated particles with various
  !! spatial distribution functions. @endnote
  TYPE(KORC_PARAMS), INTENT(INOUT) 			  :: params
  !! Core KORC simulation parameters.
  TYPE(SPECIES), DIMENSION(:), ALLOCATABLE, INTENT(INOUT) :: spp
  !! An instance of the derived type SPECIES containing all the parameters and
  !! simulation variables of the different species in the simulation.
  TYPE(PROFILES), INTENT(IN)                              :: P
  !! An instance of the KORC derived type PROFILES.
  TYPE(FIELDS), INTENT(IN)                                   :: F
  !! An instance of the KORC derived type FIELDS.
  INTEGER 						  :: ss
  !! Species iterator.
  INTEGER 				:: mpierr

  do ss=1_idef,params%num_species
     SELECT CASE (TRIM(spp(ss)%spatial_distribution))
     CASE ('UNIFORM')
        call uniform(spp(ss))
     CASE ('DISK')
        call disk(params,spp(ss))
     CASE ('TORUS')
        call torus(params,spp(ss))
     CASE ('EXPONENTIAL-TORUS')
        call exponential_torus(params,spp(ss))
     CASE ('GAUSSIAN-TORUS')
        call gaussian_torus(params,spp(ss))
     CASE ('ELLIPTIC-TORUS')
        call elliptic_torus(params,spp(ss))
     CASE ('EXPONENTIAL-ELLIPTIC-TORUS')
        call exponential_elliptic_torus(params,spp(ss))
     CASE ('GAUSSIAN-ELLIPTIC-TORUS')
        call gaussian_elliptic_torus(params,spp(ss))
     CASE ('2D-GAUSSIAN-ELLIPTIC-TORUS-MH')
        call MH_gaussian_elliptic_torus(params,spp(ss))
     CASE ('AVALANCHE-4D')
        call get_Avalanche_4D(params,spp(ss),P,F)
        !! In addition to spatial distribution function, [[Avalanche_4D]]
        !! samples the avalanche distribution function used to initialize
        !! the components of velocity for all particles.
     CASE ('TRACER')
        spp(ss)%vars%X(:,1)=spp(ss)%Xtrace(1)
        spp(ss)%vars%X(:,2)=spp(ss)%Xtrace(2)
        spp(ss)%vars%X(:,3)=spp(ss)%Xtrace(3)
     CASE ('SPONG-3D')
        call Spong_3D(params,spp(ss))
     CASE ('HOLLMANN-3D')
        call get_Hollmann_distribution_3D(params,spp(ss),F)
     CASE ('HOLLMANN-3D-PSI')
        call get_Hollmann_distribution_3D_psi(params,spp(ss),F)
     CASE('MH_psi')

        if (spp(ss)%ppp*params%mpi_params%nmpi.lt.10) then
           if(params%mpi_params%rank.eq.0) then
              write(6,*) &
                   'num_samples need to be atleast 10 but is only: ', &
                   spp(ss)%ppp*params%mpi_params%nmpi
           end if
           call korc_abort
        end if

        call MH_psi(params,spp(ss),F)
     CASE DEFAULT
        call torus(params,spp(ss))
     END SELECT
  end do
end subroutine intitial_spatial_distribution

END MODULE korc_spatial_distribution


      subroutine OPERATORLAP4(
     &           lofphi
     &           ,ilofphilo0
     &           ,ilofphihi0
     &           ,nlofphicomp
     &           ,phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           ,alpha
     &           ,beta
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nlofphicomp
      integer ilofphilo0
      integer ilofphihi0
      REAL*8 lofphi(
     &           ilofphilo0:ilofphihi0,
     &           0:nlofphicomp-1)
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      REAL*8 alpha
      REAL*8 beta
      REAL*8 dxinv, lap
      integer n,ncomp
      integer i
      ncomp = nphicomp
      if(ncomp .ne. nlofphicomp) then
         call MAYDAYERROR()
      endif
      dxinv = (1.0d0)/(dx*dx)
      do n = 0, ncomp-1
      do i = iregionlo0,iregionhi0
          lap = (
     &   (16.0d0)*phi(i-1,n) - phi(i-2,n)
     & + (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &                     -((30.0d0)*1)*phi(i,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lofphi(i,n) = alpha*phi(i,n)+beta*lap
      enddo
      enddo
      return
      end
      subroutine OPERATORLAPRES4(
     &           r
     &           ,irlo0
     &           ,irhi0
     &           ,nrcomp
     &           ,phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0
     &           ,irhshi0
     &           ,nrhscomp
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           ,alpha
     &           ,beta
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nrcomp
      integer irlo0
      integer irhi0
      REAL*8 r(
     &           irlo0:irhi0,
     &           0:nrcomp-1)
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0
      integer irhshi0
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           0:nrhscomp-1)
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      REAL*8 alpha
      REAL*8 beta
      REAL*8 dxinv, lap, lhs
      integer n,ncomp
      integer i
      ncomp = nphicomp
      dxinv = (1.0d0)/(dx*dx)
      do n = 0, ncomp-1
      do i = iregionlo0,iregionhi0
          lap = (
     &   (16.0d0)*phi(i-1,n) - phi(i-2,n)
     & + (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &                     -((30.0d0)*1)*phi(i,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lhs = alpha*phi(i,n) + beta*lap
          r(i,n) = rhs(i,n) - lhs
      enddo
      enddo
      return
      end
      subroutine RESTRICTRES4(
     &           res
     &           ,ireslo0
     &           ,ireshi0
     &           ,nrescomp
     &           ,phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0
     &           ,irhshi0
     &           ,nrhscomp
     &           ,alpha
     &           ,beta
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nrescomp
      integer ireslo0
      integer ireshi0
      REAL*8 res(
     &           ireslo0:ireshi0,
     &           0:nrescomp-1)
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0
      integer irhshi0
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           0:nrhscomp-1)
      REAL*8 alpha
      REAL*8 beta
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      REAL*8 denom,dxinv,lofphi,lap
      integer n,ncomp
      integer i
      integer ii
      ncomp = nphicomp
      dxinv = (1.0d0) / (dx*dx)
      denom = 2
      do n = 0, ncomp-1
      do i = iregionlo0,iregionhi0
          ii = i/2
          lap = (
     &   (16.0d0)*phi(i-1,n) - phi(i-2,n)
     & + (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &                     -((30.0d0)*1)*phi(i,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lofphi = alpha*phi(i,n) + beta*lap
          res(ii,n) = res(ii,n)
     &                            + (rhs(i,n) - lofphi) / denom
      enddo
      enddo
      return
      end
      subroutine GSRBLAPLACIAN4(
     &           phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0
     &           ,irhshi0
     &           ,nrhscomp
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           ,tmp
     &           ,itmplo0
     &           ,itmphi0
     &           ,ntmpcomp
     &           ,redBlack
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0
      integer irhshi0
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           0:nrhscomp-1)
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      integer ntmpcomp
      integer itmplo0
      integer itmphi0
      REAL*8 tmp(
     &           itmplo0:itmphi0,
     &           0:ntmpcomp-1)
      integer redBlack
      REAL*8 dx2t, thD
      integer i
      integer n,ncomp,indtot,imin,imax,red,black
      red = 0
      black = 1
      dx2t = (12.0d0)*dx*dx
      thD  = (1.000d0 / 30.000d0)/1
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
         if (redBlack .eq. red) then
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               tmp(i,n) = thD*(
     &           (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &         + (16.0d0)*phi(i-1,n) - phi(i-2,n)
     &         - dx2t*rhs(i,n) )
            enddo
        else if (redBlack .eq. black) then
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+black, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,n) = thD*(
     &           (16.0d0)*tmp(i+1,n) - tmp(i+2,n)
     &         + (16.0d0)*tmp(i-1,n) - tmp(i-2,n)
     &         - dx2t*rhs(i,n) )
            enddo
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,n) = tmp(i,n)
            enddo
        else
           call MAYDAYERROR()
        end if
      enddo
      return
      end
      subroutine SORLAPLACIAN4(
     &           phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0
     &           ,irhshi0
     &           ,nrhscomp
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0
      integer irhshi0
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           0:nrhscomp-1)
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      REAL*8 dx2t, thD, tmp, omega
      integer i
      integer n,ncomp
      dx2t = (12.0d0)*dx*dx
      thD  = (1.000d0 / 30.000d0)/1
      omega = 0.47
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
      do i = iregionlo0,iregionhi0
         tmp = thD*(
     &        (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &        + (16.0d0)*phi(i-1,n) - phi(i-2,n)
     &        - dx2t*rhs(i,n) )
         phi(i,n) = omega*tmp
     &        + ((1.0d0)-omega)*phi(i,n)
      enddo
      enddo
      return
      end
      subroutine GSRBHELMHOLTZ4(
     &           phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0
     &           ,irhshi0
     &           ,nrhscomp
     &           ,iregionlo0
     &           ,iregionhi0
     &           ,dx
     &           ,tmp
     &           ,itmplo0
     &           ,itmphi0
     &           ,ntmpcomp
     &           ,alpha
     &           ,beta
     &           ,redBlack
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0
      integer irhshi0
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           0:nrhscomp-1)
      integer iregionlo0
      integer iregionhi0
      REAL*8 dx
      integer ntmpcomp
      integer itmplo0
      integer itmphi0
      REAL*8 tmp(
     &           itmplo0:itmphi0,
     &           0:ntmpcomp-1)
      REAL*8 alpha
      REAL*8 beta
      integer redBlack
      REAL*8 dx2t, lambda, lap, dxinv, helm
      integer i
      integer n,ncomp,indtot,imin,imax,red,black
      red = 0
      black = 1
      dx2t = (12.0d0)*dx*dx
      dxinv = (1.0d0)/(dx*dx)
      lambda = (1.0d0)/(alpha - beta*(30.0d0)*1*(1.000d0 / 12.000d0)*dxi
     &nv)
      lambda = lambda*(0.60)
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
         if (redBlack .eq. red) then
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
          lap = (
     &   (16.0d0)*phi(i-1,n) - phi(i-2,n)
     & + (16.0d0)*phi(i+1,n) - phi(i+2,n)
     &                     -((30.0d0)*1)*phi(i,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          helm = alpha*phi(i,n) + beta*lap
          tmp(i,n) = phi(i,n) +
     &      lambda*( rhs(i,n) - helm )
            enddo
        else if (redBlack .eq. black) then
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+black, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               lap = (
     &           (16.0d0)*tmp(i+1,n) - tmp(i+2,n)
     &         + (16.0d0)*tmp(i-1,n) - tmp(i-2,n)
     &                     -((30.0d0)*1)*tmp(i,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
               helm = alpha*tmp(i,n) + beta*lap
               phi(i,n) = tmp(i,n) +
     &              lambda*( rhs(i,n) - helm )
            enddo
            imin = iregionlo0
            indtot = imin
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,n) = tmp(i,n)
            enddo
        else
           call MAYDAYERROR()
        end if
      enddo
      return
      end
      subroutine NEWGETFLUX4(
     &           flux
     &           ,ifluxlo0
     &           ,ifluxhi0
     &           ,nfluxcomp
     &           ,phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,iboxlo0
     &           ,iboxhi0
     &           ,beta_dx
     &           ,a_idir
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer CHF_ID(0:5,0:5)
      data CHF_ID/ 1,0,0,0,0,0 ,0,1,0,0,0,0 ,0,0,1,0,0,0 ,0,0,0,1,0,0 ,0
     &,0,0,0,1,0 ,0,0,0,0,0,1 /
      integer nfluxcomp
      integer ifluxlo0
      integer ifluxhi0
      REAL*8 flux(
     &           ifluxlo0:ifluxhi0,
     &           0:nfluxcomp-1)
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer iboxlo0
      integer iboxhi0
      REAL*8 beta_dx
      integer a_idir
      INTEGER ncomp,n
      integer ii
      integer i
      ncomp = nphicomp
      ii = CHF_ID(a_idir, 0)
      do n = 0, ncomp-1
      do i = iboxlo0,iboxhi0
          flux(i,n) = beta_dx * (1.000d0 / 12.000d0) *
     &        ( (15.0d0)*phi(i,n)
     &           + phi(i-2*ii,n)
     &           - phi(i+ii,n)
     &           - (15.0d0)*phi(i-ii,n) )
      enddo
      enddo
      return
      end
      subroutine PROLONGLINEAR(
     &           phi
     &           ,iphilo0
     &           ,iphihi0
     &           ,nphicomp
     &           ,coarse
     &           ,icoarselo0
     &           ,icoarsehi0
     &           ,ncoarsecomp
     &           ,ifineBoxlo0
     &           ,ifineBoxhi0
     &           ,icrseBoxlo0
     &           ,icrseBoxhi0
     &           ,r
     &           )
      implicit none
      integer*8 ch_flops
      COMMON/ch_timer/ ch_flops
      integer nphicomp
      integer iphilo0
      integer iphihi0
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           0:nphicomp-1)
      integer ncoarsecomp
      integer icoarselo0
      integer icoarsehi0
      REAL*8 coarse(
     &           icoarselo0:icoarsehi0,
     &           0:ncoarsecomp-1)
      integer ifineBoxlo0
      integer ifineBoxhi0
      integer icrseBoxlo0
      integer icrseBoxhi0
      integer r
      INTEGER ncomp, n
      integer i
      integer ic
      ncomp = nphicomp
      do n = 0, ncomp-1
      do i = ifineBoxlo0,ifineBoxhi0
           ic = i/r
           phi(i,n) =  phi(i,n) +
     &          coarse(ic,n)
           if (ic.ne.icrseBoxhi0 .and.
     &         (ic*r.lt.i .or. ic.eq.icrseBoxlo0)) then
              phi(i,n) =  phi(i,n) +
     &             (coarse(ic+1,n)
     &              - coarse(ic,n))/r*(i+(0.500d0)-ic*r-(0.500d0)*r)
           else
              phi(i,n) =  phi(i,n) +
     &             (- coarse(ic-1,n)
     &              + coarse(ic,n))/r*(i+(0.500d0)-ic*r-(0.500d0)*r)
           endif
      enddo
      enddo
      return
      end


use FEMDomainClass

type(FEMDomain_) :: domain, domain2
type(ShapeFunction_) :: sf
type(IO_) :: f
real(real64) :: position(3)

call f%open("ContactMatrix.txt")

call domain%create(meshtype="Cube3D",x_num=2,y_num=2,z_num=2)
call domain%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domain%msh("domain")

call domain2%create(meshtype="Cube3D",x_num=2,y_num=2,z_num=2)
call domain2%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domain2%move(x=1.9d0,z=0.10d0)
call domain2%msh("domain2")

! set global coordinate
position(:) = domain2%mesh%nodcoord(7,:)

! Get the nearest Element to (x,y,z) = (0,0,1)
print *, "domain2, Node ID : ", 7
print *, "domain1, ELEMENT ID: ", domain%mesh%nearestElementID(x=position(1),y=position(2),z=position(3) )
print *, "domain1, Node ID: ", domain%connectivity(domain%mesh%nearestElementID(x=position(1),y=position(2),z=position(3) ))
call f%write( "ELEMENT ID: "//str(domain%mesh%nearestElementID(x=position(1),y=position(2),z=position(3) )))

! get shape-function
sf = domain%getShapeFunction(position=position)

call f%write(domain%connectMatrix(position,DOF=1) )
call f%write(domain%connectMatrix(position,DOF=1),sparse=.true. )
call f%close()

! show global coordinate
print *, "x",position(1)
print *, "y",position(2)
print *, "z",position(3)

end


module m_cov_sqexp
  use m_util
  use m_cov

  implicit none

  private
  public cov_sqexp

  type, extends(cov_fn) :: cov_sqexp
   contains
     procedure, nopass :: ntheta_required
     procedure, nopass :: cov_val
     procedure, nopass :: dcov_x1
     procedure, nopass :: dcov_x2
     procedure, nopass :: d2cov_xx
  end type cov_sqexp

contains
  pure function ntheta_required(dims)
    integer ntheta_required
    integer, intent(in) :: dims
 ! the scale parameter plus one `r' parameter for each dimension
    ntheta_required = dims+1
  end function ntheta_required

  pure function cov_val(x,y,hypers)
    real(dp) :: cov_val
    real(dp), intent(in), dimension(:) :: x, y, hypers
    real(dp) :: scale, r(size(x,1))
    scale = hypers(1)
    r(:) = hypers(2:)
    cov_val = scale * exp(-0.5*sum((x-y)**2/r**2))
  end function cov_val

  pure function dcov_x1(n,x,y,hypers)
    real(dp) :: dcov_x1
    real(dp), intent(in), dimension(:) :: x, y, hypers
    integer, intent(in) :: n
    real(dp) :: r(size(x,1))
    r(:) = hypers(2:)
    dcov_x1 = -(x(n)-y(n))/r(n)**2 * cov_val(x,y,hypers)
  end function dcov_x1

  pure function dcov_x2(n,x,y,hypers)
    real(dp) :: dcov_x2
    real(dp), intent(in), dimension(:) :: x, y, hypers
    integer, intent(in) :: n
    real(dp) :: r(size(x,1))
    r(:) = hypers(2:)
    dcov_x2 = (x(n)-y(n))/r(n)**2 * cov_val(x,y,hypers)
  end function dcov_x2

  pure function d2cov_xx(n,m,x,y,hypers)
    real(dp) :: d2cov_xx
    real(dp), intent(in), dimension(:) :: x, y, hypers
    integer, intent(in) :: n, m
    real(dp) :: r(size(x,1))
    r(:) = hypers(2:)
    d2cov_xx = merge(cov_val(x,y,hypers)/r(n)**2, 0.0_dp, n.eq.m) &
         - (x(n)-y(n))/r(n)**2 * dcov_x2(m,x,y,hypers)
  end function d2cov_xx
end module m_cov_sqexp