!moduleMeshCyl: 2D axial symmetric extension of generic mesh from GMSH format.
!               x == z
!               y == r
!               z == theta (unused)
MODULE moduleMeshCyl
  USE moduleMesh
  IMPLICIT NONE

  !Values for Gauss integral
  REAL(8), PARAMETER:: corQuad(1:3) = (/ -DSQRT(3.D0/5.D0), 0.D0,      DSQRT(3.D0/5.D0) /)
  REAL(8), PARAMETER:: wQuad(1:3)   = (/        5.D0/9.D0,  8.D0/9.D0,       5.D0/9.D0  /)

  REAL(8), PARAMETER:: xi1Tria(1:4) = (/   1.D0/3.D0,   1.D0/5.D0,   3.D0/5.D0,   1.D0/5.D0  /)
  REAL(8), PARAMETER:: xi2Tria(1:4) = (/   1.D0/3.D0,   1.D0/5.D0,   1.D0/5.D0,   3.D0/5.D0  /)
  REAL(8), PARAMETER:: wTria(1:4)   = (/ -27.D0/96.D0, 25.D0/96.D0, 25.D0/96.D0, 25.D0/96.D0 /)

  TYPE, PUBLIC, EXTENDS(meshNode):: meshNodeCyl
    !Element coordinates
    REAL(8):: r = 0.D0, z = 0.D0
    CONTAINS
      PROCEDURE, PASS:: init => initNodeCyl
      PROCEDURE, PASS:: getCoordinates => getCoordCyl

  END TYPE meshNodeCyl

  TYPE, PUBLIC, EXTENDS(meshEdge):: meshEdgeCyl
    !Element coordinates
    REAL(8):: r(1:2) = 0.D0, z(1:2) = 0.D0
    !Connectivity to nodes
    CLASS(meshNode), POINTER:: n1 => NULL(), n2 => NULL()
    CONTAINS
      PROCEDURE, PASS:: init     => initEdgeCyl
      PROCEDURE, PASS:: getNodes => getNodesCyl
      PROCEDURE, PASS:: randPos  => randPosCyl

  END TYPE meshEdgeCyl

  !Boundary functions defined in the submodule Boundary
  INTERFACE
    MODULE SUBROUTINE reflection(edge, part)
      USE moduleSpecies
      IMPLICIT NONE

      CLASS(meshEdge), INTENT(inout):: edge
      CLASS(particle), INTENT(inout):: part

    END SUBROUTINE reflection

    MODULE SUBROUTINE absorption(edge, part)
      USE moduleSpecies
      IMPLICIT NONE

      CLASS(meshEdge), INTENT(inout):: edge
      CLASS(particle), INTENT(inout):: part

    END SUBROUTINE absorption

    MODULE SUBROUTINE symmetryAxis(edge, part)
      USE moduleSpecies
      IMPLICIT NONE

      CLASS(meshEdge), INTENT(inout):: edge
      CLASS(particle), INTENT(inout):: part
        
    END SUBROUTINE symmetryAxis

  END INTERFACE

  TYPE, PUBLIC, ABSTRACT, EXTENDS(meshVol):: meshVolCyl
    CONTAINS
      PROCEDURE, PASS:: detJac    => detJCyl
      PROCEDURE, PASS:: invJac    => invJCyl
      PROCEDURE(fPsi_interface), DEFERRED, NOPASS:: fPsi
      PROCEDURE(dPsi_interface), DEFERRED, NOPASS:: dPsi
      PROCEDURE(partialDer_interface), DEFERRED, PASS:: partialDer

  END TYPE meshVolCyl

  ABSTRACT INTERFACE
    PURE FUNCTION fPsi_interface(xi) RESULT(fPsi)
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: fPsi(:)

    END FUNCTION fPsi_interface

    PURE FUNCTION dPsi_interface(xi) RESULT(dPsi)
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: dPsi(:,:)

    END FUNCTION dPsi_interface

    PURE SUBROUTINE partialDer_interface(self, dPsi, dz, dr)
      IMPORT meshVolCyl
      CLASS(meshVolCyl), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dz, dr

    END SUBROUTINE partialDer_interface

  END INTERFACE

  !Quadrilateral volume element
  TYPE, PUBLIC, EXTENDS(meshVolCyl):: meshVolCylQuad
    !Element coordinates
    REAL(8):: r(1:4) = 0.D0, z(1:4) = 0.D0
    !Connectivity to nodes
    CLASS(meshNode), POINTER:: n1 => NULL(), n2 => NULL(), n3 => NULL(), n4 => NULL()
    !Connectivity to adjacent elements
    CLASS(*), POINTER:: e1 => NULL(), e2 => NULL(), e3 => NULL(), e4 => NULL()
    REAL(8):: arNodes(1:4) = 0.D0

    CONTAINS
      PROCEDURE, PASS::   init        => initVolQuadCyl
      PROCEDURE, PASS::   area        => areaQuad
      PROCEDURE, NOPASS:: fPsi        => fPsiQuad
      PROCEDURE, NOPASS:: dPsi        => dPsiQuad
      PROCEDURE, NOPASS:: dPsiXi1     => dPsiQuadXi1
      PROCEDURE, NOPASS:: dPsiXi2     => dPsiQuadXi2
      PROCEDURE, PASS::   partialDer  => partialDerQuad
      PROCEDURE, PASS::   elemK       => elemKQuad
      PROCEDURE, PASS::   elemF       => elemFQuad
      PROCEDURE, NOPASS:: weight      => weightQuad
      PROCEDURE, NOPASS:: inside      => insideQuad
      PROCEDURE, PASS::   scatter     => scatterQuad
      PROCEDURE, PASS::   gatherEF    => gatherEFQuad
      PROCEDURE, PASS::   getNodes    => getNodesQuad
      PROCEDURE, PASS::   phy2log     => phy2logQuad
      PROCEDURE, PASS::   nextElement => nextElementQuad
      PROCEDURE, PASS::   resetOutput => resetOutputQuad

  END TYPE meshVolCylQuad

  !Triangular volume element
  TYPE, PUBLIC, EXTENDS(meshVolCyl):: meshVolCylTria
    !Element coordinates
    REAL(8):: r(1:3) = 0.D0, z(1:3) = 0.D0
    !Connectivity to nodes
    CLASS(meshNode), POINTER:: n1 => NULL(), n2 => NULL(), n3 => NULL()
    !Connectivity to adjacent elements
    CLASS(*), POINTER:: e1 => NULL(), e2 => NULL(), e3 => NULL()
    REAL(8):: arNodes(1:3) = 0.D0
    !Derivatives in z,r real space
    REAL(8), DIMENSION(1:3):: dPsiZ, dPsiR

    CONTAINS
      PROCEDURE, PASS::   init        => initVolTriaCyl
      PROCEDURE, PASS::   area        => areaTria
      PROCEDURE, NOPASS:: fPsi        => fPsiTria
      PROCEDURE, NOPASS:: dPsi        => dPsiTria
      PROCEDURE, NOPASS:: dPsiXi1     => dPsiTriaXi1
      PROCEDURE, NOPASS:: dPsiXi2     => dPsiTriaXi2
      PROCEDURE, PASS::   partialDer  => partialDerTria
      PROCEDURE, PASS::   elemK       => elemKTria
      PROCEDURE, PASS::   elemF       => elemFTria
      PROCEDURE, NOPASS:: weight      => weightTria
      PROCEDURE, NOPASS:: inside      => insideTria
      PROCEDURE, PASS::   scatter     => scatterTria
      PROCEDURE, PASS::   gatherEF    => gatherEFTria
      PROCEDURE, PASS::   getNodes    => getNodesTria
      PROCEDURE, PASS::   phy2log     => phy2logTria
      PROCEDURE, PASS::   nextElement => nextElementTria
      PROCEDURE, PASS::   resetOutput => resetOutputTria

  END TYPE meshVolCylTria

  CONTAINS
    !NODE FUNCTIONS
    !Inits node element
    SUBROUTINE initNodeCyl(self, n, r)
      USE moduleSpecies
      USE moduleRefParam
      IMPLICIT NONE

      CLASS(meshNodeCyl), INTENT(out):: self
      INTEGER, INTENT(in):: n
      REAL(8), INTENT(in):: r(1:3)

      self%n = n
      self%z = r(2)/L_ref
      self%r = r(1)/L_ref
      !Node volume, to be determined in mesh
      self%v = 0.D0

      !Allocates output:
      ALLOCATE(self%output(1:nSpecies))

    END SUBROUTINE initNodeCyl

    !Get coordinates from node
    PURE FUNCTION getCoordCyl(self) RESULT(r)
      IMPLICIT NONE

      CLASS(meshNodeCyl), INTENT(in):: self
      REAL(8):: r(1:3)

      r = (/self%z, self%r, 0.D0/)

    END FUNCTION getCoordCyl

    !EDGE FUNCTIONS
    !Inits edge element
    SUBROUTINE initEdgeCyl(self, n, p, bt, physicalSurface)
      USE moduleSpecies
      USE moduleBoundary
      USE moduleErrors
      IMPLICIT NONE

      CLASS(meshEdgeCyl), INTENT(out):: self
      INTEGER, INTENT(in):: n
      INTEGER, INTENT(in):: p(:)
      INTEGER, INTENT(in):: bt
      INTEGER, INTENT(in):: physicalSurface
      REAL(8), DIMENSION(1:3):: r1, r2
      INTEGER:: s

      self%n  = n
      self%n1 => mesh%nodes(p(1))%obj
      self%n2 => mesh%nodes(p(2))%obj
      !Get element coordinates
      r1 = self%n1%getCoordinates()
      r2 = self%n2%getCoordinates()
      self%z  = (/r1(1), r2(1)/)
      self%r  = (/r1(2), r2(2)/)
      !Normal vector
      self%normal = (/ self%r(2)-self%r(1), &
                       self%z(2)-self%z(1), &
                       0.D0 /)
      !Boundary index
      self%boundary => boundary(bt)
      ALLOCATE(self%fboundary(1:nSpecies))
      !Assign functions to boundary
      DO s = 1, nSpecies
        SELECT TYPE(obj => self%boundary%bTypes(s)%obj)
        TYPE IS(boundaryAbsorption)
          self%fBoundary(s)%apply => absorption

        TYPE IS(boundaryReflection)
          self%fBoundary(s)%apply => reflection

        TYPE IS(boundaryAxis)
          self%fBoundary(s)%apply => symmetryAxis

        CLASS DEFAULT
          CALL criticalError("Boundary type not defined in this geometry", 'initEdgeCyl')

        END SELECT

      END DO

      !Physical surface
      self%physicalSurface = physicalSurface

    END SUBROUTINE initEdgeCyl

    !Get nodes from edge
    PURE FUNCTION getNodesCyl(self) RESULT(n)
      IMPLICIT NONE

      CLASS(meshEdgeCyl), INTENT(in):: self
      INTEGER, ALLOCATABLE:: n(:)

      ALLOCATE(n(1:2))
      n = (/self%n1%n, self%n2%n /)

    END FUNCTION getNodesCyl

    !Calculates a random position in edge
    FUNCTION randPosCyl(self) RESULT(r)
      CLASS(meshEdgeCyl), INTENT(in):: self
      REAL(8):: rnd
      REAL(8):: r(1:3)
      REAL(8):: p1(1:2), p2(1:2)

      CALL RANDOM_NUMBER(rnd)

      p1 = (/self%z(1), self%r(1) /)
      p2 = (/self%z(2), self%r(2) /)
      r(1:2) = (1.D0 - rnd)*p1 + rnd*p2
      r(3)   =  0.D0

    END FUNCTION randPosCyl

    !VOLUME FUNCTIONS
    !QUAD FUNCTIONS
    !Inits quadrilateral element
    SUBROUTINE initVolQuadCyl(self, n, p)
      USE moduleRefParam
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(out):: self
      INTEGER, INTENT(in):: n
      INTEGER, INTENT(in):: p(:)
      REAL(8), DIMENSION(1:3):: r1, r2, r3, r4

      self%n  = n
      self%n1 => mesh%nodes(p(1))%obj
      self%n2 => mesh%nodes(p(2))%obj
      self%n3 => mesh%nodes(p(3))%obj
      self%n4 => mesh%nodes(p(4))%obj
      !Get element coordinates
      r1 = self%n1%getCoordinates()
      r2 = self%n2%getCoordinates()
      r3 = self%n3%getCoordinates()
      r4 = self%n4%getCoordinates()
      self%z  = (/r1(1), r2(1), r3(1), r4(1)/)
      self%r  = (/r1(2), r2(2), r3(2), r4(2)/)

      !Assign node volume
      CALL self%area()
      self%n1%v = self%n1%v + self%arNodes(1)
      self%n2%v = self%n2%v + self%arNodes(2)
      self%n3%v = self%n3%v + self%arNodes(3)
      self%n4%v = self%n4%v + self%arNodes(4)

      self%sigmaVrelMax = sigma_ref/L_ref**2

      CALL OMP_INIT_LOCK(self%lock)

    END SUBROUTINE initVolQuadCyl

    !Computes element area
    PURE SUBROUTINE areaQuad(self)
      USE moduleConstParam
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(inout):: self
      REAL(8):: r, xi(1:3)
      REAL(8):: detJ
      REAL(8):: fPsi(1:4)

      self%volume  = 0.D0
      self%arNodes = 0.D0
      !2D 1 point Gauss Quad Integral
      xi = 0.D0
      detJ = self%detJac(xi)*8.D0*PI !4*2*pi
      fPsi = self%fPsi(xi)
      r = DOT_PRODUCT(fPsi,self%r)
      self%volume  =      r*detJ
      self%arNodes = fPsi*r*detJ

    END SUBROUTINE areaQuad

    !Computes element functions in point xi
    PURE FUNCTION fPsiQuad(xi) RESULT(fPsi) 
      IMPLICIT NONE

      REAL(8),INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: fPsi(:)

      ALLOCATE(fPsi(1:4))

      fPsi(1) = (1.D0-xi(1))*(1.D0-xi(2))
      fPsi(2) = (1.D0+xi(1))*(1.D0-xi(2))
      fPsi(3) = (1.D0+xi(1))*(1.D0+xi(2))
      fPsi(4) = (1.D0-xi(1))*(1.D0+xi(2))
      fPsi    = fPsi*0.25D0

    END FUNCTION fPsiQuad

    !Derivative element function at coordinates xi
    PURE FUNCTION dPsiQuad(xi) RESULT(dPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: dPsi(:,:)

      ALLOCATE(dPsi(1:2,1:4))

      dPsi(1,:) = dPsiQuadXi1(xi(2))
      dPsi(2,:) = dPsiQuadXi2(xi(1))

    END FUNCTION dPsiQuad

    !Derivative element function (xi1)
    PURE FUNCTION dPsiQuadXi1(xi2) RESULT(dPsiXi1) 
      IMPLICIT NONE

      REAL(8),INTENT(in):: xi2
      REAL(8):: dPsiXi1(1:4)

      dPsiXi1(1) = -(1.D0-xi2)
      dPsiXi1(2) =  (1.D0-xi2)
      dPsiXi1(3) =  (1.D0+xi2)
      dPsiXi1(4) = -(1.D0+xi2)
      dPsiXi1    = dPsiXi1*0.25D0
      
    END FUNCTION dPsiQuadXi1

    !Derivative element function (xi2)
    PURE FUNCTION dPsiQuadXi2(xi1) RESULT(dPsiXi2) 
      IMPLICIT NONE

      REAL(8),INTENT(in):: xi1
      REAL(8):: dPsiXi2(1:4)

      dPsiXi2(1) = -(1.D0-xi1)
      dPsiXi2(2) = -(1.D0+xi1)
      dPsiXi2(3) =  (1.D0+xi1)
      dPsiXi2(4) =  (1.D0-xi1)
      dPsiXi2    = dPsiXi2*0.25D0

    END FUNCTION dPsiQuadXi2

    PURE SUBROUTINE partialDerQuad(self, dPsi, dz, dr)
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dz, dr

      dz(1) = DOT_PRODUCT(dPsi(1,:),self%z)
      dz(2) = DOT_PRODUCT(dPsi(2,:),self%z)
      dr(1) = DOT_PRODUCT(dPsi(1,:),self%r)
      dr(2) = DOT_PRODUCT(dPsi(2,:),self%r)

    END SUBROUTINE partialDerQuad

    !Computes element local stiffness matrix
    PURE FUNCTION elemKQuad(self) RESULT(ke)
      USE moduleConstParam, ONLY: PI
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8):: r, xi(1:3)
      REAL(8):: fPsi(1:4), dPsi(1:2,1:4)
      REAL(8):: ke(1:4,1:4)
      REAL(8):: invJ(1:2,1:2), detJ
      INTEGER:: l, m

      ke=0.D0
      xi=0.D0
      !Start 2D Gauss Quad Integral
      DO l=1, 3
        xi(2) = corQuad(l)
        dPsi(1,:) = self%dPsiXi1(xi(2))
        DO m = 1, 3
          xi(1)     = corQuad(m)
          dPsi(2,:) = self%dPsiXi2(xi(1))
          fPsi      = self%fPsi(xi)
          detJ      = self%detJac(xi,dPsi)
          invJ      = self%invJac(xi,dPsi)
          r         = DOT_PRODUCT(fPsi,self%r)
          ke        = ke + MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*r*wQuad(l)*wQuad(m)/detJ

        END DO
      END DO
      ke = ke*2.D0*PI

    END FUNCTION elemKQuad

    !Computes the local source vector for a force f
    PURE FUNCTION elemFQuad(self, source) RESULT(localF)
      USE moduleConstParam
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8), INTENT(in):: source(1:)
      REAL(8), ALLOCATABLE:: localF(:)
      REAL(8):: r, xi(1:3)
      REAL(8):: fPsi(1:4)
      REAL(8):: detJ, f
      INTEGER:: l, m

      ALLOCATE(localF(1:4))
      localF = 0.D0
      xi     = 0.D0
      DO l=1, 3
        xi(1) = corQuad(l)
        DO m = 1, 3
          xi(2)  = corQuad(m)
          detJ   = self%detJac(xi)
          fPsi   = self%fPsi(xi)
          r      = DOT_PRODUCT(fPsi,self%r)
          f      = DOT_PRODUCT(fPsi,source)
          localF = localF + r*f*fPsi*wQuad(l)*wQuad(m)*detJ

         END DO
      END DO
      localF = localF*2.D0*PI

    END FUNCTION elemFQuad

    !Computes weights in the element nodes
    PURE FUNCTION weightQuad(xi) RESULT(w)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      REAL(8):: w(1:4)

      w = fPsiQuad(xi)

    END FUNCTION weightQuad

    !Checks if a particle is inside a quad element
    PURE FUNCTION insideQuad(xi) RESULT(ins)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      LOGICAL:: ins

      ins = (xi(1) >= -1.D0 .AND. xi(1) <= 1.D0) .AND. &
            (xi(2) >= -1.D0 .AND. xi(2) <= 1.D0)

    END FUNCTION insideQuad

    !Scatter properties of particle into element nodes
    SUBROUTINE scatterQuad(self, part)
      USE moduleOutput
      USE moduleSpecies
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      CLASS(particle), INTENT(in):: part
      TYPE(outputNode), POINTER:: vertex
      REAL(8):: w_p(1:4)
      REAL(8):: tensorS(1:3,1:3)

      w_p = self%weight(part%xi)
      tensorS = outerProduct(part%v, part%v)

      vertex => self%n1%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(1)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(1)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(1)*tensorS

      vertex => self%n2%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(2)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(2)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(2)*tensorS

      vertex => self%n3%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(3)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(3)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(3)*tensorS

      vertex => self%n4%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(4)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(4)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(4)*tensorS

    END SUBROUTINE scatterQuad

    !Gathers the electric field at position xi
    PURE FUNCTION gatherEFQuad(self,xi) RESULT(EF)
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8):: dPsi(1:2,1:4)
      REAL(8):: dPsiR(1:2,1:4)!Derivative of shpae functions in global coordinates
      REAL(8):: invJ(1:2,1:2), detJ
      REAL(8):: phi(1:4)
      REAL(8):: EF(1:3)

      phi = (/self%n1%emData%phi, &
              self%n2%emData%phi, &
              self%n3%emData%phi, &
              self%n4%emData%phi /)

      dPsi  =  self%dPsi(xi)
      detJ  =  self%detJac(xi,dPsi)
      invJ  =  self%invJac(xi,dPsi)
      dPsiR =  MATMUL(invJ, dPsi)/detJ
      EF(1) = -DOT_PRODUCT(dPsiR(1,:), phi)
      EF(2) = -DOT_PRODUCT(dPsiR(2,:), phi)
      EF(3) =  0.D0

    END FUNCTION gatherEFQuad

    !Gets nodes from quadrilateral element
    PURE FUNCTION getNodesQuad(self) RESULT(n)
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      INTEGER, ALLOCATABLE:: n(:)

      ALLOCATE(n(1:4))
      n = (/self%n1%n, self%n2%n, self%n3%n, self%n4%n /)

    END FUNCTION getNodesQuad

    !Transforms physical coordinates to element coordinates
    PURE FUNCTION phy2logQuad(self,r) RESULT(xN) 
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8), INTENT(in):: r(1:3)
      REAL(8):: xN(1:3)
      REAL(8):: xO(1:3), detJ, invJ(1:2,1:2), f(1:2)
      REAL(8):: dPsi(1:2,1:4), fPsi(1:4)
      REAL(8):: conv

      !Iterative newton method to transform coordinates
      conv=1.D0
      xO=0.D0

      DO WHILE(conv>1.D-4)
        dPsi = self%dPsi(xO)
        invJ = self%invJac(xO, dPsi)
        fPsi = self%fPsi(xO)
        f(1) = DOT_PRODUCT(fPsi,self%z)-r(1)
        f(2) = DOT_PRODUCT(fPsi,self%r)-r(2)
        detJ = self%detJac(xO,dPsi)
        xN(1:2)=xO(1:2) - MATMUL(invJ, f)/detJ
        conv=MAXVAL(DABS(xN-xO),1)
        xO=xN

      END DO

    END FUNCTION phy2logQuad

    !Gets the next element for a logical position xi
    SUBROUTINE nextElementQuad(self, xi, nextElement)
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      CLASS(*), POINTER, INTENT(out):: nextElement
      REAL(8):: xiArray(1:4)
      INTEGER:: nextInt

      xiArray = (/ -xi(2), xi(1), xi(2), -xi(1) /)
      nextInt = MAXLOC(xiArray,1)
      !Selects the higher value of directions and searches in that direction
      NULLIFY(nextElement)
      SELECT CASE (nextInt)
      CASE (1)
        nextElement => self%e1
      CASE (2)
        nextElement => self%e2
      CASE (3)
        nextElement => self%e3
      CASE (4)
        nextElement => self%e4
      END SELECT

    END SUBROUTINE nextElementQuad

    !Reset the output of nodes in quad element
    PURE SUBROUTINE resetOutputQuad(self)
      USE moduleSpecies
      USE moduleOutput
      IMPLICIT NONE

      CLASS(meshVolCylQuad), INTENT(inout):: self
      INTEGER:: k

      DO k = 1, nSpecies
        self%n1%output(k)%den     = 0.D0
        self%n1%output(k)%mom     = 0.D0
        self%n1%output(k)%tensorS = 0.D0

        self%n2%output(k)%den     = 0.D0
        self%n2%output(k)%mom     = 0.D0
        self%n2%output(k)%tensorS = 0.D0

        self%n3%output(k)%den     = 0.D0
        self%n3%output(k)%mom     = 0.D0
        self%n3%output(k)%tensorS = 0.D0

        self%n4%output(k)%den     = 0.D0
        self%n4%output(k)%mom     = 0.D0
        self%n4%output(k)%tensorS = 0.D0

      END DO

    END SUBROUTINE resetOutputQuad

    !TRIA ELEMENT
    !Init tria element
    SUBROUTINE initVolTriaCyl(self, n, p)
      USE moduleRefParam
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(out):: self
      INTEGER, INTENT(in):: n
      INTEGER, INTENT(in):: p(:)
      REAL(8), DIMENSION(1:3):: r1, r2, r3
      REAL(8):: A

      !Assign node index
      self%n = n

      !Assign nodes to element
      self%n1 => mesh%nodes(p(1))%obj
      self%n2 => mesh%nodes(p(2))%obj
      self%n3 => mesh%nodes(p(3))%obj
      !Get element coordinates
      r1 = self%n1%getCoordinates()
      r2 = self%n2%getCoordinates()
      r3 = self%n3%getCoordinates()
      self%z  = (/r1(1), r2(1), r3(1)/)
      self%r  = (/r1(2), r2(2), r3(2)/)
      !Assign node volume
      CALL self%area()
      self%n1%v = self%n1%v + self%arNodes(1)
      self%n2%v = self%n2%v + self%arNodes(2)
      self%n3%v = self%n3%v + self%arNodes(3)

      !Derivatives in z/r for shape functions (node independent)
      !TODO: This is used because invJ.dPsi does not produce the right Electric field
      A = self%z(2)*self%r(3) - self%z(3)*self%r(2) + &
          self%z(3)*self%r(1) - self%z(1)*self%r(3) + &
          self%z(1)*self%r(2) - self%z(2)*self%r(1)

      self%dPsiZ = (/ self%r(2)-self%r(3), &
                      self%r(3)-self%r(1), &
                      self%r(1)-self%r(2) /)/A

      self%dPsiR = (/ self%z(3)-self%z(2), &
                      self%z(1)-self%z(3), &
                      self%z(2)-self%z(1) /)/A

      self%sigmaVrelMax = sigma_ref/L_ref**2

      CALL OMP_INIT_LOCK(self%lock)

    END SUBROUTINE initVolTriaCyl

    !Calculates area for triangular element
    PURE SUBROUTINE areaTria(self)
      USE moduleConstParam
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(inout):: self
      REAL(8):: r, xi(1:3)
      REAL(8):: detJ
      REAL(8):: fPsi(1:3)

      self%volume  = 0.D0
      self%arNodes = 0.D0
      !2D 1 point Gauss Quad Integral
      xi = (/1.D0/3.D0, 1.D0/3.D0, 0.D0 /)
      detJ  = self%detJac(xi)*PI !2PI*1/2
      fPsi  = self%fPsi(xi)
      r = DOT_PRODUCT(fPsi,self%r)
      self%volume  =      r*detJ
      self%arNodes = fPsi*r*detJ

    END SUBROUTINE areaTria

    !Shape functions for triangular element
    PURE FUNCTION fPsiTria(xi) RESULT(fPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: fPsi(:)

      ALLOCATE(fPsi(1:3))

      fPsi(1) = 1.D0 - xi(1) - xi(2)
      fPsi(2) =        xi(1)
      fPsi(3) =                xi(2)

    END FUNCTION fPsiTria

    !Derivative element function at coordinates xi
    PURE FUNCTION dPsiTria(xi) RESULT(dPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: dPsi(:,:)

      ALLOCATE(dPsi(1:2,1:3))

      dPsi(1,:) = dPsiTriaXi1(xi(2))
      dPsi(2,:) = dPsiTriaXi2(xi(1))

    END FUNCTION dPsiTria

    !Derivative element function (xi1)
    PURE FUNCTION dPsiTriaXi1(xi2) RESULT(dPsiXi1)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi2
      REAL(8):: dPsiXi1(1:3)

      dPsiXi1(1) = -1.D0
      dPsiXi1(2) =  1.D0
      dPsiXi1(3) =  0.D0

    END FUNCTION dPsiTriaXi1

    !Derivative element function (xi1)
    PURE FUNCTION dPsiTriaXi2(xi1) RESULT(dPsiXi2)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi1
      REAL(8):: dPsiXi2(1:3)

      dPsiXi2(1) = -1.D0
      dPsiXi2(2) =  0.D0
      dPsiXi2(3) =  1.D0

    END FUNCTION dPsiTriaXi2

    PURE SUBROUTINE partialDerTria(self, dPsi, dz, dr)
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dz, dr

      dz(1) = DOT_PRODUCT(dPsi(1,:),self%z)
      dz(2) = DOT_PRODUCT(dPsi(2,:),self%z)
      dr(1) = DOT_PRODUCT(dPsi(1,:),self%r)
      dr(2) = DOT_PRODUCT(dPsi(2,:),self%r)

    END SUBROUTINE partialDerTria

    !Computes element local stiffness matrix
    PURE FUNCTION elemKTria(self) RESULT(ke)
      USE moduleConstParam
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8):: r, xi(1:3)
      REAL(8):: fPsi(1:3), dPsi(1:2,1:3)
      REAL(8):: ke(1:3,1:3)
      REAL(8):: invJ(1:2,1:2), detJ
      INTEGER:: l

      ke=0.D0
      xi=0.D0
      !Start 2D Gauss Quad Integral
      DO l=1, 4
        xi(1) = xi1Tria(l)
        xi(2) = xi2Tria(l)
        dPsi  = self%dPsi(xi)
        detJ  = self%detJac(xi,dPsi)
        invJ  = self%invJac(xi,dPsi)
        fPsi  = self%fPsi(xi)
        r     = DOT_PRODUCT(fPsi,self%r)
        ke    = ke + MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*r*wTria(l)/detJ

      END DO
      ke = ke*2.D0*PI

    END FUNCTION elemKTria

    !Computes element local source vector
    PURE FUNCTION elemFTria(self, source) RESULT(localF)
      USE moduleConstParam
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8), INTENT(in):: source(1:)
      REAL(8), ALLOCATABLE:: localF(:)
      REAL(8):: fPsi(1:3)
      REAL(8):: r, xi(1:3)
      REAL(8):: detJ, f
      INTEGER:: l

      ALLOCATE(localF(1:3))
      localF = 0.D0
      xi     = 0.D0
      !Start 2D Gauss Quad Integral
      DO l=1, 4
        xi(1)  = xi1Tria(l)
        xi(2)  = xi2Tria(l)
        detJ   = self%detJac(xi)
        fPsi   = self%fPsi(xi)
        r      = DOT_PRODUCT(fPsi,self%r)
        f      = DOT_PRODUCT(fPsi,source)
        localF = localF + r*f*fPsi*wTria(l)*detJ

      END DO
      localF = localF*2.D0*PI

    END FUNCTION elemFTria

    !Computes weights in the element nodes
    PURE FUNCTION weightTria(xi) RESULT(w)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), ALLOCATABLE:: w(:)

      w = fPsiTria(xi)

    END FUNCTION weightTria

    PURE FUNCTION insideTria(xi) RESULT(ins)
      IMPLICIT NONE

      REAL(8), INTENT(in):: xi(1:3)
      LOGICAL:: ins

      ins =        xi(1)         >= 0.D0 .AND. &
                           xi(2) >= 0.D0 .AND. &
            1.D0 - xi(1) - xi(2) >= 0.D0

    END FUNCTION insideTria

    !Scatter properties of particles into element
    SUBROUTINE scatterTria(self, part)
      USE moduleOutput
      USE moduleSpecies
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      CLASS(particle), INTENT(in):: part
      TYPE(outputNode), POINTER:: vertex
      REAL(8):: w_p(1:3)
      REAL(8):: tensorS(1:3,1:3)

      w_p = self%weight(part%xi)
      tensorS = outerProduct(part%v, part%v)

      vertex => self%n1%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(1)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(1)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(1)*tensorS

      vertex => self%n2%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(2)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(2)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(2)*tensorS

      vertex => self%n3%output(part%sp)
      vertex%den          = vertex%den          + part%weight*w_p(3)
      vertex%mom(:)       = vertex%mom(:)       + part%weight*w_p(3)*part%v(:)
      vertex%tensorS(:,:) = vertex%tensorS(:,:) + part%weight*w_p(3)*tensorS

    END SUBROUTINE scatterTria

    !Gathers the electric field at position xi
    PURE FUNCTION gatherEFTria(self,xi) RESULT(EF)
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8):: phi(1:3)
      REAL(8):: EF(1:3)

      phi = (/self%n1%emData%phi, &
              self%n2%emData%phi, &
              self%n3%emData%phi/)

      EF(1) = -DOT_PRODUCT(self%dPsiZ(:), phi)
      EF(2) = -DOT_PRODUCT(self%dPsiR(:), phi)
      EF(3) =  0.D0

    END FUNCTION gatherEFTria

    !Gets node indexes from triangular element
    PURE FUNCTION getNodesTria(self) RESULT(n)
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      INTEGER, ALLOCATABLE:: n(:)

      ALLOCATE(n(1:3))
      n = (/self%n1%n, self%n2%n, self%n3%n /)

    END FUNCTION getNodesTria

    !Transforms physical coordinates to element coordinates
    PURE FUNCTION phy2logTria(self,r) RESULT(xi)
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8), INTENT(in):: r(1:3)
      REAL(8):: xi(1:3)
      REAL(8):: invJ(1:2,1:2), detJ
      REAL(8):: deltaR(1:2)
      REAL(8):: dPsi(1:2,1:3)

      !Direct method to convert coordinates
      xi      = 0.D0 !Irrelevant, required for input
      deltaR  = (/ r(1) - self%z(1), r(2) - self%r(1) /)
      dPsi    = self%dPsi(xi)
      invJ    = self%invJac(xi, dPsi)
      detJ    = self%detJac(xi, dPsi)
      xi(1:2) = MATMUL(invJ,deltaR)/detJ

    END FUNCTION phy2logTria

    SUBROUTINE nextElementTria(self, xi, nextElement)
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      CLASS(*), POINTER, INTENT(out):: nextElement
      REAL(8):: xiArray(1:3)
      INTEGER:: nextInt

      xiArray = (/ xi(2), 1.D0-xi(1)-xi(2), xi(1) /)
      nextInt = MINLOC(xiArray,1)
      NULLIFY(nextElement)
      SELECT CASE (nextInt)
      CASE (1)
        nextElement => self%e1
      CASE (2)
        nextElement => self%e2
      CASE (3)
        nextElement => self%e3
      END SELECT

    END SUBROUTINE nextElementTria

    !Reset the output of nodes in tria element
    PURE SUBROUTINE resetOutputTria(self)
      USE moduleSpecies
      USE moduleOutput
      IMPLICIT NONE

      CLASS(meshVolCylTria), INTENT(inout):: self
      INTEGER:: k

      DO k = 1, nSpecies
        self%n1%output(k)%den     = 0.D0
        self%n1%output(k)%mom     = 0.D0
        self%n1%output(k)%tensorS = 0.D0

        self%n2%output(k)%den     = 0.D0
        self%n2%output(k)%mom     = 0.D0
        self%n2%output(k)%tensorS = 0.D0

        self%n3%output(k)%den     = 0.D0
        self%n3%output(k)%mom     = 0.D0
        self%n3%output(k)%tensorS = 0.D0

      END DO

    END SUBROUTINE resetOutputTria


    !COMMON FUNCTIONS FOR CYLINDRICAL VOLUME ELEMENTS
    !Computes element Jacobian determinant
    PURE FUNCTION detJCyl(self, xi, dPsi_in) RESULT(dJ)
      IMPLICIT NONE

      CLASS(meshVolCyl), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), INTENT(in), OPTIONAL:: dPsi_in(1:,1:)
      REAL(8), ALLOCATABLE:: dPsi(:,:)
      REAL(8):: dJ
      REAL(8):: dz(1:2), dr(1:2)

      IF(PRESENT(dPsi_in)) THEN
        dPsi = dPsi_in

      ELSE
        dPsi = self%dPsi(xi)

      END IF

      CALL self%partialDer(dPsi, dz, dr)
      dJ = dz(1)*dr(2)-dz(2)*dr(1)

    END FUNCTION detJCyl

    !Computes element Jacobian inverse matrix (without determinant)
    PURE FUNCTION invJCyl(self,xi,dPsi_in) RESULT(invJ)
      IMPLICIT NONE

      CLASS(meshVolCyl), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), INTENT(in), OPTIONAL:: dPsi_in(1:,1:)
      REAL(8), ALLOCATABLE:: dPsi(:,:)
      REAL(8):: dz(1:2), dr(1:2)
      REAL(8):: invJ(1:2,1:2)

      IF(PRESENT(dPsi_in)) THEN
        dPsi=dPsi_in

      ELSE
        dPsi = self%dPsi(xi)

      END IF

      CALL self%partialDer(dPsi, dz, dr)
      invJ(1,:) = (/  dr(2), -dz(2) /)
      invJ(2,:) = (/ -dr(1),  dz(1) /)

    END FUNCTION invJCyl

END MODULE moduleMeshCyl