!moduleMesh3DCart: 3D Cartesian coordinate system
!                  x == x
!                  y == y
!                  z == z
MODULE moduleMesh3DCart
  USE moduleMesh
  USE moduleMeshBoundary
  IMPLICIT NONE

  TYPE, PUBLIC, EXTENDS(meshNode):: meshNode3DCart
    !Element coordinates
    REAL(8):: x, y, z
    CONTAINS
      PROCEDURE, PASS:: init           => initNode3DCart
      PROCEDURE, PASS:: getCoordinates => getCoord3DCart

  END TYPE meshNode3DCart

  !Triangular surface element
  TYPE, PUBLIC, EXTENDS(meshEdge):: meshEdge3DCartTria
    !Element coordinates
    REAL(8):: x(1:3) = 0.D0, y(1:3) = 0.D0, z(1:3) = 0.D0
    !Connectivity to nodes
    CLASS(meshNode), POINTER:: n1 => NULL(), n2 => NULL(), n3 => NULL()
    CONTAINS
      PROCEDURE, PASS::   init         => initEdge3DCartTria
      PROCEDURE, PASS::   getNodes     => getNodes3DCartTria
      PROCEDURE, PASS::   intersection => intersection3DCartTria
      PROCEDURE, PASS::   randPos      => randPosEdgeTria
      PROCEDURE, NOPASS:: fPsi         => fPsiEdgeTria

  END TYPE meshEdge3DCartTria

  TYPE, PUBLIC, ABSTRACT, EXTENDS(meshVol):: meshVol3DCart
    CONTAINS
      PROCEDURE, PASS:: detJac => detJ3DCart
      PROCEDURE, PASS:: invJac => invJ3DCart
      PROCEDURE(dPsi_interface), DEFERRED, NOPASS:: dPsi
      PROCEDURE(partialDer_interface), DEFERRED, PASS:: partialDer
  
  END TYPE meshVol3DCart

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

    END FUNCTION dPsi_interface

    PURE SUBROUTINE partialDer_interface(self, dPsi, dx, dy, dz)
      IMPORT meshVol3DCart
      CLASS(meshVol3DCart), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:,1:)
      REAL(8), INTENT(out), DIMENSION(1:3):: dx, dy, dz

    END SUBROUTINE partialDer_interface

  END INTERFACE

  !Tetrahedron volume element
  TYPE, PUBLIC, EXTENDS(meshVol3DCart):: meshVol3DCartTetra
    !Element Coordinates
    REAL(8):: x(1:4) = 0.D0, y(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(meshElement), POINTER:: e1 => NULL(), e2 => NULL(), e3 => NULL(), e4 => NULL()
    CONTAINS
      PROCEDURE, PASS::   init        => initVolTetra3DCart
      PROCEDURE, PASS::   randPos     => randPosVolTetra
      PROCEDURE, PASS::   calcVol     => volumeTetra
      PROCEDURE, NOPASS:: fPsi        => fPsiTetra
      PROCEDURE, NOPASS:: dPsi        => dPsiTetra
      PROCEDURE, NOPASS:: dPsiXi1     => dPsiTetraXii1
      PROCEDURE, NOPASS:: dPsiXi2     => dPsiTetraXii2
      PROCEDURE, PASS::   partialDer  => partialDerTetra
      PROCEDURE, PASS::   elemK       => elemKTetra
      PROCEDURE, PASS::   elemF       => elemFTetra
      PROCEDURE, NOPASS:: weight      => weightTetra
      PROCEDURE, NOPASS:: inside      => insideTetra
      PROCEDURE, PASS::   scatter     => scatterTetra
      PROCEDURE, PASS::   gatherEF    => gatherEFTetra
      PROCEDURE, PASS::   getNodes    => getNodesTetra
      PROCEDURE, PASS::   phy2log     => phy2logTetra
      PROCEDURE, PASS::   nextElement => nextElementTetra
      
  END TYPE meshVol3DCartTetra

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

      CLASS(meshNode3DCart), 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
      self%z = r(3)/L_ref
      !Node volume, to be determined in mesh
      self%v = 0.D0

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

      CALL OMP_INIT_LOCK(self%lock)

    END SUBROUTINE initNode3DCart

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

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

      r = (/self%x, self%y, self%z/)

    END FUNCTION getCoord3DCart

    !SURFACE FUNCTIONS
    !Inits surface element
    SUBROUTINE initEdge3DCartTria(self, n, p, bt, physicalSurface)
      USE moduleSpecies
      USE moduleBoundary
      USE moduleErrors
      USE moduleMath
      IMPLICIT NONE

      CLASS(meshEdge3DCartTria), 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, r3
      REAL(8), DIMENSION(1:3):: vec1, vec2
      INTEGER:: s

      self%n = n
      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)/)
      self%z  = (/r1(3), r2(3), r3(3)/)
      !Normal vector
      vec1 = (/ self%x(2) - self%x(1), &
                self%y(2) - self%y(1), &
                self%z(2) - self%z(1) /)
      vec2 = (/ self%x(3) - self%x(1), &
                self%y(3) - self%y(1), &
                self%z(3) - self%z(1) /)
      self%normal = crossProduct(vec1, vec2)
      self%normal = normalize(self%normal)

      !Boundary index
      self%boundary => boundary(bt)
      ALLOCATE(self%fBoundary(1:nSpecies))
      !Assign functions to boundary
      DO s = 1, nSpecies
        CALL pointBoundaryFunction(self, s)

      END DO

      !Physical surface
      self%physicalSurface = physicalSurface

    END SUBROUTINE initEdge3DCartTria

    !Get nodes from surface
    PURE FUNCTION getNodes3DCartTria(self) RESULT(n)
      IMPLICIT NONE

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

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

    END FUNCTION getNodes3DCartTria

    PURE FUNCTION intersection3DCartTria(self, r0) RESULT(r)
      IMPLICIT NONE

      CLASS(meshEdge3DCartTria), INTENT(in):: self
      REAL(8), INTENT(in):: r0(1:3)
      REAL(8), DIMENSION(1:3):: r
      REAL(8), DIMENSION(1:3):: edge0, edgeV
      REAL(8):: tI

      edge0 = (/self%x(1), self%y(1), self%z(1) /)
      edgeV = (/self%x(2), self%y(2), self%z(2) /) - edge0

      tI = DOT_PRODUCT(r0 - edge0, edgeV)/DOT_PRODUCT(edgeV, edgeV)

      r = edge0 + tI*edgeV

    END FUNCTION intersection3DCartTria

    !Calculates a random position in the surface
    FUNCTION randPosEdgeTria(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

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

      xii = (/random(), random(), 0.D0 /)

      fPsi = self%fPsi(xii)
      r = (/DOT_PRODUCT(fPsi, self%x), &
            DOT_PRODUCT(fPsi, self%y), &
            DOT_PRODUCT(fPsi, self%z)/)

    END FUNCTION randPosEdgeTria

    !Shape functions for triangular surface
    PURE FUNCTION fPsiEdgeTria(xii) RESULT(fPsi)
      IMPLICIT NONE

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

      ALLOCATE(fPsi(1:3))

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

    END FUNCTION fPsiEdgeTria

    !VOLUME FUNCTIONS
    !TETRA FUNCTIONS
    !Inits tetrahedron element
    SUBROUTINE initVolTetra3DCart(self, n, p, nodes)
      USE moduleRefParam
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(out):: self
      INTEGER, INTENT(in):: n
      INTEGER, INTENT(in):: p(:)
      TYPE(meshNodeCont), INTENT(in), TARGET:: nodes(:)
      REAL(8), DIMENSION(1:3):: r1, r2, r3, r4 !Positions of each node
      REAL(8):: volNodes(1:4) !Volume of each node

      self%n  = n
      self%n1 => nodes(p(1))%obj
      self%n2 => nodes(p(2))%obj
      self%n3 => nodes(p(3))%obj
      self%n4 => 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)/)
      self%z  = (/r1(3), r2(3), r3(3), r4(3)/)

      !Computes the element volume
      CALL self%calcVol()

      !Assign proportional volume to each node
      !TODO: Review this to apply to all elements in the future
      volNodes = self%fPsi((/0.25D0, 0.25D0, 0.25D0/))*self%volume
      self%n1%v = self%n1%v + volNodes(1)
      self%n2%v = self%n2%v + volNodes(2)
      self%n3%v = self%n3%v + volNodes(3)
      self%n4%v = self%n4%v + volNodes(4)

      self%sigmaVrelMax = sigma_ref/L_ref**2

      CALL OMP_INIT_LOCK(self%lock)

    END SUBROUTINE initVolTetra3DCart

    !Random position in volume tetrahedron
    FUNCTION randPosVolTetra(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), 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) = random(0.D0, 1.D0)

      ALLOCATE(fPsi(1:4))
      fPsi = self%fPsi(xii)

      r(1) = DOT_PRODUCT(fPsi, self%x)
      r(2) = DOT_PRODUCT(fPsi, self%y)
      r(3) = DOT_PRODUCT(fPsi, self%z)

    END FUNCTION randPosVolTetra

    !Computes the element volume
    PURE SUBROUTINE volumeTetra(self)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(inout):: self
      REAL(8):: xii(1:3)

      self%volume = 0.D0
      xii = (/0.25D0, 0.25D0, 0.25D0/)
      self%volume = self%detJac(xii)

    END SUBROUTINE volumeTetra

    !Computes element functions in point xii
    PURE FUNCTION fPsiTetra(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) - xi(2) - xi(3)
      fPsi(2) =        xi(1)
      fPsi(3) =                xi(2)
      fPsi(4) =                        xi(3)

    END FUNCTION fPsiTetra

    !Derivative element function at coordinates xii
    PURE FUNCTION dPsiTetra(xii) RESULT(dPsi)
      IMPLICIT NONE

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

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

      dPsi(1,:) = dPsiTetraXii1(xii(2), xii(3))
      dPsi(2,:) = dPsiTetraXii2(xii(1), xii(3))
      dPsi(3,:) = dPsiTetraXii3(xii(1), xii(2))

    END FUNCTION dPsiTetra

    !Derivative element function respect to xii1
    PURE FUNCTION dPsiTetraXii1(xii2, xii3) RESULT(dPsiXii1)
      IMPLICIT NONE
      REAL(8), INTENT(in):: xii2, xii3
      REAL(8):: dPsiXii1(1:4)

      dPsiXii1(1) = -1.D0
      dPsiXii1(2) =  1.D0
      dPsiXii1(3) =  0.D0
      dPsiXii1(4) =  0.D0

    END FUNCTION dPsiTetraXii1
      
    !Derivative element function respect to xii2
    PURE FUNCTION dPsiTetraXii2(xii1, xii3) RESULT(dPsiXii2)
      IMPLICIT NONE
      REAL(8), INTENT(in):: xii1, xii3
      REAL(8):: dPsiXii2(1:4)

      dPsiXii2(1) = -1.D0
      dPsiXii2(2) =  0.D0
      dPsiXii2(3) =  1.D0
      dPsiXii2(4) =  0.D0

    END FUNCTION dPsiTetraXii2
      
    !Derivative element function respect to xii3
    PURE FUNCTION dPsiTetraXii3(xii1, xii2) RESULT(dPsiXii3)
      IMPLICIT NONE
      REAL(8), INTENT(in):: xii1, xii2
      REAL(8):: dPsiXii3(1:4)

      dPsiXii3(1) = -1.D0
      dPsiXii3(2) =  0.D0
      dPsiXii3(3) =  0.D0
      dPsiXii3(4) =  1.D0

    END FUNCTION dPsiTetraXii3
    
    !Computes the derivatives in global coordinates
    PURE SUBROUTINE partialDerTetra(self, dPsi, dx, dy, dz)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: dPsi(1:, 1:)
      REAL(8), INTENT(out), DIMENSION(1:3):: dx, dy, dz

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

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

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

    END SUBROUTINE partialDerTetra

    PURE FUNCTION elemKTetra(self) RESULT(localK)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(in):: self
      REAL(8), ALLOCATABLE:: localK(:,:)
      REAL(8):: xii(1:3)
      REAL(8):: fPsi(1:4), dPsi(1:3, 1:4)
      REAL(8):: invJ(1:3,1:3), detJ

      ALLOCATE(localK(1:4,1:4))
      localK = 0.D0
      xii    = 0.D0
      !TODO: One point Gauss integral. Upgrade when possible
      xii    = (/ 0.25D0, 0.25D0, 0.25D0 /)
      dPsi   = self%dPsi(xii)
      detJ   = self%detJac(xii, dPsi)
      invJ   = self%invJac(xii, dPsi)
      fPsi   = self%fPsi(xii)
      localK = MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*1.D0/detJ

    END FUNCTION elemKTetra

    PURE FUNCTION elemFTetra(self, source) RESULT(localF)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: source(1:)
      REAL(8), ALLOCATABLE:: localF(:)
      REAL(8):: fPsi(1:4), dPsi(1:3, 1:4)
      REAL(8):: xii(1:3)
      REAL(8):: detJ, f

      ALLOCATE(localF(1:4))
      localF = 0.D0
      xii    = 0.D0
      !TODO: One point Gauss integral. Upgrade when possible
      xii = (/ 0.25D0, 0.25D0, 0.25D0 /)
      dPsi = self%dPsi(xii)
      detJ = self%detJac(xii, dPsi)
      fPsi = self%fPsi(xii)
      f    = DOT_PRODUCT(fPsi, source)
      localF = f*fPsi*1.D0*detJ

    END FUNCTION elemFTetra

    PURE FUNCTION weightTetra(xii) RESULT(w)
      IMPLICIT NONE
      REAL(8), INTENT(in):: xii(1:3)
      REAL(8), ALLOCATABLE:: w(:)

      w = fPsiTetra(xii)

    END FUNCTION weightTetra

    PURE FUNCTION insideTetra(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. &
                                   xi(3) >= 0.D0 .AND. &
            1.D0 - xi(1) - xi(2) - xi(3) >= 0.D0
      
    END FUNCTION insideTetra

    SUBROUTINE scatterTetra(self, part)
      USE moduleMath
      USE moduleSpecies
      USE OMP_LIB
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(in):: self
      CLASS(particle), INTENT(in):: part
      REAL(8):: w_p(1:4)
      REAL(8):: tensorS(1:3, 1:3)
      CLASS(meshNode), POINTER:: node
      INTEGER:: sp

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

      sp   =  part%species%n
      node => self%n1
      CALL OMP_SET_LOCK(node%lock)
      node%output(sp)%den          = node%output(sp)%den          + part%weight*w_p(1)
      node%output(sp)%mom(:)       = node%output(sp)%mom(:)       + part%weight*w_p(1)*part%v(:)
      node%output(sp)%tensorS(:,:) = node%output(sp)%tensorS(:,:) + part%weight*w_p(1)*tensorS
      CALL OMP_UNSET_LOCK(node%lock)

      node => self%n2
      CALL OMP_SET_LOCK(node%lock)
      node%output(sp)%den          = node%output(sp)%den          + part%weight*w_p(2)
      node%output(sp)%mom(:)       = node%output(sp)%mom(:)       + part%weight*w_p(2)*part%v(:)
      node%output(sp)%tensorS(:,:) = node%output(sp)%tensorS(:,:) + part%weight*w_p(2)*tensorS
      CALL OMP_UNSET_LOCK(node%lock)

      node => self%n3
      CALL OMP_SET_LOCK(node%lock)
      node%output(sp)%den          = node%output(sp)%den          + part%weight*w_p(3)
      node%output(sp)%mom(:)       = node%output(sp)%mom(:)       + part%weight*w_p(3)*part%v(:)
      node%output(sp)%tensorS(:,:) = node%output(sp)%tensorS(:,:) + part%weight*w_p(3)*tensorS
      CALL OMP_UNSET_LOCK(node%lock)

      node => self%n4
      CALL OMP_SET_LOCK(node%lock)
      node%output(sp)%den          = node%output(sp)%den          + part%weight*w_p(4)
      node%output(sp)%mom(:)       = node%output(sp)%mom(:)       + part%weight*w_p(4)*part%v(:)
      node%output(sp)%tensorS(:,:) = node%output(sp)%tensorS(:,:) + part%weight*w_p(4)*tensorS
      CALL OMP_UNSET_LOCK(node%lock)

    END SUBROUTINE scatterTetra

    PURE FUNCTION gatherEFTetra(self, xi) RESULT(EF)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8):: dPsi(1:3, 1:4)
      REAL(8):: dPsiR(1:3, 1:4)
      REAL(8):: invJ(1:3, 1:3), 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) = -DOT_PRODUCT(dPsiR(3,:), phi)

    END FUNCTION gatherEFTetra

    PURE FUNCTION getNodesTetra(self) RESULT(n)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), 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 getNodesTetra

    PURE FUNCTION phy2logTetra(self,r) RESULT(xi)
      IMPLICIT NONE

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

      xi     = 0.D0
      deltaR = (/r(1) - self%x(1), r(2) - self%y(1), r(3) - self%z(1) /)
      dPsi   = self%dPsi(xi)
      invJ   = self%invJac(xi, dPsi)
      detJ   = self%detJac(xi, dPsi)
      xi     = MATMUL(invJ, deltaR)/detJ

    END FUNCTION phy2logTetra
    
    SUBROUTINE nextElementTetra(self, xi, nextElement)
      IMPLICIT NONE

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

      !TODO: Review when connectivity
      xiArray = (/ xi(3), 1.D0 - xi(1) - xi(2) - xi(3), 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
      CASE (4)
        nextElement => self%e4
      END SELECT

    END SUBROUTINE nextElementTetra

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

      CLASS(meshVol3DCart), INTENT(in)::self
      REAL(8), INTENT(in):: xi(1:3)
      REAL(8), INTENT(in), OPTIONAL:: dPsi_in(1:, 1:)
      REAL(8):: dJ
      REAL(8), ALLOCATABLE:: dPsi(:,:)
      REAL(8):: dx(1:3), dy(1:3), dz(1:3)

      IF (PRESENT(dPsi_in)) THEN
        dPsi = dPsi_in

      ELSE
        dPsi = self%dPsi(xi)

      END IF

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

    END FUNCTION detJ3DCart

    PURE FUNCTION invJ3DCart(self,xi,dPsi_in) RESULT(invJ)
      IMPLICIT NONE

      CLASS(meshVol3DCart), 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), DIMENSION(1:3):: dx, dy, dz
      REAL(8):: invJ(1:3,1:3)

      IF(PRESENT(dPsi_in)) THEN
        dPsi=dPsi_in

      ELSE
        dPsi = self%dPsi(xi)

      END IF

      CALL self%partialDer(dPsi, dx, dy, dz)
      invJ(1,1) =  (dy(2)*dz(3) - dy(3)*dz(2))
      invJ(1,2) = -(dy(1)*dz(3) - dy(3)*dz(1))
      invJ(1,3) =  (dy(1)*dz(2) - dy(2)*dz(1))

      invJ(2,1) = -(dx(2)*dz(3) - dx(3)*dz(2))
      invJ(2,2) =  (dx(1)*dz(3) - dx(3)*dz(1))
      invJ(2,3) = -(dx(1)*dz(2) - dx(2)*dz(1))

      invJ(3,1) =  (dx(2)*dy(3) - dx(3)*dy(2))
      invJ(3,2) = -(dx(1)*dy(3) - dx(3)*dy(1))
      invJ(3,3) =  (dx(1)*dy(2) - dx(2)*dy(1))

      invJ = TRANSPOSE(invJ)

    END FUNCTION invJ3DCart

    !Selects type of elements to build connection
    SUBROUTINE connectVolVol(elemA, elemB)
      IMPLICIT NONE

      CLASS(meshVol), INTENT(inout):: elemA
      CLASS(meshVol), INTENT(inout):: elemB

      SELECT TYPE(elemA)
      TYPE IS(meshVol3DCartTetra)
        !Element A is a tetrahedron
        SELECT TYPE(elemB)
        TYPE IS(meshVol3DCartTetra)
          !Element B is a tetrahedron
          CALL connectTetraTetra(elemA, elemB)

        END SELECT

      END SELECT

    END SUBROUTINE connectVolVol

    SUBROUTINE connectVolEdge(elemA, elemB)
      IMPLICIT NONE

      CLASS(meshVol),  INTENT(inout):: elemA
      CLASS(meshEdge), INTENT(inout):: elemB

      SELECT TYPE(elemB)
      CLASS IS(meshEdge3DCartTria)
        SELECT TYPE(elemA)
        TYPE IS(meshVol3DCartTetra)
          !Element A is a tetrahedron
          CALL connectTetraEdge(elemA, elemB)

        END SELECT

      END SELECT

    END SUBROUTINE connectVolEdge

    SUBROUTINE connectMesh3DCart(self)
      IMPLICIT NONE

      CLASS(meshGeneric), INTENT(inout):: self
      INTEGER:: e, et

      DO e = 1, self%numVols
        !Connect Vol-Vol
        DO et = 1, self%numVols
          IF (e /= et) THEN
            CALL connectVolVol(self%vols(e)%obj, self%vols(et)%obj)

          END IF

        END DO

        SELECT TYPE(self)
        TYPE IS(meshParticles)
          !Connect Vol-Edge
          DO et = 1, self%numEdges
            CALL connectVolEdge(self%vols(e)%obj, self%edges(et)%obj)

          END DO

        END SELECT

      END DO

    END SUBROUTINE connectMesh3DCart

    !Checks if two sets of nodes are coincidend in any order
    PURE FUNCTION coincidentNodes(nodesA, nodesB) RESULT(coincident)
      IMPLICIT NONE

      INTEGER, DIMENSION(1:3), INTENT(in):: nodesA, nodesB
      LOGICAL:: coincident
      INTEGER:: i

      coincident = .FALSE.
      DO i = 1, 3
        IF (ANY(nodesA(i) == nodesB)) THEN
          coincident = .TRUE.

        ELSE
          coincident = .FALSE.
          EXIT

        END IF

      END DO

    END FUNCTION coincidentNodes

    SUBROUTINE connectTetraTetra(elemA, elemB)
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(inout), TARGET:: elemA
      CLASS(meshVol3DCartTetra), INTENT(inout), TARGET:: elemB

      !Check surface 1
      IF (.NOT. ASSOCIATED(elemA%e1)) THEN
        IF     (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n3%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n3%n/))) THEN

          elemA%e1 => elemB
          elemB%e1 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n3%n/), &
                                (/elemB%n2%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e1 => elemB
          elemB%e2 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n3%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n4%n/))) THEN

          elemA%e1 => elemB
          elemB%e3 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n3%n/), &
                                (/elemB%n1%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e1 => elemB
          elemB%e4 => elemA

        END IF

      END IF

      !Check surface 2
      IF (.NOT. ASSOCIATED(elemA%e2)) THEN
        IF     (coincidentNodes((/elemA%n2%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n3%n/))) THEN

          elemA%e2 => elemB
          elemB%e1 => elemA

        ELSEIF (coincidentNodes((/elemA%n2%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n2%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e2 => elemB
          elemB%e2 => elemA

        ELSEIF (coincidentNodes((/elemA%n2%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n4%n/))) THEN

          elemA%e2 => elemB
          elemB%e3 => elemA

        ELSEIF (coincidentNodes((/elemA%n2%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e2 => elemB
          elemB%e4 => elemA

        END IF

      END IF

      !Check surface 3
      IF (.NOT. ASSOCIATED(elemA%e3)) THEN
        IF     (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n3%n/))) THEN

          elemA%e3 => elemB
          elemB%e1 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n4%n/), &
                                (/elemB%n2%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e3 => elemB
          elemB%e2 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n4%n/))) THEN

          elemA%e3 => elemB
          elemB%e3 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e3 => elemB
          elemB%e4 => elemA

        END IF

      END IF

      !Check surface 4
      IF (.NOT. ASSOCIATED(elemA%e4)) THEN
        IF     (coincidentNodes((/elemA%n1%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n3%n/))) THEN

          elemA%e4 => elemB
          elemB%e1 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n2%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e4 => elemB
          elemB%e2 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n2%n, elemB%n4%n/))) THEN

          elemA%e4 => elemB
          elemB%e3 => elemA

        ELSEIF (coincidentNodes((/elemA%n1%n, elemA%n3%n, elemA%n4%n/), &
                                (/elemB%n1%n, elemB%n3%n, elemB%n4%n/))) THEN

          elemA%e4 => elemB
          elemB%e4 => elemA

        END IF

      END IF

    END SUBROUTINE connectTetraTetra

    SUBROUTINE connectTetraEdge(elemA, elemB)
      USE moduleMath
      IMPLICIT NONE

      CLASS(meshVol3DCartTetra), INTENT(inout), TARGET:: elemA
      CLASS(meshEdge3DCartTria), INTENT(inout), TARGET:: elemB
      INTEGER:: nodesEdge(1:3)
      REAL(8), DIMENSION(1:3):: vec1, vec2
      REAL(8):: normVol(1:3)

      nodesEdge = (/ elemB%n1%n, elemB%n2%n, elemB%n3%n /)

      !Check surface 1
      IF (.NOT. ASSOCIATED(elemA%e1)) THEN
        IF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elema%n3%n/), &
                              nodesEdge)) THEN

          vec1 = (/ elemA%x(2) - elemA%x(1), &
                    elemA%y(2) - elemA%y(1), &
                    elemA%z(2) - elemA%z(1) /)
          vec2 = (/ elemA%x(3) - elemA%x(1), &
                    elemA%y(3) - elemA%y(1), &
                    elemA%z(3) - elemA%z(1) /)
          normVol = crossProduct(vec1, vec2)
          normVol = normalize(normVol)

          IF (DOT_PRODUCT(elemB%normal, normVol) == -1.D0) THEN

            elemA%e1 => elemB
            elemB%e1 => elemA

          ELSE

            elemA%e1 => elemB
            elemB%e2 => elemA

            !Revers the normal to point inside the domain
            elemB%normal = -elemB%normal

          END IF

        END IF

      END IF

      !Check surface 2
      IF (.NOT. ASSOCIATED(elemA%e2)) THEN
        IF (coincidentNodes((/elemA%n2%n, elemA%n3%n, elemA%n4%n/), &
                              nodesEdge)) THEN

          vec1 = (/ elemA%x(3) - elemA%x(2), &
                    elemA%y(3) - elemA%y(2), &
                    elemA%z(3) - elemA%z(2) /)
          vec2 = (/ elemA%x(4) - elemA%x(2), &
                    elemA%y(4) - elemA%y(2), &
                    elemA%z(4) - elemA%z(2) /)
          normVol = crossProduct(vec1, vec2)
          normVol = normalize(normVol)

          IF (DOT_PRODUCT(elemB%normal, normVol) == -1.D0) THEN

            elemA%e2 => elemB
            elemB%e1 => elemA

          ELSE

            elemA%e2 => elemB
            elemB%e2 => elemA

            !Revers the normal to point inside the domain
            elemB%normal = -elemB%normal

          END IF

        END IF

      END IF

      !Check surface 3
      IF (.NOT. ASSOCIATED(elemA%e3)) THEN
        IF (coincidentNodes((/elemA%n1%n, elemA%n2%n, elema%n4%n/), &
                              nodesEdge)) THEN

          vec1 = (/ elemA%x(2) - elemA%x(1), &
                    elemA%y(2) - elemA%y(1), &
                    elemA%z(2) - elemA%z(1) /)
          vec2 = (/ elemA%x(4) - elemA%x(1), &
                    elemA%y(4) - elemA%y(1), &
                    elemA%z(4) - elemA%z(1) /)
          normVol = crossProduct(vec1, vec2)
          normVol = normalize(normVol)

          IF (DOT_PRODUCT(elemB%normal, normVol) == -1.D0) THEN

            elemA%e3 => elemB
            elemB%e1 => elemA

          ELSE

            elemA%e3 => elemB
            elemB%e2 => elemA

            !Revers the normal to point inside the domain
            elemB%normal = -elemB%normal

          END IF

        END IF

      END IF

      !Check surface 4
      IF (.NOT. ASSOCIATED(elemA%e4)) THEN
        IF (coincidentNodes((/elemA%n1%n, elemA%n3%n, elema%n4%n/), &
                              nodesEdge)) THEN

          vec1 = (/ elemA%x(3) - elemA%x(1), &
                    elemA%y(3) - elemA%y(1), &
                    elemA%z(3) - elemA%z(1) /)
          vec2 = (/ elemA%x(4) - elemA%x(1), &
                    elemA%y(4) - elemA%y(1), &
                    elemA%z(4) - elemA%z(1) /)
          normVol = crossProduct(vec1, vec2)
          normVol = normalize(normVol)

          IF (DOT_PRODUCT(elemB%normal, normVol) == -1.D0) THEN

            elemA%e4 => elemB
            elemB%e1 => elemA


          ELSE

            elemA%e4 => elemB
            elemB%e2 => elemA

            !Revers the normal to point inside the domain
            elemB%normal = -elemB%normal

          END IF

        END IF

      END IF

    END SUBROUTINE connectTetraEdge

END MODULE moduleMesh3DCart