!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(meshCell):: meshCell3DCart
    CONTAINS
      PROCEDURE, PASS:: detJac => detJ3DCart
      PROCEDURE, PASS:: invJac => invJ3DCart
      PROCEDURE(partialDer_interface), DEFERRED, PASS:: partialDer
  
  END TYPE meshCell3DCart

  ABSTRACT INTERFACE
    PURE SUBROUTINE partialDer_interface(self, nNodes, dPsi, dx, dy, dz)
      IMPORT meshCell3DCart
      CLASS(meshCell3DCart), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in):: dPsi(1:3,1:nNodes)
      REAL(8), INTENT(out), DIMENSION(1:3):: dx, dy, dz

    END SUBROUTINE partialDer_interface

  END INTERFACE

  !Tetrahedron volume element
  TYPE, PUBLIC, EXTENDS(meshCell3DCart):: 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
      PROCEDURE, PASS::   init        => initCellTetra
      PROCEDURE, PASS::   randPos     => randPosCellTetra
      PROCEDURE, PASS::   calcCell     => volumeTetra
      PROCEDURE, PASS:: fPsi        => fPsiTetra
      PROCEDURE, PASS:: dPsi        => dPsiTetra
      PROCEDURE, NOPASS, PRIVATE:: dPsiXi1 => dPsiTetraXi1
      PROCEDURE, NOPASS, PRIVATE:: dPsiXi2 => dPsiTetraXi2
      PROCEDURE, PASS::   partialDer  => partialDerTetra
      PROCEDURE, PASS::   elemK       => elemKTetra
      PROCEDURE, PASS::   elemF       => elemFTetra
      PROCEDURE, PASS:: gatherElectricField => gatherEFTetra
      PROCEDURE, PASS:: gatherMagneticField => gatherMFTetra
      PROCEDURE, NOPASS:: inside      => insideTetra
      PROCEDURE, PASS::   getNodes    => getNodesTetra
      PROCEDURE, PASS::   phy2log     => phy2logTetra
      PROCEDURE, PASS::   nextElement => nextElementTetra
      
  END TYPE meshCell3DCartTetra

  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%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

    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):: 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), ALLOCATABLE:: fPsi(:)

      ALLOCATE(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
    !Inits tetrahedron 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
      REAL(8):: Xi(1:3), fPsi(1:4)
      REAL(8):: volNodes(1:4) !Cellume of each node

      self%n  = n
      self%nNodes = SIZE(p)
      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%calcCell()

      !Assign proportional volume to each node
      Xi = (/0.25D0, 0.25D0, 0.25D0/)
      fPsi = self%fPsi(Xi, 4)
      volNodes = fPsi*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)

      CALL OMP_INIT_LOCK(self%lock)

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

    END SUBROUTINE initCellTetra

    !Random position in volume tetrahedron
    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

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

      CLASS(meshCell3DCartTetra), INTENT(inout):: self
      REAL(8):: Xi(1:3)

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

    END SUBROUTINE volumeTetra

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

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      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

    !Derivative element function at coordinates Xi
    PURE FUNCTION dPsiTetra(self, Xi, nNodes) RESULT(dPsi)
      IMPLICIT NONE

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

      dPsi = 0.D0

      dPsi(1,:) = dPsiTetraXi1(Xi(2), Xi(3))
      dPsi(2,:) = dPsiTetraXi2(Xi(1), Xi(3))
      dPsi(3,:) = dPsiTetraXi3(Xi(1), Xi(2))

    END FUNCTION dPsiTetra

    !Derivative element function respect to Xi1
    PURE FUNCTION dPsiTetraXi1(Xi2, Xi3) RESULT(dPsiXi1)
      IMPLICIT NONE
      REAL(8), INTENT(in):: Xi2, Xi3
      REAL(8):: dPsiXi1(1:4)

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

    END FUNCTION dPsiTetraXi1
      
    !Derivative element function respect to Xi2
    PURE FUNCTION dPsiTetraXi2(Xi1, Xi3) RESULT(dPsiXi2)
      IMPLICIT NONE
      REAL(8), INTENT(in):: Xi1, Xi3
      REAL(8):: dPsiXi2(1:4)

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

    END FUNCTION dPsiTetraXi2
      
    !Derivative element function respect to Xi3
    PURE FUNCTION dPsiTetraXi3(Xi1, Xi2) RESULT(dPsiXi3)
      IMPLICIT NONE
      REAL(8), INTENT(in):: Xi1, Xi2
      REAL(8):: dPsiXi3(1:4)

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

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

      CLASS(meshCell3DCartTetra), INTENT(in):: self
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in):: dPsi(1:3, 1:nNodes)
      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, 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):: 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)
      detJ   = self%detJac(Xi, 4, dPsi)
      invJ   = self%invJac(Xi, 4, dPsi)
      fPsi = self%fPsi(Xi, 4)
      localK = MATMUL(TRANSPOSE(MATMUL(invJ,dPsi)),MATMUL(invJ,dPsi))*1.D0/detJ

    END FUNCTION elemKTetra

    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):: fPsi(1:4), dPsi(1:3, 1:4)
      REAL(8):: Xi(1:3)
      REAL(8):: detJ, f

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

    END FUNCTION elemFTetra

    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

    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

    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

    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

    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):: 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, 4)
      invJ   = self%invJac(Xi, 4, dPsi)
      detJ   = self%detJac(Xi, 4, dPsi)
      Xi     = MATMUL(invJ, deltaR)/detJ

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

      CLASS(meshCell3DCartTetra), 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, nNodes, dPsi_in) RESULT(dJ)
      IMPLICIT NONE

      CLASS(meshCell3DCart), INTENT(in)::self
      REAL(8), INTENT(in):: Xi(1:3)
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in), OPTIONAL:: dPsi_in(1:3, 1:nNodes)
      REAL(8):: dJ
      REAL(8):: dPsi(1:3, 1:nNodes)
      REAL(8):: dx(1:3), dy(1:3), dz(1:3)

      IF (PRESENT(dPsi_in)) THEN
        dPsi = dPsi_in

      ELSE
        dPsi = self%dPsi(Xi, 4)

      END IF

      CALL self%partialDer(nNodes, 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, nNodes, dPsi_in) RESULT(invJ)
      IMPLICIT NONE

      CLASS(meshCell3DCart), INTENT(in):: self
      REAL(8), INTENT(in):: Xi(1:3)
      INTEGER, INTENT(in):: nNodes
      REAL(8), INTENT(in), OPTIONAL:: dPsi_in(1:3, 1:nNodes)
      REAL(8):: dPsi(1:3, 1:nNodes)
      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, 4)

      END IF

      CALL self%partialDer(nNodes, 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 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

    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

    !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