MODULE moduleMesh3DCartRead
  USE moduleMesh
  USE moduleMesh3DCart

  TYPE, EXTENDS(meshGeneric):: mesh3DCartGeneric
    CONTAINS
      PROCEDURE, PASS:: init     => init3DCartMesh
      PROCEDURE, PASS:: readMesh => readMesh3DCartGmsh

  END TYPE

  INTERFACE connected
    MODULE PROCEDURE connectedVolVol, connectedVolEdge

  END INTERFACE connected

  CONTAINS
    !Init mesh
    SUBROUTINE init3DCartMesh(self, meshFormat)
      USE moduleMesh
      USE moduleErrors
      IMPLICIT NONE

      CLASS(mesh3DCartGeneric), INTENT(out):: self
      CHARACTER(:), ALLOCATABLE, INTENT(in):: meshFormat

      SELECT CASE(meshFormat)
      CASE ("gmsh")
        self%printOutput => printOutputGmsh
        self%printColl   => printCollGmsh
        self%printEM     => printEMGmsh

      CASE DEFAULT
        CALL criticalError("Mesh type " // meshFormat // " not supported.", "init3DCartMesh")

      END SELECT

    END SUBROUTINE init3DCartMesh

    !Read mesh from gmsh file
    SUBROUTINE readMesh3DCartGmsh(self, filename)
      USE moduleBoundary
      IMPLICIT NONE

      CLASS(mesh3DCartGeneric), INTENT(inout):: self
      CHARACTER(:), ALLOCATABLE, INTENT(in):: filename
      REAL(8):: x, y, z
      INTEGER:: p(1:4)
      INTEGER:: e = 0, et = 0, n = 0, eTemp = 0, elemType = 0, bt = 0
      INTEGER:: totalNumElem
      INTEGER:: boundaryType

      !Read mesh
      OPEN(10, FILE=TRIM(filename))
      !Skip header
      READ(10, *)
      READ(10, *)
      READ(10, *)
      READ(10, *)
      !Read number of nodes
      READ(10, *) self%numNodes
      !Allocate required matrices and vectors
      ALLOCATE(self%nodes(1:self%numNodes))
      ALLOCATE(self%K(1:self%numNodes, 1:self%numNodes))
      ALLOCATE(self%IPIV(1:self%numNodes, 1:self%numNodes))
      self%K = 0.D0
      self%IPIV = 0

      !Read node cartesian coordinates (x = x, y = y, z = z)
      DO e = 1, self%numNodes
        READ(10, *) n, x, y, z
        ALLOCATE(meshNode3Dcart::self%nodes(n)%obj)
        CALL self%nodes(n)%obj%init(n, (/x, y, z /))

      END DO

      !Skip comments
      READ(10, *)
      READ(10, *)

      !Reads total number of elements
      READ(10, *) totalNumElem
      !conts edges and volume elements
      self%numEdges = 0
      DO e = 1, totalNumElem
        READ(10, *) eTemp, elemType
        IF (elemType == 2) THEN
          self%numEdges = e

        END IF

      END DO

      !Substract the number of edges to the total number of elements to obtain the number
      !of volume elements
      self%numVols = totalNumElem - self%numEdges

      !Allocate required arrays
      ALLOCATE(self%edges(1:self%numEdges))
      ALLOCATE(self%vols(1:self%numVols))

      !Go back to the beggining to read each specific element
      DO e = 1, totalNumElem
        BACKSPACE(10)

      END DO

      !Reads surfaces
      DO e = 1, self%numEdges
        READ(10, *) n, elemType
        BACKSPACE(10)

        SELECT CASE(elemType)
        CASE(2)
          !Triangular surface
          READ(10, *) n, elemType, eTemp, boundaryType, eTemp, p(1:3)
          bt = getBoundaryID(boundaryType)

          ALLOCATE(meshEdge3DCartTria:: self%edges(e)%obj)

          CALL self%edges(e)%obj%init(n, p(1:3), bt, boundaryType)

        END SELECT

      END DO

      !Read and initialize volumes
      DO e = 1, self%numVols
        READ(10, *) n, elemType
        BACKSPACE(10)

        SELECT CASE(elemType)
        CASE(4)
          !Tetrahedron element
          READ(10, *) n, elemType, eTemp, eTemp, eTemp, p(1:4)
          ALLOCATE(meshVol3DCartTetra::  self%vols(e)%obj)
          CALL self%vols(e)%obj%init(n - self%numEdges, p(1:4))

        END SELECT

      END DO

      CLOSE(10)

      !Build connectivy between elements
      DO e = 1, self%numVols
        !Connectivity between volumes
        DO et = 1, self%numVols
          IF (e /= et) THEN
            CALL connected(self%vols(e)%obj, self%vols(et)%obj)

          END IF

        END DO

        !Connectivity between vols and surfaces
        DO et = 1, self%numEdges
          CALL connected(self%vols(e)%obj, self%edges(et)%obj)

        END DO

        !Constructs the global K matrix
        CALL constructGlobalK(self%K, self%vols(e)%obj)
        
      END DO

    END SUBROUTINE readMesh3DCartGmsh

    !Selects type of elements to build connection
    SUBROUTINE connectedVolVol(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 connectedTetraTetra(elemA, elemB)

        END SELECT

      END SELECT

    END SUBROUTINE connectedVolVol


    SUBROUTINE connectedVolEdge(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 connectedTetraEdge(elemA, elemB)

        END SELECT

      END SELECT

    END SUBROUTINE connectedVolEdge

    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 connectedTetraTetra(elemA, elemB)
      IMPLICIT NONE

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

      !TODO: Try to find a much clear way to do this

      !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 connectedTetraTetra

    SUBROUTINE connectedTetraEdge(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 connectedTetraEdge

    SUBROUTINE constructGlobalK(K, elem)
      IMPLICIT NONE

      REAL(8), INTENT(inout):: K(1:, 1:)
      CLASS(meshVol), INTENT(in):: elem
      REAL(8), ALLOCATABLE:: localK(:,:)
      INTEGER:: nNodes, i, j
      INTEGER, ALLOCATABLE:: n(:)

      SELECT TYPE(elem)
      TYPE IS(meshVol3DCartTetra)
        nNodes = 4
        ALLOCATE(localK(1:nNodes,1:nNodes))
        localK = elem%elemK()
        ALLOCATE(n(1:nNodes))
        n = (/ elem%n1%n, elem%n2%n, &
               elem%n3%n, elem%n4%n /)

      CLASS DEFAULT
        nNodes = 0
        ALLOCATE(localK(1:1, 1:1))
        localK = 0.D0
        ALLOCATE(n(1:1))
        n = 0

      END SELECT
      
      DO i = 1, nNodes
        DO j = 1, nNodes
          K(n(i), n(j)) = K(n(i), n(j)) + localK(i, j)
        END DO
      END DO

    END SUBROUTINE constructGlobalK

END MODULE moduleMesh3DCartRead