!>
!> @file multigrid_mod.f90
!>
!> @brief
!>
!> @copyright
!> Copyright (©) 2021 EPFL (Ecole Polytechnique Fédérale de Lausanne)
!> SPC (Swiss Plasma Center)
!>
!> SPClibs 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 3 of the License, or (at your option)
!> any later version.
!>
!> SPClibs 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 General Public License for more details.
!>
!> You should have received a copy of the GNU Lesser General Public License
!> along with this program. If not, see <https://www.gnu.org/licenses/>.
!>
!> @author
!> (in alphabetical order)
!> @author Trach-Minh Tran <trach-minh.tran@epfl.ch>
!>
MODULE multigrid
!
!    MULTIGRID: Implement Multigrid solver for Finite Elements
!               and Fiinite Differences.
!
!    T.M. Tran, CRPP-EPFL
!    September 2012
!
  USE bsplines
  USE matrix
  USE conmat_mod
  USE csr
  USE cds
  IMPLICIT NONE
!
  TYPE grid1d
     INTEGER                         :: n    ! Number of intervals
     INTEGER                         :: rank ! Dimension of FE space
     DOUBLE PRECISION                :: h
     DOUBLE PRECISION, ALLOCATABLE   :: x(:)
     DOUBLE PRECISION, ALLOCATABLE   :: v(:)
     DOUBLE PRECISION, ALLOCATABLE   :: f(:)
     TYPE(spline1d)                  :: spl
     TYPE(gemat)                     :: transf    ! Coarse to fine transfer matrix
     TYPE(gbmat), ALLOCATABLE        :: mata      ! FE matrix
     TYPE(gbmat), ALLOCATABLE        :: matm      ! mass matrix
     TYPE(gbmat), ALLOCATABLE        :: mata_copy ! Used for direct_solve
     TYPE(gemat), ALLOCATABLE        :: matap     ! FE matrix
     TYPE(gemat), ALLOCATABLE        :: matmp     ! mass matrix
     TYPE(gemat), ALLOCATABLE        :: matap_copy! Used for direct_solve
  END TYPE grid1d
!
  TYPE grid2d
     INTEGER                         :: n(2)    ! Number of intervals
     INTEGER                         :: rank(2) ! Dimension of FE space
     DOUBLE PRECISION                :: h(2)
     DOUBLE PRECISION, ALLOCATABLE   :: x(:), y(:)
!
     DOUBLE PRECISION, ALLOCATABLE :: v(:,:) ! sol
     DOUBLE PRECISION, ALLOCATABLE :: f(:,:) ! rhs
     DOUBLE PRECISION, POINTER     :: v1d(:) ! flatten sol
     DOUBLE PRECISION, POINTER     :: f1d(:) ! flatten rhs
!
     TYPE(csr_mat),ALLOCATABLE  :: mata
     TYPE(cds_mat),ALLOCATABLE  :: mata_cds
     TYPE(spline2d) :: spl
     TYPE(gemat)    :: transf(2)
!
     TYPE(csr_mat)  :: matp(2)
  END TYPE grid2d
!
  TYPE mg_info
     INTEGER :: nu1    ! Relaxation down sweeps
     INTEGER :: nu2    ! Relaxation up sweeps
     INTEGER :: mu     ! mu-cycle number
     INTEGER :: nu0    ! Number of FMG cycles
     INTEGER :: levels ! Number of mg levels
     CHARACTER(len=4) :: relax ! Type of relation
     DOUBLE PRECISION :: omega ! for weighted Jacobi relaxation
     LOGICAL          :: nlscale=.FALSE. ! Scale restriction if .TRUE.
  END TYPE mg_info
!
  INTERFACE create_grid
     MODULE PROCEDURE create_grid_1d, create_grid_2d
  END INTERFACE create_grid
  INTERFACE disrhs
     MODULE PROCEDURE disrhs_1d, disrhs_2d
  END INTERFACE disrhs
  INTERFACE direct_solve
     MODULE PROCEDURE direct_solve_1d, direct_solve_2d
  END INTERFACE direct_solve
  INTERFACE mg
     MODULE PROCEDURE mg_1d, mg_2d
  END INTERFACE mg
  INTERFACE disc_err
     MODULE PROCEDURE disc_err_1d, disc_err_2d
  END INTERFACE disc_err
  INTERFACE jacobi
     MODULE PROCEDURE jacobi_cds, jacobi_csr, jacobi_gb, jacobi_ge
  END INTERFACE jacobi
  INTERFACE gs
     MODULE PROCEDURE gs_cds, gs_csr, gs_gb, gs_ge
  END INTERFACE gs
  INTERFACE restrict
     MODULE PROCEDURE restrict_1d, restrict_2d, restrict_2d_csr
  END INTERFACE restrict
  INTERFACE prolong
     MODULE PROCEDURE prolong_1d, prolong_2d, prolong_2d_csr
  END INTERFACE prolong
  INTERFACE printmat
     MODULE PROCEDURE printmat_mat, printmat_ge, printmat_gb, printmat_periodic
  END INTERFACE printmat
  INTERFACE massmat
     MODULE PROCEDURE massmat_ge, massmat_gb, massmat_periodic
  END INTERFACE massmat
  INTERFACE femat
     MODULE PROCEDURE femat_2d_csr, femat_ge, femat_gb, femat_periodic
  END INTERFACE femat
  INTERFACE ibcmat
     MODULE PROCEDURE ibcmat_1d, ibcmat_2d
  END INTERFACE ibcmat
  INTERFACE mod_transf
     MODULE PROCEDURE mod_transf_full, mod_transf_csr
  END INTERFACE mod_transf
  INTERFACE normf
     MODULE PROCEDURE normf_gb, normf_ge
  END INTERFACE normf
  INTERFACE residue
     MODULE PROCEDURE residue_gen, residue_csr, residue_cds, residue_gb, residue_ge
  END INTERFACE residue
!
CONTAINS
!--------------------------------------------------------------------------------
  SUBROUTINE create_grid_1d(n, nidbas, ng_in, alpha, grids, period)
!
!    Create an array of levels  grids
!    Compute mass matrix and prolongation matrices.
!
    INTEGER, INTENT(in) :: n      ! Number of intervals in the finest grid
    INTEGER, INTENT(in) :: nidbas ! Order of splines
    INTEGER, INTENT(in) :: ng_in  ! Number of proposed Gauss points
    INTEGER, INTENT(in) :: alpha  ! geometric exponent
    TYPE(grid1d), INTENT(out)     :: grids(:)
    LOGICAL, INTENT(in), OPTIONAL :: period
!
    LOGICAL :: nlper
    INTEGER :: n_current, nrank, ngauss
    INTEGER :: levels, l, i
    DOUBLE PRECISION :: h_current
    TYPE(gbmat) :: matm
    TYPE(gemat) :: matmp
!
    levels = SIZE(grids)
    nlper = .FALSE.
    IF(PRESENT(period)) nlper = period
!    
    ngauss = CEILING(REAL(2*nidbas+alpha+1,8)/2.d0)
    ngauss = MAX(ng_in, ngauss)
    WRITE(*,'(a,i0)') 'ngauss = ', ngauss
!
!    Allocate some matrices
!
    DO l=1,levels
       IF(nlper) THEN
          ALLOCATE(grids(l)%matmp)
          ALLOCATE(grids(l)%matap)
       ELSE
          ALLOCATE(grids(l)%matm)
          ALLOCATE(grids(l)%mata)
       END IF
    END DO
!
    n_current = n
    h_current = 1.0d0/REAL(n_current,8)
    DO l=1,levels
       IF(n_current .LT. 2 ) THEN
          PRINT*, 'CREATE_GRID: number intervals too small!'
          STOP
       END IF
       grids(l)%n = n_current
       grids(l)%h = h_current
       ALLOCATE(grids(l)%x(0:n_current))
       grids(l)%x(0:n_current) = (/ (REAL(i,8)*h_current, i=0,n_current) /)
       CALL set_spline(nidbas, ngauss, grids(l)%x, grids(l)%spl, period=nlper)
       CALL get_dim(grids(l)%spl, nrank)
       IF(nlper) nrank = n_current
       grids(l)%rank = nrank
       ALLOCATE(grids(l)%v(nrank))
       ALLOCATE(grids(l)%f(nrank))
       IF(nlper) THEN
          CALL massmat(grids(l)%spl, alpha, grids(l)%matmp)
       ELSE
          CALL massmat(grids(l)%spl, alpha, grids(l)%matm)
       END IF
       IF(l.GT.1) THEN 
          CALL ctof_massmat(grids(l-1)%spl, grids(l)%spl, alpha, grids(l)%transf)
          IF(nlper) THEN
             CALL mcopy(grids(l-1)%matmp, matmp)
             CALL factor(matmp)
             CALL bsolve(matmp, grids(l)%transf%val)
             CALL destroy(matmp)
          ELSE
             CALL mcopy(grids(l-1)%matm, matm)
             CALL factor(matm)
             CALL bsolve(matm, grids(l)%transf%val)
             CALL destroy(matm)
          END IF
      END IF
       n_current = n_current/2
       h_current = 2.0d0*h_current
    END DO
  END SUBROUTINE create_grid_1d
!--------------------------------------------------------------------------------
  SUBROUTINE create_grid_2d(x, y, nidbas, ng_in, alpha, grids, mat_type, period, &
       &                   debug_in)
!
!    Create an array of levels  grids
!    Compute mass matrix and prolongation matrices.
!
    DOUBLE PRECISION, INTENT(in) :: x(0:), y(0:) ! Finest grid points
    INTEGER, INTENT(in)          :: nidbas(2)    ! Order of splines
    INTEGER, INTENT(in)          :: alpha(2)     ! geometric exponent
    INTEGER, INTENT(in)          :: ng_in(2)     ! Number of proposed Gauss points
    TYPE(grid2d), INTENT(out), TARGET :: grids(:)
    CHARACTER(*), INTENT(in), OPTIONAL :: mat_type ! csr (default) or cds
    LOGICAL, INTENT(in), OPTIONAL     :: period(2)
    LOGICAL, INTENT(in), OPTIONAL     :: debug_in
!
    LOGICAL, DIMENSION(2) :: nlper
    INTEGER, DIMENSION(2) :: n, sp_dim, ngauss
    LOGICAL     :: nlcds
    LOGICAL     :: debug
    INTEGER     :: levels, l, rank2d
    TYPE(gemat) :: matm
!
    DOUBLE PRECISION, PARAMETER :: pi = 4.0d0*ATAN(1.0d0)
!
!  Process input args
!
    n(1) = SIZE(x)-1
    n(2) = SIZE(y)-1
    levels = SIZE(grids)
    nlper = .FALSE.
    IF(PRESENT(period)) THEN 
       nlper = period
    END IF
    IF(PRESENT(debug_in)) THEN
       debug = debug_in
    ELSE
       debug = .FALSE.
    END IF
    IF(PRESENT(mat_type)) THEN  ! CSR matrix by default
       nlcds = mat_type.EQ.'cds'
    ELSE
       nlcds = .FALSE.
    END IF
!
!  WARNING: Assume that only 2nd dim can be periodic!!!
    IF(nlper(1)) THEN
       WRITE(*,'(A)') 'CREATE_GRID: First dimension could not be periodic!'
       STOP
    END IF
!
    ngauss = CEILING(REAL(2*nidbas+1,8)/2.d0)
    ngauss = MAX(ng_in, ngauss)
    WRITE(*,'(a,2i4)') 'ngauss = ', ngauss
!
    DO l=1,levels
!
!  Create mesh from finest grid mesh
       IF(MINVAL(n) .LT. 2 ) THEN
          PRINT*, 'CREATE_GRID: number intervals too small!'
          STOP
       END IF
       grids(l)%n = n
       ALLOCATE(grids(l)%x(0:n(1)))
       ALLOCATE(grids(l)%y(0:n(2)))
       IF(l.EQ.1) THEN
          grids(1)%x = x
          grids(1)%y = y
       ELSE
          grids(l)%x(:) = grids(l-1)%x(0::2)
          grids(l)%y(:) = grids(l-1)%y(0::2)
       END IF
       IF(debug) THEN
          WRITE(*,'(/a,i4,a,2l2)') 'l =', l, '  nlper =', nlper
          WRITE(*,'(a/(10(1pe12.3)))') 'x', grids(l)%x
          WRITE(*,'(a/(10(1pe12.3)))') 'y', grids(l)%y
       END IF
!
!  Allocate mem for solution v and RHS f
       CALL set_spline(nidbas, ngauss, grids(l)%x, grids(l)%y, grids(l)%spl, period=nlper)
       CALL get_dim(grids(l)%spl%sp1, sp_dim(1))
       CALL get_dim(grids(l)%spl%sp2, sp_dim(2))
       ALLOCATE(grids(l)%v(sp_dim(1), sp_dim(2)))
       ALLOCATE(grids(l)%f(sp_dim(1), sp_dim(2)))
!
!  WARNING: Assume that only 2nd dim can be periodic!!!
       grids(l)%rank = sp_dim
       IF(nlper(2)) THEN
          grids(l)%rank(2) = n(2)
       END IF
       IF(debug) THEN
          WRITE(*,'(a,2i6)') 'Grid ranks', grids(l)%rank
       END IF
!
!  Flatten version of sol and rhs
       rank2d = PRODUCT(grids(l)%rank)
       grids(l)%v1d(1:rank2d) => grids(l)%v
       grids(l)%f1d(1:rank2d) => grids(l)%f
!
!  Matrix format for FE matrix
       IF(nlcds) THEN
          ALLOCATE(grids(l)%mata_cds)
       ELSE
          ALLOCATE(grids(l)%mata)
       END IF
!
!  Coarse to fine mesh transfers for l>1
       IF(l.GT.1) THEN
          CALL ctof_massmat(grids(l-1)%spl%sp1, grids(l)%spl%sp1, alpha(1), grids(l)%transf(1))
          CALL ctof_massmat(grids(l-1)%spl%sp2, grids(l)%spl%sp2, alpha(2), grids(l)%transf(2))
!
          CALL massmat(grids(l-1)%spl%sp1, alpha(1), matm)
          CALL factor(matm)
          CALL bsolve(matm, grids(l)%transf(1)%val)
          CALL full_to_csr(grids(l)%transf(1)%val, grids(l)%matp(1))
          CALL destroy(matm)
          CALL destroy(grids(l)%transf(1))
!
          CALL massmat(grids(l-1)%spl%sp2, alpha(2), matm)
          CALL factor(matm)
          CALL bsolve(matm, grids(l)%transf(2)%val)
          CALL full_to_csr(grids(l)%transf(2)%val, grids(l)%matp(2))
          CALL destroy(matm)
          CALL destroy(grids(l)%transf(2))
       END IF
!
!   Next coarse grid
       n = n/2
    END DO
  END SUBROUTINE create_grid_2d
!--------------------------------------------------------------------------------
  SUBROUTINE create_grid_fd(x, y, grids, info, mat_type, period, debug)
!
!    FD version of create_grid
!
    DOUBLE PRECISION, INTENT(in)       :: x(0:), y(0:)
    TYPE(grid2d), INTENT(out), TARGET  :: grids(:)
    TYPE(mg_info), INTENT(inout)       :: info      ! info for MG
    CHARACTER(*), INTENT(in), OPTIONAL :: mat_type  ! csr (default) or cds
    LOGICAL, INTENT(in), OPTIONAL      :: period(2)
    LOGICAL, INTENT(in), OPTIONAL      :: debug
!
    INTEGER :: nidbas(2)=1, ngauss(2)=4    ! Linear Splines \equiv 1st FD
    INTEGER :: alpha(2) = 1                ! Cartesian coordinate
    LOGICAL, DIMENSION(2) :: nlper
    LOGICAL :: nldebug
    LOGICAL :: nlcds
    INTEGER :: levels, n(2), sp_dim(2)
    INTEGER :: l, rank2d    
    TYPE(gemat)  :: matm
!--------------------------------------------------------------------------------    
!
!  Process input args
!
    n(1) = SIZE(x)-1
    n(2) = SIZE(y)-1
    levels = SIZE(grids)
    info%nlscale = .TRUE.   ! Restriction should be scaled for FD
    nlper = .FALSE.
    IF(PRESENT(period)) nlper = period
    nldebug = .FALSE.
    IF(PRESENT(debug))  nldebug = debug
    IF(PRESENT(mat_type)) THEN  ! CSR matrix by default
       nlcds = mat_type.EQ.'cds'
    ELSE
       nlcds = .FALSE.
    END IF
!
    DO l=1,levels
!
!  Create mesh from finest grid mesh
       IF(MINVAL(n) .LT. 2 ) THEN
          PRINT*, 'CREATE_GRID: number intervals too small!'
          STOP
       END IF
       grids(l)%n = n
       ALLOCATE(grids(l)%x(0:n(1)))
       ALLOCATE(grids(l)%y(0:n(2)))
       IF(l.EQ.1) THEN
          grids(1)%x = x
          grids(1)%y = y
       ELSE
          grids(l)%x(:) = grids(l-1)%x(0::2)
          grids(l)%y(:) = grids(l-1)%y(0::2)
       END IF
       IF(nldebug) THEN
          WRITE(*,'(/a,i4,a,2l2)') 'l =', l, '  nlper =', nlper
          WRITE(*,'(a/(10(1pe12.3)))') 'x', grids(l)%x
          WRITE(*,'(a/(10(1pe12.3)))') 'y', grids(l)%y
       END IF
!
!  Allocate mem for solution v and RHS f
       CALL set_spline(nidbas, ngauss, grids(l)%x, grids(l)%y, grids(l)%spl, period=nlper)
       CALL get_dim(grids(l)%spl%sp1, sp_dim(1))
       CALL get_dim(grids(l)%spl%sp2, sp_dim(2))
       ALLOCATE(grids(l)%v(0:sp_dim(1)-1, 0:sp_dim(2)-1))
       ALLOCATE(grids(l)%f(0:sp_dim(1)-1, 0:sp_dim(2)-1))
!
!  WARNING: Assume that only 2nd dim can be periodic!!!
       grids(l)%rank = sp_dim
       IF(nlper(2)) THEN
          grids(l)%rank(2) = n(2)
       END IF
       IF(nldebug) THEN
          WRITE(*,'(a,2i6)') 'Grid ranks', grids(l)%rank
       END IF
!
!  Flatten version of sol and rhs
       rank2d = PRODUCT(grids(l)%rank)
       grids(l)%v1d(1:rank2d) => grids(l)%v
       grids(l)%f1d(1:rank2d) => grids(l)%f
!
!  Matrix format for FD matrix
       IF(nlcds) THEN
          ALLOCATE(grids(l)%mata_cds)
       ELSE
          ALLOCATE(grids(l)%mata)
       END IF
!
!  Coarse to fine mesh transfers for l>1
       IF(l.GT.1) THEN
          CALL ctof_massmat(grids(l-1)%spl%sp1, grids(l)%spl%sp1, alpha(1), grids(l)%transf(1))
          CALL ctof_massmat(grids(l-1)%spl%sp2, grids(l)%spl%sp2, alpha(2), grids(l)%transf(2))
!
          CALL massmat(grids(l-1)%spl%sp1, alpha(1), matm)
          CALL factor(matm)
          CALL bsolve(matm, grids(l)%transf(1)%val)
          CALL full_to_csr(grids(l)%transf(1)%val, grids(l)%matp(1))
          CALL destroy(matm)
          CALL destroy(grids(l)%transf(1))
!
          CALL massmat(grids(l-1)%spl%sp2, alpha(2), matm)
          CALL factor(matm)
          CALL bsolve(matm, grids(l)%transf(2)%val)
          CALL full_to_csr(grids(l)%transf(2)%val, grids(l)%matp(2))
          CALL destroy(matm)
          CALL destroy(grids(l)%transf(2))
       END IF
!
!   Next coarse grid
       n = n/2
    END DO
  END SUBROUTINE create_grid_fd
!--------------------------------------------------------------------------------
  RECURSIVE SUBROUTINE fmg(grids, info, l)
!
!   Execute a full multigrid V-cycle
!
    TYPE(grid1d), INTENT(inout) :: grids(:)
    TYPE(mg_info), INTENT(in)    :: info
    INTEGER, INTENT(in)          :: l
    INTEGER :: levels, k
    levels = info%levels
!
    IF(l.EQ.levels) THEN
       CALL direct_solve(grids(levels), grids(levels)%v)
    ELSE
       grids(l+1)%f = restrict(grids(l+1)%transf,grids(l)%f)
       CALL fmg(grids, info, l+1)       
       grids(l)%v = prolong(grids(l+1)%transf,grids(l+1)%v)
       DO k=1,info%nu0
          CALL mg(grids, info, l)
       END DO
    END IF
  END SUBROUTINE fmg
!--------------------------------------------------------------------------------
  RECURSIVE SUBROUTINE mg_1d(grids, info, l)
!
!   Execute a recursive V-cycle
!
    TYPE(grid1d), INTENT(inout)  :: grids(:)
    TYPE(mg_info), INTENT(in)    :: info
    INTEGER, INTENT(in)          :: l
    INTEGER :: levels, k
    LOGICAL :: nlper
!
    levels = info%levels
    nlper = grids(1)%spl%period
!
    IF(l.EQ.levels) THEN
       CALL direct_solve(grids(levels), grids(levels)%v)
    ELSE
       CALL relax(info%nu1)
       IF(nlper) THEN
          grids(l+1)%f = restrict(grids(l+1)%transf, &
               &   grids(l)%f-vmx(grids(l)%matap, grids(l)%v))
       ELSE
          grids(l+1)%f = restrict(grids(l+1)%transf, &
               &   grids(l)%f-vmx(grids(l)%mata, grids(l)%v))
       END IF
       grids(l+1)%v = 0.0d0
!
!   Only 1 call to the coarsest level
       DO k=1,MERGE(info%mu, 1, (l+1).NE.levels) 
          CALL mg(grids, info, l+1)
       END DO
!
       grids(l)%v = grids(l)%v + prolong(grids(l+1)%transf,grids(l+1)%v)
       CALL relax(info%nu2)
    END IF
!
  CONTAINS
    SUBROUTINE relax(nu)
      INTEGER, INTENT(in) :: nu
      SELECT CASE (TRIM(info%relax))
      CASE ("jac")
         IF(nlper) THEN
            CALL jacobi(grids(l)%matap, info%omega, nu, grids(l)%v, grids(l)%f)
         ELSE
            CALL jacobi(grids(l)%mata, info%omega, nu, grids(l)%v, grids(l)%f)
         END IF
      CASE ("gs")
         IF(nlper) THEN
            CALL gs(grids(l)%matap, nu, grids(l)%v, grids(l)%f)
         ELSE
            CALL gs(grids(l)%mata, nu, grids(l)%v, grids(l)%f)
         END IF
      CASE default
         PRINT*, "relax ", info%relax, " NOT IMPLEMENTED!"
         STOP
      END SELECT
    END SUBROUTINE relax
  END SUBROUTINE mg_1d
!--------------------------------------------------------------------------------
  RECURSIVE SUBROUTINE mg_2d(grids, info, l)
!
!   Execute a recursive V-cycle
!
    TYPE(grid2d), INTENT(inout)  :: grids(:)
    TYPE(mg_info), INTENT(in)    :: info
    INTEGER, INTENT(in)          :: l
!
    DOUBLE PRECISION, ALLOCATABLE, TARGET :: resid(:,:)
    DOUBLE PRECISION, POINTER             :: resid1d(:)
    INTEGER :: levels, k, m1, m2
!
    levels = info%levels
    m1 = SIZE(grids(l)%v,1)
    m2 = SIZE(grids(l)%v,2)
!
    IF(l.EQ.levels) THEN
       grids(levels)%v = grids(levels)%f
       CALL direct_solve(grids(levels), grids(levels)%v1d)
    ELSE
       CALL relax(info%nu1)
       ALLOCATE(resid(m1,m2)); resid1d(1:m1*m2) => resid
       IF(ALLOCATED(grids(l)%mata)) THEN
          resid1d = grids(l)%f1d - vmx(grids(l)%mata, grids(l)%v1d)
       ELSE
          resid1d = grids(l)%f1d - vmx(grids(l)%mata_cds, grids(l)%v1d)
       END IF
       grids(l+1)%f = restrict(grids(l+1)%matp, resid)
       IF(info%nlscale) grids(l+1)%f = 0.25d0*grids(l+1)%f
       DEALLOCATE(resid)
       grids(l+1)%v = 0.0d0
!
!   Only 1 call to the coarsest level
       DO k=1,MERGE(info%mu, 1, (l+1).NE.levels) 
          CALL mg(grids, info, l+1)
       END DO
!
       grids(l)%v = grids(l)%v + prolong(grids(l+1)%matp,grids(l+1)%v)
       CALL relax(info%nu2)
    END IF
!
  CONTAINS
    SUBROUTINE relax(nu)
      INTEGER, INTENT(in) :: nu
      SELECT CASE (TRIM(info%relax))
      CASE ("jac")
         IF(ALLOCATED(grids(l)%mata)) THEN
            CALL jacobi(grids(l)%mata, info%omega, nu, grids(l)%v1d, grids(l)%f1d)
         ELSE
            CALL jacobi(grids(l)%mata_cds, info%omega, nu, grids(l)%v1d, grids(l)%f1d)
         END IF
      CASE ("gs")
         IF(ALLOCATED(grids(l)%mata)) THEN
            CALL gs(grids(l)%mata, nu, grids(l)%v1d, grids(l)%f1d)
         ELSE
            CALL gs(grids(l)%mata_cds, nu, grids(l)%v1d, grids(l)%f1d)
         END IF
      CASE default
         PRINT*, "relax ", info%relax, " NOT IMPLEMENTED!"
         STOP
      END SELECT
    END SUBROUTINE relax
  END SUBROUTINE mg_2d
!--------------------------------------------------------------------------------
  RECURSIVE SUBROUTINE mg_cyl(grids, info, l, nluniq_in)
!
!   Execute a recursive V-cycle
!
    TYPE(grid2d), INTENT(inout)   :: grids(:)
    TYPE(mg_info), INTENT(in)     :: info
    INTEGER, INTENT(in)           :: l
    LOGICAL, INTENT(in), OPTIONAL :: nluniq_in
!
    DOUBLE PRECISION, ALLOCATABLE, TARGET :: resid(:,:)
    DOUBLE PRECISION, POINTER             :: resid1d(:)
    INTEGER :: levels, k, m1, m2, r1, r2
    LOGICAL :: nluniq
!
    levels = info%levels
    m1 = SIZE(grids(l)%v,1)
    m2 = SIZE(grids(l)%v,2)
    r1 = grids(l)%rank(1)     ! r1 = m1
    r2 = grids(l)%rank(2)     ! r2 = m2-1
    IF(PRESENT(nluniq_in)) THEN
       nluniq = nluniq_in
    ELSE
       nluniq = .TRUE.
    END IF
!
    IF(l.EQ.levels) THEN
       grids(levels)%v1d = grids(levels)%f1d
       CALL direct_solve(grids(levels), grids(levels)%v1d, debug=.FALSE.)
    ELSE
       CALL relax(info%nu1)
       ALLOCATE(resid(m1,m2)); resid1d(1:r1*r2) => resid
       IF(ALLOCATED(grids(l)%mata)) THEN
          resid1d(:) = grids(l)%f1d(:) - vmx(grids(l)%mata, grids(l)%v1d)
       ELSE
          resid1d(:) = grids(l)%f1d(:) - vmx(grids(l)%mata_cds, grids(l)%v1d)
       END IF
!
       grids(l+1)%f(:,:) = restrict_cyl(grids(l+1), resid, nluniq)
!
       DEALLOCATE(resid)
       grids(l+1)%v1d = 0.0d0
!
!   Only 1 call to the coarsest level
       DO k=1,MERGE(info%mu, 1, (l+1).NE.levels) 
          CALL mg_cyl(grids, info, l+1, nluniq)
       END DO
!
       grids(l)%v(:,1:r2) = grids(l)%v(:,1:r2) + &
            &               prolong_cyl(grids(l+1),grids(l+1)%v, nluniq)
!
       CALL relax(info%nu2)
    END IF
!
  CONTAINS
    SUBROUTINE relax(nu)
      INTEGER, INTENT(in) :: nu
      SELECT CASE (TRIM(info%relax))
      CASE ("jac")
         IF(ALLOCATED(grids(l)%mata)) THEN
            CALL jacobi(grids(l)%mata, info%omega, nu, grids(l)%v1d, grids(l)%f1d)
         ELSE
            CALL jacobi(grids(l)%mata_cds, info%omega, nu, grids(l)%v1d, grids(l)%f1d)
         END IF
      CASE ("gs")
         IF(ALLOCATED(grids(l)%mata)) THEN
            CALL gs(grids(l)%mata, nu, grids(l)%v1d, grids(l)%f1d)
         ELSE
            CALL gs(grids(l)%mata_cds, nu, grids(l)%v1d, grids(l)%f1d)
        END IF
      CASE default
         PRINT*, "relax ", info%relax, " NOT IMPLEMENTED!"
         STOP
      END SELECT
    END SUBROUTINE relax
  END SUBROUTINE mg_cyl
!--------------------------------------------------------------------------------
  FUNCTION prolong_1d(matp,vcoarse) RESULT(vfine)
!
!   Prolongation
!
    TYPE(gemat), INTENT(in) :: matp
    DOUBLE PRECISION, INTENT(in) :: vcoarse(:)
    DOUBLE PRECISION             :: vfine(matp%mrows)
!
    vfine = vmx(matp,vcoarse)
  END FUNCTION prolong_1d
!--------------------------------------------------------------------------------
  FUNCTION restrict_1d(matp,vfine) RESULT(vcoarse)
!
!   Restriction
!
    TYPE(gemat), INTENT(in) :: matp
    DOUBLE PRECISION, INTENT(in) :: vfine(:)
    DOUBLE PRECISION             :: vcoarse(matp%ncols)
!
    vcoarse = vmx(matp,vfine,'T')
  END FUNCTION restrict_1d
!--------------------------------------------------------------------------------
  FUNCTION prolong_2d(matp,vcoarse) RESULT(vfine)
!
!   Prolongation
!
    TYPE(gemat), INTENT(in) :: matp(2)
    DOUBLE PRECISION, INTENT(in)  :: vcoarse(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: vfine(:,:)
!
    DOUBLE PRECISION, POINTER     :: pmat1(:,:), pmat2(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    DOUBLE PRECISION :: one=1.0d0, zero=0.0d0
    INTEGER :: m1, m1p, m2, m2p
!
    pmat1 => matp(1)%val
    pmat2 => matp(2)%val
    m1 = SIZE(pmat1,1); m1p = SIZE(pmat1,2)
    m2 = SIZE(pmat2,1); m2p = SIZE(pmat2,2)
    ALLOCATE(vfine(m1,m2))
    ALLOCATE(temp(m1,m2p))
!
!    Compute (P1) * V
!
    CALL dgemm('N', 'N', m1, m2p, m1p, one, pmat1, m1, vcoarse, m1p, zero, &
         &     temp, m1)
!
!    Compute (P1) * V * (P2)^T
!
    CALL dgemm('N', 'T', m1, m2, m2p, one, temp, m1, pmat2, m2, zero, &
         &     vfine, m1)
!
    DEALLOCATE(temp)
  END FUNCTION prolong_2d
!--------------------------------------------------------------------------------
  FUNCTION prolong_2d_csr(matp,vcoarse) RESULT(vfine)
!
!   Prolongation using CSR prolongation matrix
!
    TYPE(csr_mat), INTENT(in)     :: matp(2)
    DOUBLE PRECISION, INTENT(in)  :: vcoarse(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: vfine(:,:)
!
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    INTEGER :: m1, m1p, m2, m2p
    INTEGER :: i, j, k, kk
!
    m1 = matp(1)%mrows; m1p = matp(1)%ncols
    m2 = matp(2)%mrows; m2p = matp(2)%ncols
    ALLOCATE(vfine(m1,m2))
    ALLOCATE(temp(m1p,m2))
    temp = 0.0d0
    vfine = 0.0d0
!
!    Compute temp = V * (P2)^T
!     t_ij = sum_{k=1}^{m2p} V_ik (P2)_jk, i=1:m1p, j=1:m2
!
    DO j=1,m2
       DO kk=matp(2)%irow(j),matp(2)%irow(j+1)-1
          k = matp(2)%cols(kk)
          temp(:,j) = temp(:,j) + vcoarse(:,k)*matp(2)%val(kk)
       END DO
    END DO
!
!    Compute (P1) * V * (P2)^T
!     V_ij = sum_{k=1}^{m1p} (P1)_ik t_kj, i=1:m1, j=1:m2
!
    DO i=1,m1
       DO kk=matp(1)%irow(i),matp(1)%irow(i+1)-1
          k = matp(1)%cols(kk)
          vfine(i,:) = vfine(i,:) + matp(1)%val(kk)*temp(k,:)
       END DO
    END DO
!
    DEALLOCATE(temp)
  END FUNCTION prolong_2d_csr
!--------------------------------------------------------------------------------
  FUNCTION restrict_2d(matp,vfine) RESULT(vcoarse)
!
!   Restriction
!
    TYPE(gemat), INTENT(in) :: matp(2)
    DOUBLE PRECISION, INTENT(in)  :: vfine(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: vcoarse(:,:)
!
    DOUBLE PRECISION, POINTER     :: pmat1(:,:), pmat2(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    DOUBLE PRECISION :: one=1.0d0, zero=0.0d0
    INTEGER :: m1, m1p, m2, m2p
!
    pmat1 => matp(1)%val
    pmat2 => matp(2)%val
    m1 = SIZE(pmat1,1); m1p = SIZE(pmat1,2)
    m2 = SIZE(pmat2,1); m2p = SIZE(pmat2,2)
    ALLOCATE(vcoarse(m1p,m2p))
    ALLOCATE(temp(m1p,m2))
!
!    Compute (P1)^T * V
!
    CALL dgemm('T', 'N', m1p, m2, m1, one, pmat1, m1, vfine, m1, zero, &
         &     temp, m1p)
!
!    Compute (P1)^T * V * P2
!
    CALL dgemm('N', 'N', m1p, m2p, m2, one, temp, m1p, pmat2, m2, zero, &
         &     vcoarse, m1p)
!
    DEALLOCATE(temp)
  END FUNCTION restrict_2d
!--------------------------------------------------------------------------------
  FUNCTION restrict_2d_csr(matp,vfine) RESULT(vcoarse)
!
!   Restriction using CSR prolongation matrix
!
    TYPE(csr_mat), INTENT(in)     :: matp(2)
    DOUBLE PRECISION, INTENT(in)  :: vfine(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: vcoarse(:,:)
!
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    INTEGER :: m1, m1p, m2, m2p
    INTEGER :: i, ii, j, jj, k
!
    m1 = matp(1)%mrows; m1p = matp(1)%ncols
    m2 = matp(2)%mrows; m2p = matp(2)%ncols
    ALLOCATE(vcoarse(m1p,m2p))
    ALLOCATE(temp(m1,m2p))
    temp = 0.0d0
    vcoarse = 0.0d0
!
!    Compute temp = V * (R2)^T = V * (P2)
!     t_ij = sum_{k=1}^{m2} V_ik (P2)_kj, i=1:m1, j=1:m2p
!
    DO k=1,m2
       DO jj=matp(2)%irow(k),matp(2)%irow(k+1)-1
          j = matp(2)%cols(jj)
          temp(:,j) = temp(:,j) + vfine(:,k)*matp(2)%val(jj)
       END DO
    END DO
!
!    Compute (R1) * V * (R2)^T = (P1)^T) * V * (P2)
!     V_ij = sum_{k=1}^{m1p} (P1)_ki t_kj, i=1:m1p, j=1:m2p
!
    DO k=1,m1
       DO ii=matp(1)%irow(k),matp(1)%irow(k+1)-1
          i = matp(1)%cols(ii)
          vcoarse(i,:) = vcoarse(i,:) + matp(1)%val(ii)*temp(k,:)
       END DO
    END DO
!
    DEALLOCATE(temp)
  END FUNCTION restrict_2d_csr
!--------------------------------------------------------------------------------
  FUNCTION prolong_cyl(grid, vcoarse, nluniq) RESULT(vfine)
!
!   Prolongation (cylindrical case)
!
    TYPE(grid2d)                    :: grid
    DOUBLE PRECISION, INTENT(inout) :: vcoarse(:,:)
    DOUBLE PRECISION, ALLOCATABLE   :: vfine(:,:)
    LOGICAL, INTENT(in)             :: nluniq
!
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    INTEGER :: m1, m1p, m2, m2p
    INTEGER :: i, j, k, kk
!
    m1 = grid%matp(1)%mrows; m1p = grid%matp(1)%ncols
    m2 = grid%matp(2)%mrows; m2p = grid%matp(2)%ncols
    ALLOCATE(vfine(m1,m2))
    ALLOCATE(temp(m1p,m2))
    temp = 0.0d0
    vfine = 0.0d0
!
    IF(nluniq) vcoarse(1,1:m2p-1) = vcoarse(1,m2p)
!
!    Compute temp = V * (P2)^T
!     t_ij = sum_{k=1}^{m2p} V_ik (P2)_jk, i=1:m1p, j=1:m2
!
    DO j=1,m2
       DO kk=grid%matp(2)%irow(j),grid%matp(2)%irow(j+1)-1
          k = grid%matp(2)%cols(kk)
          temp(:,j) = temp(:,j) + vcoarse(:,k)*grid%matp(2)%val(kk)
       END DO
    END DO
!
!    Compute (P1) * V * (P2)^T
!     V_ij = sum_{k=1}^{m1p} (P1)_ik t_kj, i=1:m1, j=1:m2
!
    DO i=1,m1
       DO kk=grid%matp(1)%irow(i),grid%matp(1)%irow(i+1)-1
          k = grid%matp(1)%cols(kk)
          vfine(i,:) = vfine(i,:) + grid%matp(1)%val(kk)*temp(k,:)
       END DO
    END DO
!!$!
!!$!    Compute (P1) * V
!!$!
!!$    CALL dgemm('N', 'N', m1, m2p, m1p, one, pmat1, m1, vcoarse, m1p, zero, &
!!$         &     temp, m1)
!!$!
!!$!    Compute (P1) * V * (P2)^T
!!$!
!!$    CALL dgemm('N', 'T', m1, m2, m2p, one, temp, m1, pmat2, m2, zero, &
!!$         &     vfine, m1)
!
    IF(nluniq) THEN
       vcoarse(1,1:m2p-1) = vcoarse(1,1:m2p-1) - vcoarse(1,m2p)
       vfine(1,1:m2-1) =  vfine(1,1:m2-1) - vfine(1,m2)
    END IF
!
    DEALLOCATE(temp)
  END FUNCTION prolong_cyl
!--------------------------------------------------------------------------------
  FUNCTION restrict_cyl(grid, vfine, nluniq) RESULT(vcoarse)
!
!   Restriction (cylindrical case)
!
    TYPE(grid2d)                    :: grid
    DOUBLE PRECISION, INTENT(inout) :: vfine(:,:)
    DOUBLE PRECISION, ALLOCATABLE   :: vcoarse(:,:)
    LOGICAL, INTENT(in)             :: nluniq
!
    DOUBLE PRECISION, ALLOCATABLE :: temp(:,:)
    INTEGER :: m1, m1p, m2, m2p
    INTEGER :: i, ii, j, jj, k
!
    m1 = grid%matp(1)%mrows; m1p = grid%matp(1)%ncols
    m2 = grid%matp(2)%mrows; m2p = grid%matp(2)%ncols
    ALLOCATE(vcoarse(m1p,m2p))
    ALLOCATE(temp(m1,m2p))
    temp = 0.0d0
    vcoarse = 0.0d0
!
    IF(nluniq) vfine(1,1:m2) = vfine(1,m2)/REAL(m2,8)
!
!    Compute temp = V * (R2)^T = V * (P2)
!     t_ij = sum_{k=1}^{m2} V_ik (P2)_kj, i=1:m1, j=1:m2p
!
    DO k=1,m2
       DO jj=grid%matp(2)%irow(k),grid%matp(2)%irow(k+1)-1
          j = grid%matp(2)%cols(jj)
          temp(:,j) = temp(:,j) + vfine(:,k)*grid%matp(2)%val(jj)
       END DO
    END DO
!
!    Compute (R1) * V * (R2)^T = (P1)^T) * V * (P2)
!     V_ij = sum_{k=1}^{m1p} (P1)_ki t_kj, i=1:m1p, j=1:m2p
!
    DO k=1,m1
       DO ii=grid%matp(1)%irow(k),grid%matp(1)%irow(k+1)-1
          i = grid%matp(1)%cols(ii)
          vcoarse(i,:) = vcoarse(i,:) + grid%matp(1)%val(ii)*temp(k,:)
       END DO
    END DO
!
!!$!    Compute (P1)^T * V
!!$!
!!$    CALL dgemm('T', 'N', m1p, m2, m1, one, pmat1, m1, vfine, m1, zero, &
!!$         &     temp, m1p)
!!$!
!!$!    Compute (P1)^T * V * P2
!!$!
!!$    CALL dgemm('N', 'N', m1p, m2p, m2, one, temp, m1p, pmat2, m2, zero, &
!!$         &     vcoarse, m1p)
!
    IF(nluniq) THEN
       vfine(1,m2) = SUM(vfine(1,1:m2)); vfine(1,1:m2-1) = 0.0d0
       vcoarse(1,m2p) =  SUM(vcoarse(1,1:m2p)); vcoarse(1,m2p-1) = 0.0d0
    END IF
!
    DEALLOCATE(temp)
!
  END FUNCTION restrict_cyl
!--------------------------------------------------------------------------------
  SUBROUTINE massmat_ge(spl, alpha, matm)
!
!   Compute mass matrix
!
    TYPE(spline1d), INTENT(in)   :: spl
    INTEGER, INTENT(in)          :: alpha
    TYPE(gemat), INTENT(out)     :: matm
!
    INTEGER :: nrank, nx, nidbas, kl, ku
!
    CALL get_dim(spl, nrank, nx, nidbas)
    kl=nidbas; ku=kl
    IF(spl%period) nrank = nx
    CALL init(nrank, 1, matm)
    CALL conmat(spl, matm, coefeq)
  CONTAINS
    SUBROUTINE coefeq(x, idt, idw, c)
      DOUBLE PRECISION, INTENT(in) :: x
      INTEGER, INTENT(out) :: idt(:), idw(:)
      DOUBLE PRECISION, INTENT(out) :: c(:)
      c(1) = x**alpha
      idt(1) = 0
      idw(1) = 0
    END SUBROUTINE coefeq
  END SUBROUTINE massmat_ge
!--------------------------------------------------------------------------------
  SUBROUTINE massmat_gb(spl, alpha, matm)
!
!   Compute mass matrix
!
    TYPE(spline1d), INTENT(in)   :: spl
    INTEGER, INTENT(in)          :: alpha
    TYPE(gbmat), INTENT(out)     :: matm
!
    INTEGER :: nrank, nx, nidbas, kl, ku
!
    CALL get_dim(spl, nrank, nx, nidbas)
    kl=nidbas; ku=kl
    CALL init(kl, ku, nrank, 1, matm)
    CALL conmat(spl, matm, coefeq)
  CONTAINS
    SUBROUTINE coefeq(x, idt, idw, c)
      DOUBLE PRECISION, INTENT(in) :: x
      INTEGER, INTENT(out) :: idt(:), idw(:)
      DOUBLE PRECISION, INTENT(out) :: c(:)
      c(1) = x**alpha
      idt(1) = 0
      idw(1) = 0
    END SUBROUTINE coefeq
  END SUBROUTINE massmat_gb
!--------------------------------------------------------------------------------
  SUBROUTINE massmat_periodic(spl, alpha, matm)
!
!   Compute mass matrix (periodic case)
!
    TYPE(spline1d), INTENT(in)      :: spl
    INTEGER, INTENT(in)             :: alpha
    TYPE(periodic_mat), INTENT(out) :: matm
!
    INTEGER :: dim, nrank, nx, nidbas, kl, ku
!
    CALL get_dim(spl, dim, nx, nidbas)
    kl=nidbas; ku=kl
    nrank = nx
    CALL init(kl, ku, nrank, 1, matm)
    CALL conmat(spl, matm, coefeq)
  CONTAINS
    SUBROUTINE coefeq(x, idt, idw, c)
      DOUBLE PRECISION, INTENT(in) :: x
      INTEGER, INTENT(out) :: idt(:), idw(:)
      DOUBLE PRECISION, INTENT(out) :: c(:)
      c(1) = x**alpha
      idt(1) = 0
      idw(1) = 0
    END SUBROUTINE coefeq
  END SUBROUTINE massmat_periodic
!--------------------------------------------------------------------------------
  SUBROUTINE femat_2d_csr(spl, mat, coefeq, nterms, maxder_in, nat_order_in, &
       &                  noinit)
!
!   Compute 2d fe CSR matrix
!
    TYPE(spline2d), INTENT(in)    :: spl
    TYPE(csr_mat), INTENT(inout)  :: mat
    INTEGER, INTENT(in)           :: nterms
    INTEGER, INTENT(in), OPTIONAL :: maxder_in(2)
    LOGICAL, INTENT(in), OPTIONAL :: nat_order_in
    LOGICAL, INTENT(in), OPTIONAL :: noinit
    INTERFACE
       SUBROUTINE coefeq(x, y, idt, idw, c)
         DOUBLE PRECISION, INTENT(in) :: x, y
         INTEGER, INTENT(out) :: idt(:,:), idw(:,:)
         DOUBLE PRECISION, INTENT(out) :: c(:)
       END SUBROUTINE coefeq
    END INTERFACE
!
    INTEGER :: nrank, ndim(2), nints(2), maxder(2)
    LOGICAL :: nat_order, run_init
!
    CALL get_dim(spl, ndim, nints)
    IF(spl%sp2%period) THEN
       nrank = ndim(1)*nints(2)
    ELSE
       nrank = PRODUCT(ndim)
    END IF
!
    maxder = 1;         IF(PRESENT(maxder_in)) maxder = maxder_in
    nat_order = .TRUE.; IF(PRESENT(nat_order_in)) nat_order = nat_order_in
!
    run_init = .TRUE.
    IF(PRESENT(noinit)) run_init = .NOT.noinit
    IF(run_init)       CALL init(nrank, nterms, mat)
!
    CALL conmat(spl, mat, coefeq, maxder, nat_order)
  END SUBROUTINE femat_2d_csr
!--------------------------------------------------------------------------------
  SUBROUTINE femat_ge(spl, mat, coefeq)
!
!   Compute fe matrix
!
    TYPE(spline1d), INTENT(in)   :: spl
    TYPE(gemat), INTENT(out)     :: mat
    INTERFACE
       SUBROUTINE coefeq(x, idt, idw, c)
         DOUBLE PRECISION, INTENT(in) :: x
         INTEGER, INTENT(out) :: idt(:), idw(:)
         DOUBLE PRECISION, INTENT(out) :: c(:)
       END SUBROUTINE coefeq
    END INTERFACE
!
    INTEGER :: nrank, nx, nidbas, kl, ku
!
    CALL get_dim(spl, nrank, nx, nidbas)
    kl=nidbas; ku=kl
    IF(spl%period) nrank = nx
    CALL init(nrank, 2, mat)
    CALL conmat(spl, mat, coefeq)
  END SUBROUTINE femat_ge
!--------------------------------------------------------------------------------
  SUBROUTINE femat_gb(spl, mat, coefeq)
!
!   Compute fe matrix
!
    TYPE(spline1d), INTENT(in)   :: spl
    TYPE(gbmat), INTENT(out)     :: mat
    INTERFACE
       SUBROUTINE coefeq(x, idt, idw, c)
         DOUBLE PRECISION, INTENT(in) :: x
         INTEGER, INTENT(out) :: idt(:), idw(:)
         DOUBLE PRECISION, INTENT(out) :: c(:)
       END SUBROUTINE coefeq
    END INTERFACE
!
    INTEGER :: nrank, nx, nidbas, kl, ku
!
    CALL get_dim(spl, nrank, nx, nidbas)
    kl=nidbas; ku=kl
    CALL init(kl, ku, nrank, 2, mat)
    CALL conmat(spl, mat, coefeq)
  END SUBROUTINE femat_gb
!--------------------------------------------------------------------------------
  SUBROUTINE femat_periodic(spl, mat, coefeq)
!
!   Compute fe matrix
!
    TYPE(spline1d), INTENT(in)      :: spl
    TYPE(periodic_mat), INTENT(out) :: mat
    INTERFACE
       SUBROUTINE coefeq(x, idt, idw, c)
         DOUBLE PRECISION, INTENT(in) :: x
         INTEGER, INTENT(out) :: idt(:), idw(:)
         DOUBLE PRECISION, INTENT(out) :: c(:)
       END SUBROUTINE coefeq
    END INTERFACE
!
    INTEGER :: nrank, dim, nx, nidbas, kl, ku
!
    CALL get_dim(spl, dim, nx, nidbas)
    kl=nidbas; ku=kl
    nrank = nx
    CALL init(kl, ku, nrank, 2, mat)
    CALL conmat(spl, mat, coefeq)
  END SUBROUTINE femat_periodic
!--------------------------------------------------------------------------------
  SUBROUTINE ibcmat_1d(irow, mat)
!
!   Set BC at row irow to 0
!
    INTEGER, INTENT(in)        :: irow
    TYPE(gbmat), INTENT(inout) :: mat
!
    DOUBLE PRECISION :: a(mat%rank)
!
    a(:)=0.0d0; a(irow)=1.0d0
    CALL putrow(mat, irow, a)
    CALL putcol(mat, irow, a)
  END SUBROUTINE ibcmat_1d
!--------------------------------------------------------------------------------
 SUBROUTINE ibcmat_2d(grid, mat, nluniq_in)
!
!   Impose BC on matrix (asume natural ordering)
!      I = (j-1)*N1 + i, i=1:N1, j=1:N2
!
    TYPE(grid2d), INTENT(in)      :: grid
    TYPE(csr_mat), INTENT(inout)  :: mat
    LOGICAL, INTENT(in), OPTIONAL :: nluniq_in
!
    DOUBLE PRECISION :: temp(mat%rank)
    INTEGER :: n1e, n2e, nrank, i, j, irow, jcol
    LOGICAL :: nlper1, nlper2, nlcart, nluniq
!
    n1e = grid%rank(1)
    n2e = grid%rank(2)
    nrank = mat%rank
    nlper1 = grid%spl%sp1%period
    nlper2 = grid%spl%sp2%period
    nlcart = (.NOT.nlper1) .AND. (.NOT.nlper2)
    IF(PRESENT(nluniq_in)) THEN
       nluniq = nluniq_in
    ELSE
       nluniq = .TRUE.
    END IF
!
! BC at x=0   ! Dirichlet for Cartesian, unicity for cylindrical problem
    IF(nlcart) THEN
       i=1
       DO j=1,n2e
          irow = (j-1)*n1e + i
          temp = 0.0d0; temp(irow) = 1.0d0
          CALL putrow(mat, irow, temp)
          CALL putcol(mat, irow, temp)
       END DO
    ELSE
       i=1
       IF(nluniq) THEN
          CALL unicity
       END IF
    END IF
!
! BC at x=1  ! For both Cartesian and cylindrical
    i=n1e
    DO j=1,n2e
       irow = (j-1)*n1e + i
       temp = 0.0d0; temp(irow) = 1.0d0
       CALL putrow(mat, irow, temp)
       CALL putcol(mat, irow, temp)
    END DO
!
! BC at y=0   ! Only for Cartesian problem
    IF(nlcart) THEN
       j=1
       DO i=1,n1e
          irow = (j-1)*n1e + i
          temp = 0.0d0; temp(irow) = 1.0d0
          CALL putrow(mat, irow, temp)
          CALL putcol(mat, irow, temp)
       END DO
    END IF
!
! BC at y=1   ! Only for Cartesian problem
    IF(nlcart) THEN
       j=n2e
       DO i=1,n1e
          irow = (j-1)*n1e + i
          temp = 0.0d0; temp(irow) = 1.0d0
          CALL putrow(mat, irow, temp)
          CALL putcol(mat, irow, temp)
       END DO
    END IF
!
  CONTAINS
    SUBROUTINE unicity
      INTEGER          :: irow0, jcol0
      DOUBLE PRECISION :: temp_sum(mat%rank) 
!
      irow0 = (n2e-1)*n1e + i
      jcol0 = irow0
!
!   Vertical sum
      temp_sum(:) = 0.0d0
      DO j=1,n2e
         irow = (j-1)*n1e + i
         temp = 0.0d0
         CALL getrow(mat, irow, temp)
         temp_sum(:) = temp_sum(:) + temp(:)
      END DO
      CALL putrow(mat, irow0, temp_sum)
!
!  Horizontal sum
      temp_sum(:) = 0.0d0
      DO j=1,n2e
         jcol = (j-1)*n1e + i
         temp = 0.0d0
         CALL getcol(mat, jcol, temp)
         temp_sum(:) = temp_sum(:) + temp(:)
      END DO
      CALL putcol(mat, jcol0, temp_sum)
!
!  The away operator
      DO j=1,n2e-1
         irow = (j-1)*n1e + i
         temp = 0.0d0; temp(irow) = 1.0d0
         CALL putrow(mat, irow, temp)
         CALL putcol(mat, irow, temp)
      END DO
    END SUBROUTINE unicity
  END SUBROUTINE ibcmat_2d
!--------------------------------------------------------------------------------
  SUBROUTINE mod_transf_full(mat,k)
!
!    Modify grid transfer matrix.
!
    DOUBLE PRECISION, INTENT(inout) :: mat(:,:)
    INTEGER, INTENT(in) :: k
    INTEGER :: m, n
!
    m=SIZE(mat,1)
    n=SIZE(mat,2)
!
! Clear matrix small elements.
    WHERE( ABS(mat) < 1.d-8) mat=0.0d0
!
! Left boundary
    IF(k.EQ.1 .OR. k.EQ.3) THEN
       mat(2:m,1) = 0.0d0
    END IF
!
! Right boundary
    IF(k.EQ.2 .OR. k.EQ.3) THEN
       mat(1:m-1,n) = 0.0d0
    END IF
  END SUBROUTINE mod_transf_full
!--------------------------------------------------------------------------------
  SUBROUTINE mod_transf_csr(mat,k)
!
!    Modify grid transfer matrix.
!
    TYPE(csr_mat), INTENT(inout) :: mat
    INTEGER, INTENT(in)          :: k
!
    DOUBLE PRECISION :: acol(mat%mrows)
    INTEGER :: m, n
!
    m=mat%mrows
    n=mat%ncols
!
! Left boundary
    acol = 0.0d0
    IF(k.EQ.1 .OR. k.EQ.3) THEN
       CALL getele(mat, 1, 1, acol(1))
       CALL putcol(mat, 1, acol)
    END IF
!
! Right boundary
    acol = 0.0d0
    IF(k.EQ.2 .OR. k.EQ.3) THEN
       CALL getele(mat, m, n, acol(m))
       CALL putcol(mat, n, acol)
    END IF
  END SUBROUTINE mod_transf_csr
!--------------------------------------------------------------------------------
  SUBROUTINE calc_pmat(grid1, grid2, pmat, debug_in)
!
!   Compute prolongation matrix by collocation
!
    TYPE(grid1d), INTENT(in)      :: grid1, grid2
    TYPE(gemat), INTENT(out)      :: pmat
    LOGICAL, OPTIONAL, INTENT(in) :: debug_in
!
    TYPE(gemat) :: mat_interp
    DOUBLE PRECISION, ALLOCATABLE :: fun(:,:), fun2(:,:)
    DOUBLE PRECISION :: xinter
    INTEGER, ALLOCATABLE :: jcol(:)
    INTEGER :: nidbas, nfine, ncoarse, nderv
    LOGICAL :: nlper, debug
    INTEGER :: i, i0, ii, k, irow
!================================================================================
!                       0. Prologue
!
    debug = .FALSE.
    IF(PRESENT(debug_in)) debug = debug_in
!
    nfine = grid1%n
    ncoarse = grid2%n
    nidbas = grid1%spl%order - 1
    nlper = grid1%spl%period
!
    IF(nlper) THEN
       IF(ncoarse .LT. nidbas+1) THEN
          WRITE(*,'(/a)') '** NX/2 should be larger or equal to NIDBAS+1 **'
          STOP
       END IF
   END IF
!
    IF(debug) THEN
       WRITE(*,'(/2(a,i0))') 'nfine = ', nfine, ', ncoarse = ', ncoarse
       IF(nlper) WRITE(*,'(a)') 'Grids are periodic!'
    END IF
!
    ALLOCATE(jcol(0:nidbas))
    ALLOCATE(fun(0:nidbas,1))
    IF(nlper) THEN
       CALL init(ncoarse, 1, pmat, mrows=nfine)
       CALL init(nfine, 1, mat_interp)
    ELSE
       CALL init(ncoarse+nidbas, 1, pmat, mrows=nfine+nidbas)
       CALL init(nfine+nidbas, 1, mat_interp)
    END IF
!================================================================================
!                       1. Interpolation matrix
!
    irow = 0
    i0 = 1
!
!    Left bound
    IF(.NOT.nlper) THEN 
       IF(MODULO(nidbas,2).EQ.1) THEN
          nderv = (nidbas-1)/2   ! ndidbas = 1, 3, 5, ...
          ALLOCATE(fun2(0:nidbas,nderv+1))
          CALL basfun(grid1%x(0), grid1%spl, fun2, 1)
          jcol = 1 + (/ (i, i=0,nidbas) /)
          DO k=1,nderv+1
             irow = irow+1
             mat_interp%val(irow,jcol) = fun2(0:nidbas,k)
          END DO
          i0 = 2  ! Skip the first grid point
       ELSE
          nderv = nidbas/2-1   ! ndidbas = 2, 4, ...
          ALLOCATE(fun2(0:nidbas,nderv+1))
          CALL basfun(grid1%x(0), grid1%spl, fun2, 1)
          jcol = 1 + (/ (i, i=0,nidbas) /)
          DO k=1,nderv+1
             irow = irow+1
             mat_interp%val(irow,jcol) = fun2(0:nidbas,k)
          END DO
       END IF
    END IF
    DO i=i0,nfine
       IF(MODULO(nidbas,2).EQ.0) THEN
          xinter = (grid1%x(i-1)+grid1%x(i))/2.0d0  ! Left bound of interval
       ELSE
          xinter = grid1%x(i-1)                     ! Left bound of interval
       END IF
       CALL basfun(xinter, grid1%spl, fun, i)
       irow = irow+1
       DO k=0,nidbas
          jcol(k) = i+k
       END DO
       IF(nlper) jcol = MODULO(jcol-1,nfine)+1
       mat_interp%val(irow,jcol) = fun(0:nidbas,1)
    END DO
!
!   Right bound
    IF(.NOT.nlper) THEN
       CALL basfun(grid1%x(nfine), grid1%spl, fun2, nfine)
       jcol = nfine + (/ (i, i=0,nidbas) /)
       DO k=nderv+1,1,-1
          irow = irow+1
          mat_interp%val(irow,jcol) = fun2(0:nidbas,k)
       END DO
    END IF
    IF(debug) CALL printmat('** Interpolation matrix **', mat_interp)
!================================================================================
!                       2. RHS matrix
!
    irow = 0
    i0 = 1
    DO i=1,ncoarse
       ii = 2*i-1
       CALL comp_rhs(ii)
       CALL comp_rhs(ii+1)
    END DO
    IF(debug) CALL printmat('** RHS matrix **', pmat)
!================================================================================
!                       3. Compute prolongation matrix
!
    CALL factor(mat_interp)
    CALL bsolve(mat_interp, pmat%val)
!================================================================================
!                       9. Epilogue
!
    CALL destroy(mat_interp)
    DEALLOCATE(jcol)
    DEALLOCATE(fun)
    IF(ALLOCATED(fun2)) DEALLOCATE(fun2)
!
  CONTAINS
    SUBROUTINE comp_rhs(ii)
      INTEGER, INTENT(in) :: ii
      INTEGER :: k
!
!    Left  bounds for non-periodic cases
      IF(.NOT.nlper .AND. ii.EQ.1) THEN
         CALL basfun(grid1%x(0), grid2%spl, fun2, 1)
         jcol = 1 + (/ (k, k=0,nidbas) /)
         DO k=1,nderv+1
            irow = irow+1
            pmat%val(irow,jcol) = fun2(0:nidbas,k)
         END DO
         IF(MODULO(nidbas,2).EQ.1) RETURN   ! Skip
      END IF
!
      IF(MODULO(nidbas,2).EQ.0) THEN
         xinter = (grid1%x(ii-1)+grid1%x(ii))/2.0d0 ! Left bound of interval
      ELSE
         xinter = grid1%x(ii-1)                     ! Left bound of interval
      END IF
      CALL basfun(xinter, grid2%spl, fun, i)
      irow = irow+1
      DO k=0,nidbas
         jcol(k) = i+k
      END DO
      IF(nlper) jcol = MODULO(jcol-1,ncoarse)+1
      pmat%val(irow,jcol) = fun(0:nidbas,1)
!
!    Right  bounds for non-periodic cases
      IF(.NOT.nlper .AND. ii.EQ.nfine) THEN
         CALL basfun(grid1%x(nfine), grid2%spl, fun2, ncoarse)
         jcol = ncoarse + (/ (k, k=0,nidbas) /)
         DO k=nderv+1,1,-1
            irow = irow+1
            pmat%val(irow,jcol) = fun2(0:nidbas,k)
         END DO
      END IF
    END SUBROUTINE comp_rhs
  END SUBROUTINE calc_pmat
!--------------------------------------------------------------------------------
  SUBROUTINE disrhs_1d(spl, farr, frhs)
!
!  Projection of RHS on spline basis functions
!
    TYPE(spline1d)                :: spl
    DOUBLE PRECISION, INTENT(out) :: farr(:)
    INTERFACE
       DOUBLE PRECISION FUNCTION frhs(x)
         DOUBLE PRECISION, INTENT(in) :: x
       END FUNCTION frhs
    END INTERFACE
    DOUBLE PRECISION :: contrib
!
    DOUBLE PRECISION, POINTER :: xg(:,:), wg(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: fun(:,:)
    INTEGER :: ndim, n, nidbas, ng
    INTEGER :: i, ig, it, irow
    LOGICAL :: nlper
!
    CALL get_dim(spl, ndim, n, nidbas)
    nlper = spl%period
    xg => spl%gausx ! xg(ng,n)
    wg => spl%gausw ! wg(ng,n)
    ng = SIZE(xg,1)
    ALLOCATE(fun(0:nidbas,1))
!
    farr = 0.0d0
    DO i=1,n
       DO ig=1,ng
          CALL basfun(xg(ig,i), spl, fun, i)
          contrib = wg(ig,i)*frhs(xg(ig,i))
          DO it=0,nidbas
             irow = i+it
             IF(nlper) irow = MODULO(irow-1,n) +1
             farr(irow) = farr(irow)+contrib*fun(it,1)
          END DO
       END DO
    END DO
!
    DEALLOCATE(fun)
  END SUBROUTINE disrhs_1d
!--------------------------------------------------------------------------------
  SUBROUTINE disrhs_2d(spl, farr, frhs)
!
!  Projection of RHS on spline basis functions
!
    TYPE(spline2d)                :: spl
    DOUBLE PRECISION, INTENT(out) :: farr(:,:)
    INTERFACE
       DOUBLE PRECISION FUNCTION frhs(x,y)
         DOUBLE PRECISION, INTENT(in) :: x,y
       END FUNCTION frhs
    END INTERFACE
    DOUBLE PRECISION :: contrib
!
    DOUBLE PRECISION, POINTER :: xg1(:,:), wg1(:,:)
    DOUBLE PRECISION, POINTER :: xg2(:,:), wg2(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: fun1(:,:)
    DOUBLE PRECISION, ALLOCATABLE :: fun2(:,:)
    INTEGER :: ndim1, n1, nidbas1, ng1
    INTEGER :: ndim2, n2, nidbas2, ng2
    INTEGER :: i1, ig1, it1, irow
    INTEGER :: i2, ig2, it2, jcol
!
    CALL get_dim(spl%sp1, ndim1, n1, nidbas1)
    CALL get_dim(spl%sp2, ndim2, n2, nidbas2)
!
    xg1 => spl%sp1%gausx ! xg(ng,n)
    wg1 => spl%sp1%gausw ! wg(ng,n)
    ng1 = SIZE(xg1,1)
    xg2 => spl%sp2%gausx ! xg(ng,n)
    wg2 => spl%sp2%gausw ! wg(ng,n)
    ng2 = SIZE(xg2,1)
!
    ALLOCATE(fun1(0:nidbas1,1))
    ALLOCATE(fun2(0:nidbas2,1))
!
    farr = 0.0d0
    DO i1=1,n1
       DO ig1=1,ng1
          CALL basfun(xg1(ig1,i1), spl%sp1, fun1, i1)
          DO i2=1,n2
             DO ig2=1,ng2
                CALL basfun(xg2(ig2,i2), spl%sp2, fun2, i2)
                contrib = wg1(ig1,i1)*wg2(ig2,i2)* &
                     & frhs(xg1(ig1,i1), xg2(ig2,i2))
                DO it1=0,nidbas1
                   irow = i1+it1
                   DO it2=0,nidbas2
                      jcol = i2+it2
                      farr(irow,jcol) = farr(irow,jcol) + &
                           &   contrib*fun1(it1,1)*fun2(it2,1)
                   END DO
                END DO
             END DO
          END DO
       END DO
    END DO
!
!  Case of periodic BC (only in 2nd dimension!)
!
    IF(spl%sp2%period) THEN
       DO jcol=1,nidbas2
          farr(:,jcol) = farr(:,jcol)+farr(:,jcol+n2)
          farr(:,jcol+n2) = 0.0d0
       END DO
    END IF
    DEALLOCATE(fun1)
    DEALLOCATE(fun2)
  END SUBROUTINE disrhs_2d
!--------------------------------------------------------------------------------
  SUBROUTINE ibcrhs(grid, f, nluniq_in)
!
! Impose BC on RHS
!
    TYPE(grid2d)                    :: grid
    DOUBLE PRECISION, INTENT(inout) :: f(:,:)
    LOGICAL, INTENT(in), OPTIONAL   :: nluniq_in
!
    DOUBLE PRECISION :: temp
    INTEGER :: n1, n2
    LOGICAL :: nlper1, nlper2, nlcyl, nluniq
!
    n1 = grid%rank(1)
    n2 = grid%rank(2)
    nlper1 = grid%spl%sp1%period
    nlper2 = grid%spl%sp2%period
    nlcyl = (.NOT.nlper1) .AND. (nlper2)
    IF(PRESENT(nluniq_in)) THEN 
       nluniq = nluniq_in
    ELSE
       nluniq=.TRUE.
    END IF
!
!  Cylindrical case, unicity at the axis, 0 at the right side
    IF(nlcyl) THEN  ! 
       IF(nluniq) THEN
          temp = SUM(f(1,1:n2))
          f(1,n2) = temp
          f(1,1:n2-1) = 0.0d0
       END IF
       f(n1,1:n2) = 0.0d0
!
!  Cartesian case: 0 on all 4 boundaries
    ELSE
       f(1,:) = 0.0d0; f(n1,:) = 0.0d0
       f(:,1) = 0.0d0; f(:,n2) = 0.0d0
    END IF
  END SUBROUTINE ibcrhs
!--------------------------------------------------------------------------------
  FUNCTION disc_err_1d(spl, f, fexact) RESULT(disc_err)
!
!  L2 norm of discretization error
!
    TYPE(spline1d)               :: spl
    DOUBLE PRECISION, INTENT(in) :: f(:)
    DOUBLE PRECISION             :: disc_err
    INTERFACE
       FUNCTION fexact(x)
         DOUBLE PRECISION, INTENT(in) :: x(:)
         DOUBLE PRECISION :: fexact(SIZE(x))
       END FUNCTION fexact
    END INTERFACE
!
    DOUBLE PRECISION, ALLOCATABLE :: err(:,:)
    DOUBLE PRECISION, POINTER :: xg(:,:), wg(:,:)
    INTEGER :: ndim, n, nidbas, ng
    INTEGER :: ig
!
    CALL get_dim(spl, ndim, n, nidbas)
    xg => spl%gausx ! xg(ng,n)
    wg => spl%gausw ! wg(ng,n)
    ng = SIZE(xg,1)
!
    ALLOCATE(err(ng,n))
    CALL gridval(spl, xg(1,:), err(1,:), 0, f)
    err(1,:) = (err(1,:) - fexact(xg(1,:)))**2
    DO ig=2,ng
       CALL gridval(spl, xg(ig,:), err(ig,:), 0)
       err(ig,:) = (err(ig,:) - fexact(xg(ig,:)))**2
    END DO
!
    disc_err = SQRT(SUM(wg*err))
!
    DEALLOCATE(err)
  END FUNCTION disc_err_1d
!--------------------------------------------------------------------------------
  FUNCTION disc_err_2d(spl, f, fexact) RESULT(disc_err)
!
!  L2 norm of discretization error
!
    TYPE(spline2d)               :: spl
    DOUBLE PRECISION, INTENT(in) :: f(:,:)
    DOUBLE PRECISION             :: disc_err
    INTERFACE
       FUNCTION fexact(x,y)
         DOUBLE PRECISION, INTENT(in) :: x(:), y(:)
         DOUBLE PRECISION :: fexact(SIZE(x),SIZE(y))
       END FUNCTION fexact
    END INTERFACE
!
    DOUBLE PRECISION, ALLOCATABLE :: err(:,:)
    DOUBLE PRECISION, POINTER :: xg(:,:), wg1(:,:), yg(:,:), wg2(:,:)
    INTEGER, DIMENSION(2) :: ndim, n, ng
    INTEGER :: i, j, ig, jg
    LOGICAL :: nlper1, nlper2, nlcyl
!
    CALL get_dim(spl, ndim, n)
    xg => spl%sp1%gausx ! xg(ng,n)
    wg1 => spl%sp1%gausw ! wg(ng,n)
    ng(1) = SIZE(xg,1)
    yg => spl%sp2%gausx ! xg(ng,n)
    wg2 => spl%sp2%gausw ! wg(ng,n)
    ng(2) = SIZE(yg,1)
!
    nlper1 = spl%sp1%period
    nlper2 = spl%sp2%period
    nlcyl = (.NOT.nlper1) .AND. (nlper2)
!
    disc_err = 0.0d0
    ALLOCATE(err(n(1),n(2)))
    DO ig=1,ng(1)
       DO jg=1,ng(2)
          IF(ig.EQ.1.AND.jg.EQ.1) THEN
             CALL gridval(spl, xg(ig,:), yg(jg,:), err, [0,0], f)
          ELSE
             CALL gridval(spl, xg(ig,:), yg(jg,:), err, [0,0])
          END IF
          err = (err - fexact(xg(ig,:), yg(jg,:)))**2
          DO i=1,n(1)
             DO j=1,n(2)
                IF(nlcyl) THEN
                   disc_err = disc_err + xg(ig,i)*wg1(ig,i)*wg2(jg,j)*err(i,j)
                ELSE
                   disc_err = disc_err + wg1(ig,i)*wg2(jg,j)*err(i,j)
                END IF
             END DO
          END DO
       END DO
    END DO
    disc_err = SQRT(disc_err)
!
    DEALLOCATE(err)
  END FUNCTION disc_err_2d
!--------------------------------------------------------------------------------
  SUBROUTINE back_transf(grid, u, nluniq_in)
!
!   Back transform solution and use  periodicity (cylindrical problem)
!
    TYPE(grid2d), INTENT(in)        :: grid
    DOUBLE PRECISION, INTENT(inout) :: u(:,:)
    LOGICAL, INTENT(in), OPTIONAL   :: nluniq_in
!
    LOGICAL :: nluniq
    INTEGER :: n, nidbas, j
!
    n = grid%n(2)
    nidbas = grid%spl%sp2%order-1
    IF(PRESENT(nluniq_in)) THEN
       nluniq = nluniq_in
    ELSE
       nluniq = .TRUE.
    END IF
!
!  Back transform
    IF(nluniq) THEN
       u(1,1:n-1) = u(1,n)
    END IF
!
!  Periodicity
    DO j=1,nidbas
       u(:,j+n) = u(:,j)
    END DO
  END SUBROUTINE back_transf
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION normf_gb(matm, f)
!
!    L2 norm of f represented by its expansion coefficients.
!
    TYPE(gbmat), INTENT(in)      :: matm
    DOUBLE PRECISION, INTENT(in) :: f(:)
    normf_gb = SQRT(DOT_PRODUCT(f, vmx(matm,f)))
  END FUNCTION normf_gb
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION normf_ge(matm, f)
!
!    L2 norm of f represented by its expansion coefficients.
!
    TYPE(gemat), INTENT(in)      :: matm
    DOUBLE PRECISION, INTENT(in) :: f(:)
    normf_ge = SQRT(DOT_PRODUCT(f, vmx(matm,f)))
  END FUNCTION normf_ge
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION residue_gen(grid, f, u, p)
!
!   Generic version of residue
!
    TYPE(grid2d) :: grid
    DOUBLE PRECISION, INTENT(in) :: f(:), u(:)
    DOUBLE PRECISION :: r(SIZE(f))
    CHARACTER(len=*), OPTIONAL, INTENT(in) :: p
!
    IF(ALLOCATED(grid%mata)) THEN
       residue_gen = residue_csr(grid%mata, f, u, p)
    ELSE
       residue_gen = residue_cds(grid%mata_cds, f, u, p)
    END IF
  END FUNCTION residue_gen
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION residue_csr(mat, f, u, p)
!
!    L2 norm of residue ||f-Av||
!
    TYPE(csr_mat), INTENT(in)    :: mat
    DOUBLE PRECISION, INTENT(in) :: f(:), u(:)
    DOUBLE PRECISION :: r(SIZE(f))
    CHARACTER(len=*), OPTIONAL, INTENT(in) :: p
!
    CHARACTER(len=4) :: norm_type
    norm_type = '2'
    IF(PRESENT(p)) norm_type = p
!
    r = f-vmx(mat,u)
    SELECT CASE (norm_type)
    CASE('1') 
       residue_csr = SUM(ABS(r))
    CASE ('2')
       residue_csr = SQRT(DOT_PRODUCT(r,r))
    CASE ('inf')
       residue_csr = MAXVAL(ABS(r))
    END SELECT
  END FUNCTION residue_csr
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION residue_cds(mat, f, u, p)
!
!    L2 norm of residue ||f-Av||
!
    TYPE(cds_mat), INTENT(in)    :: mat
    DOUBLE PRECISION, INTENT(in) :: f(:), u(:)
    DOUBLE PRECISION :: r(SIZE(f))
    CHARACTER(len=*), OPTIONAL, INTENT(in) :: p
!
    CHARACTER(len=4) :: norm_type
    norm_type = '2'
    IF(PRESENT(p)) norm_type = p
!
    r = f-vmx(mat,u)
    SELECT CASE (norm_type)
    CASE('1') 
       residue_cds = SUM(ABS(r))
    CASE ('2')
       residue_cds = SQRT(DOT_PRODUCT(r,r))
    CASE ('inf')
       residue_cds = MAXVAL(ABS(r))
    END SELECT
  END FUNCTION residue_cds
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION residue_ge(mat, f, u)
!
!    L2 norm of residue ||f-Av||
!
    TYPE(gemat), INTENT(in)      :: mat
    DOUBLE PRECISION, INTENT(in) :: f(:), u(:)
    DOUBLE PRECISION :: r(SIZE(f))
    r = f-vmx(mat,u)
    residue_ge = SQRT(DOT_PRODUCT(r,r))
  END FUNCTION residue_ge
!--------------------------------------------------------------------------------
  DOUBLE PRECISION FUNCTION residue_gb(mat, f, u)
!
!    L2 norm of residue ||f-Av||
!
    TYPE(gbmat), INTENT(in)      :: mat
    DOUBLE PRECISION, INTENT(in) :: f(:), u(:)
    DOUBLE PRECISION :: r(SIZE(f))
    r = f-vmx(mat,u)
    residue_gb = SQRT(DOT_PRODUCT(r,r))
  END FUNCTION residue_gb
!--------------------------------------------------------------------------------
  SUBROUTINE ctof_massmat(splf, splc, alpha, matm)
!
!   Compute coarse to fine mass matrix M(h,2h)
!
    TYPE(spline1d), INTENT(in)   :: splf ! Spline on fine mesh
    TYPE(spline1d), INTENT(in)   :: splc ! Spline on coarse mesh
    INTEGER, INTENT(in)          :: alpha
    TYPE(gemat), INTENT(out)     :: matm
!
    LOGICAL :: nlper
    INTEGER :: nf, nxf, nc, nxc, nidbas, kl, ku
    INTEGER :: ig, ngauss
    INTEGER :: i, ic, it, jw, irow, jcol
!
    DOUBLE PRECISION              :: contrib
    DOUBLE PRECISION, ALLOCATABLE :: xg(:), wg(:)
    DOUBLE PRECISION, ALLOCATABLE :: funf(:,:), func(:,:)
!
    nlper = splf%period .OR. splc%period
    CALL get_dim(splf, nf, nxf, nidbas)
    CALL get_gauss(splf, ngauss)
    CALL get_dim(splc, nc, nxc, nidbas)
    kl=nidbas; ku=kl
    IF(nlper) THEN
       nf = nxf
       nc = nxc
    END IF
    CALL init(nc, 1, matm, mrows=nf)  ! Defne nf x nc matrix
!
    ALLOCATE(xg(ngauss),wg(ngauss))
    ALLOCATE(funf(0:nidbas,1))
    ALLOCATE(func(0:nidbas,1))
    DO i=1,nxf
       ic = (i-1)/2+1
       CALL get_gauss(splf, ngauss, i, xg, wg)
       DO ig=1,ngauss
          CALL basfun(xg(ig), splf, funf, i)
          CALL basfun(xg(ig), splc, func, ic)
          DO it=0,nidbas
             DO jw=0,nidbas
                contrib = wg(ig)*funf(it,1)*func(jw,1)*xg(ig)**alpha
                irow = i+it;  IF(nlper) irow=MODULO(irow-1,nxf)+1
                jcol = ic+jw; IF(nlper) jcol=MODULO(jcol-1,nxc)+1
                CALL updtmat(matm, irow, jcol, contrib)
             END DO
          END DO
       END DO
    END DO
!
    DEALLOCATE(xg,wg)
    DEALLOCATE(funf,func)
  END SUBROUTINE ctof_massmat
!--------------------------------------------------------------------------------
  SUBROUTINE direct_solve_1d(grid, v)
!
!   1D direct solver
!
    TYPE(grid1d), INTENT(inout)   :: grid
    DOUBLE PRECISION, INTENT(out) :: v(:)
    LOGICAL :: nlper
!
    nlper = grid%spl%period
    IF(nlper) THEN
       IF(.NOT.ALLOCATED(grid%matap_copy)) THEN 
          ALLOCATE(grid%matap_copy)
          CALL mcopy(grid%matap, grid%matap_copy)
          CALL factor(grid%matap_copy)
       END IF
       CALL bsolve(grid%matap_copy, grid%f, v)
    ELSE
       IF(.NOT.ALLOCATED(grid%mata_copy)) THEN 
          ALLOCATE(grid%mata_copy)
          CALL mcopy(grid%mata, grid%mata_copy)
          CALL factor(grid%mata_copy)
       END IF
       CALL bsolve(grid%mata_copy, grid%f, v)
    END IF
  END SUBROUTINE direct_solve_1d
!--------------------------------------------------------------------------------
  SUBROUTINE direct_solve_2d(grid, v, debug)
!
!   2D direct solver
!
    TYPE(grid2d), INTENT(inout)     :: grid
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    LOGICAL, INTENT(in), OPTIONAL   :: debug
    LOGICAL :: dbg
!
    dbg = .FALSE.
    IF(PRESENT(debug)) dbg=debug
!
    IF(ALLOCATED(grid%mata)) THEN
       IF(.NOT.ALLOCATED(grid%mata%mumps)) THEN
          ALLOCATE(grid%mata%mumps)
          CALL csr2mumps(grid%mata, grid%mata%mumps)
          CALL factor(grid%mata%mumps, debug=dbg)
       END IF
       CALL bsolve(grid%mata%mumps, v, debug=dbg)
    ELSE
       IF(.NOT.ALLOCATED(grid%mata_cds%mumps)) THEN
          ALLOCATE(grid%mata_cds%mumps)
          CALL cds2mumps(grid%mata_cds, grid%mata_cds%mumps)
          CALL factor(grid%mata_cds%mumps, debug=dbg)
       END IF
       CALL bsolve(grid%mata_cds%mumps, v, debug=dbg)
    END IF
!
!  Only cylindrical case
    IF(.NOT.grid%spl%sp1%period .AND. grid%spl%sp2%period) THEN
    END IF
  END SUBROUTINE direct_solve_2d
!--------------------------------------------------------------------------------
  SUBROUTINE jacobi_gb(mat, omega, nu, v, f)
!
!   Weighted Jacobi relaxation
!
    TYPE(gbmat),INTENT(in) :: mat
    DOUBLE PRECISION, INTENT(in)    :: omega
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: temp(SIZE(v))
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, kl, ku, n, i, j, jmin, jmax
!
    kl = mat%kl
    ku = mat%ku
    n = mat%rank
!
    inv_diag(:) = omega/mat%val(kl+ku+1,:)
    DO k=1,nu
       DO i=1,n
          jmin = MAX(1,i-kl)
          jmax = MIN(n, i+ku)
          temp(i) = f(i)
          DO j=jmin,i-1
             temp(i) = temp(i) - mat%val(kl+ku+i-j+1,j)*v(j)
          END DO
          DO j=i+1,jmax
             temp(i) = temp(i) - mat%val(kl+ku+i-j+1,j)*v(j)
          END DO
          temp(i) = temp(i)*inv_diag(i)
       END DO
       v(:) = (1.d0-omega)*v(:) + temp(:)   
    END DO
  END SUBROUTINE jacobi_gb
!--------------------------------------------------------------------------------
  SUBROUTINE jacobi_ge(mat, omega, nu, v, f)
!
!   Weighted Jacobi relaxation
!
    TYPE(gemat),INTENT(in) :: mat
    DOUBLE PRECISION, INTENT(in)    :: omega
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: temp(SIZE(v))
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, n, i, j
!
    n = mat%rank
!
    DO i=1,n
       inv_diag(i) = omega/mat%val(i,i)
    END DO
!
    DO k=1,nu
       DO i=1,n
          temp(i) = f(i)
          DO j=1,i-1
             temp(i) = temp(i) - mat%val(i,j)*v(j)
          END DO
          DO j=i+1,n
             temp(i) = temp(i) - mat%val(i-j+1,j)*v(j)
          END DO
          temp(i) = temp(i)*inv_diag(i)
       END DO
       v(:) = (1.d0-omega)*v(:) + temp(:)   
    END DO
  END SUBROUTINE jacobi_ge
!--------------------------------------------------------------------------------
  SUBROUTINE jacobi_csr(mat, omega, nu, v, f)
!
!   Weighted Jacobi relaxation
!
    TYPE(csr_mat),INTENT(in)        :: mat
    DOUBLE PRECISION, INTENT(in)    :: omega
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: temp(SIZE(v))
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, n, i, j, jcol
!
    n = mat%rank
!
    inv_diag(:) = omega/mat%val(mat%idiag)
    DO k=1,nu
       temp(:) = f(:)
       DO i=1,n
          DO j = mat%irow(i), mat%irow(i+1)-1
             jcol = mat%cols(j)
             IF(jcol.NE.i) THEN   ! The diagonal
                temp(i) = temp(i) - mat%val(j)*v(jcol)
             END IF
          END DO
       END DO
       temp(:) = temp(:)*inv_diag(:)
       v(:)    = (1.d0-omega)*v(:) + temp(:)   
    END DO
  END SUBROUTINE jacobi_csr
!--------------------------------------------------------------------------------
  SUBROUTINE jacobi_cds(mat, omega, nu, v, f)
!
!   Weighted Jacobi relaxation
!
    TYPE(cds_mat),INTENT(in)        :: mat
    DOUBLE PRECISION, INTENT(in)    :: omega
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: temp(SIZE(v))
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, n, i, id, d
!
    n = mat%rank
!
    inv_diag(:) = omega/mat%val(:,mat%dists(0))
    DO k=1,nu
       temp(:) = f(:)
       DO id=-mat%kl,mat%ku  ! f - (L+U)*v
          IF(id.EQ.0) CYCLE
          d = mat%dists(id)
          DO i=MAX(1,1-d), MIN(n,mat%rank-d)
             temp(i) = temp(i) - mat%val(i,id)*v(i+d)
          END DO
       END DO
       temp(:) = temp(:)*inv_diag(:)
       v(:)    = (1.d0-omega)*v(:) + temp(:)   
    END DO
  END SUBROUTINE jacobi_cds
!--------------------------------------------------------------------------------
  SUBROUTINE gs_gb(mat, nu, v, f)
!
!   Gauss-Seidel relaxation
!
    TYPE(gbmat),INTENT(in) :: mat
    INTEGER, INTENT(in)    :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    INTEGER          :: k, kl, ku, n, i, j, jmin, jmax
    DOUBLE PRECISION :: inv_diag(SIZE(v))
!
    kl = mat%kl
    ku = mat%ku
    n = mat%rank
!
    inv_diag(:) = 1.d0/mat%val(kl+ku+1,:)
    DO k=1,nu
       DO i=1,n
          jmin = MAX(1,i-kl)
          jmax = MIN(n, i+ku)
          v(i) = f(i)
          DO j=jmin,i-1
             v(i) = v(i) - mat%val(kl+ku+i-j+1,j)*v(j)
          END DO
          DO j=i+1,jmax
             v(i) = v(i) - mat%val(kl+ku+i-j+1,j)*v(j)
          END DO
          v(i) = inv_diag(i)*v(i)
       END DO
    END DO
  END SUBROUTINE gs_gb
!--------------------------------------------------------------------------------
  SUBROUTINE gs_ge(mat, nu, v, f)
!
!   Gauss-Seidel relaxation
!
    TYPE(gemat),INTENT(in) :: mat
    INTEGER, INTENT(in)    :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    INTEGER          :: k, n, i, j
    DOUBLE PRECISION :: inv_diag(SIZE(v))
!
    n = mat%rank
!
    DO i=1,n
       inv_diag(i) = 1.d0/mat%val(i,i)
    END DO
    DO k=1,nu
       DO i=1,n
          v(i) = f(i)
          DO j=1,i-1
             v(i) = v(i) - mat%val(i,j)*v(j)
          END DO
          DO j=i+1,n
             v(i) = v(i) - mat%val(i,j)*v(j)
          END DO
          v(i) = inv_diag(i)*v(i)
       END DO
    END DO
  END SUBROUTINE gs_ge
!--------------------------------------------------------------------------------
  SUBROUTINE gs_csr(mat, nu, v, f)
!
!   Gauss-Seidel relaxation
!
    TYPE(csr_mat),INTENT(in)        :: mat
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, n, i, j, jcol
!
    n = mat%rank
!
    inv_diag(:) = 1.0d0/mat%val(mat%idiag)
    DO k=1,nu
       DO i=1,n
          v(i) = f(i)
          DO j = mat%irow(i), mat%irow(i+1)-1
             jcol = mat%cols(j)
             IF(jcol.NE.i) THEN   ! The diagonal
                v(i) = v(i) - mat%val(j)*v(jcol)
             END IF
          END DO
          v(i) = v(i)*inv_diag(i)
       END DO
    END DO
  END SUBROUTINE gs_csr
!--------------------------------------------------------------------------------
  SUBROUTINE gs_cds(mat, nu, v, f)
!
!   Gauss-Seidel relaxation
!
    TYPE(cds_mat),INTENT(in)        :: mat
    INTEGER, INTENT(in)             :: nu
    DOUBLE PRECISION, INTENT(inout) :: v(:)
    DOUBLE PRECISION, INTENT(in)    :: f(:)
!
    DOUBLE PRECISION :: temp(SIZE(v))
    DOUBLE PRECISION :: inv_diag(SIZE(v))
    INTEGER          :: k, n, i, id, d
!
    n = mat%rank
!
    inv_diag(:) = 1.0d0/mat%val(:,mat%dists(0))
    DO k=1,nu
!
       temp(:) = f(:)
       DO id=1,mat%ku  ! t <- f - U*v
          d = mat%dists(id)
          DO i=MAX(1,1-d), MIN(n,n-d)
             temp(i) = temp(i) - mat%val(i,id)*v(i+d)
          END DO
       END DO
!
       DO i=1,n        ! Solve (L+D)v=t
          v(i) = temp(i)
          DO id=-1,-mat%kl,-1
             d = mat%dists(id)
             IF(i+d.LT.1) EXIT
             v(i) = v(i) - mat%val(i,id)*v(i+d)
          END DO
          v(i) = v(i)*inv_diag(i)
       END DO
    END DO
  END SUBROUTINE gs_cds
!--------------------------------------------------------------------------------
  SUBROUTINE printmat_mat(str, val)
!
    CHARACTER(len=*), INTENT(in) :: str
    DOUBLE PRECISION, INTENT(in)  :: val(:,:)
    INTEGER :: mrows, ncols,i
    mrows=SIZE(val,1)
    ncols=SIZE(val,2)
    WRITE(*,'(/a)') TRIM(str)
    WRITE(*,'(2(a,i6))') 'M =', mrows, ', N =', ncols
    DO i=1,mrows
       WRITE(*,'(12(1pe12.3))') val(i,:)
    END DO
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or rows', SUM(val,2)
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or cols', SUM(val,1)
  END SUBROUTINE printmat_mat
!--------------------------------------------------------------------------------
  SUBROUTINE printmat_ge(str, mat)
!
    CHARACTER(len=*), INTENT(in) :: str
    TYPE(gemat), INTENT(in)      :: mat
    INTEGER :: i
    DOUBLE PRECISION :: arow(mat%ncols)
    DOUBLE PRECISION :: sum_cols(mat%ncols), sum_rows(mat%mrows)
    sum_cols = 0.0d0
    arow = 0.0d0
    WRITE(*,'(/a)') TRIM(str)
    WRITE(*,'(2(a,i6))') 'M =', mat%mrows, ', N =', mat%ncols
    DO i=1,mat%mrows
       CALL getrow(mat,i,arow)
       sum_rows(i) = SUM(arow)
       sum_cols(:) = sum_cols(:) + arow(:)
       WRITE(*,'(12(1pe12.3))') arow
    END DO
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or rows', sum_rows
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or cols', sum_cols
  END SUBROUTINE printmat_ge
!--------------------------------------------------------------------------------
  SUBROUTINE printmat_gb(str, mat)
!
    CHARACTER(len=*), INTENT(in) :: str
    TYPE(gbmat), INTENT(in)      :: mat
    INTEGER :: i
    DOUBLE PRECISION :: arow(mat%ncols)
    DOUBLE PRECISION :: sum_cols(mat%ncols), sum_rows(mat%mrows)
    sum_cols = 0.0d0
    WRITE(*,'(/a)') TRIM(str)
    WRITE(*,'(2(a,i6))') 'M =', mat%mrows, ', N =', mat%ncols
    DO i=1,mat%mrows
       CALL getrow(mat,i,arow)
       sum_rows(i) = SUM(arow)
       sum_cols(:) = sum_cols(:) + arow(:)
       WRITE(*,'(8(1pe12.3))') arow
    END DO
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or rows', sum_rows
    WRITE(*,'(a/(12(1pe12.3)))') 'Sum or cols', sum_cols
  END SUBROUTINE printmat_gb
!--------------------------------------------------------------------------------
  SUBROUTINE printmat_periodic(str, mat)
!
    CHARACTER(len=*), INTENT(in) :: str
    TYPE(periodic_mat), INTENT(in)      :: mat
    INTEGER :: i
    DOUBLE PRECISION :: arow(mat%mat%ncols)
    DOUBLE PRECISION :: sum_cols(mat%mat%ncols), sum_rows(mat%mat%mrows)
    sum_cols = 0.0d0
    WRITE(*,'(/a)') TRIM(str)
    WRITE(*,'(2(a,i6))') 'M =', mat%mat%mrows, ', N =', mat%mat%ncols
    DO i=1,mat%mat%mrows
       CALL getrow(mat,i,arow)
       sum_rows(i) = SUM(arow)
       sum_cols(:) = sum_cols(:) + arow(:)
       WRITE(*,'(8(1pe12.3))') arow
    END DO
    WRITE(*,'(a/(8(1pe12.3)))') 'Sum or rows', sum_rows
    WRITE(*,'(a/(8(1pe12.3)))') 'Sum or cols', sum_cols
  END SUBROUTINE printmat_periodic
!--------------------------------------------------------------------------------
  SUBROUTINE printdiag_gb(str, mat)
!
    CHARACTER(len=*), INTENT(in) :: str
    TYPE(gbmat), INTENT(in)      :: mat
    INTEGER :: ku, kl
    kl = mat%kl
    ku = mat%ku
    WRITE(*,'(a/(8(1pe12.3)))') str, mat%val(kl+ku+1,:)
  END SUBROUTINE printdiag_gb
!--------------------------------------------------------------------------------
  INTEGER FUNCTION get_lmax(n)
    INTEGER, INTENT(in) :: n
    INTEGER :: l, ncur
    l=1
    ncur = n
    DO
       IF(ncur.EQ.2 .OR. MODULO(ncur,2).NE.0) EXIT  ! Minimum N is 2 or odd.
       l=l+1
       ncur = ncur/2
    END DO
    get_lmax = l
  END FUNCTION get_lmax
!--------------------------------------------------------------------------------
  SUBROUTINE ibc_transf(grids, dir, k)
!
!   Impose BC on transfer matrix
!
    TYPE(grid2d), INTENT(inout) :: grids(:)
    INTEGER, INTENT(in)         :: dir
    INTEGER, INTENT(in)         :: k
!
    INTEGER :: levels, l
    levels = SIZE(grids)
    DO l=2,levels
     CALL mod_transf(grids(l)%matp(dir),k)
    END DO
  END SUBROUTINE ibc_transf
!--------------------------------------------------------------------------------
END MODULE multigrid