InterpolateMeshToMesh.F90

!/*****************************************************************************/
! *
! *  Elmer, A Finite Element Software for Multiphysical Problems
! *
! *  Copyright 1st April 1995 - , CSC - IT Center for Science Ltd., Finland
! * 
! *  This library is free software; you can redistribute it and/or
! *  modify it under the terms of the GNU Lesser General Public
! *  License as published by the Free Software Foundation; either
! *  version 2.1 of the License, or (at your option) any later version.
! *
! *  This library is distributed in the hope that it will be useful,
! *  but WITHOUT ANY WARRANTY; without even the implied warranty of
! *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
! *  Lesser General Public License for more details.
! * 
! *  You should have received a copy of the GNU Lesser General Public
! *  License along with this library (in file ../LGPL-2.1); if not, write 
! *  to the Free Software Foundation, Inc., 51 Franklin Street, 
! *  Fifth Floor, Boston, MA  02110-1301  USA
! *
! *****************************************************************************/
!
!/******************************************************************************
! *
! *  Authors: Juha Ruokolainen, Peter Råback, Joe Todd, Mika Malinen
! *  Email:   [email protected]
! *  Web:     http://www.csc.fi/elmer
! *  Address: CSC - IT Center for Science Ltd.
! *           Keilaranta 14
! *           02101 Espoo, Finland 
! *
! *
! ******************************************************************************/


!------------------------------------------------------------------------------
!> Map results from mesh to mesh. The from-mesh is stored in an octree from 
!> which it is relatively fast to find the to-nodes. When the node is found
!> interpolation is performed. Optionally there may be an existing projector
!> that speeds up the interpolation.
!------------------------------------------------------------------------------
     SUBROUTINE InterpolateMeshToMesh( OldMesh, NewMesh, OldVariables, &
         NewVariables, UseQuadrantTree, Projector, MaskName, UnfoundNodes )
!------------------------------------------------------------------------------
       USE Lists
       USE SParIterComm
       USE Interpolation
       USE CoordinateSystems
       USE MeshUtils, ONLY: ReleaseMesh
!-------------------------------------------------------------------------------
       TYPE(Mesh_t), TARGET  :: OldMesh, NewMesh
       TYPE(Variable_t), POINTER, OPTIONAL :: OldVariables, NewVariables
       LOGICAL, OPTIONAL :: UseQuadrantTree
       TYPE(Projector_t), POINTER, OPTIONAL :: Projector
       CHARACTER(LEN=*),OPTIONAL :: MaskName
       LOGICAL, POINTER, OPTIONAL :: UnfoundNodes(:)
!-------------------------------------------------------------------------------
       INTEGER, ALLOCATABLE :: perm(:), vperm(:)
       INTEGER, POINTER :: nperm(:)
       LOGICAL, ALLOCATABLE :: FoundNodes(:), FoundNodesPar(:)
       TYPE(Mesh_t), POINTER :: nMesh
       TYPE(VAriable_t), POINTER :: Var, nVar
       INTEGER :: i,j,k,l,nfound,maxrecv,n,ierr,nvars,npart,proc,status(MPI_STATUS_SIZE)
       REAL(KIND=dp) :: myBB(6), eps2, dn
       REAL(KIND=dp), POINTER :: store(:)
       REAL(KIND=dp), ALLOCATABLE, TARGET :: astore(:),vstore(:,:), BB(:,:), &
             nodes_x(:),nodes_y(:),nodes_z(:), xpart(:), ypart(:), zpart(:)
       LOGICAL :: al, Stat
       INTEGER :: PassiveCoordinate

       TYPE ProcRecv_t
         INTEGER :: n = 0
         REAL(KIND=dp), ALLOCATABLE :: nodes_x(:),nodes_y(:),nodes_z(:)
       END TYPE ProcRecv_t
       TYPE(ProcRecv_t),  ALLOCATABLE, TARGET :: ProcRecv(:)

       TYPE ProcSend_t
         INTEGER :: n = 0
         INTEGER, ALLOCATABLE :: perm(:)
       END TYPE ProcSend_t
       TYPE(ProcSend_t),  ALLOCATABLE :: ProcSend(:)

!-------------------------------------------------------------------------------
       INTERFACE
         SUBROUTINE InterpolateMeshToMeshQ( OldMesh, NewMesh, OldVariables, NewVariables, &
             UseQuadrantTree, Projector, MaskName, FoundNodes, NewMaskPerm, KeepUnfoundNodes )
           USE Types
           TYPE(Variable_t), POINTER, OPTIONAL :: OldVariables, NewVariables
           TYPE(Mesh_t), TARGET  :: OldMesh, NewMesh
           LOGICAL, OPTIONAL :: UseQuadrantTree,FoundNodes(:)
           CHARACTER(LEN=*),OPTIONAL :: MaskName
           TYPE(Projector_t), POINTER, OPTIONAL :: Projector
           INTEGER, OPTIONAL, POINTER :: NewMaskPerm(:)  !< Mask the new variable set by the given MaskName when trying to define the interpolation.
           LOGICAL, OPTIONAL :: KeepUnfoundNodes  !< Do not disregard unfound nodes from projector
         END SUBROUTINE InterpolateMeshToMeshQ
       END INTERFACE
!-------------------------------------------------------------------------------

      ALLOCATE( FoundNodes(NewMesh % NumberOfNodes) ); FoundNodes=.FALSE.

      IF(PRESENT(UnfoundNodes)) THEN
         IF(ASSOCIATED(UnfoundNodes)) DEALLOCATE(UnfoundNodes)
         ALLOCATE(UnfoundNodes(NewMesh % NumberOfNodes))
      END IF

      ! In serial interpolation is simple
      !------------------------------------
      IF ( ParEnv % PEs<=1 ) THEN
         CALL InterpolateMeshToMeshQ( OldMesh, NewMesh, OldVariables, &
            NewVariables, UseQuadrantTree, Projector, MaskName, FoundNodes )

         IF( InfoActive(20) ) THEN
           n = COUNT(.NOT. FoundNodes )
           IF(n>0) CALL Info('InterpolateMeshToMesh','Number of unfound nodes in serial: '//I2S(n))
         END IF
                    
         IF(PRESENT(UnfoundNodes)) UnfoundNodes = .NOT. FoundNodes
         RETURN
      END IF

      ! Passive coordinate is needed also here in order not to use that direction
      ! for the bounding box checks.
      !---------------------------------------------------------------------------
      PassiveCoordinate = ListGetInteger( CurrentModel % Simulation, &
          'Interpolation Passive Coordinate', Stat ) 
      IF (.NOT. Stat .AND. ASSOCIATED(CurrentModel % Solver)) THEN
        PassiveCoordinate = ListGetInteger( CurrentModel % Solver % Values, &
            'Interpolation Passive Coordinate', Stat ) 
      END IF
            
      ! Interpolate within our own partition, flag the points we found:
      ! ---------------------------------------------------------------      
      CALL InterpolateMeshToMeshQ( OldMesh, NewMesh, OldVariables, &
         NewVariables, UseQuadrantTree, MaskName=MaskName, FoundNodes=FoundNodes )

      IF(PRESENT(UnfoundNodes)) UnfoundNodes = .NOT. FoundNodes
      
      ! special case "all found":
      !--------------------------
      n = COUNT(.NOT.FoundNodes); dn = n

      IF( InfoActive(20) ) THEN
        IF(n>0) CALL Info('InterpolateMeshToMesh','Number of unfound nodes in own partition: '//I2S(n))
      END IF
      
      AL = .FALSE.
      IF (.NOT.ASSOCIATED(ParEnv % Active) ) THEN
        ALLOCATE(Parenv % Active(PArEnv % PEs))
        AL = .TRUE.
        ParEnv % Active = .TRUE.
      END IF

      CALL SParActiveSUM(dn,2)
      IF ( dn==0 ) RETURN

      ! No use to continue even in parallel, since the OldMeshes are all the same!
      IF( OldMesh % SingleMesh ) THEN
        CALL Warn('InterpolateMeshToMesh','Could not find all dofs in single mesh: '//I2S(NINT(dn)))
        RETURN
      END IF

      ! Exchange partition bounding boxes:
      ! This is needed to eliminate the amount of data to send among partitions.
      ! ------------------------------------------------------------------------
      myBB = HUGE(mybb(1))
      IF(OldMesh % NumberOfNodes /= 0) THEN
        myBB(1) = MINVAL(OldMesh % Nodes % x)
        myBB(2) = MINVAL(OldMesh % Nodes % y)
        myBB(3) = MINVAL(OldMesh % Nodes % z)
        myBB(4) = MAXVAL(OldMesh % Nodes % x)
        myBB(5) = MAXVAL(OldMesh % Nodes % y)
        myBB(6) = MAXVAL(OldMesh % Nodes % z)

        eps2 = 0.1_dp * MAXVAL(myBB(4:6)-myBB(1:3))
        myBB(1:3) = myBB(1:3) - eps2
        myBB(4:6) = myBB(4:6) + eps2
      END IF

      ALLOCATE(BB(6,ParEnv % PEs))
      CALL CheckBuffer(ParEnv % PEs*(6+MPI_BSEND_OVERHEAD))
      DO i=1,ParEnv % PEs
        IF ( Parenv % mype == i-1 .OR. .NOT. ParEnv % Active(i) ) CYCLE
        proc = i-1
        CALL MPI_BSEND( myBB, 6, MPI_DOUBLE_PRECISION, proc, &
                 999, ELMER_COMM_WORLD, ierr )
      END DO
      DO i=1,COUNT(ParEnv % Active)-1
        CALL MPI_RECV( myBB, 6, MPI_DOUBLE_PRECISION, MPI_ANY_SOURCE, &
                 999, ELMER_COMM_WORLD, status, ierr )
        proc = status(MPI_SOURCE)
        BB(:,proc+1) = myBB
      END DO

      CALL CheckBuffer(Parenv % PEs*(n*(3 * 2 + 2)+MPI_BSEND_OVERHEAD)) !3 x double precision coord, 2 x count

      IF ( n==0 ) THEN
        ! We have found all nodes, nothing to do except sent the info to others!
        !----------------------------------------------------------------------
        DEALLOCATE(BB)
        DO i=1,ParEnv % PEs
          IF ( Parenv % mype == i-1 .OR. .NOT. ParEnv % Active(i) ) CYCLE
          proc = i-1
          CALL MPI_BSEND( n, 1, MPI_INTEGER, proc, &
                1001, ELMER_COMM_WORLD, ierr )
        END DO
      ELSE
        ! Extract nodes that we didn't find from our own partition...
        ! ------------------------------------------------------------
        ALLOCATE( Perm(n), nodes_x(n), nodes_y(n),nodes_z(n) ); Perm=0
        j = 0
        DO i=1,NewMesh % NumberOfNodes
          IF ( FoundNodes(i) ) CYCLE
          j = j + 1
          perm(j) = i
          nodes_x(j) = NewMesh % Nodes % x(i)
          nodes_y(j) = NewMesh % Nodes % y(i)
          nodes_z(j) = NewMesh % Nodes % z(i)
        END DO

        ! ...and ask those from others
        ! -------------------------------
        ALLOCATE(ProcSend(ParEnv % PEs))
        DO i=1,ParEnv % PEs
          IF ( Parenv % mype == i-1 .OR. .NOT. ParEnv % Active(i) ) CYCLE
          proc = i-1

          ! extract those of the missing nodes that are within the other
          ! partitions bounding box:
          ! ------------------------------------------------------------
          myBB = BB(:,i)
          npart = 0
          DO j=1,n
            IF ( ( nodes_x(j)<myBB(1) .OR. nodes_x(j)>myBB(4) ) .AND. PassiveCoordinate /= 1 ) CYCLE
            IF ( ( nodes_y(j)<myBB(2) .OR. nodes_y(j)>myBB(5) ) .AND. PassiveCoordinate /= 2 ) CYCLE
            IF ( ( nodes_z(j)<myBB(3) .OR. nodes_z(j)>myBB(6) ) .AND. PassiveCoordinate /= 3 ) CYCLE
            npart = npart+1
          END DO
          ProcSend(proc+1) % n = npart
          IF ( npart>0 ) THEN
            ALLOCATE( xpart(npart),ypart(npart),zpart(npart),ProcSend(proc+1) % perm(npart) )
            npart = 0
            DO j=1,n
              IF ( ( nodes_x(j)<myBB(1) .OR. nodes_x(j)>myBB(4) ) .AND. PassiveCoordinate /= 1 ) CYCLE
              IF ( ( nodes_y(j)<myBB(2) .OR. nodes_y(j)>myBB(5) ) .AND. PassiveCoordinate /= 2 ) CYCLE
              IF ( ( nodes_z(j)<myBB(3) .OR. nodes_z(j)>myBB(6) ) .AND. PassiveCoordinate /= 3 ) CYCLE
              npart=npart+1
              ProcSend(proc+1) % perm(npart)=j
              xpart(npart) = Nodes_x(j)
              ypart(npart) = Nodes_y(j)
              zpart(npart) = Nodes_z(j)
            END DO
          END IF

          ! send count...
          ! -------------
          CALL MPI_BSEND( npart, 1, MPI_INTEGER, proc, &
                  1001, ELMER_COMM_WORLD, ierr )

          IF ( npart==0 ) CYCLE

          ! ...and points
          ! -------------
          CALL MPI_BSEND( xpart, npart, MPI_DOUBLE_PRECISION, proc, &
                  1002, ELMER_COMM_WORLD, ierr )
          CALL MPI_BSEND( ypart, npart, MPI_DOUBLE_PRECISION, proc, &
                  1003, ELMER_COMM_WORLD, ierr )
          CALL MPI_BSEND( zpart, npart, MPI_DOUBLE_PRECISION, proc, &
                  1004, ELMER_COMM_WORLD, ierr )

          DEALLOCATE(xpart,ypart,zpart)
        END DO
        DEALLOCATE(nodes_x,nodes_y,nodes_z,BB)
      END IF

       
      ! receive points from others:
      ! ----------------------------
      ALLOCATE(ProcRecv(Parenv % Pes))
      DO i=1,COUNT(ParEnv % Active)-1
        CALL MPI_RECV( n, 1, MPI_INTEGER, MPI_ANY_SOURCE, &
              1001, ELMER_COMM_WORLD, status, ierr )

        proc = status(MPI_SOURCE)
        ProcRecv(proc+1) % n = n

        IF ( n<=0 ) CYCLE

        ALLOCATE(ProcRecv(proc+1) % Nodes_x(n), &
              ProcRecv(proc+1) % Nodes_y(n),ProcRecv(proc+1) % Nodes_z(n))

        CALL MPI_RECV( ProcRecv(proc+1) % nodes_x, n, MPI_DOUBLE_PRECISION, proc, &
               1002, ELMER_COMM_WORLD, status, ierr )
        CALL MPI_RECV( ProcRecv(proc+1) % nodes_y, n, MPI_DOUBLE_PRECISION, proc, &
               1003, ELMER_COMM_WORLD, status, ierr )
        CALL MPI_RECV( ProcRecv(proc+1) % nodes_z, n, MPI_DOUBLE_PRECISION, proc, &
               1004, ELMER_COMM_WORLD, status, ierr )
      END DO

      ! Count variables and received nodes, and check MPI buffer is 
      ! sufficiently large:
      ! -----------------------------------------------------------
      Var => OldVariables
      nvars = 0
      DO WHILE(ASSOCIATED(Var))
        IF(LegitInterpVar(Var)) THEN         
          nvars = nvars + 1
          IF ( ASSOCIATED(Var % PrevValues) ) THEN
            j = SIZE(Var % PrevValues,2)
            nvars = nvars+j
          END IF
        END IF
        Var => Var % Next
      END DO

      maxrecv = 0
      DO i=1,SIZE(ProcRecv)
         maxrecv = MAX(maxrecv, ProcRecv(i) % n)
      END DO
      
      !For each node, we send a single integer perm and 
      !a real(dp) per variable. Also sending two counts
      CALL CheckBuffer(SIZE(ProcRecv) * maxrecv * ((2 * nvars) + 1) + 2)

      ! Check the received points and extract values for the to-be-interpolated-
      ! variables, if we have the points within our domain: 
      ! ------------------------------------------------------------------------
      DO i=1,ParEnv % PEs
        IF ( Parenv % mype == i-1 .OR. .NOT. ParEnv % Active(i) ) CYCLE

        proc = i-1
        n = ProcRecv(i) % n

        IF ( n==0 ) THEN
          CALL MPI_BSEND( n, 1, MPI_INTEGER, proc, 2001, ELMER_COMM_WORLD, ierr )
          CYCLE
        END IF
      
        ! Construct temporary mesh structure for the received points:
        ! -----------------------------------------------------------
        Nmesh => AllocateMesh()
        Nmesh % Nodes % x => ProcRecv(i) % nodes_x
        Nmesh % Nodes % y => ProcRecv(i) % nodes_y
        Nmesh % Nodes % z => ProcRecv(i) % nodes_z
        Nmesh % NumberOfNodes = n

        ALLOCATE(nperm(n))
        DO j=1,n
          nPerm(j)=j
        END DO

        Var => OldVariables
        nvars = 0
        DO WHILE(ASSOCIATED(Var))
          IF(LegitInterpVar(Var)) THEN         
            ALLOCATE(store(n)); store=0
            nvars = nvars+1
            CALL VariableAdd(nMesh % Variables,nMesh,Var % Solver, &
                Var % Name,1,store,nperm )
            IF ( ASSOCIATED(Var % PrevValues) ) THEN
              j = SIZE(Var % PrevValues,2)
              nvars = nvars+j
              Nvar => VariableGet( Nmesh % Variables,Var % Name,ThisOnly=.TRUE.)
              ALLOCATE(Nvar % PrevValues(n,j))
              Nvar % PrevValues = 0._dp
            END IF
          END IF
          Var => Var % Next
        END DO

        ! try interpolating values for the points:
        ! ----------------------------------------
        ALLOCATE( FoundNodesPar(n) ); FoundNodesPar=.FALSE.
        
        CALL InterpolateMeshToMeshQ( OldMesh, nMesh, OldVariables, &
           nMesh % Variables, UseQuadrantTree, MaskName=MaskName, FoundNodes=FoundNodesPar )

        nfound = COUNT(FoundNodesPar)

        CALL MPI_BSEND( nfound, 1, MPI_INTEGER, proc, 2001, ELMER_COMM_WORLD, ierr )

        ! send interpolated values back to the owner:
        ! -------------------------------------------
        IF ( nfound>0 ) THEN
          ALLOCATE(vstore(nfound,nvars), vperm(nfound)); vstore=0
          k = 0
          DO j=1,n
            IF ( .NOT.FoundNodesPar(j)) CYCLE   
            k = k + 1
            vperm(k) = j
            Var => OldVariables
            nvars = 0
            DO WHILE(ASSOCIATED(Var))
              IF(LegitInterpVar(Var)) THEN
                Nvar => VariableGet( Nmesh % Variables,Var % Name,ThisOnly=.TRUE.)
                nvars=nvars+1
                vstore(k,nvars)=Nvar % Values(j)
                IF ( ASSOCIATED(Var % PrevValues) ) THEN
                  DO l=1,SIZE(Var % PrevValues,2)
                    nvars = nvars+1
                    vstore(k,nvars)=Nvar % PrevValues(j,l)
                  END DO
                END IF
              END IF
              Var => Var % Next
            END DO
          END DO

          CALL MPI_BSEND( vperm, nfound, MPI_INTEGER, proc, &
                2002, ELMER_COMM_WORLD, ierr )

          DO j=1,nvars
            CALL MPI_BSEND( vstore(:,j), nfound, MPI_DOUBLE_PRECISION, proc, &
                       2002+j, ELMER_COMM_WORLD, ierr )
          END DO

          DEALLOCATE(vstore, vperm)
        END IF

        !Pointers to ProcRev, deallocated automatically below
        NULLIFY(Nmesh % Nodes % x,&
                Nmesh % Nodes % y,&
                Nmesh % Nodes % z)

        CALL ReleaseMesh(Nmesh)
        DEALLOCATE(FoundNodesPar, Nmesh)
      END DO
      DEALLOCATE(ProcRecv)

     ! Receive interpolated values:
     ! ----------------------------
      DO i=1,COUNT(ParEnv % Active)-1

        ! recv count:
        ! -----------
        CALL MPI_RECV( n, 1, MPI_INTEGER, MPI_ANY_SOURCE, &
              2001, ELMER_COMM_WORLD, status, ierr )

        proc = status(MPI_SOURCE)
        IF ( n<=0 ) THEN
          IF ( ALLOCATED(ProcSend) ) THEN
            IF ( ALLOCATED(ProcSend(proc+1) % Perm)) &
                       DEALLOCATE(ProcSend(proc+1) % Perm)
          END IF
          CYCLE
        END IF

        ALLOCATE(astore(n),vperm(n))

        ! recv permutation (where in the original array the
        ! points the partition found are):
        ! --------------------------------------------------
        CALL MPI_RECV( vperm, n, MPI_INTEGER, proc, &
              2002, ELMER_COMM_WORLD, status, ierr )

        !Mark nodes as found 
        DO j=1,n
          k=perm(ProcSend(proc+1) % Perm(vperm(j)))          
          FoundNodes(k) = .TRUE.
        END DO
                
        ! recv values and store:
        ! ----------------------
        Var => OldVariables
        nvars=0
        DO WHILE(ASSOCIATED(Var))
          IF(LegitInterpVar(Var)) THEN
              nvars=nvars+1
              CALL MPI_RECV( astore, n, MPI_DOUBLE_PRECISION, proc, &
                  2002+nvars, ELMER_COMM_WORLD, status, ierr )

              Nvar => VariableGet( NewMesh % Variables,Var % Name,ThisOnly=.TRUE.)

              IF ( ASSOCIATED(Nvar) ) THEN
                DO j=1,n
                  k=perm(ProcSend(proc+1) % Perm(vperm(j)))
                  IF(ASSOCIATED(nvar % Perm)) THEN
                    IF ( Nvar % perm(k)>0 ) &
                      Nvar % Values(Nvar % Perm(k)) = astore(j)
                  ELSE
                      Nvar % Values(k) = astore(j)
                  END IF
                END DO
              END IF

              IF ( ASSOCIATED(Var % PrevValues) ) THEN
                DO l=1,SIZE(Var % PrevValues,2)
                  nvars=nvars+1
                  CALL MPI_RECV( astore, n, MPI_DOUBLE_PRECISION, proc, &
                      2002+nvars, ELMER_COMM_WORLD, status, ierr )

                  IF ( ASSOCIATED(Nvar) ) THEN
                    DO j=1,n
                      k=perm(ProcSend(proc+1) % Perm(vperm(j)))
                      IF ( ASSOCIATED(Nvar % perm)) THEN
                        IF ( Nvar % perm(k)>0 ) &
                          Nvar % PrevValues(Nvar % Perm(k),l) = astore(j)
                      ELSE
                          Nvar % PrevValues(k,l) = astore(j)
                      END IF
                    END DO
                  END IF
                END DO
              END IF
          END IF
          Var => Var % Next
        END DO
        DEALLOCATE(astore,vperm,ProcSend(proc+1) % perm)
      END DO
      IF ( ALLOCATED(Perm) ) DEALLOCATE(Perm,ProcSend)

      CALL MPI_BARRIER(ParEnv % ActiveComm,ierr)

      IF(AL) THEN
        DEALLOCATE(Parenv % Active)
        ParEnv % Active => NULL()
      END IF

      n = COUNT(.NOT. FoundNodes )           
      IF(n>0) CALL Info('InterpolateMeshToMesh',&
	'Number of unfound nodes in all partitions: '//I2S(n),Level=6)
      
      IF(PRESENT(UnfoundNodes)) UnfoundNodes = .NOT. FoundNodes
      DEALLOCATE( FoundNodes ) 
      
      
CONTAINS

  ! Collect here all the historical ways how a variable might be not good for interpolation.
  !-----------------------------------------------------------------------------------------
  FUNCTION LegitInterpVar(Var) RESULT ( IsLegit )
    TYPE(Variable_t), POINTER :: Var
    LOGICAL :: IsLegit

    IF(.NOT.ASSOCIATED(Var)) THEN
      IsLegit = .FALSE.
      RETURN
    END IF

    ! Only nodal and discontinuous galerkin fields can be interpolated as for now. 
    IsLegit = ( Var % TYPE == Variable_on_nodes_on_elements .OR. Var % Type == Variable_on_nodes ) 
    ! Even for vectors the interpolation is done for each scalar component. 
    IF( Var % Dofs > 1 ) IsLegit = .FALSE.
    !IF( Var % Secondary ) IsLegit = .FALSE.
    ! Coordinates are special and should not be interpolated. 
    IF(LEN(Var % Name) >= 10) THEN
      IF( Var % Name(1:10) == 'coordinate' ) IsLegit = .FALSE.
    END IF
    ! This is global variable for which the type has not been properly set.
    IF(.NOT. ASSOCIATED(Var % Perm) ) THEN
      IF(ASSOCIATED(Var % Values) ) THEN
       IF (SIZE(Var % Values)==1 ) IsLegit = .FALSE.
     END IF
    END IF
    
  END FUNCTION LegitInterpVar

  
!------------------------------------------------------------------------------
   FUNCTION AllocateMesh() RESULT(Mesh)
!------------------------------------------------------------------------------
     TYPE(Mesh_t), POINTER :: Mesh
!------------------------------------------------------------------------------
     INTEGER :: istat

     ALLOCATE( Mesh, STAT=istat )
     IF ( istat /= 0 ) &
        CALL Fatal( 'AllocateMesh', 'Unable to allocate a few bytes of memory?' )

!    Nothing computed on this mesh yet!
!    ----------------------------------
     Mesh % SavesDone    = 0
     Mesh % OutputActive = .FALSE.

     Mesh % AdaptiveDepth = 0
     Mesh % Changed   = .FALSE. !  TODO: Change this sometime

     Mesh % Stabilize = .FALSE.

     Mesh % Variables => NULL()
     Mesh % Parent => NULL()
     Mesh % Child => NULL()
     Mesh % Next => NULL()
     Mesh % RootQuadrant => NULL()
     Mesh % Elements => NULL()
     Mesh % Edges => NULL()
     Mesh % Faces => NULL()
     Mesh % Projector => NULL()
     Mesh % NumberOfEdges = 0
     Mesh % NumberOfFaces = 0
     Mesh % NumberOfNodes = 0
     Mesh % NumberOfBulkElements = 0
     Mesh % NumberOfBoundaryElements = 0

     Mesh % MaxFaceDOFs = 0
     Mesh % MaxEdgeDOFs = 0
     Mesh % MaxBDOFs = 0
     Mesh % MaxElementDOFs  = 0
     Mesh % MaxElementNodes = 0

     Mesh % ViewFactors => NULL()

     ALLOCATE( Mesh % Nodes, STAT=istat )
     IF ( istat /= 0 ) &
        CALL Fatal( 'AllocateMesh', 'Unable to allocate a few bytes of memory?' )
     NULLIFY( Mesh % Nodes % x )
     NULLIFY( Mesh % Nodes % y )
     NULLIFY( Mesh % Nodes % z )
     Mesh % Nodes % NumberOfNodes = 0

     Mesh % ParallelInfo % NumberOfIfDOFs =  0
     NULLIFY( Mesh % ParallelInfo % GlobalDOFs )
     NULLIFY( Mesh % ParallelInfo % GInterface )
     NULLIFY( Mesh % ParallelInfo % NeighbourList )
         
  END FUNCTION AllocateMesh
!-------------------------------------------------------------------------------
END SUBROUTINE InterpolateMeshToMesh
!-------------------------------------------------------------------------------


!------------------------------------------------------------------------------
!>    Interpolates values of all variables from a mesh associated with
!>    the old model to the mesh of the new model.
!------------------------------------------------------------------------------
     SUBROUTINE InterpolateMeshToMeshQ( OldMesh, NewMesh, OldVariables, NewVariables, &
         UseQuadrantTree, Projector, MaskName, FoundNodes, NewMaskPerm, KeepUnfoundNodes )
!------------------------------------------------------------------------------
       USE DefUtils
!-------------------------------------------------------------------------------
       TYPE(Mesh_t), TARGET  :: OldMesh   !< Old mesh structure
       TYPE(Mesh_t), TARGET  :: NewMesh   !< New mesh structure
       TYPE(Variable_t), POINTER, OPTIONAL :: OldVariables  !< Old model variable structure
       TYPE(Variable_t), POINTER, OPTIONAL :: NewVariables  !< New model variable structure. NewVariables defines the variables to be interpolated
       LOGICAL, OPTIONAL :: UseQuadrantTree  !< If true use the RootQuadrant of the old mesh in interpolation.
       TYPE(Projector_t), POINTER, OPTIONAL :: Projector  !< Use projector between meshes for interpolation, if available
       CHARACTER(LEN=*),OPTIONAL :: MaskName  !< Mask the old variable set by the given MaskName when trying to define the interpolation.
       LOGICAL, OPTIONAL :: FoundNodes(:)     !< List of nodes where the interpolation was a success
       INTEGER, OPTIONAL, POINTER :: NewMaskPerm(:)  !< Mask the new variable set by the given MaskName when trying to define the interpolation.
       LOGICAL, OPTIONAL :: KeepUnfoundNodes  !< Do not disregard unfound nodes from projector
!------------------------------------------------------------------------------
       INTEGER :: dim
       TYPE(Nodes_t) :: ElementNodes
       INTEGER :: nBulk, i, j, k, l, n, np, bf_id, QTreeFails, TotFails, FoundCnt
       REAL(KIND=dp), DIMENSION(3) :: Point
       INTEGER, POINTER :: Indexes(:)
       REAL(KIND=dp), DIMENSION(3) :: LocalCoordinates
       TYPE(Variable_t), POINTER :: OldSol, NewSol, Var
       REAL(KIND=dp), POINTER :: OldValue(:), NewValue(:), ElementValues(:)
       TYPE(Quadrant_t), POINTER :: LeafQuadrant
       TYPE(Element_t),POINTER :: Element, Parent
       
       REAL(KIND=dp), ALLOCATABLE :: Basis(:),Vals(:),dVals(:,:), &
                          RotWBasis(:,:), WBasis(:,:)
       REAL(KIND=dp) :: BoundingBox(6), detJ, u,v,w,s,val,rowsum, F(3,3), G(3,3)
       
       LOGICAL :: UseQTree, TryQTree, Stat, UseProjector, EdgeBasis, PiolaT, Parallel, &
           TryLinear, KeepUnfoundNodesL, InterpolatePartial
       TYPE(Quadrant_t), POINTER :: RootQuadrant
       
       INTEGER, POINTER   CONTIG :: Rows(:), Cols(:)
       INTEGER, POINTER    :: Diag(:), OldPerm(:), NewPerm(:)

       TYPE Epntr_t
         TYPE(Element_t), POINTER :: Element
       END TYPE Epntr_t
       
       TYPE(Epntr_t), ALLOCATABLE :: ElemPtrs(:)
       
       INTEGER, ALLOCATABLE, TARGET :: RInd(:), Unitperm(:)
       LOGICAL :: Found, EpsAbsGiven,EpsRelGiven, MaskExists, CylProject, ProjectorAllocated
       INTEGER :: eps_tries, nrow, PassiveCoordinate
       REAL(KIND=dp) :: eps1 = 0.1, eps2, eps_global, eps_local, eps_basis,eps_numeric
       REAL(KIND=dp), POINTER CONTIG :: Values(:) 
       REAL(KIND=dp), POINTER :: LocalU(:), LocalV(:), LocalW(:)

       TYPE(Nodes_t), SAVE :: Nodes
       INTEGER, ALLOCATABLE :: OneDGIndex(:)
              
       !$OMP THREADPRIVATE(eps1,Nodes)

!------------------------------------------------------------------------------

       Parallel = (ParEnv % PEs > 1)

       FoundCnt = 0
       IF ( OldMesh % NumberOfNodes == 0 ) RETURN
!
!      If projector argument given, search for existing
!      projector matrix, or generate new projector, if
!      not already there:
!      ------------------------------------------------
       IF ( PRESENT(Projector) ) THEN
         Projector => NewMesh % Projector
         
         DO WHILE( ASSOCIATED( Projector ) )
           IF ( ASSOCIATED(Projector % Mesh, OldMesh) ) THEN
              CALL Info('InterpolateMesh2Mesh','Applying exiting projector in interpolation',Level=12)
              IF ( PRESENT(OldVariables) ) CALL ApplyProjector()
             RETURN
           END IF
           Projector => Projector % Next
         END DO

         n = NewMesh % NumberOfNodes
         ALLOCATE( LocalU(n), LocalV(n), LocalW(n), ElemPtrs(n) )
         DO i=1,n
           NULLIFY( ElemPtrs(i) % Element )
         END DO
       END IF
!
!      Check if using the spatial division hierarchy for the search:
!      -------------------------------------------------------------

       RootQuadrant => OldMesh % RootQuadrant
       dim = CoordinateSystemDimension()

       dim = MAX(dim,OldMesh % MeshDim)
       
       IF ( .NOT. PRESENT( UseQuadrantTree ) ) THEN
         UseQTree = .TRUE.
       ELSE
         UseQTree = UseQuadrantTree
       ENDIF
    
       IF ( UseQTree ) THEN
         IF ( .NOT.ASSOCIATED( RootQuadrant ) ) THEN
           BoundingBox(1) = MINVAL(OldMesh % Nodes % x)
           BoundingBox(2) = MINVAL(OldMesh % Nodes % y)
           BoundingBox(3) = MINVAL(OldMesh % Nodes % z)
           BoundingBox(4) = MAXVAL(OldMesh % Nodes % x)
           BoundingBox(5) = MAXVAL(OldMesh % Nodes % y)
           BoundingBox(6) = MAXVAL(OldMesh % Nodes % z)
           
           eps2 = 0.1_dp * MAXVAL(BoundingBox(4:6)-BoundingBox(1:3))
           BoundingBox(1:3) = BoundingBox(1:3) - eps2
           BoundingBox(4:6) = BoundingBox(4:6) + eps2

           CALL Info('InterpolateMeshToMeshQ','Creating quadrant tree for faster interpolation!',Level=10)
           CALL BuildQuadrantTree( OldMesh,BoundingBox,OldMesh % RootQuadrant)
           RootQuadrant => OldMesh % RootQuadrant
         END IF
       END IF
       
! Use mask or not
!---------------------------------------
       MaskExists = PRESENT( MaskName )

!------------------------------------------------------------------------------

       n = OldMesh % MaxElementNodes
       ALLOCATE( ElementNodes % x(n), ElementNodes % y(n), &
           ElementNodes % z(n), ElementValues(n) )
       
       eps_global = ListGetConstReal( CurrentModel % Simulation,  &
           'Interpolation Global Epsilon', Stat)
       IF(.NOT. Stat) eps_global = 2.0d-10
       
       eps_local = ListGetConstReal( CurrentModel % Simulation,  &
           'Interpolation Local Epsilon', Stat )
       IF(.NOT. Stat) eps_local = 1.0d-10

       eps_tries = ListGetInteger( CurrentModel % Simulation,  &
           'Interpolation Max Iterations', Stat )
       IF(.NOT. Stat) eps_tries = 12

       eps_numeric = ListGetConstReal( CurrentModel % Simulation, &
           'Interpolation Numeric Epsilon', Stat)
       IF(.NOT. Stat) eps_numeric = 1.0e-10

       PassiveCoordinate = ListGetInteger( CurrentModel % Simulation, &
            'Interpolation Passive Coordinate', Stat ) 
       IF (.NOT. Stat .AND. ASSOCIATED(CurrentModel % Solver)) THEN
         PassiveCoordinate = ListGetInteger( CurrentModel % Solver % Values, &
               'Interpolation Passive Coordinate', Stat ) 
       END IF
              
       CylProject = ListGetLogical( CurrentModel % Simulation, &
            'Interpolation Cylindric', Stat )                     
       IF (.NOT. Stat .AND. ASSOCIATED(CurrentModel % Solver)) THEN
         CylProject = ListGetLogical( CurrentModel % Solver % Values, &
               'Interpolation Cylindric', Stat ) 
       END IF

       InterpolatePartial = ListGetLogical( CurrentModel % Simulation, &
            'Interpolation Partial Hit', Stat )                     

       
       QTreeFails = 0
       TotFails = 0

       EdgeBasis = .FALSE.
       IF (ASSOCIATED(CurrentModel % Solver)) &
           EdgeBasis = ListGetLogical(CurrentModel % Solver % Values,'Edge Basis',Found)

       PiolaT = .FALSE.
       IF (EdgeBasis) THEN
         IF (.NOT. ASSOCIATED(CurrentModel % Solver % Mesh)) CALL Fatal('InterpolateMeshToMeshQ', &
             'Edge basis functions need an associated mesh')
         PiolaT = ListGetLogical(CurrentModel % Solver % Values,'Use Piola Transform',Found)
       END IF

       TryLinear = ListGetLogical( CurrentModel % Simulation, 'Try Linear Search If Qtree Fails', Found)
       IF(.NOT.Found) TryLinear = .TRUE.

       IF ( PRESENT(KeepUnfoundNodes) ) THEN
         KeepUnfoundNodesL = KeepUnfoundNodes
       ELSE
         KeepUnfoundNodesL = .TRUE.
       END IF        
       
       FoundCnt = 0

       i = MAX(NewMesh % NumberOfNodes,OldMesh % NumberOfNodes)
       ALLOCATE(Unitperm(i))
       Unitperm = [(j,j=1,i)]
!------------------------------------------------------------------------------
! Loop over all nodes in the new mesh
!------------------------------------------------------------------------------
       DO i=1,NewMesh % NumberOfNodes
!------------------------------------------------------------------------------

         ! Only get the variable for the requested nodes
         IF( PRESENT( NewMaskPerm ) ) THEN
           IF( NewMaskPerm(i) == 0 ) CYCLE
         END IF

         Point(1) = NewMesh % Nodes % x(i)
         Point(2) = NewMesh % Nodes % y(i)
         Point(3) = NewMesh % Nodes % z(i)

         IF( PassiveCoordinate /= 0 ) THEN
           Point(PassiveCoordinate) = 0.0_dp
         END IF

         IF( CylProject ) THEN
           Point(1) = SQRT( Point(1)**2 + Point(2)**2 )
           Point(2) = Point(3)
           Point(3) = 0.0_dp
         END IF
         
!------------------------------------------------------------------------------
! Find in which old mesh bulk element the point belongs to
!------------------------------------------------------------------------------
         Found = .FALSE.
         TryQTree = ASSOCIATED(RootQuadrant) .AND. UseQTree 

         IF( TryQTree ) THEN
!------------------------------------------------------------------------------
! Find the last existing quadrant that the point belongs to
!------------------------------------------------------------------------------
           Element => NULL()
           CALL FindLeafElements(Point, dim, RootQuadrant, LeafQuadrant)
           
           IF ( ASSOCIATED(LeafQuadrant) ) THEN
             ! Go through the bulk elements in the last ChildQuadrant
             ! only.  Try to find matching element with progressively
             ! sloppier tests. Allow at most 100 % of slack:
             ! -------------------------------------------------------
             Eps1 = eps_global
             Eps2 = eps_local
             
             DO j=1,eps_tries
               DO k=1, LeafQuadrant % NElemsInQuadrant
                 Element => OldMesh % Elements(LeafQuadrant % Elements(k))
                 
                 IF( MaskExists ) THEN
                   bf_id = ListGetInteger( CurrentModel % Bodies(Element % BodyId) % Values, &
                       'Body Force', Found )
                   IF( .NOT. Found ) CYCLE
                   IF(.NOT. ListCheckPresent( &
                       CurrentModel % BodyForces(bf_id) % Values,MaskName) ) CYCLE
                 END IF
                  
                 Indexes => Element % NodeIndexes
                 n = Element % TYPE % NumberOfNodes
                 
                 ElementNodes % x(1:n) = OldMesh % Nodes % x(Indexes)
                 ElementNodes % y(1:n) = OldMesh % Nodes % y(Indexes)
                 ElementNodes % z(1:n) = OldMesh % Nodes % z(Indexes)

                 IF( PassiveCoordinate > 0 ) THEN
                   IF( PassiveCoordinate == 1 ) THEN
                     ElementNodes % x(1:n) = 0.0_dp
                   ELSE IF( PassiveCoordinate == 2 ) THEN
                     ElementNodes % y(1:n) = 0.0_dp
                   ELSE IF( PassiveCoordinate == 3 ) THEN
                     ElementNodes % z(1:n) = 0.0_dp
                   END IF
                 END IF
                 
                 Found = PointInElement( Element, ElementNodes, &
                     Point, LocalCoordinates, Eps1, Eps2, NumericEps=eps_numeric,EdgeBasis=PiolaT)
                 IF ( Found ) EXIT
               END DO
               IF ( Found ) EXIT  
               
               Eps1 = 10 * Eps1
               Eps2 = 10 * Eps2               
               IF( Eps1 > 1.0_dp ) EXIT
             END DO
           END IF
         END IF
         
         IF( .NOT. TryQTree .OR. (.NOT. Found .AND. .NOT. Parallel .AND. TryLinear ) ) THEN
           !------------------------------------------------------------------------------
           ! Go through all old mesh bulk elements
           !------------------------------------------------------------------------------
           DO k=1,OldMesh % NumberOfBulkElements
             Element => OldMesh % Elements(k)
             
             n = Element % TYPE % NumberOfNodes
             Indexes => Element % NodeIndexes
             
             ElementNodes % x(1:n) = OldMesh % Nodes % x(Indexes)
             ElementNodes % y(1:n) = OldMesh % Nodes % y(Indexes)
             ElementNodes % z(1:n) = OldMesh % Nodes % z(Indexes)

             IF( PassiveCoordinate > 0 ) THEN
               IF( PassiveCoordinate == 1 ) THEN
                 ElementNodes % x(1:n) = 0.0_dp
               ELSE IF( PassiveCoordinate == 2 ) THEN
                 ElementNodes % y(1:n) = 0.0_dp
               ELSE IF( PassiveCoordinate == 3 ) THEN
                 ElementNodes % z(1:n) = 0.0_dp
               END IF
             END IF
             
             Found =  PointInElement( Element, ElementNodes, &
                 Point, LocalCoordinates  ) 
             IF( Found ) THEN
               IF( TryQTree ) QTreeFails = QtreeFails + 1
               EXIT
             END IF
           END DO
         END IF
         
         IF (.NOT.Found) THEN
           Element => NULL()
           IF (.NOT. Parallel ) THEN
             WRITE( Message,'(A,I0,A,3ES10.2,A)' ) 'Point ',i,' at ',Point,' not found!'
             CALL Info( 'InterpolateMeshToMesh', Message, Level=30 )             
             TotFails = TotFails + 1
           END IF
           CYCLE
         END IF
         IF ( PRESENT(FoundNodes) ) FoundNodes(i) = .TRUE.

!------------------------------------------------------------------------------
!
!         Found Element in OldModel:
!         ---------------------------------

         IF ( PRESENT(Projector) ) THEN
             FoundCnt = FoundCnt + 1
             IF ( KeepUnfoundNodesL ) THEN
               ElemPtrs(i) % Element => Element
               LocalU(i) = LocalCoordinates(1)
               LocalV(i) = LocalCoordinates(2)
               LocalW(i) = LocalCoordinates(3)
             ELSE
               ElemPtrs(FoundCnt) % Element => Element
               LocalU(FoundCnt) = LocalCoordinates(1)
               LocalV(FoundCnt) = LocalCoordinates(2)
               LocalW(FoundCnt) = LocalCoordinates(3)
             END IF
          END IF

          IF ( .NOT.PRESENT(OldVariables) .OR. PRESENT(Projector) ) CYCLE
!------------------------------------------------------------------------------
!
!         Go through all variables to be interpolated:
!         --------------------------------------------


          Var => OldVariables
          DO WHILE( ASSOCIATED( Var ) )

            IF(LegitInterpVar(Var)) THEN                         
               
!------------------------------------------------------------------------------
!
!               Interpolate variable at Point in Element:
!               ------------------------------------------------

                NewSol => VariableGet( NewMesh % Variables, Var % Name, .TRUE. )
                IF ( .NOT. ASSOCIATED( NewSol ) ) THEN
                   Var => Var % Next
                   CYCLE
                END IF

                IF(PRESENT(OldVariables)) THEN
                  OldSol => VariableGet( OldVariables, Var % Name, .TRUE. )
                ELSE
                  OldSol => VariableGet( OldMesh % Variables, Var % Name, .TRUE. )
                END IF
                IF( .NOT. ASSOCIATED( OldSol ) ) THEN
                  CALL Fatal('InterpolateMeshToMesh','Variable not associated: '//TRIM(Var % Name))
                END IF
                  
                
                ! Check that the node was found in the old mesh:
                ! ----------------------------------------------
                IF ( ASSOCIATED (Element) ) THEN
                  !------------------------------------------------------------------------------
!
!                  Check for rounding errors:
!                  --------------------------
                  IF( OldSol % TYPE == Variable_on_nodes_on_elements ) THEN
                    Indexes => Element % DGIndexes
                  ELSE
                    Indexes => Element % NodeIndexes
                  END IF

                   OldPerm => OldSol % Perm
                   IF (.NOT.ASSOCIATED(OldPerm)) OldPerm => Unitperm

                   NewPerm => NewSol % Perm
                   IF (.NOT.ASSOCIATED(NewPerm)) NewPerm => Unitperm

                   k = COUNT( OldPerm(Indexes) > 0 )
                   
                   IF ( k == SIZE(Indexes) .OR. (InterpolatePartial .AND. k>0) ) THEN
                    IF( NewSol % TYPE == Variable_on_nodes_on_elements ) THEN
                      IF(.NOT. ALLOCATED(OneDGIndex) ) THEN                        
                        CALL CreateOneDGIndex()
                      END IF
                      IF( OneDGIndex(i) > 0 ) THEN
                        k = NewPerm( OneDGIndex(i) )
                      ELSE
                        k = 0
                      END IF
                    ELSE
                      k = NewPerm(i)
                    END IF
                      
                    IF ( k /= 0 ) THEN
                      WHERE( OldPerm(Indexes(1:n)) > 0 ) 
                        ElementValues(1:n) = OldSol % Values(OldPerm(Indexes))
                      ELSE WHERE
                        ElementValues(1:n) = 0.0_dp
                      END WHERE
                        
                      val = InterpolateInElement( Element, ElementValues, &
                          LocalCoordinates(1), LocalCoordinates(2), LocalCoordinates(3) )
                      
                      NewSol % Values(k) = val

                      IF ( ASSOCIATED( OldSol % PrevValues ) ) THEN
                        DO j=1,SIZE(OldSol % PrevValues,2)

                          WHERE( OldPerm(Indexes(1:n)) > 0 )                           
                            ElementValues(1:n) = OldSol % PrevValues(OldPerm(Indexes),j)
                          END WHERE
                            
                          val = InterpolateInElement( Element, ElementValues, &
                              LocalCoordinates(1), LocalCoordinates(2), LocalCoordinates(3) )

                          NewSol % PrevValues(k,j) = val
                        END DO
                      END IF
                    END IF
                  END IF
                ELSE
                  IF ( NewPerm(i)/=0 ) NewValue(NewPerm(i))=0.0_dp
                END IF

!------------------------------------------------------------------------------
              END IF
             Var => Var % Next
           END DO
!------------------------------------------------------------------------------
         END DO

         IF( .NOT. Parallel ) THEN
           IF( QtreeFails > 0 ) THEN
             WRITE( Message,'(A,I0)' ) 'Number of points not found in quadtree: ',QtreeFails
             CALL Info( 'InterpolateMeshToMesh', Message )
             IF( TotFails == 0 ) THEN
               CALL Info( 'InterpolateMeshToMesh','All nodes still found by N^2 dummy search!' )               
             END IF
           END IF
           IF( TotFails == 0 ) THEN
             CALL Info('InterpolateMeshToMesh','Found all nodes in the target mesh',Level=6)
           ELSE
             WRITE( Message,'(A,I0,A,I0,A)') 'Points not found: ',TotFails,' (found ',&
                 NewMesh % NumberOfNodes - TotFails,')'
             CALL Warn( 'InterpolateMeshToMesh', Message )
             IF( ListGetLogical( CurrentModel % Simulation,'Interpolation Abort Not Found',Stat) ) THEN
               CALL Fatal('InterpolateMeshToMesh','Can not continue with incomplete interpolation!')
             END IF
           END IF
         END IF

!------------------------------------------------------------------------------
!
!      Construct mesh projector, if requested. Next time around
!      will use the existing projector to interpolate values:
!      ---------------------------------------------------------
       IF ( PRESENT(Projector) ) THEN

          IF ( KeepUnfoundNodesL ) THEN
            n = NewMesh % NumberOfNodes
          ELSE
            n = FoundCnt
          END IF
          ALLOCATE( Basis(100),Vals(100), Indexes(100))

          ! The critical value of basis function that is accepted to the 
          ! projector. Note that the sum of weights is one, so this
          ! we know the scale for this one. 
          eps_basis = ListGetConstReal( CurrentModel % Simulation,  &
                 'Interpolation Basis Epsilon', Stat )
          IF(.NOT. Stat) eps_basis = 0.0d-12


          ALLOCATE( Rows(n+1) )
          Rows(1) = 1
          ProjectorAllocated = .FALSE.

100       nrow = 1

          DO i=1,n

            IF(EdgeBasis.AND.ASSOCIATED(OldMesh % Parent)) THEN
              Element => OldMesh % Parent % Faces(ElemPtrs(i) % Element % ElementIndex)
              IF(ASSOCIATED(Element % BoundaryInfo)) THEN
                Parent => Element % BoundaryInfo% Left
                IF (ASSOCIATED(Parent)) THEN
                  k  = Element % TYPE % NumberOfNodes
                  np = Parent  % TYPE % NumberOfNodes
                END IF
              END IF
            ELSE
              Element => ElemPtrs(i) % Element
            END IF
            Found = ASSOCIATED( Element ) 
            
            IF( .NOT. Found ) THEN
             ! It seems unnecessary to make a matrix entry in case no target element is found!
              IF(.FALSE.) THEN
                IF( ProjectorAllocated ) THEN
                  Cols(nrow) = 1
                  Values(nrow) = 0._dp
                END IF
                nrow = nrow + 1
              END IF
            ELSE

              u = LocalU(i)
              v = LocalV(i)
              w = LocalW(i)

              IF (EdgeBasis) THEN
                CALL GetElementNodes(Nodes,Element)
              ELSE
                CALL GetElementNodes(Nodes,Element,UMesh=OldMesh)
              END IF

              np = GetElementNOFNodes(Element)              
              IF (EdgeBasis) THEN
                k = GetElementDOFs( Indexes, Element, NotDG=.TRUE.)                
              ELSE
                !
                ! In this case calling GetElementDOFs appears to generate warnings
                ! (since CurrentModel % Solver % Mesh may not be associated for some reason),
                ! now we proceed silently assuming the Lagrange interpolation
                !
                k = np
                Indexes(1:k) = Element % NodeIndexes(1:k)
              END IF
              
              IF (ANY(Indexes(1:np)>Element % NodeIndexes)) np=0

              IF( EdgeBasis) THEN
                IF(.NOT.ALLOCATED(dVals)) THEN
                  ALLOCATE(dVals(k,3),WBasis(k,3),RotWBasis(k,3))
                ELSE IF(SIZE(dVals,1)<k) THEN
                  DEALLOCATE(dVals,WBasis,RotWBasis)
                  ALLOCATE(dVals(k,3),WBasis(k,3),RotWBasis(k,3))
                END IF

                IF(PiolaT) THEN 
                  stat = ElementInfo(Element,Nodes,u,v,w,detJ,Vals,EdgeBasis=WBasis )
                ELSE
                  stat = ElementInfo(Element,Nodes,u,v,w,detJ,Vals,dVals)
                  CALL GetEdgeBasis(Element,WBasis,RotWBasis,Vals,dVals)
                END IF
              ELSE
                stat = ElementInfo(Element,Nodes,u,v,w,detJ,Vals)
              END IF
              

              rowsum = 0.0_dp
              DO j=1,k
!               IF( ABS( vals(j) ) < eps_basis ) CYCLE
                IF(j<=np) rowsum = rowsum + vals(j)
                IF (.NOT. ProjectorAllocated) THEN
                  IF (.NOT.EdgeBasis.OR.(EdgeBasis.AND.j<=np)) THEN
                    nrow = nrow+1
                  ELSE
                    nrow = nrow+3
                  END IF
                END IF
              END DO
              

              IF( ProjectorAllocated ) THEN
                DO j=1,k
!                 IF( ABS( vals(j) ) < eps_basis ) CYCLE
                  IF(.NOT.EdgeBasis) Rind(Indexes(j)) = Rind(Indexes(j)) + 1

                  ! Always normalize the weights to one!
                  IF (.NOT.EdgeBasis.OR.(EdgeBasis.AND.j<=np)) THEN
                    Cols(nrow) = Indexes(j)
                    Values(nrow) = vals(j) / rowsum
                    nrow = nrow + 1                  
                  ELSE
                    Cols(nrow) = -Indexes(j)
                    Values(nrow) = WBasis(j-np,1)
                    nrow = nrow + 1                  
                    Cols(nrow) = -Indexes(j)
                    Values(nrow) = WBasis(j-np,2)
                    nrow = nrow + 1                  
                    Cols(nrow) = -Indexes(j)
                    Values(nrow) = WBasis(j-np,3)
                    nrow = nrow + 1                  
                  END IF
                END DO
              END IF                
            END IF

            Rows(i+1) = nrow
         END DO

         IF( .NOT. ProjectorAllocated ) THEN
            ALLOCATE( Cols(Rows(n+1)-1), Values(Rows(n+1)-1) )
            Cols   = 0
            Values = 0
            
            ALLOCATE( Projector )
            Projector % Matrix => AllocateMatrix()
            Projector % Matrix % NumberOfRows = n
            Projector % Matrix % Rows   => Rows
            Projector % Matrix % Cols   => Cols 
            Projector % Matrix % Values => Values
            
            Projector % Next => NewMesh % Projector
            NewMesh % Projector => Projector
            NewMesh % Projector % Mesh => OldMesh
            
            IF( .NOT.EdgeBasis) THEN
              ALLOCATE(Rind(OldMesh % NumberOfNodes)); Rind = 0
            END IF

            ProjectorAllocated = .TRUE.

            GOTO 100
          END IF

          DEALLOCATE( Basis, Vals, ElemPtrs, LocalU, LocalV, LocalW, Indexes )

!         Store also the transpose of the projector:
!         ------------------------------------------ 
          Projector % TMatrix => NULL()
          IF(.NOT.EdgeBasis) THEN
            IF ( Found ) THEN
              n = OldMesh % NumberOfNodes
              ! Needed for some matrices
              n = MAX( n, MAXVAL( Projector % Matrix % Cols ) )

              ALLOCATE( Rows(n+1) )
              Rows(1) = 1
              DO i=2,n+1
                 Rows(i) = Rows(i-1)+Rind(i-1)
              END DO

              ALLOCATE( Cols(Rows(n+1)-1), Values(Rows(n+1)-1) )
              Projector % TMatrix => AllocateMatrix()
              Projector % TMatrix % NumberOfRows = n
              Projector % TMatrix % Rows   => Rows
              Projector % TMatrix % Cols   => Cols 
              Projector % TMatrix % Values => Values

              RInd = 0
              DO i=1,Projector % Matrix % NumberOfRows
                DO j=Projector % Matrix % Rows(i), Projector % Matrix % Rows(i+1)-1
                   k = Projector % Matrix % Cols(j)
                   l = Rows(k) + Rind(k)
                   Rind(k) = Rind(k) + 1
                   Cols(l) = i
                   Values(l) = Projector % Matrix % Values(j)
                END DO
              END DO
            END IF

            DEALLOCATE(Rind)
          END IF

          IF ( PRESENT(OldVariables) ) CALL ApplyProjector
       END IF

       DEALLOCATE( ElementNodes % x, ElementNodes % y, &
                   ElementNodes % z, ElementValues )

CONTAINS

  FUNCTION LegitInterpVar(Var) RESULT ( IsLegit )
    TYPE(Variable_t), POINTER :: Var
    LOGICAL :: IsLegit
        
    IsLegit = ( Var % TYPE == Variable_on_nodes_on_elements .OR. Var % Type == Variable_on_nodes ) 
    IF( Var % Dofs > 1 ) IsLegit = .FALSE.
    !IF( Var % Secondary ) IsLegit = .FALSE.
    IF(LEN(Var % Name) >= 10) THEN
      IF( Var % Name(1:10) == 'coordinate' ) IsLegit = .FALSE.
    END IF
    IF(.NOT. ASSOCIATED(Var % Perm) .AND. SIZE(Var % Values) == 1 ) IsLegit = .FALSE.
    
  END FUNCTION LegitInterpVar

  

  ! Create a representative dg index to be used for interpolation.
  ! This is cheating since it does not work in general. It does work
  ! for the reduced basis DG. Even there it works only at intersections
  ! if there is an additional mask that is used to pick the correct element.
  ! For generic cases we would need a table to all DG indexes. 
  !------------------------------------------------------------------------
  SUBROUTINE CreateOneDGIndex()
    INTEGER :: i,j,k,t,n
    TYPE(Element_t), POINTER :: Element,Parent
    INTEGER, TARGET :: TmpIndexes(20)
    INTEGER, POINTER :: pIndexes(:)
    

    CALL Info('InterpolateMesh2Mesh','Creating representative DG reindexing table!',Level=12)
    
    ALLOCATE(OneDGIndex(NewMesh % NumberOfNodes))
    OneDGIndex = 0

    DO t=1,NewMesh % NumberOfBulkElements + NewMesh % NumberOfBoundaryElements
      Element => NewMesh % Elements(t)

      ! This might take away all bulk elements so we need to be able to deal with
      ! boundary elements as well. 
      IF( PRESENT( NewMaskPerm ) ) THEN
        IF( ANY( NewMaskPerm(Element % NodeIndexes) == 0 ) ) CYCLE
      END IF
      
      n = Element % Type % NumberOfNodes
      IF( ASSOCIATED( Element % DGIndexes ) ) THEN
        pIndexes => Element % DGIndexes
      ELSE IF( ASSOCIATED( Element % BoundaryInfo) ) THEN
        pIndexes => TmpIndexes
        pIndexes(1:n) = 0
        DO k=1,2
          IF(k==1) THEN
            Parent => Element % BoundaryInfo % Left
          ELSE
            Parent => Element % BoundaryInfo % Right
          END IF
          IF(.NOT. ASSOCIATED(Parent)) CYCLE
          DO i=1,n
            IF(pIndexes(i) > 0 ) CYCLE
            DO j=1,Parent % TYPE % NumberOfNodes
              IF(Element % NodeIndexes(i) == Parent % NodeIndexes(j) ) THEN
                pIndexes(i) = Parent % DGIndexes(j)
                EXIT
              END IF
            END DO
          END DO
        END DO
      ELSE
        CYCLE
      END IF
      
      DO i=1,n
        j = Element % NodeIndexes(i)
        IF( OneDGIndex(j) > 0) CYCLE
        OneDGIndex(j) = pIndexes(i)
      END DO      
    END DO

    
  END SUBROUTINE CreateOneDGIndex

  

!------------------------------------------------------------------------------
     SUBROUTINE ApplyProjector
!------------------------------------------------------------------------------
        INTEGER :: i
        TYPE(Variable_t), POINTER :: Var
!------------------------------------------------------------------------------
        Var => OldVariables
        DO WHILE( ASSOCIATED(Var) )
          IF(LegitInterpVar(Var)) THEN
              OldSol => VariableGet( OldMesh % Variables, Var % Name, .TRUE. )
              NewSol => VariableGet( NewMesh % Variables, Var % Name, .TRUE. )
              IF ( .NOT. (ASSOCIATED (NewSol) ) ) THEN
                 Var => Var % Next
                 CYCLE
              END IF

              CALL CRS_ApplyProjector( Projector % Matrix, &
                   OldSol % Values, OldSol % Perm,         &
                   NewSol % Values, NewSol % Perm )

              IF ( ASSOCIATED( OldSol % PrevValues ) ) THEN
                 DO i=1,SIZE(OldSol % PrevValues,2)
                    CALL CRS_ApplyProjector( Projector % Matrix,  &
                         OldSol % PrevValues(:,i), OldSol % Perm, &
                         NewSol % PrevValues(:,i), NewSol % Perm )
                 END DO
              END IF
           END IF
           Var => Var % Next
         END DO         
         
!------------------------------------------------------------------------------
     END SUBROUTINE ApplyProjector
!------------------------------------------------------------------------------
  END SUBROUTINE InterpolateMeshToMeshQ
!------------------------------------------------------------------------------



  !---------------------------------------------------------------------------
  !> Create a projector for mapping between interfaces using the Galerkin method
  !> A temporal mesh structure with a node for each Gaussian integration point is 
  !> created. This projector matrix is transferred to a projector on the nodal
  !> coordinates.
  !> Note that this approach is very suboptimal compared to the version where
  !> a temporal supermesh is used for in the integration. 
  !---------------------------------------------------------------------------
   FUNCTION WeightedProjector(BMesh2, BMesh1, InvPerm2, InvPerm1, &
       UseQuadrantTree, Repeating, AntiRepeating, PeriodicScale, & 
       NodalJump ) &
      RESULT ( Projector )
  !---------------------------------------------------------------------------
    USE DefUtils
    USE MeshUtils, ONLY : PreRotationalProjector, PostRotationalProjector
    IMPLICIT NONE

    TYPE(Mesh_t), POINTER :: BMesh1, BMesh2
    REAL(KIND=dp) :: PeriodicScale
    INTEGER, POINTER :: InvPerm1(:), InvPerm2(:)
    LOGICAL :: UseQuadrantTree, Repeating, AntiRepeating
    TYPE(Matrix_t), POINTER :: Projector
    LOGICAL :: NodalJump
    !--------------------------------------------------------------------------
    LOGICAL, ALLOCATABLE :: MirrorNode(:)
    TYPE(Matrix_t), POINTER :: GaussProjector
    TYPE(Nodes_t), POINTER :: GaussNodes, RealNodes, ElementNodes
    TYPE(Element_t), POINTER :: Element
    INTEGER :: i,j,k,l,n,p,q,NoNodes, NoGaussPoints,Indexes(100),&
        nodesize, totsize, eqindsize, RelOrder
    INTEGER, POINTER :: NodeIndexes(:),Rows(:),Cols(:)
    REAL(KIND=dp), POINTER :: Basis(:), Values(:)
    REAL(KIND=dp) :: u,v,w,val,detJ,weight,x
    TYPE(GaussIntegrationPoints_t) :: IntegStuff
    LOGICAL :: Stat, EdgeBasis,Found,AxisSym, PiolaT
    TYPE(Nodes_t), SAVE :: Nodes

    REAL(KIND=dp) :: vq(3),wq(3),f(3,3),g(3,3)
    REAL(KIND=dp), ALLOCATABLE ::WBasis(:,:),RotWbasis(:,:),dBasisdx(:,:)

    INTEGER :: ind,np,qq,pp
    INTEGER, ALLOCATABLE :: Eqind(:), xPerm(:)


    CALL Info('WeightedProjector','Creating Galerkin projector between two boundaries',Level=7)

    ALLOCATE( GaussNodes, ElementNodes )
    RealNodes => Bmesh1 % Nodes
    NoNodes = Bmesh1 % NumberOfNodes

    EdgeBasis = .FALSE.
    IF (ASSOCIATED(CurrentModel % Solver)) THEN
      EdgeBasis = ListGetLogical(CurrentModel % Solver % Values,'Edge Basis',Found)
    END IF

    PiolaT = .FALSE.
    IF( EdgeBasis ) THEN
      PiolaT = ListGetLogical( CurrentModel % Solver % Values, 'Use Piola Transform', Found)
      CALL Info('weightedProjector','Accounting for edge elements in projector.',Level=7)
    END IF

    RelOrder = ListGetInteger( CurrentModel % Solver % Values, &
        'Projector Relative Integration Order', Found, minv=-1,maxv=1)

    ! Calculate the total number of Gaussian integration points
    ! and allocate space for the node structures. 
    !----------------------------------------------------------
    NoGaussPoints = 0
    DO i=1, BMesh1 % NumberOfBulkElements
      Element => BMesh1 % Elements(i)
      IntegStuff = GaussPoints( Element, RelOrder=RelOrder, EdgeBasis=PiolaT )
      NoGaussPoints = NoGaussPoints + IntegStuff % n
    END DO
    
    WRITE( Message,'(A,I0,A,I0)') 'Number of nodes and gauss points:'&
        ,NoNodes,' and ',NoGaussPoints
    CALL Info('WeightedProjector',Message,Level=10)

    ALLOCATE( GaussNodes % x(NoGaussPoints), GaussNodes % y(NoGaussPoints), GaussNodes % z(NoGaussPoints))

   ! Change the local coordinates of the BMesh2 to match to corresponding faces:
    ! ---------------------------------------------------------------------------
    IF(EdgeBasis) THEN
      ALLOCATE(xPerm(Bmesh2 % Parent % NumberofNodes)); xPerm=0
      DO i=1,SIZE(InvPerm2)
        xPerm(InvPerm2(i)) = i
      END DO

      DO i=1, BMesh2 % NumberOfBulkElements
        Element => BMesh2 % Parent % Faces(BMesh2 % Elements(i) % ElementIndex)
        BMesh2 % Elements(i) % NodeIndexes = xPerm(Element % NodeIndexes)
      END DO
    END IF

    AxisSym = .FALSE.
    IF ( CurrentCoordinateSystem() == AxisSymmetric .OR. &
        CurrentCoordinateSystem() == CylindricSymmetric ) THEN
      IF( NodalJump ) THEN
        AxisSym = .TRUE.
      ELSE IF (ASSOCIATED(CurrentModel % Solver)) THEN
        AxisSym = ListGetLogical(CurrentModel % Solver % Values,'Projector Metrics',Found)
      END IF
      IF( AxisSym ) CALL Info('weightedProjector','Projector will be weighted for axi symmetry',Level=7)
    END IF


    nodesize = BMesh1 % Parent % NumberOfNodes
    totsize  = BMesh1 % Parent % NumberOfNodes + BMesh1 % Parent % NumberOfEdges
    IF(ASSOCIATED(CurrentModel % Solver)) THEN
      totsize = CurrentModel % Solver % Matrix % NumberOfRows / &
         CurrentModel % Solver % Variable % Dofs
    END IF

    IF(EdgeBasis) THEN
      n = BMesh1 % Parent % MaxElementDOFs
      ALLOCATE( ElementNodes % x(n), ElementNodes % y(n), ElementNodes % z(n), &
                Basis(n), dBasisdx(n,3), WBasis(n,3), RotWBasis(n,3) )
      eqindsize = totsize
    ELSE
      n = BMesh1 % MaxElementNodes
      ALLOCATE( ElementNodes % x(n), ElementNodes % y(n), ElementNodes % z(n), Basis(n) )
      eqindsize = BMesh1 % NumberOfNodes
    END IF

    eqindsize = 0
    DO i=1, BMesh1 % NumberOfBulkElements
      IF(EdgeBasis) THEN
         Element => BMesh1 % Parent % Faces(BMesh1 % Elements(i) % ElementIndex)
         n  = GetElementDOFs(Indexes,Element)
         np = GetElementNOFNodes(Element)
       ELSE
         Element => BMesh1 % Elements(i)
         n = Element % TYPE % NumberOfNodes
         np = n
         Indexes(1:n) = Element % NodeIndexes
       END IF
       eqindsize = MAX( eqindsize, MAXVAL(Indexes(1:n)) )
    END DO


    ! Create the nodal coordinates for all Gaussian integration points
    !-----------------------------------------------------------------    
    NoGaussPoints = 0
    DO i=1, BMesh1 % NumberOfBulkElements
      Element => BMesh1 % Elements(i)
      n = Element % TYPE % NumberOfNodes        
      NodeIndexes => Element % NodeIndexes
      ElementNodes % x(1:n) = RealNodes % x(NodeIndexes(1:n))
      ElementNodes % y(1:n) = RealNodes % y(NodeIndexes(1:n))
      ElementNodes % z(1:n) = RealNodes % z(NodeIndexes(1:n))
      
      IntegStuff = GaussPoints( Element, RelOrder=RelOrder, EdgeBasis=PiolaT )
      DO j=1,IntegStuff % n
        NoGaussPoints = NoGaussPoints + 1
        u = IntegStuff % u(j)
        v = IntegStuff % v(j)
        w = IntegStuff % w(j)
        IF(PiolaT) THEN
          stat = ElementInfo( Element, ElementNodes, u,v,w, &
                    detJ, Basis, EdgeBasis=WBasis )
        ELSE
           Stat = ElementInfo( Element, ElementNodes, u, v, w, detJ, Basis )
        END IF
        GaussNodes % x(NoGaussPoints) = SUM( Basis(1:n) * ElementNodes % x(1:n) )
        GaussNodes % y(NoGaussPoints) = SUM( Basis(1:n) * ElementNodes % y(1:n) )
        GaussNodes % z(NoGaussPoints) = SUM( Basis(1:n) * ElementNodes % z(1:n) )
      END DO
    END DO

    BMesh1 % Nodes => GaussNodes
    BMesh1 % NumberOfNodes = NoGaussPoints

    ! Create the mirror node flag and map the nodes of Mesh1 to be 
    ! in the interval of Mesh2.    
    !-----------------------------------------------------------------
    IF( Repeating ) THEN
      IF( AntiRepeating ) THEN
        ALLOCATE( MirrorNode( BMesh1 % NumberOfNodes ) )
        MirrorNode = .FALSE.
      END IF
      CALL PreRotationalProjector(BMesh1, BMesh2, MirrorNode )
    END IF

    ! Create the projector for Gaussian integration points
    !-----------------------------------------------------------------
    GaussProjector => MeshProjector( BMesh2, BMesh1, UseQuadrantTree )    
    Rows => GaussProjector % Rows
    Cols => GaussProjector % Cols
    Values => GaussProjector % Values

    ! If there are mirror nodes change the sign
    !-----------------------------------------------------------------------------
    IF( AntiRepeating ) THEN
      CALL PostRotationalProjector( GaussProjector, MirrorNode )
      IF( ALLOCATED( MirrorNode) ) DEALLOCATE( MirrorNode ) 
    END IF

    ! Transfer the projector on the Gaussian points to that on
    ! nodal points utilizing the flexibility of the list matrix.
    !-----------------------------------------------------------
    Projector => AllocateMatrix()
    Projector % FORMAT = MATRIX_LIST
    Projector % ProjectorType = PROJECTOR_TYPE_GALERKIN

    ALLOCATE(Eqind(eqindsize))
    EqInd = 0

    ALLOCATE(Projector % InvPerm(eqindsize))
    Projector % InvPerm = 0

    Ind   = 0

    NoGaussPoints = 0
    DO i=1, BMesh1 % NumberOfBulkElements
      IF(EdgeBasis) THEN
        Element => BMesh1 % Parent % Faces(BMesh1 % Elements(i) % ElementIndex)
        n  = GetElementDOFs(Indexes,Element)
        np = GetElementNOFNodes(Element)
        IF(ANY(Indexes(1:np)>Element % NodeIndexes)) np=0
        CALL GetElementNodes(Nodes,Element)
      ELSE
        Element => BMesh1 % Elements(i)
        n = Element % TYPE % NumberOfNodes        
        np = n
        Indexes(1:n) = Element % NodeIndexes
        ElementNodes % x(1:n) = RealNodes % x(Indexes(1:n))
        ElementNodes % y(1:n) = RealNodes % y(Indexes(1:n))
        ElementNodes % z(1:n) = RealNodes % z(Indexes(1:n))
      END IF

      IntegStuff = GaussPoints(Element, RelOrder=RelOrder, EdgeBasis=PiolaT )
      DO j=1,IntegStuff % n
        NoGaussPoints = NoGaussPoints + 1
        u = IntegStuff % u(j)
        v = IntegStuff % v(j)
        w = IntegStuff % w(j)

        IF(EdgeBasis) THEN
          IF(PiolaT) THEN
            stat = ElementInfo( Element,Nodes,u,v,w,detJ,Basis,EdgeBasis=WBasis)
          ELSE
            Stat = ElementInfo(Element, Nodes, u, v, w, detJ, Basis,dBasisdx)
            CALL GetEdgeBasis(Element,WBasis,RotWBasis,Basis,dBasisdx)
          END IF
        ELSE
          Stat = ElementInfo(Element, ElementNodes, u, v, w, detJ, Basis)
        END IF

        ! Modify weight so that the projector is consistent with the coordinate system.
        weight = detJ * IntegStuff % s(j)
        IF( AxisSym ) THEN
          IF( EdgeBasis ) THEN
            x = SUM( Basis(1:np) * Nodes % x(1:np) )
          ELSE
            x = SUM( Basis(1:np) * ElementNodes % x(1:np) )
          END IF
          weight = weight * x
        END IF


        ! Do the numbering of new dofs
        ! This needs to be done here because the nodal jump 
        ! needs the index related to (p,q) pair.  
        DO p=1,np
          IF (EQind(Indexes(p))==0) THEN
            Ind = Ind+1
            EQind(Indexes(p)) = Ind
            IF( EdgeBasis ) THEN
              Projector % InvPerm(Ind) = Indexes(p)
            ELSE
              Projector % InvPerm(Ind) = InvPerm1(Indexes(p))
            END IF
          END IF
        END DO

        DO p=1,np
          val = weight * Basis(p)
 
          DO q=1,np
            qq = Indexes(q)
            IF(.NOT.EdgeBasis) qq=InvPerm1(qq)
            CALL List_AddToMatrixElement(Projector % ListMatrix, EQind(Indexes(p)), qq, Basis(q) * val ) 

            ! Add a diagonal entry to the future constrained system.
            ! This will enable a jump to the discontinuous boundary.
            ! So far no value is added just the sparse matrix entry.
            !IF( NodalJump ) THEN
            !  IF( Indexes(p) <= nodesize .AND. Indexes(q) <= nodesize ) THEN 
            !    CALL List_AddToMatrixElement(Projector % ListMatrix, EQind(Indexes(p)),&
            !        totsize + EQInd(Indexes(q)), 0.0_dp )
            !  END IF
            !END IF
          END DO

          DO q = Rows(NoGaussPoints), Rows(NoGaussPoints+1)-1
            qq = Cols(q)
            IF (qq<=0) EXIT
            IF(.NOT.EdgeBasis) qq=InvPerm2(qq)
            CALL List_AddToMatrixElement(Projector % ListMatrix, &
                EQind(Indexes(p)), qq, -PeriodicScale * Values(q) * val ) 
          END DO
        END DO

        IF(EdgeBasis)THEN
          DO p=np+1,n
            pp=p-np

            wq = WBasis(pp,:)

            IF (EQind(Indexes(p))==0) THEN
              Ind  = Ind+1
              EQind(Indexes(p)) = Ind
              Projector % InvPerm(Ind) = Indexes(p)
            END IF

            DO q=np+1,n
              qq = q-np

              vq = Wbasis(qq,:)
              CALL List_AddToMatrixElement(Projector % ListMatrix, EQind(Indexes(p)),&
                       Indexes(q), weight * SUM(vq*wq))
            END DO


            DO q = Rows(NoGaussPoints)+np, Rows(NoGaussPoints+1)-1,3
              IF(Cols(q)>=0) STOP 'q'

              vq(1) = Values(q)
              vq(2) = Values(q+1)
              vq(3) = Values(q+2)

              CALL List_AddToMatrixElement( Projector % ListMatrix,&
                  EQind(Indexes(p)), -Cols(q), -PeriodicScale * weight * SUM(vq*wq))
            END DO
          END DO
        ENDIF
      END DO
    END DO


    CALL List_ToCRSMatrix(Projector)

    BMesh1 % Nodes => RealNodes
    BMesh1 % NumberOfNodes = NoNodes

    ! Finally, deallocate permanent storage
    !----------------------------------------------------------------
    DEALLOCATE( ElementNodes % x, ElementNodes % y, ElementNodes % z )
    DEALLOCATE( ElementNodes )

    DEALLOCATE( GaussNodes % x, GaussNodes % y, GaussNodes % z)
    DEALLOCATE( GaussNodes )

    DEALLOCATE( Basis )
    IF(EdgeBasis) DEALLOCATE( dBasisdx, WBasis, RotWBasis ) 

!------------------------------------------------------------------------------
  END FUNCTION WeightedProjector
!------------------------------------------------------------------------------


  !------------------------------------------------------------------------------
  !> When we have a field defined on IP points we may temporarily swap it to be a field
  !> defined on DG points. This is done by solving small linear system for each element.
  !------------------------------------------------------------------------------------
  SUBROUTINE Ip2DgFieldInElement( Mesh, Element, nip, fip, ndg, fdg )
    !------------------------------------------------------------------------------
    USE Types
    USE Integration
    USE ElementDescription
    IMPLICIT NONE
    
    TYPE(Mesh_t), POINTER :: Mesh
    TYPE(Element_t), POINTER :: Element
    INTEGER :: nip, ndg
    REAL(KIND=dp) :: fip(:), fdg(:)
    !------------------------------------------------------------------------------
    REAL(KIND=dp) :: Weight, DetJ
    REAL(KIND=dp), ALLOCATABLE :: Basis(:), MASS(:,:), LOAD(:)
    INTEGER :: i,t,p,q,n
    TYPE(GaussIntegrationPoints_t) :: IP
    TYPE(Nodes_t) :: Nodes
    LOGICAL :: Stat, CSymmetry, AllocationsDone = .FALSE.
    CHARACTER(*), PARAMETER :: Caller = 'Ip2DgFieldInElement'
    TYPE(Element_t), POINTER :: PrevElement => NULL()

    SAVE Nodes, Basis, MASS, LOAD, CSymmetry, PrevElement, AllocationsDone
    !------------------------------------------------------------------------------

    IF( .NOT. AllocationsDone ) THEN
      n = Mesh % MaxElementNodes
      ALLOCATE( Basis(n), LOAD(n), MASS(n,n) )
      CSymmetry = CurrentCoordinateSystem() == AxisSymmetric .OR. &
          CurrentCoordinateSystem() == CylindricSymmetric
      ALLOCATE( Nodes % x(n), Nodes % y(n), Nodes % z(n) )
      AllocationsDone = .TRUE.
    END IF

    IF(nip == 0) THEN
      CALL Warn(Caller,'No IP variables given')
      fdg(1:ndg) = 0.0_dp
      RETURN
    END IF
    
    n = Element % TYPE % NumberOfNodes 
    IF( n /= ndg ) CALL Fatal(Caller,'Mismatch in sizes!')

    ! We could probably do more to utilize the previous visit to save resources...
    IF(.NOT. ASSOCIATED( Element, PrevElement ) ) THEN
      Nodes % x(1:n) = Mesh % Nodes % x(Element % NodeIndexes)
      Nodes % y(1:n) = Mesh % Nodes % y(Element % NodeIndexes)
      Nodes % z(1:n) = Mesh % Nodes % z(Element % NodeIndexes)
      PrevElement => Element
    END IF

    MASS  = 0._dp
    LOAD = 0._dp

    ! Numerical integration:
    !-----------------------
    IP = GaussPoints( Element, nip )

    DO t=1,IP % n
      stat = ElementInfo( Element, Nodes, IP % U(t), IP % V(t), IP % W(t), detJ, Basis )
      Weight = IP % s(t) * DetJ

      IF( CSymmetry ) THEN
        Weight = Weight * SUM( Basis(1:n) * Nodes % x(1:n) )
      END IF

      DO p=1,n
        LOAD(p) = LOAD(p) + Weight * Basis(p) * fip(t)
        DO q=1,n
          MASS(p,q) = MASS(p,q) + Weight * Basis(q) * Basis(p)
        END DO
      END DO
    END DO

    CALL LuSolve(n,MASS,LOAD) 

    fdg(1:n) = LOAD(1:n)
!------------------------------------------------------------------------------
  END SUBROUTINE Ip2DgFieldInElement
!------------------------------------------------------------------------------
  
!------------------------------------------------------------------------------
!> Given a finite element field expressed in terms of the hierarchic p-basis
!> find its elementwise Lagrange interpolant of a specified degree. At the moment
!> this subroutine has limited functionality, since the coordinates of the Lagrange
!> nodes are not yet defined for all element types in the case of high p.
!------------------------------------------------------------------------------
  SUBROUTINE HierarchicPToLagrange(PElement, Degree, PSol, LSol, DOFs, PSolver)
!------------------------------------------------------------------------------
    USE MeshUtils, ONLY: AllocateElement
    USE ElementDescription
    IMPLICIT NONE

    TYPE(Element_t), POINTER, INTENT(IN) :: PElement         !< An element structure capable for p-approximation
    INTEGER, INTENT(IN) :: Degree                            !< The desired order of the Lagrange interpolant
    REAL(KIND=dp), INTENT(IN) :: PSol(:,:)                   !< The DOFs of p-solution
    REAL(KIND=dp), INTENT(INOUT) :: LSol(:,:)                !< The DOFs for the Lagrange interpolation
    INTEGER, OPTIONAL, INTENT(IN) :: DOFs                    !< The first dimension of PSol
    TYPE(Solver_t), POINTER, OPTIONAL, INTENT(IN) :: PSolver !< For seeking p-definitions from this solver
!------------------------------------------------------------------------------
    INTEGER, PARAMETER :: MAX_LINE_DEGREE = 8
    INTEGER, PARAMETER :: MAX_TRIANGLE_DEGREE = 8
    INTEGER, PARAMETER :: MAX_TRIANGLE_NODES = 45
    INTEGER, PARAMETER :: MAX_QUAD_DEGREE = 8
    INTEGER, PARAMETER :: MAX_TETRA_DEGREE = 4
    INTEGER, PARAMETER :: MAX_TETRA_NODES = 35
    INTEGER, PARAMETER :: MAX_PYRAMID_DEGREE = 2
    INTEGER, PARAMETER :: MAX_PYRAMID_NODES = 15
    INTEGER, PARAMETER :: MAX_PRISM_DEGREE = 4
    INTEGER, PARAMETER :: MAX_PRISM_NODES = 75
    INTEGER, PARAMETER :: MAX_BRICK_DEGREE = 8
    INTEGER, PARAMETER :: MAX_LAGRANGE_NODES = 729

    INTEGER, PARAMETER :: MAX_DEGREE = 8  ! The maximal polynomial degree over any element shape 

    TYPE(Mesh_t), POINTER :: Mesh
    TYPE(Nodes_t) :: PNodes
    TYPE(Element_t), POINTER :: LElement => NULL()
    LOGICAL ::  AllocationsDone = .FALSE.
    LOGICAL :: PVersion, stat
    INTEGER, ALLOCATABLE :: BasisDegree(:)
    INTEGER :: Family, Fields, MaxFields, i, j, k, n, t, nd
    INTEGER :: i_start, i_end
    INTEGER :: Offsets(MAX_DEGREE)
    INTEGER :: Edge(2), Face(4)
    REAL(KIND=dp), ALLOCATABLE :: PBasis(:)
    REAL(KIND=dp), POINTER :: NodesU(:), NodesV(:), NodesW(:)
    REAL(KIND=dp) :: ut, vt, wt, detJ
    REAL(KIND=dp) :: r(3), delta

    REAL(KIND=dp), TARGET, SAVE :: VTKLineU(MAX_LINE_DEGREE+1,MAX_LINE_DEGREE+1)

    REAL(KIND=dp), TARGET, SAVE :: VTKTriangleU(MAX_TRIANGLE_DEGREE,MAX_TRIANGLE_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKTriangleV(MAX_TRIANGLE_DEGREE,MAX_TRIANGLE_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKTriangleW(MAX_TRIANGLE_NODES)
    INTEGER, SAVE :: TriangleNodeCounts(MAX_TRIANGLE_DEGREE)

    REAL(KIND=dp), TARGET, SAVE :: VTKQuadU(MAX_QUAD_DEGREE,(MAX_QUAD_DEGREE+1)**2)
    REAL(KIND=dp), TARGET, SAVE :: VTKQuadV(MAX_QUAD_DEGREE,(MAX_QUAD_DEGREE+1)**2)
    REAL(KIND=dp), TARGET, SAVE :: VTKQuadW((MAX_QUAD_DEGREE+1)**2)
    INTEGER, SAVE :: QuadNodeCounts(MAX_QUAD_DEGREE)

    REAL(KIND=dp), TARGET, SAVE :: VTKTetraU(MAX_TETRA_DEGREE,MAX_TETRA_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKTetraV(MAX_TETRA_DEGREE,MAX_TETRA_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKTetraW(MAX_TETRA_DEGREE,MAX_TETRA_NODES)
    INTEGER, SAVE :: TetraNodeCounts(MAX_TETRA_DEGREE)
    INTEGER :: VTKTetraFaceMap(4,3)

    REAL(KIND=dp), TARGET, SAVE :: VTKPyramidU(MAX_PYRAMID_DEGREE,MAX_PYRAMID_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKPyramidV(MAX_PYRAMID_DEGREE,MAX_PYRAMID_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKPyramidW(MAX_PYRAMID_DEGREE,MAX_PYRAMID_NODES)
    INTEGER, SAVE :: PyramidNodeCounts(MAX_PYRAMID_DEGREE)

    REAL(KIND=dp), TARGET, SAVE :: VTKPrismU(MAX_PRISM_DEGREE,MAX_PRISM_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKPrismV(MAX_PRISM_DEGREE,MAX_PRISM_NODES)
    REAL(KIND=dp), TARGET, SAVE :: VTKPrismW(MAX_PRISM_DEGREE,MAX_PRISM_NODES)
    INTEGER, SAVE :: PrismNodeCounts(MAX_PRISM_DEGREE)

    REAL(KIND=dp), TARGET, SAVE :: VTKBrickU(MAX_BRICK_DEGREE,(MAX_BRICK_DEGREE+1)**3)
    REAL(KIND=dp), TARGET, SAVE :: VTKBrickV(MAX_BRICK_DEGREE,(MAX_BRICK_DEGREE+1)**3)
    REAL(KIND=dp), TARGET, SAVE :: VTKBrickW(MAX_BRICK_DEGREE,(MAX_BRICK_DEGREE+1)**3)
    INTEGER, SAVE :: BrickNodeCounts(MAX_BRICK_DEGREE)
    INTEGER :: VTKBrickFaceMap(6,4)

    CHARACTER(*), PARAMETER :: Caller = 'HierarchicPToLagrange'

    SAVE PNodes, LElement, AllocationsDone, PBasis, BasisDegree
!------------------------------------------------------------------------------
    LSol = 0.0d0

    MaxFields = MIN(SIZE(PSol,1), SIZE(LSol,1))
    IF (PRESENT(DOFs)) THEN
      IF (DOFs > MaxFields) THEN
        CALL Warn(Caller, 'Too small arrays to write all fields')
        Fields = MaxFields
      ELSE
        Fields = DOFs
      END IF
    ELSE
      Fields = MaxFields ! Write as many fields as seems to be possible
    END IF

    Family = PElement % TYPE % ElementCode / 100

    IF (Degree == 1) THEN
      DO k=1,Fields
        LSol(k,1:Family) = PSol(k,1:Family)
      END DO
      RETURN
    END IF

    Mesh => CurrentModel % Solver % Mesh
    IF (PRESENT(PSolver)) THEN
      IF(ASSOCIATED(PSolver)) Mesh => PSolver % Mesh
    END IF

    IF (.NOT. AllocationsDone) THEN
      LElement => AllocateElement()
      n = Mesh % MaxElementDOFs
      ALLOCATE(PBasis(n), BasisDegree(n), PNodes % x(n), &
          PNodes % y(n), PNodes % z(n))

      ! --------------------------------------------------------------------------
      ! Create the nodes for line elements (works for any given degree):
      ! --------------------------------------------------------------------------
      VTKLineU(1:MAX_LINE_DEGREE,1) = -1.0d0
      VTKLineU(1:MAX_LINE_DEGREE,2) = 1.0d0
      DO k=2,MAX_LINE_DEGREE
        delta = 2.0d0/k
        ut = -1.0d0
        DO i=1,k-1
          ut = ut + delta 
          VTKLineU(k,i+2) = ut
        END DO
      END DO
      VTKLineU(MAX_LINE_DEGREE+1,:) = 0.0d0  ! One extra row to get zero values for non-active parameters

      ! --------------------------------------------------------------------------
      ! Create the nodes for triangles (basically works for any given degree):
      ! --------------------------------------------------------------------------
      VTKTriangleU(1:MAX_TRIANGLE_DEGREE,1) = -1.0d0
      VTKTriangleU(1:MAX_TRIANGLE_DEGREE,2) = 1.0d0
      VTKTriangleU(1:MAX_TRIANGLE_DEGREE,3) = 0.0d0
      VTKTriangleV(1:MAX_TRIANGLE_DEGREE,1) = 0.0d0
      VTKTriangleV(1:MAX_TRIANGLE_DEGREE,2) = 0.0d0
      VTKTriangleV(1:MAX_TRIANGLE_DEGREE,3) = SQRT(3.0d0)
      VTKTriangleW = 0.0d0

      ! The edges:
      TriangleNodeCounts = 3
      DO k=1,3
        Edge = GetTriangleEdgeMap(k)
        Offsets(1:MAX_TRIANGLE_DEGREE) = TriangleNodeCounts(1:MAX_TRIANGLE_DEGREE)
        CALL NodesOnEdge(VTKTriangleU, VTKTriangleV, Edge(1), Edge(2), MAX_TRIANGLE_DEGREE, &
            Offsets, TriangleNodeCounts)
      END DO

      ! The bubbles:
      VTKTriangleU(3,10) = 0.0d0
      VTKTriangleV(3,10) = SQRT(3.0d0)/3.0d0
      TriangleNodeCounts(3) = TriangleNodeCounts(3) + 1

      DO j=4,MAX_TRIANGLE_DEGREE
        !
        ! We can re-use the nodes of (j-3)th order triangle:
        !
        i_start = 3*j + 1
        SELECT CASE(j)
        CASE(4)
          n = 3
        CASE(5)
          n = 6
        CASE(6)
          n = 10
        CASE(7)
          n = 15
        CASE(8)
          n = 21
        CASE DEFAULT
          CALL Fatal(Caller, 'Triangle interior nodes supported up to degree 8')
        END SELECT
        i_end = 3*j + n

        VTKTriangleU(j,i_start:i_end) = (j-3.0d0)/j * VTKTriangleU(j-3,1:n)
        vt = SQRT(3.0d0)/j
        VTKTriangleV(j,i_start:i_end) = vt + (j-3.0d0)/j * VTKTriangleV(j-3,1:n)
        TriangleNodeCounts(j) = TriangleNodeCounts(j) + n
      END DO

      ! --------------------------------------------------------------------------
      ! Create the nodes for quads (works for any given degree):
      ! --------------------------------------------------------------------------
      VTKQuadU(1:MAX_QUAD_DEGREE,1) = -1.0d0
      VTKQuadU(1:MAX_QUAD_DEGREE,2:3) = 1.0d0
      VTKQuadU(1:MAX_QUAD_DEGREE,4) = -1.0d0
      VTKQuadV(1:MAX_QUAD_DEGREE,1:2) = -1.0d0
      VTKQuadV(1:MAX_QUAD_DEGREE,3:4) = 1.0d0

      ! The edges:
      QuadNodeCounts = 4
      DO k=1,4
        Edge = GetQuadEdgeMap(k)
        Offsets(1:MAX_QUAD_DEGREE) = QuadNodeCounts(1:MAX_QUAD_DEGREE)
        CALL NodesOnEdge(VTKQuadU, VTKQuadV, Edge(1), Edge(2), MAX_QUAD_DEGREE, &
            Offsets, QuadNodeCounts)
      END DO

      ! The bubbles:
      Offsets(1:MAX_QUAD_DEGREE) = QuadNodeCounts(1:MAX_QUAD_DEGREE)
      DO k=2,MAX_QUAD_DEGREE
        delta = 2.0d0/k
        vt = -1.0d0
        DO i=1,k-1
          vt = vt + delta
          ut = -1.0d0
          DO j=1,k-1
            ut = ut + delta 
            VTKQuadU(k,j+Offsets(k)) = ut
            VTKQuadV(k,j+Offsets(k)) = vt         
            QuadNodeCounts(k) = QuadNodeCounts(k) + 1
          END DO
          Offsets(k) = QuadNodeCounts(k)
        END DO
      END DO
      VTKQuadW = 0.0d0

      ! --------------------------------------------------------------------------
      ! Create the nodes for tetrahedra:
      ! --------------------------------------------------------------------------
      LElement % Type => GetElementType(504, .FALSE.)
      CALL GetRefPElementNodes(LElement % Type, VTKTetraU(1,1:4), &
          VTKTetraV(1,1:4), VTKTetraW(1,1:4)) 

      DO k=2,MAX_TETRA_DEGREE
        VTKTetraU(k,1:4) = VTKTetraU(1,1:4)
        VTKTetraV(k,1:4) = VTKTetraV(1,1:4)
        VTKTetraW(k,1:4) = VTKTetraW(1,1:4)
      END DO

      ! The edges:
      TetraNodeCounts = 4
      DO k=1,6
        Edge = GetTetraEdgeMap(k)
        Offsets(1:MAX_TETRA_DEGREE) = TetraNodeCounts(1:MAX_TETRA_DEGREE)
        IF (k == 3) THEN
          CALL NodesOnEdge(VTKTetraU, VTKTetraV, Edge(2), Edge(1), MAX_TETRA_DEGREE, &
              Offsets, TetraNodeCounts, VTKTetraW)
        ELSE
          CALL NodesOnEdge(VTKTetraU, VTKTetraV, Edge(1), Edge(2), MAX_TETRA_DEGREE, &
              Offsets, TetraNodeCounts, VTKTetraW)
        END IF
      END DO
      
      ! The faces:
      Offsets(1:MAX_TETRA_DEGREE) = TetraNodeCounts(1:MAX_TETRA_DEGREE)
      VTKTetraFaceMap(1,:) = (/ 1,2,4 /)
      VTKTetraFaceMap(2,:) = (/ 3,4,2 /)
      VTKTetraFaceMap(3,:) = (/ 1,4,3 /)
      VTKTetraFaceMap(4,:) = (/ 1,3,2 /)

      DO j=3,MAX_TETRA_DEGREE
        SELECT CASE(j)
        CASE(3)
          i_start = 10
          i_end = 10
        CASE(4)
          i_start = 13
          i_end = 15
        CASE DEFAULT
          CALL Fatal(Caller, 'Tetra face nodes supported up to degree 4')
        END SELECT
        n = i_end - i_start + 1 ! The number of new nodes per face

        DO k=1,4
          DO i=1,n
            ! Pick the positions of face nodes from a lower dimensional element 
            r(1) = VTKTriangleU(j,i+i_start-1)
            r(2) = VTKTriangleV(j,i+i_start-1)
            ut = 0.0d0
            vt = 0.0d0
            wt = 0.0d0
            DO t=1,3
              PBasis(1) = TriangleNodalPBasis(t, r(1), r(2))
              ut = ut + PBasis(1) * VTKTetraU(1,VTKTetraFaceMap(k,t))
              vt = vt + PBasis(1) * VTKTetraV(1,VTKTetraFaceMap(k,t))
              wt = wt + PBasis(1) * VTKTetraW(1,VTKTetraFaceMap(k,t))
            END DO
            VTKTetraU(j,i+Offsets(j)) = ut
            VTKTetraV(j,i+Offsets(j)) = vt
            VTKTetraW(j,i+Offsets(j)) = wt
            TetraNodeCounts(j) = TetraNodeCounts(j) + 1
          END DO
          Offsets(j) = TetraNodeCounts(j)
        END DO
      END DO

      ! Bubbles:
      !DO j=4,MAX_TETRA_DEGREE
      r(1) = SUM(VTKTetraU(1,1:4))/4
      r(2) = SUM(VTKTetraV(1,1:4))/4
      r(3) = SUM(VTKTetraW(1,1:4))/4
      VTKTetraU(4,1+Offsets(4)) = r(1)
      VTKTetraV(4,1+Offsets(4)) = r(2)
      VTKTetraW(4,1+Offsets(4)) = r(3)
      TetraNodeCounts(4) = TetraNodeCounts(4) + 1
      !END DO

      ! --------------------------------------------------------------------------
      ! Create the nodes for pyramids:
      ! --------------------------------------------------------------------------
      LElement % Type => GetElementType(605, .FALSE.)
      CALL GetRefPElementNodes(LElement % Type, VTKPyramidU(1,1:5), &
          VTKPyramidV(1,1:5), VTKPyramidW(1,1:5)) 

      DO k=2,MAX_PYRAMID_DEGREE
        VTKPyramidU(k,1:5) = VTKPyramidU(1,1:5)
        VTKPyramidV(k,1:5) = VTKPyramidV(1,1:5)
        VTKPyramidW(k,1:5) = VTKPyramidW(1,1:5)
      END DO

      ! The edges:
      PyramidNodeCounts = 5
      DO k=1,8
        Edge = GetPyramidEdgeMap(k)
        Offsets(1:MAX_PYRAMID_DEGREE) = PyramidNodeCounts(1:MAX_PYRAMID_DEGREE)
        CALL NodesOnEdge(VTKPyramidU, VTKPyramidV, Edge(1), Edge(2), MAX_PYRAMID_DEGREE, &
            Offsets, PyramidNodeCounts, VTKPyramidW)
      END DO
      Offsets(1:MAX_PYRAMID_DEGREE) = PyramidNodeCounts(1:MAX_PYRAMID_DEGREE)

      ! The quad face:
      Face = GetPyramidFaceMap(1)
      DO j=2,MAX_PYRAMID_DEGREE
        SELECT CASE(j)
        CASE(2)
          i_start = 9
          i_end = 9
        CASE(3)
          i_start = 13
          i_end = 16
        CASE DEFAULT
          CALL Fatal(Caller, 'Pyramid (quad) face nodes supported up to degree 3')
        END SELECT
        n = i_end - i_start + 1 ! The number of new nodes per face

        DO i=1,n
          ! Pick the positions of face nodes from a lower dimensional element 
          r(1) = VTKQuadU(j,i+i_start-1)
          r(2) = VTKQuadV(j,i+i_start-1)
          ut = 0.0d0
          vt = 0.0d0
          wt = 0.0d0
          DO t=1,4
            PBasis(1) = QuadNodalPBasis(t, r(1), r(2))
            ut = ut + PBasis(1) * VTKPyramidU(1,Face(t))
            vt = vt + PBasis(1) * VTKPyramidV(1,Face(t))
            wt = wt + PBasis(1) * VTKPyramidW(1,Face(t))
          END DO
          VTKPyramidU(j,i+Offsets(j)) = ut
          VTKPyramidV(j,i+Offsets(j)) = vt
          VTKPyramidW(j,i+Offsets(j)) = wt
          PyramidNodeCounts(j) = PyramidNodeCounts(j) + 1
        END DO
        Offsets(j) = PyramidNodeCounts(j) 
      END DO

      ! The triangular faces (TO DO: support for degrees p > 3):

      ! The bubbles (TO DO: support for degrees p > 2):
      r(1) = SUM(VTKPyramidU(1,1:5))/5
      r(2) = SUM(VTKPyramidV(1,1:5))/5
      r(3) = SUM(VTKPyramidW(1,1:5))/5
      VTKPyramidU(2,1+Offsets(2)) = r(1)
      VTKPyramidV(2,1+Offsets(2)) = r(2)
      VTKPyramidW(2,1+Offsets(2)) = r(3)
      PyramidNodeCounts(2) = PyramidNodeCounts(2) + 1

      ! --------------------------------------------------------------------------
      ! Create the nodes for prisms:
      ! --------------------------------------------------------------------------
      LElement % Type => GetElementType(706, .FALSE.)
      CALL GetRefPElementNodes(LElement % Type, VTKPrismU(1,1:6), &
          VTKPrismV(1,1:6), VTKPrismW(1,1:6)) 

      DO k=2,MAX_PRISM_DEGREE
        VTKPrismU(k,1:6) = VTKPrismU(1,1:6)
        VTKPrismV(k,1:6) = VTKPrismV(1,1:6)
        VTKPrismW(k,1:6) = VTKPrismW(1,1:6)
      END DO

      ! The edges:
      PrismNodeCounts = 6
      DO k=1,9
        Edge = GetWedgeEdgeMap(k)
        Offsets(1:MAX_PRISM_DEGREE) = PrismNodeCounts(1:MAX_PRISM_DEGREE)
        CALL NodesOnEdge(VTKPrismU, VTKPrismV, Edge(1), Edge(2), MAX_PRISM_DEGREE, &
            Offsets, PrismNodeCounts, VTKPrismW)
      END DO
      Offsets(1:MAX_PRISM_DEGREE) = PrismNodeCounts(1:MAX_PRISM_DEGREE)

      ! The triangular faces:
      DO j=3,MAX_PRISM_DEGREE
        SELECT CASE(j)
        CASE(3)
          i_start = 10
          i_end = 10
        CASE(4)
          i_start = 13
          i_end = 15
        CASE DEFAULT
          CALL Fatal(Caller, 'Prism (triangular) face nodes supported up to degree 4')
        END SELECT
        n = i_end - i_start + 1 ! The number of new nodes per face
        DO k=1,2
          Face = GetWedgeFaceMap(k)
          DO i=1,n
            ! Pick the positions of face nodes from a lower dimensional element 
            r(1) = VTKTriangleU(j,i+i_start-1)
            r(2) = VTKTriangleV(j,i+i_start-1)
            ut = 0.0d0
            vt = 0.0d0
            wt = 0.0d0
            DO t=1,3
              PBasis(1) = TriangleNodalPBasis(t, r(1), r(2))
              ut = ut + PBasis(1) * VTKPrismU(1,Face(t))
              vt = vt + PBasis(1) * VTKPrismV(1,Face(t))
              wt = wt + PBasis(1) * VTKPrismW(1,Face(t))
            END DO
            VTKPrismU(j,i+Offsets(j)) = ut
            VTKPrismV(j,i+Offsets(j)) = vt
            VTKPrismW(j,i+Offsets(j)) = wt
            PrismNodeCounts(j) = PrismNodeCounts(j) + 1
          END DO
          Offsets(j) = PrismNodeCounts(j)
        END DO
      END DO

      ! The quad faces:
      DO j=2,MAX_PRISM_DEGREE
        i_start = 4*j + 1
        i_end = (j+1)**2
        n = (j-1)**2  ! The number of new nodes per face n = i_end - i_start + 1

        DO k=3,5
          Face = GetWedgeFaceMap(k)
          DO i=1,n
            ! Pick the positions of face nodes from a lower dimensional element 
            r(1) = VTKQuadU(j,i+i_start-1)
            r(2) = VTKQuadV(j,i+i_start-1)
            ut = 0.0d0
            vt = 0.0d0
            wt = 0.0d0
            DO t=1,4
              PBasis(1) = QuadNodalPBasis(t, r(1), r(2))
              ut = ut + PBasis(1) * VTKPrismU(1,Face(t))
              vt = vt + PBasis(1) * VTKPrismV(1,Face(t))
              wt = wt + PBasis(1) * VTKPrismW(1,Face(t))
            END DO
            VTKPrismU(j,i+Offsets(j)) = ut
            VTKPrismV(j,i+Offsets(j)) = vt
            VTKPrismW(j,i+Offsets(j)) = wt
            PrismNodeCounts(j) = PrismNodeCounts(j) + 1
          END DO
          Offsets(j) = PrismNodeCounts(j) 
        END DO
      END DO

      DO j=3,MAX_PRISM_DEGREE
        i_start = 3*j + 1
        n = (j-1)*(j-2)/2
        i_end = 3*j + n
        DO i=1,j-1
          VTKPrismU(j,Offsets(j)+1:Offsets(j)+n) = VTKTriangleU(j,i_start:i_end)
          VTKPrismV(j,Offsets(j)+1:Offsets(j)+n) = VTKTriangleV(j,i_start:i_end)
          VTKPrismW(j,Offsets(j)+1:Offsets(j)+n) = VTKLineU(j,2+i)
          PrismNodeCounts(j) = PrismNodeCounts(j) + n
          Offsets(j) = PrismNodeCounts(j)
        END DO
      END DO

      ! --------------------------------------------------------------------------
      ! Create the nodes for hexahedra:
      ! --------------------------------------------------------------------------
      LElement % Type => GetElementType(808, .FALSE.)
      CALL GetRefPElementNodes(LElement % Type, VTKBrickU(1,1:8), &
          VTKBrickV(1,1:8), VTKBrickW(1,1:8)) 

      DO k=2,MAX_BRICK_DEGREE
        VTKBrickU(k,1:8) = VTKBrickU(1,1:8)
        VTKBrickV(k,1:8) = VTKBrickV(1,1:8)
        VTKBrickW(k,1:8) = VTKBrickW(1,1:8)
      END DO
      BrickNodeCounts = 8

      ! The edges:
      ! It seems that VTK cell types 72 and 12/29 are not interchangeable.
      ! We first pick the edge map which works for 72 and then re-run
      ! the same code up to the degree 2 with a different edge map to obtain consistency.
      DO k=1,12
        ! The following is needed for 72:
        SELECT CASE(k)
        CASE(11)
          Edge = GetBrickEdgeMap(12)
        CASE(12)
          Edge = GetBrickEdgeMap(11)
        CASE DEFAULT
          Edge = GetBrickEdgeMap(k)
        END SELECT
        Offsets(1:MAX_BRICK_DEGREE) = BrickNodeCounts(1:MAX_BRICK_DEGREE)
        CALL NodesOnEdge(VTKBrickU, VTKBrickV, Edge(1), Edge(2), MAX_BRICK_DEGREE, &
            Offsets, BrickNodeCounts, VTKBrickW)
      END DO
      ! The re-run needs a reset:
      BrickNodeCounts(2) = 8
      DO k=1,12
        ! The following is needed for 29:
        Edge = GetBrickEdgeMap(k)
        Offsets(2) = BrickNodeCounts(2)
        CALL NodesOnEdge(VTKBrickU, VTKBrickV, Edge(1), Edge(2), 2, &
            Offsets, BrickNodeCounts, VTKBrickW)
      END DO

      ! The faces:
      Offsets(1:MAX_BRICK_DEGREE) = BrickNodeCounts(1:MAX_BRICK_DEGREE)
      VTKBrickFaceMap(1,:) = (/ 1,4,8,5 /)
      VTKBrickFaceMap(2,:) = (/ 2,3,7,6 /)
      VTKBrickFaceMap(3,:) = (/ 1,2,6,5 /)
      VTKBrickFaceMap(4,:) = (/ 4,3,7,8 /)
      VTKBrickFaceMap(5,:) = (/ 1,2,3,4 /)
      VTKBrickFaceMap(6,:) = (/ 5,6,7,8 /)

      DO j=2,MAX_BRICK_DEGREE
        i_start = 4*j + 1
        i_end = (j+1)**2
        n = (j-1)**2  ! The number of new nodes per face n = i_end - i_start + 1

        DO k=1,6
          DO i=1,n
            ! Pick the positions of face nodes from a lower dimensional element 
            r(1) = VTKQuadU(j,i+i_start-1)
            r(2) = VTKQuadV(j,i+i_start-1)
            ut = 0.0d0
            vt = 0.0d0
            wt = 0.0d0
            DO t=1,4
              PBasis(1) = QuadNodalPBasis(t, r(1), r(2))
              ut = ut + PBasis(1) * VTKBrickU(1,VTKBrickFaceMap(k,t))
              vt = vt + PBasis(1) * VTKBrickV(1,VTKBrickFaceMap(k,t))
              wt = wt + PBasis(1) * VTKBrickW(1,VTKBrickFaceMap(k,t))
            END DO
            VTKBrickU(j,i+Offsets(j)) = ut
            VTKBrickV(j,i+Offsets(j)) = vt
            VTKBrickW(j,i+Offsets(j)) = wt
            BrickNodeCounts(j) = BrickNodeCounts(j) + 1
          END DO
          Offsets(j) = BrickNodeCounts(j)
        END DO
      END DO

      ! Bubbles:
      VTKBrickU(2,1+Offsets(2)) = 0.0d0
      VTKBrickV(2,1+Offsets(2)) = 0.0d0
      VTKBrickW(2,1+Offsets(2)) = 0.0d0
      BrickNodeCounts(2) = BrickNodeCounts(2) + 1

      DO j=3,MAX_BRICK_DEGREE
        i_start = 4*j + 1
        i_end = (j+1)**2
        n = (j-1)**2
        DO i=1,j-1
          VTKBrickU(j,Offsets(j)+1:Offsets(j)+n) = VTKQuadU(j,i_start:i_end)
          VTKBrickV(j,Offsets(j)+1:Offsets(j)+n) = VTKQuadV(j,i_start:i_end)
          VTKBrickW(j,Offsets(j)+1:Offsets(j)+n) = VTKLineU(j,2+i)
          BrickNodeCounts(j) = BrickNodeCounts(j) + n
          Offsets(j) = BrickNodeCounts(j)
        END DO
      END DO
      AllocationsDone = .TRUE.
    END IF

    IF (PRESENT(PSolver)) THEN
      PVersion = IsActivePElement(PElement, PSolver)
    ELSE
      PVersion = IsPElement(PElement)
    END IF
    
    IF (.NOT. PVersion) THEN
      CALL Warn(Caller, 'The input element is not a p-element, returning')
      RETURN
    END IF
    
    PNodes % x(:) = 0.0d0
    PNodes % y(:) = 0.0d0
    PNodes % z(:) = 0.0d0
    !
    ! NOTE: We shall not need the derivatives of basis functions or
    ! the determinant of the element mapping. Hence using the following
    ! nodal coordinates is sufficient, although in principle we
    ! might disregard some values 
    !
    n = PElement % TYPE % NumberOfNodes
    PNodes % x(1:n) = Mesh % Nodes % x(PElement % NodeIndexes(1:n))
    PNodes % y(1:n) = Mesh % Nodes % y(PElement % NodeIndexes(1:n))
    PNodes % z(1:n) = Mesh % Nodes % z(PElement % NodeIndexes(1:n))

    SELECT CASE(Family)
    CASE(2)
      IF (Degree > MAX_LINE_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (lines) exceeds the supported maximum')
      END IF
      n = Degree + 1
      NodesU => VTKLineU(Degree,1:n)
      NodesV => VTKLineU(MAX_LINE_DEGREE+1,1:n)
      NodesW => VTKLineU(MAX_LINE_DEGREE+1,1:n)

    CASE(3)
      IF (Degree > MAX_TRIANGLE_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (triangles) exceeds the supported maximum')
      END IF
      n = TriangleNodeCounts(Degree)
      NodesU => VTKTriangleU(Degree,1:n)
      NodesV => VTKTriangleV(Degree,1:n)
      NodesW => VTKTriangleW(1:n)

    CASE(4)
      IF (Degree > MAX_QUAD_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (quads) exceeds the supported maximum')
      END IF
      n = QuadNodeCounts(Degree)
      NodesU => VTKQuadU(Degree,1:n)
      NodesV => VTKQuadV(Degree,1:n)
      NodesW => VTKQuadW(1:n)

    CASE(5)
      IF (Degree > MAX_TETRA_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (tetrahedra) exceeds the supported maximum')
      END IF
      n = TetraNodeCounts(Degree)
      NodesU => VTKTetraU(Degree,1:n)
      NodesV => VTKTetraV(Degree,1:n)
      NodesW => VTKTetraW(Degree,1:n)

    CASE(6)
      IF (Degree > MAX_PYRAMID_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (pyramids) exceeds the supported maximum')
      END IF
      n = PyramidNodeCounts(Degree)
      NodesU => VTKPyramidU(Degree,1:n)
      NodesV => VTKPyramidV(Degree,1:n)
      NodesW => VTKPyramidW(Degree,1:n)

    CASE(7)
      IF (Degree > MAX_PRISM_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (prisms) exceeds the supported maximum')
      END IF
      n = PrismNodeCounts(Degree)
      NodesU => VTKPrismU(Degree,1:n)
      NodesV => VTKPrismV(Degree,1:n)
      NodesW => VTKPrismW(Degree,1:n)

    CASE(8)
      IF (Degree > MAX_BRICK_DEGREE) THEN
        CALL Fatal(Caller, 'Lagrange Element Degree (bricks) exceeds the supported maximum')
      END IF
      n = BrickNodeCounts(Degree)
      NodesU => VTKBrickU(Degree,1:n)
      NodesV => VTKBrickV(Degree,1:n)
      NodesW => VTKBrickW(Degree,1:n)

    CASE DEFAULT
      CALL Fatal(Caller, 'An unknown element family')
    END SELECT

    BasisDegree(:) = 0

    DO t = 1,n
      ut = NodesU(t)
      vt = NodesV(t)
      wt = NodesW(t)

      !PRINT *, 'CALLING AT POINT ', t,ut,vt,wt      

      ! It seems that the p-basis for pyramids cannot be evaluated at the fifth node,
      ! so we make a small "error":
      IF (Family == 6 .AND. t==5) wt = wt - 1.0d-8

      stat = ElementInfo(PElement, PNodes, ut, vt, wt, detJ, PBasis, BasisDegree=BasisDegree, &
          USolver=PSolver)
      IF (t == 1) THEN
        nd = COUNT(BasisDegree > 0) 
        IF (nd == 0) CALL Fatal(Caller, 'p-basis needed but the classical basis returned')
        ! PRINT *, 'p-basis dimension ', nd, SUM( PBasis(1:nd)), SUM(PBasis(1:PElement % Type % NumberOfNodes))
      END IF
        
      DO k = 1,Fields
        LSol(k,t) = SUM(PBasis(1:nd) * PSol(k,1:nd))
      END DO
    END DO

  CONTAINS

    ! --------------------------------------------------------------------------------
    ! Given the vertices of a 2-node edge in the first rows of the coordinate arrays
    ! NodesU, NodesV and NodesW, create additional nodes corresponding to the Lagrange
    ! interpolation up to the polynomial degree Maxdegree. The edge is defined by
    ! specifying its start and end indices and is assumed to be of length 2.
    ! The array Offsets gives the node counts before adding new nodes on the edge,
    ! while the array NodeCounts gives the counts after this action.
    ! --------------------------------------------------------------------------------
    SUBROUTINE NodesOnEdge(NodesU, NodesV, ind_start, ind_end, MaxDegree, Offsets, &
        NodeCounts, NodesW)

      IMPLICIT NONE
      REAL(KIND=dp), INTENT(INOUT):: NodesU(:,:), NodesV(:,:) 
      INTEGER, INTENT(IN) :: ind_start, ind_end
      INTEGER, INTENT(IN) :: MaxDegree
      INTEGER, INTENT(IN) :: OffSets(:)
      INTEGER, INTENT(INOUT) :: NodeCounts(:)
      REAL(KIND=dp), OPTIONAL, INTENT(INOUT):: NodesW(:,:)
      ! --------------------------------------------------------------
      REAL(KIND=dp), PARAMETER :: L = 2.0_dp
      LOGICAL :: CreateW
      INTEGER :: i, k
      REAL(KIND=dp) :: r(3), ut, vt, wt, delta
      ! --------------------------------------------------------------
      CreateW = PRESENT(NodesW)
      r(1) =  NodesU(1,ind_end) - NodesU(1,ind_start)
      r(2) =  NodesV(1,ind_end) - NodesV(1,ind_start)
      IF (CreateW) THEN
        r(3) =  NodesW(1,ind_end) - NodesW(1,ind_start)
        k = 3
      ELSE
        k = 2
      END IF
      r(1:k) = r(1:k)/SQRT(SUM(r(1:k)**2))

      DO k=2,MaxDegree
        delta = L/k
        ut = NodesU(1,ind_start)
        vt = NodesV(1,ind_start)
        IF (CreateW) wt = NodesW(1,ind_start)
        DO i=1,k-1
          ut = ut + delta * r(1)
          vt = vt + delta * r(2)
          NodesU(k,i+Offsets(k)) = ut
          NodesV(k,i+Offsets(k)) = vt
          IF (CreateW) THEN
            wt = wt + delta * r(3)
            NodesW(k,i+Offsets(k)) = wt
          END IF
          NodeCounts(k) = NodeCounts(k) + 1
        END DO
      END DO
    END SUBROUTINE NodesOnEdge

!------------------------------------------------------------------------------
  END SUBROUTINE HierarchicPToLagrange
!------------------------------------------------------------------------------