!moduleMesh2DCart: 2D Cartesian coordinate system
!                  x == x
!                  y == y
!                  z == unused
MODULE moduleMesh2DCart
  USE moduleMesh
  USE moduleMeshBoundary
  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):: meshNode2DCart
    !Element coordinates
    REAL(8):: x = 0.D0, y = 0.D0
    CONTAINS
      PROCEDURE, PASS:: init => initNode2DCart
      PROCEDURE, PASS:: getCoordinates => getCoord2DCart

  END TYPE meshNode2DCart

  TYPE, PUBLIC, EXTENDS(meshEdge):: meshEdge2DCart
    !Element coordinates
    REAL(8):: x(1:2) = 0.D0, y(1:2) = 0.D0
    !Connectivity to nodes
    CLASS(meshNode), POINTER:: n1 => NULL(), n2 => NULL()
    CONTAINS
      PROCEDURE, PASS:: init     => initEdge2DCart
      PROCEDURE, PASS:: getNodes => getNodes2DCart
      PROCEDURE, PASS:: intersection => intersection2DCartEdge
      PROCEDURE, PASS:: randPos  => randPosEdge

  END TYPE meshEdge2DCart

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

  END TYPE meshVol2DCart

  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, dx, dy)
      IMPORT meshVol2DCart
      CLASS(meshVol2DCart), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dx, dy

    END SUBROUTINE partialDer_interface

  END INTERFACE

  !Quadrilateral volume element
  TYPE, PUBLIC, EXTENDS(meshVol2DCart):: meshVol2DCartQuad
    !Element coordinates
    REAL(8):: x(1:4) = 0.D0, y(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        => initVolQuad2DCart
      PROCEDURE, PASS::   randPos     => randPosVolQuad
      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

  END TYPE meshVol2DCartQuad

  !Triangular volume element
  TYPE, PUBLIC, EXTENDS(meshVol2DCart):: meshVol2DCartTria
    !Element coordinates
    REAL(8):: x(1:3) = 0.D0, y(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

    CONTAINS
      PROCEDURE, PASS::   init        => initVolTria2DCart
      PROCEDURE, PASS::   randPos     => randPosVolTria
      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

  END TYPE meshVol2DCartTria

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

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

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

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

    END SUBROUTINE initNode2DCart

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

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

      r = (/self%x, self%y, 0.D0/)

    END FUNCTION getCoord2DCart

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

      CLASS(meshEdge2DCart), 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%x  = (/r1(1), r2(1)/)
      self%y  = (/r1(2), r2(2)/)
      !Normal vector
      self%normal = (/ self%y(2)-self%y(1), &
                       self%x(2)-self%x(1), &
                       0.D0 /)
      self%normal = self%normal/NORM2(self%normal)

      !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(boundaryTransparent)
          self%fBoundary(s)%apply => transparent

        TYPE IS(boundaryWallTemperature)
          self%fBoundary(s)%apply => wallTemperature

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

        END SELECT

      END DO

      !Physical surface
      self%physicalSurface = physicalSurface

    END SUBROUTINE initEdge2DCart

    !Random position in quadrilateral volume
    FUNCTION randPosVolQuad(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

      CLASS(meshVol2DCartQuad), INTENT(in):: self
      REAL(8):: r(1:3)
      REAL(8):: xii(1:3)
      REAL(8), ALLOCATABLE:: fPsi(:)

      xii(1) = random(-1.D0, 1.D0)
      xii(2) = random(-1.D0, 1.D0)
      xii(3) = 0.D0

      fPsi = self%fPsi(xii)

      r(1) = DOT_PRODUCT(fPsi, self%x)
      r(2) = DOT_PRODUCT(fPsi, self%y)
      r(3) = 0.D0

    END FUNCTION randposVolQuad

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

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

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

    END FUNCTION getNodes2DCart

    PURE FUNCTION intersection2DCartEdge(self, r0, v0) RESULT(r)
      IMPLICIT NONE

      CLASS(meshEdge2DCart), INTENT(in):: self
      REAL(8), DIMENSION(1:3), INTENT(in):: r0, v0
      REAL(8), DIMENSION(1:3):: r
      REAL(8), DIMENSION(1:3):: rS !base point of surface
      REAL(8):: d

      rS = (/ self%x(1), self%y(1), 0.D0 /)

      d = DOT_PRODUCT((rS - r0), self%normal)/DOT_PRODUCT(v0, self%normal)

      r = r0 + v0*d

    END FUNCTION intersection2DCartEdge

    !Calculates a random position in edge
    FUNCTION randPosEdge(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

      CLASS(meshEdge2DCart), INTENT(in):: self
      REAL(8):: rnd
      REAL(8):: r(1:3)
      REAL(8):: p1(1:2), p2(1:2)

      rnd = random()

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

    END FUNCTION randPosEdge

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

      CLASS(meshVol2DCartQuad), 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%x  = (/r1(1), r2(1), r3(1), r4(1)/)
      self%y  = (/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 initVolQuad2DCart

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

      CLASS(meshVol2DCartQuad), INTENT(inout):: self
      REAL(8):: 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)*4.D0 !4
      fPsi = self%fPsi(xi)
      self%volume  =      detJ
      self%arNodes = fPsi*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, dx, dy)
      IMPLICIT NONE

      CLASS(meshVol2DCartQuad), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dx, dy

      dx(1) = DOT_PRODUCT(dPsi(1,:),self%x)
      dx(2) = DOT_PRODUCT(dPsi(2,:),self%x)
      dy(1) = DOT_PRODUCT(dPsi(1,:),self%y)
      dy(2) = DOT_PRODUCT(dPsi(2,:),self%y)

    END SUBROUTINE partialDerQuad

    !Computes element local stiffness matrix
    PURE FUNCTION elemKQuad(self) RESULT(ke)
      IMPLICIT NONE

      CLASS(meshVol2DCartQuad), INTENT(in):: self
      REAL(8):: 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)
          ke        = ke + MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*wQuad(l)*wQuad(m)/detJ

        END DO
      END DO

    END FUNCTION elemKQuad

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

      CLASS(meshVol2DCartQuad), INTENT(in):: self
      REAL(8), INTENT(in):: source(1:)
      REAL(8), ALLOCATABLE:: localF(:)
      REAL(8):: 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)
          f      = DOT_PRODUCT(fPsi,source)
          localF = localF + f*fPsi*wQuad(l)*wQuad(m)*detJ

         END DO
      END DO

    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(meshVol2DCartQuad), 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(meshVol2DCartQuad), 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(meshVol2DCartQuad), 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(meshVol2DCartQuad), 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%x)-r(1)
        f(2) = DOT_PRODUCT(fPsi,self%y)-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(meshVol2DCartQuad), 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

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

      CLASS(meshVol2DCartTria), 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%x  = (/r1(1), r2(1), r3(1)/)
      self%y  = (/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)

      self%sigmaVrelMax = sigma_ref/L_ref**2

      CALL OMP_INIT_LOCK(self%lock)

    END SUBROUTINE initVolTria2DCart

    !Random position in quadrilateral volume
    FUNCTION randPosVolTria(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

      CLASS(meshVol2DCartTria), INTENT(in):: self
      REAL(8):: r(1:3)
      REAL(8):: xii(1:3)
      REAL(8), ALLOCATABLE:: fPsi(:)

      xii(1) = random( 0.D0, 1.D0)
      xii(2) = random( 0.D0, 1.D0)
      xii(3) = 0.D0

      fPsi = self%fPsi(xii)

      r(1) = DOT_PRODUCT(fPsi, self%x)
      r(2) = DOT_PRODUCT(fPsi, self%y)
      r(3) = 0.D0

    END FUNCTION randposVolTria

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

      CLASS(meshVol2DCartTria), INTENT(inout):: self
      REAL(8):: 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)/2.D0 
      fPsi  = self%fPsi(xi)
      self%volume  =      detJ
      self%arNodes = fPsi*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, dx, dy)
      IMPLICIT NONE

      CLASS(meshVol2DCartTria), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:2):: dx, dy

      dx(1) = DOT_PRODUCT(dPsi(1,:),self%x)
      dx(2) = DOT_PRODUCT(dPsi(2,:),self%x)
      dy(1) = DOT_PRODUCT(dPsi(1,:),self%y)
      dy(2) = DOT_PRODUCT(dPsi(2,:),self%y)

    END SUBROUTINE partialDerTria

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

      CLASS(meshVol2DCartTria), INTENT(in):: self
      REAL(8):: 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)
        ke    = ke + MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*wTria(l)/detJ

      END DO

    END FUNCTION elemKTria

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

      CLASS(meshVol2DCartTria), INTENT(in):: self
      REAL(8), INTENT(in):: source(1:)
      REAL(8), ALLOCATABLE:: localF(:)
      REAL(8):: fPsi(1:3)
      REAL(8):: 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)
        f      = DOT_PRODUCT(fPsi,source)
        localF = localF + f*fPsi*wTria(l)*detJ

      END DO

    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(meshVol2DCartTria), 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(meshVol2DCartTria), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8):: dPsi(1:2,1:3)
      REAL(8):: dPsiR(1:2,1:3)!Derivative of shpae functions in global coordinates
      REAL(8):: invJ(1:2,1:2), detJ
      REAL(8):: phi(1:3)
      REAL(8):: EF(1:3)

      phi = (/self%n1%emData%phi, &
              self%n2%emData%phi, &
              self%n3%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 gatherEFTria

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

      CLASS(meshVol2DCartTria), 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(meshVol2DCartTria), 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%x(1), r(2) - self%y(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(meshVol2DCartTria), 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

    !COMMON FUNCTIONS FOR CARTESIAN VOLUME ELEMENTS IN 2D
    !Computes element Jacobian determinant
    PURE FUNCTION detJ2DCart(self, xi, dPsi_in) RESULT(dJ)
      IMPLICIT NONE

      CLASS(meshVol2DCart), 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):: dx(1:2), dy(1:2)

      IF(PRESENT(dPsi_in)) THEN
        dPsi = dPsi_in

      ELSE
        dPsi = self%dPsi(xi)

      END IF

      CALL self%partialDer(dPsi, dx, dy)
      dJ = dx(1)*dy(2)-dx(2)*dy(1)

    END FUNCTION detJ2DCart

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

      CLASS(meshVol2DCart), 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):: dx(1:2), dy(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, dx, dy)
      invJ(1,:) = (/  dy(2), -dx(2) /)
      invJ(2,:) = (/ -dy(1),  dx(1) /)

    END FUNCTION invJ2DCart

END MODULE moduleMesh2DCart