!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
      !meshNode DEFERRED PROCEDURES
      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
      !meshEdge DEFERRED PROCEDURES
      PROCEDURE, PASS::   init         => initEdge3DCartTria
      PROCEDURE, PASS::   getNodes     => getNodes3DCartTria
      PROCEDURE, PASS::   intersection => intersection3DCartTria
      PROCEDURE, PASS::   randPos      => randPosEdgeTria
      !PARTICULAR PROCEDURES
      PROCEDURE, NOPASS, PRIVATE:: fPsi => fPsiEdgeTria

  END TYPE meshEdge3DCartTria

  !Tetrahedron volume element
  TYPE, PUBLIC, EXTENDS(meshCell):: meshCell3DCartTetra
    !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
      !meshCell DEFERRED PROCEDURES
      PROCEDURE, PASS::   init                => initCellTetra
      PROCEDURE, PASS::   getNodes            => getNodesTetra
      PROCEDURE, PASS::   randPos             => randPosCellTetra
      PROCEDURE, NOPASS:: fPsi                => fPsiTetra
      PROCEDURE, NOPASS:: dPsi                => dPsiTetra
      PROCEDURE, PASS::   partialDer          => partialDerTetra
      PROCEDURE, NOPASS:: detJac              => detJ3DCart
      PROCEDURE, NOPASS:: invJac              => invJ3DCart
      PROCEDURE, PASS::   gatherElectricField => gatherEFTetra
      PROCEDURE, PASS::   gatherMagneticField => gatherMFTetra
      PROCEDURE, PASS::   elemK               => elemKTetra
      PROCEDURE, PASS::   elemF               => elemFTetra
      PROCEDURE, NOPASS:: inside              => insideTetra
      PROCEDURE, PASS::   phy2log             => phy2logTetra
      PROCEDURE, PASS::   neighbourElement    => neighbourElementTetra
      !PARTICULAR PROCEDURES
      PROCEDURE, PASS, PRIVATE:: vol => volumeTetra
      
  END TYPE meshCell3DCartTetra

  CONTAINS
    !NODE FUNCTIONS
    !Init 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

    !EDGE FUNCTIONS
    !Init 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%nNodes = SIZE(p)
      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, nNodes) RESULT(n)
      IMPLICIT NONE

      CLASS(meshEdge3DCartTria), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      INTEGER:: n(1:nNodes)

      n = (/self%n1%n, self%n2%n, self%n3%n/)

    END FUNCTION getNodes3DCartTria

    !Calculate intersection between position and edge
    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

    !Calculate 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):: Xi(1:3)
      REAL(8):: fPsi(1:3)

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

      fPsi = self%fPsi(Xi)
      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(Xi) RESULT(fPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: Xi(1:3)
      REAL(8):: fPsi(1:3)

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

    END FUNCTION fPsiEdgeTria

    !VOLUME FUNCTIONS
    !TETRA FUNCTIONS
    !Init element
    SUBROUTINE initCellTetra(self, n, p, nodes)
      USE moduleRefParam
      IMPLICIT NONE

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

      !Assign node index
      self%n  = n

      !Assign number of nodes of cell
      self%nNodes = SIZE(p)

      !Assign nodes to element
      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%vol()

      CALL OMP_INIT_LOCK(self%lock)

      ALLOCATE(self%listPart_in(1:nSpecies))
      ALLOCATE(self%totalWeight(1:nSpecies))

    END SUBROUTINE initCellTetra

    !Gets node indexes from cell
    PURE FUNCTION getNodesTetra(self, nNodes) RESULT(n)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      INTEGER:: n(1:nNodes)

      n = (/self%n1%n, self%n2%n, self%n3%n, self%n4%n /)

    END FUNCTION getNodesTetra

    !Random position in cell
    FUNCTION randPosCellTetra(self) RESULT(r)
      USE moduleRandom
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      REAL(8):: r(1:3)
      REAL(8):: Xi(1:3)
      REAL(8):: fPsi(1:4)

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

      fPsi = self%fPsi(Xi, 4)

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

    END FUNCTION randPosCellTetra

    !Compute element functions in point Xi
    PURE FUNCTION fPsiTetra(Xi, nNodes) RESULT(fPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: Xi(1:3)
      INTEGER, INTENT(in):: nNodes
      REAL(8):: fPsi(1:nNodes)

      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

    !Compute element derivative functions in point Xi
    PURE FUNCTION dPsiTetra(Xi, nNodes) RESULT(dPsi)
      IMPLICIT NONE

      REAL(8), INTENT(in):: Xi(1:3)
      INTEGER, INTENT(in):: nNodes
      REAL(8):: dPsi(1:3, 1:nNodes)

      dPsi = 0.D0

      dPsi(1,1:4) = (/ -1.D0, 1.D0, 0.D0, 0.D0 /)
      dPsi(2,1:4) = (/ -1.D0, 0.D0, 1.D0, 0.D0 /)
      dPsi(3,1:4) = (/ -1.D0, 0.D0, 0.D0, 1.D0 /)

    END FUNCTION dPsiTetra

    !Compute the derivatives in global coordinates
    PURE FUNCTION partialDerTetra(self, nNodes, dPsi) RESULT(pDer)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in):: dPsi(1:3, 1:nNodes)
      REAL(8):: pDer(1:3, 1:3)

      pDer = 0.D0

      pDer(1, 1:3) = (/ DOT_PRODUCT(dPsi(1,1:4), self%x(1:4)), &
                        DOT_PRODUCT(dPsi(2,1:4), self%x(1:4)), &
                        DOT_PRODUCT(dPsi(3,1:4), self%x(1:4)) /)

      pDer(2, 1:3) = (/ DOT_PRODUCT(dPsi(1,1:4), self%y(1:4)), &
                        DOT_PRODUCT(dPsi(2,1:4), self%y(1:4)), &
                        DOT_PRODUCT(dPsi(3,1:4), self%y(1:4)) /)

      pDer(3, 1:3) = (/ DOT_PRODUCT(dPsi(1,1:4), self%z(1:4)), &
                        DOT_PRODUCT(dPsi(2,1:4), self%z(1:4)), &
                        DOT_PRODUCT(dPsi(3,1:4), self%z(1:4)) /)

    END FUNCTION partialDerTetra

    !Gather electric field at position Xi
    PURE FUNCTION gatherEFTetra(self, Xi) RESULT(array)
      IMPLICIT NONE
      CLASS(meshCell3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: Xi(1:3)
      REAL(8):: array(1:3)
      REAL(8):: phi(1:4)

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

      array = -self%gatherDF(Xi, 4, phi)

    END FUNCTION gatherEFTetra

    !Gather magnetic field at position Xi
    PURE FUNCTION gatherMFTetra(self, Xi) RESULT(array)
      IMPLICIT NONE
      CLASS(meshCell3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: Xi(1:3)
      REAL(8):: array(1:3)
      REAL(8):: B(1:4,1:3)

      B(:,1) = (/ self%n1%emData%B(1), &
                  self%n2%emData%B(1), &
                  self%n3%emData%B(1), &
                  self%n4%emData%B(1) /)

      B(:,2) = (/ self%n1%emData%B(2), &
                  self%n2%emData%B(2), &
                  self%n3%emData%B(2), &
                  self%n4%emData%B(2) /)

      B(:,3) = (/ self%n1%emData%B(3), &
                  self%n2%emData%B(3), &
                  self%n3%emData%B(3), &
                  self%n4%emData%B(3) /)

      array = self%gatherF(Xi, 4, B)

    END FUNCTION gatherMFTetra

    !Compute cell local stiffness matrix
    PURE FUNCTION elemKTetra(self, nNodes) RESULT(localK)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      REAL(8):: localK(1:nNodes,1:nNodes)
      REAL(8):: Xi(1:3)
      REAL(8):: fPsi(1:4), dPsi(1:3, 1:4)
      REAL(8):: pDer(1:3, 1:3)
      REAL(8):: invJ(1:3,1:3), detJ

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

    END FUNCTION elemKTetra

    !Compute element local source vector
    PURE FUNCTION elemFTetra(self, nNodes, source) RESULT(localF)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in):: source(1:nNodes)
      REAL(8):: localF(1:nNodes)
      REAL(8):: Xi(1:3)
      REAL(8):: fPsi(1:4), dPsi(1:3, 1:4)
      REAL(8):: pDer(1:3, 1:3)
      REAL(8):: detJ, f

      localF = 0.D0
      Xi   = 0.D0
      Xi   = (/ 0.25D0, 0.25D0, 0.25D0 /)
      dPsi = self%dPsi(Xi, 4)
      pDer = self%partialDer(4, dPsi)
      detJ = self%detJac(pDer)
      fPsi = self%fPsi(Xi, 4)
      f    = DOT_PRODUCT(fPsi, source)
      localF = f*fPsi*1.D0*detJ

    END FUNCTION elemFTetra

    !Check if Xi is inside the element
    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

    !Transform physical coordinates to element coordinates
    PURE FUNCTION phy2logTetra(self,r) RESULT(Xi)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: r(1:3)
      REAL(8):: Xi(1:3)
      REAL(8):: dPsi(1:3, 1:4)
      REAL(8):: pDer(1:3, 1:3)
      REAL(8):: invJ(1:3, 1:3), detJ
      REAL(8):: deltaR(1:3)

      !Direct method to convert coordinates
      Xi     = 0.D0
      deltaR = (/r(1) - self%x(1), r(2) - self%y(1), r(3) - self%z(1) /)
      dPsi   = self%dPsi(Xi, 4)
      pDer   = self%partialDer(4, dPsi)
      invJ   = self%invJac(pDer)
      detJ   = self%detJac(pDer)
      Xi     = MATMUL(invJ, deltaR)/detJ

    END FUNCTION phy2logTetra
    
    !Get the neighbour cell for a logical position Xi
    SUBROUTINE neighbourElementTetra(self, Xi, neighbourElement)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      REAL(8), INTENT(in):: Xi(1:3)
      CLASS(meshElement), POINTER, INTENT(out):: neighbourElement
      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(neighbourElement)
      SELECT CASE(nextInt)
      CASE (1)
        neighbourElement => self%e1
      CASE (2)
        neighbourElement => self%e2
      CASE (3)
        neighbourElement => self%e3
      CASE (4)
        neighbourElement => self%e4
      END SELECT

    END SUBROUTINE neighbourElementTetra

    !Calculate volume for triangular element
    PURE SUBROUTINE volumeTetra(self)
      IMPLICIT NONE

      CLASS(meshCell3DCartTetra), INTENT(inout):: self
      REAL(8):: Xi(1:3)
      REAL(8):: detJ
      REAL(8):: fPsi(1:4)
      REAL(8):: dPsi(1:3, 1:4), pDer(1:3, 1:3)

      self%volume = 0.D0
      !2D 1 point Gauss Quad Integral
      Xi = (/0.25D0, 0.25D0, 0.25D0/)
      dPsi = self%dPsi(Xi, 4)
      pDer = self%partialDer(4, dPsi)
      detJ = self%detJac(pDer)
      !Computes total volume of the cell
      self%volume = detJ
      !Computes volume per node
      fPsi = self%fPsi(Xi, 4)
      self%n1%v = self%n1%v + fPsi(1)*self%volume
      self%n2%v = self%n2%v + fPsi(2)*self%volume
      self%n3%v = self%n3%v + fPsi(3)*self%volume
      self%n4%v = self%n4%v + fPsi(4)*self%volume

    END SUBROUTINE volumeTetra

    !COMMON FUNCTIONS FOR CARTESIAN VOLUME ELEMENTS IN 3D
    !Compute element Jacobian determinant
    PURE FUNCTION detJ3DCart(pDer) RESULT(dJ)
      IMPLICIT NONE

      REAL(8), INTENT(in):: pDer(1:3, 1:3)
      REAL(8):: dJ

      dJ =   pDer(1,1)*(pDer(2,2)*pDer(3,3) - pDer(2,3)*pDer(3,2)) &
           - pDer(1,2)*(pDer(2,1)*pDer(3,3) - pDer(2,3)*pDer(3,1)) &
           + pDer(1,3)*(pDer(2,1)*pDer(3,2) - pDer(2,2)*pDer(3,1))

    END FUNCTION detJ3DCart

    !Compute element Jacobian inverse matrix (without determinant)
    PURE FUNCTION invJ3DCart(pDer) RESULT(invJ)
      IMPLICIT NONE

      REAL(8), INTENT(in):: pDer(1:3, 1:3)
      REAL(8):: invJ(1:3,1:3)

      invJ(1,1:3) =  (/ (pDer(2,2)*pDer(3,3) - pDer(2,3)*pDer(3,2)), &
                       -(pDer(2,1)*pDer(3,3) - pDer(2,3)*pDer(3,1)), &
                        (pDer(2,1)*pDer(3,2) - pDer(2,2)*pDer(3,1)) /)

      invJ(2,1:3) = (/ -(pDer(1,2)*pDer(3,3) - pDer(1,3)*pDer(3,2)), &
                        (pDer(1,1)*pDer(3,3) - pDer(1,3)*pDer(3,1)), &
                       -(pDer(1,1)*pDer(3,2) - pDer(1,2)*pDer(3,1)) /)

      invJ(3,1:3) =  (/ (pDer(1,2)*pDer(2,3) - pDer(1,3)*pDer(2,2)), &
                       -(pDer(1,1)*pDer(2,3) - pDer(1,3)*pDer(2,1)), &
                        (pDer(1,1)*pDer(2,2) - pDer(1,2)*pDer(2,1)) /)

      invJ = TRANSPOSE(invJ)

    END FUNCTION invJ3DCart

    SUBROUTINE connectMesh3DCart(self)
      IMPLICIT NONE

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

      DO e = 1, self%numCells
        !Connect Cell-Cell
        DO et = 1, self%numCells
          IF (e /= et) THEN
            CALL connectCellCell(self%cells(e)%obj, self%cells(et)%obj)

          END IF

        END DO

        SELECT TYPE(self)
        TYPE IS(meshParticles)
          !Connect Cell-Edge
          DO et = 1, self%numEdges
            CALL connectCellEdge(self%cells(e)%obj, self%edges(et)%obj)

          END DO

        END SELECT

      END DO

    END SUBROUTINE connectMesh3DCart

    !Select type of elements to build connection
    SUBROUTINE connectCellCell(elemA, elemB)
      IMPLICIT NONE

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

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

        END SELECT

      END SELECT

    END SUBROUTINE connectCellCell

    SUBROUTINE connectCellEdge(elemA, elemB)
      IMPLICIT NONE

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

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

        END SELECT

      END SELECT

    END SUBROUTINE connectCellEdge

    !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(meshCell3DCartTetra), INTENT(inout), TARGET:: elemA
      CLASS(meshCell3DCartTetra), 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(meshCell3DCartTetra), INTENT(inout), TARGET:: elemA
      CLASS(meshEdge3DCartTria), INTENT(inout), TARGET:: elemB
      INTEGER:: nodesEdge(1:3)
      REAL(8), DIMENSION(1:3):: vec1, vec2
      REAL(8):: normCell(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) /)
          normCell = crossProduct(vec1, vec2)
          normCell = normalize(normCell)

          IF (DOT_PRODUCT(elemB%normal, normCell) == -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) /)
          normCell = crossProduct(vec1, vec2)
          normCell = normalize(normCell)

          IF (DOT_PRODUCT(elemB%normal, normCell) == -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) /)
          normCell = crossProduct(vec1, vec2)
          normCell = normalize(normCell)

          IF (DOT_PRODUCT(elemB%normal, normCell) == -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) /)
          normCell = crossProduct(vec1, vec2)
          normCell = normalize(normCell)

          IF (DOT_PRODUCT(elemB%normal, normCell) == -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