solve_LINEQ_contact_elim.f90

Go to the documentation of this file.

!-------------------------------------------------------------------------------
! Copyright (c) 2019 FrontISTR Commons
! This software is released under the MIT License, see LICENSE.txt
!-------------------------------------------------------------------------------
module m_solve_lineq_contact_elim
  use hecmw_util
  use hecmw_local_matrix
  use m_hecmw_contact_comm
  use hecmw_solver
  use hecmw_matrix_misc
  use m_hecmw_comm_f
  use hecmw_solver_misc
  use hecmw_solver_las
  use hecmw_mpc_prepost
 
  implicit none
 
  private
  public :: solve_lineq_contact_elim_init
  public :: solve_lineq_contact_elim
 
  logical, save :: INITIALIZED = .false.
  integer, save :: SymType = 0
 
  integer, parameter :: DEBUG = 0  ! 0: no message, 1: some messages, 2: more messages, 3: even more messages
  logical, parameter :: DEBUG_VECTOR = .false.
  logical, parameter :: DEBUG_MATRIX = .false.
 
contains
 
  subroutine solve_lineq_contact_elim_init(hecMESH, hecMAT, hecLagMAT, is_sym)
    type(hecmwst_local_mesh),      intent(in)    :: hecmesh
    type(hecmwst_matrix),          intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange), intent(in)    :: heclagmat
    logical,                       intent(in)    :: is_sym
 
    if (initialized) then
      initialized = .false.
    endif
 
    hecmat%Iarray(98) = 1
    hecmat%Iarray(97) = 1
 
    if (is_sym) then
      symtype = 1
    else
      symtype = 0
    endif
 
    initialized = .true.
  end subroutine solve_lineq_contact_elim_init
 
  subroutine solve_lineq_contact_elim(hecMESH, hecMAT, hecLagMAT, istat, conMAT, is_contact_active)
    type(hecmwst_local_mesh),      intent(inout) :: hecmesh
    type(hecmwst_matrix),          intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange), intent(inout) :: heclagmat
    integer(kind=kint),            intent(out)   :: istat
    type(hecmwst_matrix),          intent(in)    :: conmat
    logical,                       intent(in)    :: is_contact_active
    !
    integer(kind=kint) :: solver_type, method_org
    integer(kind=kint) :: is_contact
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    hecmat%Iarray(97) = 1
 
    is_contact = 0
    if (is_contact_active) is_contact = 1
    call hecmw_allreduce_i1(hecmesh, is_contact, hecmw_max)
 
    if (is_contact == 0) then
      if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: no contact'
      solver_type = hecmw_mat_get_solver_type(hecmat)
      if (solver_type == 1) then
        ! use CG because the matrix is symmetric
        method_org = hecmw_mat_get_method(hecmat)
        call hecmw_mat_set_method(hecmat, 1)
      endif
      ! solve
      call solve_with_mpc(hecmesh, hecmat)
      if (solver_type == 1) then
        ! restore solver setting
        call hecmw_mat_set_method(hecmat, method_org)
      endif
    else
      if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: with contact'
      call solve_eliminate(hecmesh, hecmat, heclagmat, conmat)
    endif
 
    istat = hecmw_mat_get_flag_diverged(hecmat)
  end subroutine solve_lineq_contact_elim
 
  subroutine solve_with_mpc(hecMESH, hecMAT)
    type(hecmwst_local_mesh),      intent(inout) :: hecmesh
    type(hecmwst_matrix),          intent(inout) :: hecmat
    !
    type (hecmwst_local_mesh), pointer :: hecmeshmpc
    type (hecmwst_matrix), pointer :: hecmatmpc
    integer(kind=kint) :: method
    logical            :: fg_cg, fg_amg
 
    fg_cg = (hecmw_mat_get_method(hecmat) == 1)
    fg_amg = (hecmw_mat_get_precond(hecmat) == 5)
 
    call hecmw_mpc_mat_init(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
    call hecmw_mpc_mat_ass(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
    call hecmw_mpc_trans_rhs(hecmesh, hecmat, hecmatmpc)
    call hecmw_solve(hecmeshmpc,hecmatmpc)
    if (fg_cg .and. fg_amg .and. hecmw_mat_get_flag_diverged(hecmatmpc) /= 0) then
      ! avoid ML and retry when diverged
      call hecmw_mat_set_precond(hecmatmpc, 3) ! set diag-scaling
      hecmatmpc%Iarray(97:98) = 1
      hecmatmpc%X(:) = 0.d0
      call hecmw_solve(hecmeshmpc,hecmatmpc)
      call hecmw_mat_set_precond(hecmatmpc, 5) ! restore amg
    endif
    call hecmw_mpc_tback_sol(hecmesh, hecmat, hecmatmpc)
    call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
  end subroutine solve_with_mpc
 
  subroutine solve_eliminate(hecMESH,hecMAT,hecLagMAT,conMAT)
    type(hecmwst_local_mesh),      intent(inout) :: hecmesh
    type(hecmwst_matrix),          intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange), intent(inout) :: heclagmat
    type(hecmwst_matrix),          intent(in)    :: conmat
    !
    type(hecmwst_local_mesh)        :: hecmeshtmp
    type (hecmwst_local_mesh), pointer :: hecmeshmpc
    type (hecmwst_matrix), pointer :: hecmatmpc
    integer(kind=kint), allocatable :: slaves4lag(:)
    real(kind=kreal),   allocatable :: bls_inv(:)     
    real(kind=kreal),   allocatable :: bus_inv(:)     
    type(hecmwst_local_matrix)      :: tmat
    type(hecmwst_local_matrix)      :: ttmat
    integer(kind=kint), allocatable :: slaves(:)
    type(hecmwst_contact_comm)      :: concomm
    type(hecmwst_local_matrix)      :: kmat
    real(kind=kreal),   allocatable :: btot(:)        
    type(hecmwst_matrix)            :: hectkt
    integer(kind=kint)              :: ndof
    integer(kind=kint)              :: myrank
    real(kind=kreal)                :: t0, t1, t2
 
    myrank = hecmw_comm_get_rank()
 
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: solve_eliminate start'
    t0 = hecmw_wtime()
 
    ndof=hecmat%NDOF
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: num_lagrange',heclagmat%num_lagrange
 
    call copy_mesh(hecmesh, hecmeshtmp)
 
    allocate(slaves4lag(heclagmat%num_lagrange), bls_inv(heclagmat%num_lagrange), &
      bus_inv(heclagmat%num_lagrange))
 
    t1 = hecmw_wtime()
    call make_transformation_matrices(hecmesh, hecmeshtmp, hecmat, heclagmat, &
        slaves4lag, bls_inv, bus_inv, slaves, tmat, ttmat)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: made trans matrices', t2-t1
 
    t1 = t2
    call make_contact_comm_table(hecmesh, hecmat, heclagmat, concomm)
    t2 = hecmw_wtime()
    if (debug >= 2) write(0,*) '  DEBUG2: make contact comm_table done', hecmw_wtime()-t1
 
    t1 = t2
    allocate(btot(hecmat%NP*ndof+heclagmat%num_lagrange))
    call assemble_equation(hecmesh, hecmeshtmp, hecmat, conmat, heclagmat%num_lagrange, &
        slaves, kmat, btot)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: assembled equation ', t2-t1
 
    if (hecmw_comm_get_size() > 1) then
      if (kmat%nc /= ttmat%nc) then
        if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: node migrated with Kmat',kmat%nc-ttmat%nc
        tmat%nc = kmat%nc
        ttmat%nc = kmat%nc
      endif
    endif
 
    t1 = t2
    call convert_equation(hecmeshtmp, hecmat, kmat, tmat, ttmat, btot, slaves, &
        slaves4lag, bls_inv, concomm, hectkt)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: converted equation ', t2-t1
 
    t1 = t2
    call solve_with_mpc(hecmeshtmp, hectkt)
    if (debug_vector) call debug_write_vector(hectkt%X, 'Solution(converted)', 'hecTKT%X', ndof, hectkt%N)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: linear solver done ', t2-t1
 
    t1 = t2
    call recover_solution(hecmeshtmp, hecmat, hectkt, tmat, kmat, btot, &
        slaves4lag, bls_inv, bus_inv, concomm, slaves)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: recovered solution ', t2-t1
 
    if (debug >= 1) call check_solution(hecmesh, hecmeshtmp, hecmat, hectkt, heclagmat, kmat, btot, &
       concomm, slaves)
    if (debug >= 2) call check_solution2(hecmesh, hecmat, conmat, heclagmat, concomm, slaves)
 
    call hecmw_localmat_free(tmat)
    call hecmw_localmat_free(ttmat)
    call hecmw_mat_finalize(hectkt)
    call hecmw_localmat_free(kmat)
    call hecmw_contact_comm_finalize(concomm)
    call free_mesh(hecmeshtmp)
    deallocate(slaves4lag)
    t2 = hecmw_wtime()
    if ((debug >= 1 .and. myrank==0) .or. debug >= 2) write(0,*) 'DEBUG: solve_eliminate end', t2-t0
  end subroutine solve_eliminate
 
  subroutine copy_mesh(src, dst)
    type(hecmwst_local_mesh), intent(in)  :: src
    type(hecmwst_local_mesh), intent(out) :: dst
 
    dst%zero          = src%zero
    dst%MPI_COMM      = src%MPI_COMM
    dst%PETOT         = src%PETOT
    dst%PEsmpTOT      = src%PEsmpTOT
    dst%my_rank       = src%my_rank
    dst%n_subdomain   = src%n_subdomain
    dst%n_node        = src%n_node
    dst%nn_internal   = src%nn_internal
    dst%n_elem        = src%n_elem
    dst%ne_internal   = src%ne_internal
    dst%n_elem_type   = src%n_elem_type
    dst%n_dof         = src%n_dof
    dst%n_neighbor_pe = src%n_neighbor_pe
    if (src%n_neighbor_pe > 0) then
      allocate(dst%neighbor_pe(dst%n_neighbor_pe))
      dst%neighbor_pe(:) = src%neighbor_pe(:)
      allocate(dst%import_index(0:dst%n_neighbor_pe))
      dst%import_index(:)= src%import_index(:)
      allocate(dst%export_index(0:dst%n_neighbor_pe))
      dst%export_index(:)= src%export_index(:)
      allocate(dst%import_item(dst%import_index(dst%n_neighbor_pe)))
      dst%import_item(1:dst%import_index(dst%n_neighbor_pe)) = src%import_item(1:dst%import_index(dst%n_neighbor_pe))
      allocate(dst%export_item(dst%export_index(dst%n_neighbor_pe)))
      dst%export_item(1:dst%export_index(dst%n_neighbor_pe)) = src%export_item(1:dst%export_index(dst%n_neighbor_pe))
    else
      dst%neighbor_pe => null()
      dst%import_index => null()
      dst%export_index => null()
      dst%import_item => null()
      dst%export_item => null()
    endif
    allocate(dst%global_node_ID(dst%n_node))
    dst%global_node_ID(1:dst%n_node) = src%global_node_ID(1:dst%n_node)
    allocate(dst%node_ID(2*dst%n_node))
    dst%node_ID(1:2*dst%n_node) = src%node_ID(1:2*dst%n_node)
    allocate(dst%elem_type_item(dst%n_elem_type))
    dst%elem_type_item(:) = src%elem_type_item(:)
    !
    dst%mpc%n_mpc =  src%mpc%n_mpc
    dst%mpc%mpc_index => src%mpc%mpc_index
    dst%mpc%mpc_item => src%mpc%mpc_item
    dst%mpc%mpc_dof => src%mpc%mpc_dof
    dst%mpc%mpc_val => src%mpc%mpc_val
    dst%mpc%mpc_const => src%mpc%mpc_const
    !
    dst%node_group%n_grp = src%node_group%n_grp
    dst%node_group%n_bc = src%node_group%n_bc
    dst%node_group%grp_name => src%node_group%grp_name
    dst%node_group%grp_index => src%node_group%grp_index
    dst%node_group%grp_item => src%node_group%grp_item
    dst%node_group%bc_grp_ID => src%node_group%bc_grp_ID
    dst%node_group%bc_grp_type => src%node_group%bc_grp_type
    dst%node_group%bc_grp_index => src%node_group%bc_grp_index
    dst%node_group%bc_grp_dof => src%node_group%bc_grp_dof
    dst%node_group%bc_grp_val => src%node_group%bc_grp_val
    !
    dst%node => src%node
  end subroutine copy_mesh
 
  subroutine free_mesh(hecMESH)
    type(hecmwst_local_mesh), intent(inout) :: hecmesh
 
    if (hecmesh%n_neighbor_pe > 0) then
      deallocate(hecmesh%neighbor_pe)
      deallocate(hecmesh%import_index)
      deallocate(hecmesh%export_index)
      deallocate(hecmesh%import_item)
      deallocate(hecmesh%export_item)
      deallocate(hecmesh%global_node_ID)
    endif
    deallocate(hecmesh%node_ID)
    deallocate(hecmesh%elem_type_item)
    !hecMESH%node => null()
  end subroutine free_mesh
 
  subroutine make_transformation_matrices(hecMESH, hecMESHtmp, hecMAT, hecLagMAT, &
      slaves4lag, BLs_inv, BUs_inv, slaves, Tmat, Ttmat)
    type(hecmwst_local_mesh),        intent(in)    :: hecmesh
    type(hecmwst_local_mesh),        intent(inout) :: hecmeshtmp
    type(hecmwst_matrix),            intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange),   intent(inout) :: heclagmat
    integer(kind=kint),              intent(out)   :: slaves4lag(:)
    real(kind=kreal),                intent(out)   :: bls_inv(:)    
    real(kind=kreal),                intent(out)   :: bus_inv(:)    
    integer(kind=kint), allocatable, intent(out)   :: slaves(:)
    type(hecmwst_local_matrix),      intent(out)   :: tmat
    type(hecmwst_local_matrix),      intent(out)   :: ttmat
    !
    integer(kind=kint) :: myrank, n
 
    myrank = hecmw_comm_get_rank()
    n = hecmat%NP
 
    ! choose slave DOFs to be eliminated with Lag. DOFs
    call choose_slaves(hecmat, heclagmat, n, slaves4lag)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: slave DOFs chosen'
 
    call make_bls_inv(heclagmat, hecmat%NDOF, slaves4lag, bls_inv)
    call make_bus_inv(heclagmat, hecmat%NDOF, slaves4lag, bus_inv)
 
    call add_c_to_tmat(hecmat, heclagmat, n, slaves4lag, bls_inv, tmat)
    if (debug_matrix) call debug_write_matrix(tmat, 'Tmat (local, C only)')
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: add C to Tmat done'
 
    call add_ct_to_ttmat(hecmat, heclagmat, n, slaves4lag, bus_inv, ttmat)
    if (debug_matrix) call debug_write_matrix(ttmat, 'Ttmat (local, Ct only)')
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: add Ct to Tt done'
 
    if (hecmw_comm_get_size() > 1) then
      ! communicate and assemble Tmat (updating hecMESHtmp)
      call hecmw_localmat_assemble(tmat, hecmesh, hecmeshtmp)
      if (debug >= 2) then
        write(0,*) '  DEBUG2[',myrank,']: assemble T done'
        if (tmat%nc /= hecmesh%n_node) write(0,*) '  DEBUG2[',myrank,']: node migrated with T',tmat%nc-hecmesh%n_node
      endif
      if (debug_matrix) call debug_write_matrix(tmat, 'Tmat (assembled, C only)')
 
      ! communicate and assemble Ttmat (updating hecMESHtmp)
      call hecmw_localmat_assemble(ttmat, hecmesh, hecmeshtmp)
      if (debug >= 2) then
        write(0,*) '  DEBUG2[',myrank,']: assemble Tt done'
        if (ttmat%nc /= tmat%nc) write(0,*) '  DEBUG2[',myrank,']: node migrated with Ttmat',ttmat%nc-tmat%nc
        tmat%nc = ttmat%nc
      endif
      if (debug_matrix) call debug_write_matrix(ttmat, 'Ttmat (assembled, Ct only)')
    endif
 
    ! add Ip to Tmat and Ttmat
    call make_slave_list(hecmeshtmp, tmat%ndof, slaves4lag, slaves)
    call add_ip_to_tmat(tmat, slaves)
    if (debug_matrix) call debug_write_matrix(tmat, 'Tmat (final)')
    call add_ip_to_tmat(ttmat, slaves)
    if (debug_matrix) call debug_write_matrix(ttmat, 'Ttmat (final)')
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: place 1 on diag of T and Tt done'
  end subroutine make_transformation_matrices
 
  subroutine choose_slaves(hecMAT, hecLagMAT, n, slaves4lag)
    type(hecmwst_matrix),          intent(in)  :: hecmat
    type(hecmwst_matrix_lagrange), intent(in)  :: heclagmat
    integer(kind=kint),            intent(in)  :: n
    integer(kind=kint),            intent(out) :: slaves4lag(:)
    !
    integer(kind=kint) :: ndof, i, j, idof, jdof, l, ls, le, idx, imax, iwmin
    real(kind=kreal)   :: val, vmax
    integer(kind=kint), allocatable :: mark_slave4lag(:)
    integer(kind=kint), allocatable :: iw1l(:), iw1u(:)
    integer(kind=kint) :: n_slave_in, n_slave_out, ilag
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    ndof=hecmat%NDOF
 
    allocate(mark_slave4lag(n*ndof), source=0)
    slaves4lag=0
 
    if (heclagmat%num_lagrange == 0) return
 
    allocate(iw1l(n*ndof))
    allocate(iw1u(n*ndof))
    iw1l=0
    iw1u=0
 
    ! Count how many times each dof appear in Lagrange matrix
    ! lower
    do i=1,heclagmat%num_lagrange
      ls=heclagmat%indexL_lagrange(i-1)+1
      le=heclagmat%indexL_lagrange(i)
      do l=ls,le
        j=heclagmat%itemL_lagrange(l)
        do jdof=1,ndof
          idx=(j-1)*ndof+jdof
          iw1l(idx)=iw1l(idx)+1
        enddo
      enddo
    enddo
    ! upper
    do i=1,n
      ls=heclagmat%indexU_lagrange(i-1)+1
      le=heclagmat%indexU_lagrange(i)
      do l=ls,le
        j=heclagmat%itemU_lagrange(l)
        do idof=1,ndof
          idx=(i-1)*ndof+idof
          iw1u(idx)=iw1u(idx)+1
        enddo
      enddo
    enddo
    !!$    write(0,*) 'iw1L, iw1U:'
    !!$    do i=1,n*ndof
    !!$      if (iw1L(i) > 0 .or. iw1U(i) > 0) write(0,*) i, iw1L(i), iw1U(i)
    !!$    enddo
 
    ! Choose dofs that
    ! - appear only once in both lower and upper Lag. and
    ! - has greatest coefficient among them (in lower Lag.)
    do i=1,heclagmat%num_lagrange
      ls=heclagmat%indexL_lagrange(i-1)+1
      le=heclagmat%indexL_lagrange(i)
      vmax = 0.d0
      imax = -1
      iwmin = n
      do l=ls,le
        j=heclagmat%itemL_lagrange(l)
        do jdof=1,ndof
          idx=(j-1)*ndof+jdof
          val=heclagmat%AL_lagrange((l-1)*ndof+jdof)
          if (iw1l(idx) < iwmin .and. iw1u(idx) < iwmin) then
            iwmin = min(iw1l(idx),iw1u(idx))
            vmax = 0.d0
          endif
          if (iw1l(idx) == iwmin .and. iw1u(idx) == iwmin) then
            if (abs(val) > abs(vmax)) then
              imax=idx
              vmax=val
            endif
          endif
        enddo
      enddo
      if (imax == -1) stop "ERROR: iterative solver for contact failed"
      mark_slave4lag(imax)=i
      slaves4lag(i)=imax
    enddo
    !!$    write(0,*) 'mark_slave4lag:'
    !!$    do i=1,n*ndof
    !!$      if (mark_slave4lag(i) > 0) write(0,*) i, mark_slave4lag(i), iw1L(i), iw1U(i)
    !!$    enddo
    !!$    write(0,*) 'slaves4lag:'
    !!$    write(0,*) slaves4lag(:)
    if (debug >= 2) then
      n_slave_in = 0
      n_slave_out = 0
      do ilag=1,heclagmat%num_lagrange
        i = slaves4lag(ilag)
        if (0 < i .and. i <= hecmat%N*ndof) then
          n_slave_in = n_slave_in + 1
        elseif (hecmat%N*ndof < i .and. i <= hecmat%NP*ndof) then
          n_slave_out = n_slave_out + 1
        endif
      enddo
      write(0,*) '  DEBUG2[',myrank,']: n_slave(in,out,tot)',n_slave_in,n_slave_out,heclagmat%num_lagrange
    endif
 
    deallocate(mark_slave4lag)
    deallocate(iw1l, iw1u)
  end subroutine choose_slaves
 
  subroutine make_bls_inv(hecLagMAT, ndof, slaves4lag, BLs_inv)
    type(hecmwst_matrix_lagrange), intent(in)  :: heclagmat
    integer(kind=kint),            intent(in)  :: ndof
    integer(kind=kint),            intent(in)  :: slaves4lag(:)
    real(kind=kreal),              intent(out) :: bls_inv(:)    
    !
    integer(kind=kint) :: ilag, ls, le, l, j, jdof, idx
 
    if (heclagmat%num_lagrange == 0) return
 
    bls_inv=0.d0
    do ilag=1,heclagmat%num_lagrange
      ls=heclagmat%indexL_lagrange(ilag-1)+1
      le=heclagmat%indexL_lagrange(ilag)
      lloop: do l=ls,le
        j=heclagmat%itemL_lagrange(l)
        do jdof=1,ndof
          idx=(j-1)*ndof+jdof
          if (idx==slaves4lag(ilag)) then
            bls_inv(ilag) = 1.0d0/heclagmat%AL_lagrange((l-1)*ndof+jdof)
            exit lloop
          endif
        enddo
      enddo lloop
    enddo
    !write(0,*) BLs_inv
  end subroutine make_bls_inv
 
  subroutine make_bus_inv(hecLagMAT, ndof, slaves4lag, BUs_inv)
    type(hecmwst_matrix_lagrange), intent(in)  :: heclagmat
    integer(kind=kint),            intent(in)  :: ndof
    integer(kind=kint),            intent(in)  :: slaves4lag(:)
    real(kind=kreal),              intent(out) :: bus_inv(:)    
    !
    integer(kind=kint) :: ilag, i, idof, js, je, j, k
 
    if (heclagmat%num_lagrange == 0) return
 
    bus_inv=0.d0
    do ilag=1,size(slaves4lag)
      i=(slaves4lag(ilag)+ndof-1)/ndof
      idof=slaves4lag(ilag)-(i-1)*ndof
      js=heclagmat%indexU_lagrange(i-1)+1
      je=heclagmat%indexU_lagrange(i)
      do j=js,je
        k=heclagmat%itemU_lagrange(j)
        if (k==ilag) then
          bus_inv(ilag) = 1.0d0/heclagmat%AU_lagrange((j-1)*ndof+idof)
          exit
        endif
      enddo
    enddo
    !write(0,*) BUs_inv
  end subroutine make_bus_inv
 
  subroutine add_c_to_tmat(hecMAT, hecLagMAT, n, slaves4lag, BLs_inv, Tmat)
    type(hecmwst_matrix),          intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange), intent(inout) :: heclagmat
    integer(kind=kint),            intent(in)    :: n
    integer(kind=kint),            intent(in)    :: slaves4lag(:)
    real(kind=kreal),              intent(in)    :: bls_inv(:)    
    type(hecmwst_local_matrix),    intent(out)   :: tmat
    !
    type(hecmwst_local_matrix) :: tmat11
    integer(kind=kint), allocatable :: nz_cnt(:)
    integer(kind=kint) :: ndof, i, ilag, l, js, je, j, k, jdof, kk, jj
    real(kind=kreal)   :: factor
 
    ndof=hecmat%NDOF
    tmat11%nr=n*ndof
    tmat11%nc=tmat11%nr
    tmat11%nnz=heclagmat%numL_lagrange*ndof-heclagmat%num_lagrange
    tmat11%ndof=1
 
    allocate(tmat11%index(0:tmat11%nr))
    allocate(tmat11%item(tmat11%nnz), tmat11%A(tmat11%nnz))
    allocate(nz_cnt(tmat11%nr), source=0)
    ! index
    do ilag=1,size(slaves4lag)
      nz_cnt(slaves4lag(ilag))=ndof*(heclagmat%indexL_lagrange(ilag)-heclagmat%indexL_lagrange(ilag-1))-1
    enddo
    tmat11%index(0)=0
    do i=1,tmat11%nr
      tmat11%index(i)=tmat11%index(i-1)+nz_cnt(i)
    enddo
    deallocate(nz_cnt)
    if (tmat11%nnz /= tmat11%index(tmat11%nr)) then
      write(0,*) tmat11%nnz, tmat11%index(tmat11%nr)
      tmat11%nnz = tmat11%index(tmat11%nr)
      !stop 'ERROR: Tmat11%nnz wrong'
    endif
    ! item and A
    do ilag=1,size(slaves4lag)
      i=slaves4lag(ilag)
      l=tmat11%index(i-1)+1
      js=heclagmat%indexL_lagrange(ilag-1)+1
      je=heclagmat%indexL_lagrange(ilag)
      factor=-bls_inv(ilag)
      do j=js,je
        k=heclagmat%itemL_lagrange(j)
        do jdof=1,ndof
          kk=(k-1)*ndof+jdof
          jj=(j-1)*ndof+jdof
          if (kk==i) cycle
          tmat11%item(l)=kk
          tmat11%A(l)=heclagmat%AL_lagrange(jj)*factor
          l=l+1
        enddo
      enddo
      if (l /= tmat11%index(i)+1) then
        write(0,*) l, tmat11%index(i)+1
        stop 'ERROR: Tmat11%index wrong'
      endif
    enddo
    !call hecmw_localmat_write(Tmat11, 0)
    ! make 3x3-block version of Tmat
    call hecmw_localmat_blocking(tmat11, ndof, tmat)
    call hecmw_localmat_free(tmat11)
  end subroutine add_c_to_tmat
 
  subroutine add_ct_to_ttmat(hecMAT, hecLagMAT, n, slaves4lag, BUs_inv, Ttmat)
    type(hecmwst_matrix),          intent(inout) :: hecmat
    type(hecmwst_matrix_lagrange), intent(inout) :: heclagmat
    integer(kind=kint),            intent(in)    :: n
    integer(kind=kint),            intent(in)    :: slaves4lag(:)
    real(kind=kreal),              intent(in)    :: bus_inv(:)    
    type(hecmwst_local_matrix),    intent(out)   :: ttmat
    !
    type(hecmwst_local_matrix) :: ttmat11
    integer(kind=kint), allocatable :: nz_cnt(:)
    integer(kind=kint) :: ndof, i, idof, idx, ilag, l, js, je, j, k
 
    ndof=hecmat%NDOF
    ttmat11%nr=n*ndof
    ttmat11%nc=ttmat11%nr
    ttmat11%nnz=heclagmat%numU_lagrange*ndof-heclagmat%num_lagrange
    ttmat11%ndof=1
 
    allocate(ttmat11%index(0:ttmat11%nr))
    allocate(ttmat11%item(ttmat11%nnz), ttmat11%A(ttmat11%nnz))
    allocate(nz_cnt(ttmat11%nr), source=0)
    ! index
    if (heclagmat%num_lagrange > 0) then
      do i=1,n
        do idof=1,ndof
          idx=(i-1)*ndof+idof
          nz_cnt(idx)=heclagmat%indexU_lagrange(i)-heclagmat%indexU_lagrange(i-1)
        enddo
      enddo
      do ilag=1,size(slaves4lag)
        nz_cnt(slaves4lag(ilag))=0
      enddo
    endif
    ttmat11%index(0)=0
    do i=1,ttmat11%nr
      ttmat11%index(i)=ttmat11%index(i-1)+nz_cnt(i)
    enddo
    if (ttmat11%nnz /= ttmat11%index(ttmat11%nr)) then
      write(0,*) ttmat11%nnz, ttmat11%index(ttmat11%nr)
      !stop 'ERROR: Ttmat11%nnz wrong'
      ttmat11%nnz = ttmat11%index(ttmat11%nr)
    endif
    ! item and A
    do i=1,n
      do idof=1,ndof
        idx=(i-1)*ndof+idof
        l=ttmat11%index(idx-1)+1
        if (nz_cnt(idx) > 0) then
          ! offdiagonal
          js=heclagmat%indexU_lagrange(i-1)+1
          je=heclagmat%indexU_lagrange(i)
          do j=js,je
            k=heclagmat%itemU_lagrange(j)
            ttmat11%item(l)=slaves4lag(k)
            ttmat11%A(l)=-heclagmat%AU_lagrange((j-1)*ndof+idof)*bus_inv(k)
            l=l+1
          enddo
        endif
        if (l /= ttmat11%index(idx)+1) then
          write(0,*) l, ttmat11%index(idx)+1
          stop 'ERROR: Ttmat11%index wrong'
        endif
      enddo
    enddo
    deallocate(nz_cnt)
    !call hecmw_localmat_write(Ttmat11, 0)
    ! make 3x3-block version of Ttmat
    call hecmw_localmat_blocking(ttmat11, ndof, ttmat)
    call hecmw_localmat_free(ttmat11)
  end subroutine add_ct_to_ttmat
 
  subroutine make_slave_list(hecMESHtmp, ndof, slaves4lag, slaves)
    type(hecmwst_local_mesh),        intent(in)  :: hecmeshtmp
    integer(kind=kint),              intent(in)  :: ndof
    integer(kind=kint),              intent(in)  :: slaves4lag(:)
    integer(kind=kint), allocatable, intent(out) :: slaves(:)
    !
    integer(kind=kint), allocatable :: mark_slave(:)
    integer(kind=kint) :: n_slave
    integer(kind=kint) :: ilag, i
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    allocate(mark_slave(hecmeshtmp%n_node*ndof), source=0)
    do ilag=1,size(slaves4lag)
      mark_slave(slaves4lag(ilag))=1
    enddo
    call hecmw_assemble_i(hecmeshtmp, mark_slave, hecmeshtmp%n_node, ndof)
    n_slave = 0
    do i = 1, hecmeshtmp%nn_internal * ndof
      if (mark_slave(i) /= 0) n_slave = n_slave + 1
    enddo
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: n_slave',n_slave
    allocate(slaves(n_slave))
    n_slave = 0
    do i = 1, hecmeshtmp%nn_internal * ndof
      if (mark_slave(i) /= 0) then
        n_slave = n_slave + 1
        slaves(n_slave) = i
      endif
    enddo
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: slaves',slaves(:)
    deallocate(mark_slave)
  end subroutine make_slave_list
 
  subroutine add_ip_to_tmat(Tmat, slaves)
    type(hecmwst_local_matrix), intent(inout) :: tmat
    integer(kind=kint),         intent(in)    :: slaves(:)
    !
    type(hecmwst_local_matrix) :: imat, wmat
    integer(kind=kint) :: ndof, ndof2, i, irow, idof
 
    ndof = tmat%ndof
    ndof2 = ndof*ndof
    ! Imat: unit matrix except for slave dofs
    imat%nr = tmat%nr
    imat%nc = tmat%nc
    imat%nnz = imat%nr
    imat%ndof = ndof
    allocate(imat%index(0:imat%nr))
    allocate(imat%item(imat%nnz))
    imat%index(0) = 0
    do i = 1, imat%nr
      imat%index(i) = i
      imat%item(i) = i
    enddo
    allocate(imat%A(ndof2 * imat%nnz))
    imat%A(:) = 0.0d0
    do irow = 1, imat%nr
      do idof = 1, ndof
        imat%A(ndof2*(irow-1)+ndof*(idof-1)+idof) = 1.0d0
      enddo
    enddo
    do i = 1, size(slaves)
      irow = (slaves(i)+ndof-1)/ndof
      idof = slaves(i)-ndof*(irow-1)
      imat%A(ndof2*(irow-1)+ndof*(idof-1)+idof) = 0.0d0
    enddo
    call hecmw_localmat_add(tmat, imat, wmat)
    call hecmw_localmat_free(tmat)
    call hecmw_localmat_free(imat)
    tmat%nr = wmat%nr
    tmat%nc = wmat%nc
    tmat%nnz = wmat%nnz
    tmat%ndof = wmat%ndof
    tmat%index => wmat%index
    tmat%item => wmat%item
    tmat%A => wmat%A
  end subroutine add_ip_to_tmat
 
  subroutine make_contact_comm_table(hecMESH, hecMAT, hecLagMAT, conCOMM)
    type(hecmwst_local_mesh),      intent(in)  :: hecmesh
    type(hecmwst_matrix),          intent(in)  :: hecmat
    type(hecmwst_matrix_lagrange), intent(in)  :: heclagmat
    type(hecmwst_contact_comm),    intent(out) :: concomm
    !
    integer(kind=kint)              :: n_contact_dof
    integer(kind=kint), allocatable :: contact_dofs(:)
 
    ! make list of contact dofs including masters and slaves
    call make_contact_dof_list(hecmat, heclagmat, n_contact_dof, contact_dofs)
 
    ! make comm_table for contact dofs
    call hecmw_contact_comm_init(concomm, hecmesh, hecmat%ndof, n_contact_dof, contact_dofs)
  end subroutine make_contact_comm_table
 
  subroutine make_contact_dof_list(hecMAT, hecLagMAT, n_contact_dof, contact_dofs)
    type(hecmwst_matrix),            intent(in)  :: hecmat
    type(hecmwst_matrix_lagrange),   intent(in)  :: heclagmat
    integer(kind=kint),              intent(out) :: n_contact_dof
    integer(kind=kint), allocatable, intent(out) :: contact_dofs(:)
    !
    integer(kind=kint) :: ndof, icnt, ilag, ls, le, l, jnode, k, inode, idof, i
    integer(kind=kint), allocatable :: iw(:)
    logical :: found
    integer(kind=kint) :: myrank
 
    if (heclagmat%num_lagrange == 0) then
      n_contact_dof = 0
      return
    endif
    ! lower
    ndof = hecmat%NDOF
    allocate(iw(hecmat%NP))
    icnt = 0
    do ilag = 1, heclagmat%num_lagrange
      ls = heclagmat%indexL_lagrange(ilag-1)+1
      le = heclagmat%indexL_lagrange(ilag)
      lloop1: do l = ls, le
        jnode = heclagmat%itemL_lagrange(l)
        do k = 1, icnt
          if (iw(k) == jnode) cycle lloop1
        enddo
        icnt = icnt + 1
        iw(icnt) = jnode
      enddo lloop1
    enddo
    ! upper
    do inode = 1, hecmat%NP
      ls = heclagmat%indexU_lagrange(inode-1)+1
      le = heclagmat%indexU_lagrange(inode)
      if (ls <= le) then
        found = .false.
        do k = 1, icnt
          if (iw(k) == inode) found = .true.
        enddo
        if (.not. found) then
          icnt = icnt + 1
          iw(icnt) = inode
        endif
      endif
    enddo
    call quick_sort(iw, 1, icnt)
    allocate(contact_dofs(icnt*ndof))
    do i = 1, icnt
      do idof = 1, ndof
        contact_dofs((i-1)*ndof+idof) = (iw(i)-1)*ndof+idof
      enddo
    enddo
    n_contact_dof = icnt*ndof
    deallocate(iw)
    myrank = hecmw_comm_get_rank()
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: n_contact_dof',n_contact_dof
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: contact_dofs',contact_dofs(:)
  end subroutine make_contact_dof_list
 
  recursive subroutine quick_sort(array, id1, id2)
    integer(kind=kint), intent(inout) :: array(:)
    integer(kind=kint), intent(in)    :: id1, id2
    !
    integer(kind=kint) :: pivot, center, left, right, tmp
 
    if (id1 >= id2) return
    center = (id1 + id2) / 2
    pivot = array(center)
    left = id1
    right = id2
    do
      do while (array(left) < pivot)
        left = left + 1
      end do
      do while (pivot < array(right))
        right = right - 1
      end do
      if (left >= right) exit
      tmp = array(left)
      array(left) = array(right)
      array(right) = tmp
      left = left + 1
      right = right - 1
    end do
    if (id1 < left-1) call quick_sort(array, id1, left-1)
    if (right+1 < id2) call quick_sort(array, right+1, id2)
    return
  end subroutine quick_sort
 
  subroutine assemble_equation(hecMESH, hecMESHtmp, hecMAT, conMAT, num_lagrange, &
      slaves, Kmat, Btot)
    type(hecmwst_local_mesh),   intent(in)    :: hecmesh
    type(hecmwst_local_mesh),   intent(inout) :: hecmeshtmp
    type(hecmwst_matrix),       intent(in)    :: hecmat
    type(hecmwst_matrix),       intent(in)    :: conmat
    integer(kind=kint),         intent(in)    :: num_lagrange
    integer(kind=kint),         intent(in)    :: slaves(:)
    ! type(hecmwST_contact_comm), intent(in)    :: conCOMM      !< contact comm table for optimized communication
    type(hecmwst_local_matrix), intent(out)   :: kmat
    real(kind=kreal),           intent(out)   :: btot(:)      
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    call assemble_matrix(hecmesh, hecmeshtmp, hecmat, conmat, num_lagrange, kmat)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: assemble matrix done'
 
    call assemble_rhs(hecmesh, hecmat, conmat, num_lagrange, slaves, btot)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: assemble rhs done'
 
  end subroutine assemble_equation
 
  subroutine assemble_matrix(hecMESH, hecMESHtmp, hecMAT, conMAT, num_lagrange, Kmat)
    type(hecmwst_local_mesh),   intent(in)    :: hecmesh
    type(hecmwst_local_mesh),   intent(inout) :: hecmeshtmp
    type(hecmwst_matrix),       intent(in)    :: hecmat
    type(hecmwst_matrix),       intent(in)    :: conmat
    integer(kind=kint),         intent(in)    :: num_lagrange
    type(hecmwst_local_matrix), intent(out)   :: kmat
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    ! init Kmat and substitute conMAT
    call hecmw_localmat_init_with_hecmat(kmat, conmat, num_lagrange)
    if (debug_matrix) call debug_write_matrix(kmat, 'Kmat (conMAT local)')
 
    if (hecmw_comm_get_size() > 1) then
      ! communicate and assemble Kmat (updating hecMESHtmp)
      call hecmw_localmat_assemble(kmat, hecmesh, hecmeshtmp)
      if (debug_matrix) call debug_write_matrix(kmat, 'Kmat (conMAT assembled)')
      if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: assemble K (conMAT) done'
    endif
 
    ! add hecMAT to Kmat
    call hecmw_localmat_add_hecmat(kmat, hecmat)
    if (debug_matrix) call debug_write_matrix(kmat, 'Kmat (hecMAT added)')
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: add hecMAT to K done'
  end subroutine assemble_matrix
 
  subroutine assemble_rhs(hecMESH, hecMAT, conMAT, num_lagrange, slaves, Btot)
    type(hecmwst_local_mesh),   intent(in) :: hecmesh
    type(hecmwst_matrix),       intent(in) :: hecmat
    type(hecmwst_matrix),       intent(in) :: conmat
    integer(kind=kint),         intent(in) :: num_lagrange
    integer(kind=kint),         intent(in) :: slaves(:)
    ! type(hecmwST_contact_comm), intent(in) :: conCOMM      !< contact comm table for optimized communication
    real(kind=kreal),           intent(out) :: btot(:)     
    !
    integer(kind=kint) :: ndof, nndof, npndof, i, myrank
 
    myrank = hecmw_comm_get_rank()
 
    ndof = hecmat%NDOF
    npndof = hecmat%NP*ndof
    nndof  = hecmat%N *ndof
 
    if (debug_vector) call debug_write_vector(hecmat%B, 'RHS(hecMAT)', 'hecMAT%B', ndof, hecmat%N, &
        hecmat%NP, .false., num_lagrange, slaves)
    if (debug_vector) call debug_write_vector(conmat%B, 'RHS(conMAT)', 'conMAT%B', ndof, conmat%N, &
        conmat%NP, .true., num_lagrange, slaves)
 
    do i=1,npndof+num_lagrange
      btot(i) = conmat%B(i)
    enddo
 
    if (hecmw_comm_get_size() > 1) then
      ! next line cannot be: call hecmw_contact_comm_reduce_r(conCOMM, Btot, HECMW_SUM) !!! alag_tied fails
      call hecmw_assemble_r(hecmesh, btot, hecmat%NP, ndof)
      if (debug_vector) call debug_write_vector(btot, 'RHS(conMAT assembled)', 'Btot', ndof, conmat%N, &
          conmat%NP, .false., num_lagrange, slaves)
      if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: assemble RHS (conMAT%B) done'
    endif
 
    ! add hecMAT%B to Btot
    do i=1,nndof
      btot(i)=btot(i)+hecmat%B(i)
    enddo
    if (debug_vector) call debug_write_vector(btot, 'RHS(total)', 'Btot', ndof, conmat%N, &
        conmat%NP, .false., num_lagrange, slaves)
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: add hecMAT%B to RHS done'
  end subroutine assemble_rhs
 
  subroutine convert_equation(hecMESHtmp, hecMAT, Kmat, Tmat, Ttmat, Btot, slaves, &
      slaves4lag, BLs_inv, conCOMM, hecTKT)
    type(hecmwst_local_mesh),   intent(inout) :: hecmeshtmp
    type(hecmwst_matrix),       intent(in)    :: hecmat
    type(hecmwst_local_matrix), intent(inout) :: kmat
    type(hecmwst_local_matrix), intent(inout) :: tmat
    type(hecmwst_local_matrix), intent(in)    :: ttmat
    real(kind=kreal),           intent(in)    :: btot(:)       
    integer(kind=kint),         intent(in)    :: slaves(:)
    integer(kind=kint),         intent(in)    :: slaves4lag(:)
    real(kind=kreal),           intent(in)    :: bls_inv(:)    
    type(hecmwst_contact_comm), intent(in)    :: concomm
    type(hecmwst_matrix),       intent(out)   :: hectkt
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    call convert_matrix(hecmeshtmp, hecmat, ttmat, kmat, tmat, slaves, hectkt)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: converted matrix'
 
    call convert_rhs(hecmeshtmp, hecmat, hectkt, ttmat, kmat, &
        slaves4lag, bls_inv, btot, concomm)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: converted RHS'
  end subroutine convert_equation
 
  subroutine convert_matrix(hecMESHtmp, hecMAT, Ttmat, Kmat, Tmat, slaves, hecTKT)
    type(hecmwst_local_mesh),   intent(inout) :: hecmeshtmp
    type(hecmwst_matrix),       intent(in)    :: hecmat
    type(hecmwst_local_matrix), intent(in)    :: ttmat
    type(hecmwst_local_matrix), intent(inout) :: kmat
    type(hecmwst_local_matrix), intent(inout) :: tmat
    integer(kind=kint),         intent(in)    :: slaves(:)
    type(hecmwst_matrix),       intent(out)   :: hectkt
    !
    type(hecmwst_local_matrix) :: ttkmat, ttktmat
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    ! compute TtKmat = Ttmat * Kmat (updating hecMESHtmp)
    call hecmw_localmat_multmat(ttmat, kmat, hecmeshtmp, ttkmat)
    if (debug_matrix) call debug_write_matrix(ttkmat, 'TtKmat')
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: multiply Tt and K done'
 
    ! compute TtKTmat = TtKmat * Tmat (updating hecMESHtmp)
    call hecmw_localmat_multmat(ttkmat, tmat, hecmeshtmp, ttktmat)
    if (debug_matrix) call debug_write_matrix(ttktmat, 'TtKTmat')
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: multiply TtK and T done'
    call hecmw_localmat_free(ttkmat)
 
    ! shrink comm_table
    ! call hecmw_localmat_shrink_comm_table(TtKTmat, hecMESHtmp)
 
    call place_one_on_diag_of_slave_dof(ttktmat, slaves)
    if (debug_matrix) call debug_write_matrix(ttktmat, 'TtKTmat (place 1.0 on slave diag)')
 
    call hecmw_mat_init(hectkt)
    call hecmw_localmat_make_hecmat(hecmat, ttktmat, hectkt)
    if (debug >= 3) write(0,*) '    DEBUG3[',myrank,']: convert TtKT to hecTKT done'
    call hecmw_localmat_free(ttktmat)
  end subroutine convert_matrix
 
  subroutine place_one_on_diag_of_slave_dof(TtKTmat, slaves)
    type(hecmwst_local_matrix), intent(inout) :: ttktmat
    integer(kind=kint),         intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: ndof, ndof2, i, irow, idof, js, je, j, jcol
 
    ndof = ttktmat%ndof
    ndof2 = ndof*ndof
    do i = 1, size(slaves)
      irow = (slaves(i)+ndof-1)/ndof
      idof = slaves(i)-ndof*(irow-1)
      js = ttktmat%index(irow-1)+1
      je = ttktmat%index(irow)
      do j = js, je
        jcol = ttktmat%item(j)
        if (irow /= jcol) cycle
        if (abs(ttktmat%A(ndof2*(j-1)+ndof*(idof-1)+idof)) > tiny(0.0d0)) &
            stop 'ERROR: nonzero diag on slave dof of TtKTmat'
        ttktmat%A(ndof2*(j-1)+ndof*(idof-1)+idof) = 1.0d0
      enddo
    enddo
  end subroutine place_one_on_diag_of_slave_dof
 
  subroutine convert_rhs(hecMESHtmp, hecMAT, hecTKT, Ttmat, Kmat, &
       slaves4lag, BLs_inv, Btot, conCOMM)
    type(hecmwst_local_mesh),   intent(in)    :: hecmeshtmp
    type(hecmwst_matrix),       intent(in)    :: hecmat
    type(hecmwst_matrix),       intent(inout) :: hectkt
    type(hecmwst_local_matrix), intent(in)    :: ttmat
    type(hecmwst_local_matrix), intent(in)    :: kmat
    integer(kind=kint),         intent(in)    :: slaves4lag(:)
    real(kind=kreal),           intent(in)    :: bls_inv(:)    
    real(kind=kreal), target,   intent(in)    :: btot(:)       
    type(hecmwst_contact_comm), intent(in)    :: concomm
    !
    real(kind=kreal), allocatable :: btmp(:)
    real(kind=kreal), pointer :: blag(:)
    integer(kind=kint) :: ndof, npndof, nndof, npndof_new, num_lagrange, i
 
    ! SIZE:
    ! Btot     <=> hecMAT, hecMESH
    ! Btmp     <=> Kmat, hecMESHtmp
    ! hecTKT%B <=> hecTKT, hecMESHtmp
    ndof = hecmat%NDOF
    npndof     = hecmat%NP*ndof
    nndof      = hecmat%N *ndof
    npndof_new = hectkt%NP*ndof
    num_lagrange = size(slaves4lag)
 
    allocate(hectkt%B(npndof_new), source=0.d0)
    allocate(hectkt%X(npndof_new), source=0.d0)
    allocate(btmp(npndof_new))
    !
    !! Ttmat*(B+K*(-Bs^-1)*Blag)
    !
    ! B2=-Bs^-1*Blag
    blag => btot(npndof+1:npndof+num_lagrange)
    hectkt%B(slaves4lag(:))=-bls_inv(:)*blag(:)
    ! send external contact dof => recv internal contact dof
    ! next line can be: call hecmw_assemble_R(hecMESHtmp, hecTKT%B, hecTKT%NP, ndof)
    call hecmw_contact_comm_reduce_r(concomm, hectkt%B, hecmw_sum)
    ! Btmp=B+K*B2 (including update of hecTKT%B)
    call hecmw_update_r(hecmeshtmp, hectkt%B, hectkt%NP, ndof)
    call hecmw_localmat_mulvec(kmat, hectkt%B, btmp)
    do i=1,nndof
      btmp(i)=btot(i)+btmp(i)
    enddo
    ! B2=Ttmat*Btmp
    call hecmw_update_r(hecmeshtmp, btmp, hectkt%NP, ndof)
    call hecmw_localmat_mulvec(ttmat, btmp, hectkt%B)
    deallocate(btmp)
 
    if (debug_vector) call debug_write_vector(hectkt%B, 'RHS(converted)', 'hecTKT%B', ndof, hectkt%N)
  end subroutine convert_rhs
 
  subroutine recover_solution(hecMESHtmp, hecMAT, hecTKT, Tmat, Kmat, Btot, &
      slaves4lag, BLs_inv, BUs_inv, conCOMM, slaves)
    ! type(hecmwST_local_mesh),   intent(in)    :: hecMESH  !< original mesh
    type(hecmwst_local_mesh),   intent(in)    :: hecmeshtmp
    type(hecmwst_matrix),       intent(inout) :: hecmat
    type(hecmwst_matrix),       intent(inout) :: hectkt
    type(hecmwst_local_matrix), intent(in)    :: tmat
    type(hecmwst_local_matrix), intent(in)    :: kmat
    real(kind=kreal),           intent(in)    :: btot(:)       
    integer(kind=kint),         intent(in)    :: slaves4lag(:)
    real(kind=kreal),           intent(in)    :: bls_inv(:)    
    real(kind=kreal),           intent(in)    :: bus_inv(:)    
    type(hecmwst_contact_comm), intent(in)    :: concomm
    integer(kind=kint),         intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
 
    hecmat%Iarray=hectkt%Iarray
    hecmat%Rarray=hectkt%Rarray
 
    call comp_x_slave(hecmeshtmp, hecmat, hectkt, tmat, btot, &
        slaves4lag, bls_inv, concomm, slaves)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: recovered slave disp'
 
    call comp_lag(hecmeshtmp, hecmat, hectkt, kmat, btot, &
        slaves4lag, bus_inv, concomm, slaves)
    if (debug >= 2) write(0,*) '  DEBUG2[',myrank,']: recovered lag'
 
    if (debug_vector) call debug_write_vector(hecmat%X, 'Solution(original)', 'hecMAT%X', hecmat%NDOF, hecmat%N, &
        hecmat%NP, .false., size(slaves4lag), slaves)
  end subroutine recover_solution
 
  subroutine comp_x_slave(hecMESHtmp, hecMAT, hecTKT, Tmat, Btot, &
       slaves4lag, BLs_inv, conCOMM, slaves)
    ! type(hecmwST_local_mesh),   intent(in)    :: hecMESH  !< original mesh
    type(hecmwst_local_mesh),   intent(in)    :: hecmeshtmp
    type(hecmwst_matrix),       intent(inout) :: hecmat
    type(hecmwst_matrix),       intent(in)    :: hectkt
    type(hecmwst_local_matrix), intent(in)    :: tmat
    real(kind=kreal), target,   intent(in)    :: btot(:)       
    integer(kind=kint),         intent(in)    :: slaves4lag(:)
    real(kind=kreal),           intent(in)    :: bls_inv(:)    
    type(hecmwst_contact_comm), intent(in)    :: concomm
    integer(kind=kint),         intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: ndof, ndof2, npndof, nndof, num_lagrange
    real(kind=kreal), allocatable :: xtmp(:)
    real(kind=kreal), pointer :: blag(:)
 
    ndof = hecmat%NDOF
    ndof2 = ndof*ndof
    npndof = hecmat%NP * ndof
    nndof  = hecmat%N  * ndof
    num_lagrange = size(slaves4lag)
    !!
    !! {X} = [T] {Xp} - [-Bs^-1] {c}
    !!
    ! compute {X} = [T] {Xp}
    call hecmw_update_r(hecmeshtmp, hectkt%X, hectkt%NP, ndof)
    call hecmw_localmat_mulvec(tmat, hectkt%X, hecmat%X)
    !
    ! compute {Xtmp} = [-Bs^-1] {c}
    allocate(xtmp(npndof), source=0.0d0)
    blag => btot(npndof+1:npndof+num_lagrange)
    xtmp(slaves4lag(:)) = -bls_inv(:) * blag(:)
    !
    ! send external contact dof => recv internal contact dof
    ! next line can be: call hecmw_assemble_R(hecMESH, Xtmp, hecMAT%NP, ndof)
    call hecmw_contact_comm_reduce_r(concomm, xtmp, hecmw_sum)
    !
    ! {X} = {X} - {Xtmp}
    hecmat%X(slaves(:)) = hecmat%X(slaves(:)) - xtmp(slaves(:))
    deallocate(xtmp)
  end subroutine comp_x_slave
 
  subroutine comp_lag(hecMESHtmp, hecMAT, hecTKT, Kmat, Btot, &
       slaves4lag, BUs_inv, conCOMM, slaves)
    ! type(hecmwST_local_mesh),   intent(in)    :: hecMESH  !< original mesh
    type(hecmwst_local_mesh),   intent(in)    :: hecmeshtmp
    type(hecmwst_matrix),       intent(inout) :: hecmat
    type(hecmwst_matrix),       intent(inout) :: hectkt
    type(hecmwst_local_matrix), intent(in)    :: kmat
    real(kind=kreal),           intent(in)    :: btot(:)       
    integer(kind=kint),         intent(in)    :: slaves4lag(:)
    real(kind=kreal),           intent(in)    :: bus_inv(:)    
    type(hecmwst_contact_comm), intent(in)    :: concomm
    integer(kind=kint),         intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: ndof, npndof, nndof, npndof_new, num_lagrange
    real(kind=kreal), allocatable :: btmp(:)
    real(kind=kreal), pointer :: xlag(:)
 
    ndof=hecmat%ndof
    npndof = hecmat%NP * ndof
    nndof  = hecmat%N * ndof
    npndof_new = hectkt%NP * ndof
    num_lagrange = size(slaves4lag)
 
    !! <SUMMARY>
    !! {lag} = [Bs^-T] ( {fs} - [Ksp Kss] {u} )
    !!
    ! 1. {Btmp} = [Kmat] {X}
    hectkt%X(1:nndof) = hecmat%X(1:nndof)
    call hecmw_update_r(hecmeshtmp, hectkt%X, hectkt%NP, ndof)
    allocate(btmp(npndof))
    call hecmw_localmat_mulvec(kmat, hectkt%X, btmp)
    !
    ! 2. {Btmp_s} = {fs} - {Btmp_s}
    btmp(slaves(:)) = btot(slaves(:)) - btmp(slaves(:))
    !
    ! 3. send internal contact dof => recv external contact dof
    ! next line can be: call hecmw_update_R(hecMESH, Btmp, hecMAT%NP, ndof)
    call hecmw_contact_comm_bcast_r(concomm, btmp)
    !
    ! 4. {lag} = [Bs^-T] {Btmp_s}
    xlag => hecmat%X(npndof+1:npndof+num_lagrange)
    xlag(:)=bus_inv(:)*btmp(slaves4lag(:))
    deallocate(btmp)
  end subroutine comp_lag
 
  subroutine check_solution(hecMESH, hecMESHtmp, hecMAT, hecTKT, hecLagMAT, Kmat, Btot, &
       conCOMM, slaves)
    type(hecmwst_local_mesh),       intent(in)    :: hecmesh
    type(hecmwst_local_mesh),       intent(in)    :: hecmeshtmp
    type(hecmwst_matrix),           intent(inout) :: hecmat
    type(hecmwst_matrix),           intent(inout) :: hectkt
    type(hecmwst_matrix_lagrange) , intent(in)    :: heclagmat
    type(hecmwst_local_matrix),     intent(in)    :: kmat
    real(kind=kreal), target,       intent(in)    :: btot(:)    
    type(hecmwst_contact_comm),     intent(in)    :: concomm
    integer(kind=kint),             intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: ndof, nndof, npndof, num_lagrange, i, ls, le, l, j, idof, jdof
    real(kind=kreal), allocatable, target :: r(:)
    real(kind=kreal), allocatable :: btmp(:)
    real(kind=kreal), pointer :: rlag(:), blag(:), xlag(:)
    real(kind=kreal) :: rnrm2, rlagnrm2
    real(kind=kreal) :: bnrm2, blagnrm2
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    ndof = hecmat%NDOF
    nndof = hecmat%N * ndof
    npndof = hecmat%NP * ndof
    num_lagrange = heclagmat%num_lagrange
    !
    allocate(r(npndof + num_lagrange), source=0.0d0)
    allocate(btmp(npndof))
    !
    rlag => r(npndof+1:npndof+num_lagrange)
    blag => btot(npndof+1:npndof+num_lagrange)
    xlag => hecmat%X(npndof+1:npndof+num_lagrange)
    !
    !! {r}    = {b} - [K] {x} - [Bt] {lag}
    !! {rlag} = {c} - [B] {x}
    !
    ! {r} = {b} - [K] {x}
    do i = 1, nndof
      hectkt%X(i) = hecmat%X(i)
    enddo
    call hecmw_update_r(hecmeshtmp, hectkt%X, hectkt%NP, ndof)
    call hecmw_localmat_mulvec(kmat, hectkt%X, btmp)
    do i = 1, nndof
      r(i) = btot(i) - btmp(i)
    enddo
    !
    ! {r} = {r} - [Bt] {lag}
    btmp(:) = 0.0d0
    if (heclagmat%num_lagrange > 0) then
      do i = 1, hecmat%NP
        ls = heclagmat%indexU_lagrange(i-1)+1
        le = heclagmat%indexU_lagrange(i)
        do l = ls, le
          j = heclagmat%itemU_lagrange(l)
          do idof = 1, ndof
            btmp(ndof*(i-1)+idof) = btmp(ndof*(i-1)+idof) + heclagmat%AU_lagrange(ndof*(l-1)+idof) * xlag(j)
          enddo
        enddo
      enddo
    endif
    ! next line can be: call hecmw_assemble_R(hecMESH, Btmp, hecMAT%NP, ndof)
    call hecmw_contact_comm_reduce_r(concomm, btmp, hecmw_sum)
    do i = 1, nndof
      r(i) = r(i) - btmp(i)
    enddo
    !
    ! {rlag} = {c} - [B] {x}
    call hecmw_update_r(hecmesh, hecmat%X, hecmat%NP, ndof)
    do i = 1, num_lagrange
      rlag(i) = blag(i)
      ls = heclagmat%indexL_lagrange(i-1)+1
      le = heclagmat%indexL_lagrange(i)
      do l = ls, le
        j = heclagmat%itemL_lagrange(l)
        do jdof = 1, ndof
          rlag(i) = rlag(i) - heclagmat%AL_lagrange(ndof*(l-1)+jdof) * hecmat%X(ndof*(j-1)+jdof)
        enddo
      enddo
    enddo
    !
    ! residual in original system
    if (debug_vector) call debug_write_vector(r, 'Residual', 'R', ndof, hecmat%N, &
        hecmat%NP, .false., heclagmat%num_lagrange, slaves)
    !
    call hecmw_innerproduct_r(hecmesh, ndof, r, r, rnrm2)
    call hecmw_innerproduct_r(hecmesh, ndof, btot, btot, bnrm2)
    rlagnrm2 = dot_product(rlag, rlag)
    call hecmw_allreduce_r1(hecmesh, rlagnrm2, hecmw_sum)
    blagnrm2 = dot_product(blag, blag)
    call hecmw_allreduce_r1(hecmesh, blagnrm2, hecmw_sum)
    !
    if (myrank == 0) then
      write(0,*) 'INFO: resid(x,lag,tot)',sqrt(rnrm2),sqrt(rlagnrm2),sqrt(rnrm2+rlagnrm2)
      write(0,*) 'INFO: rhs  (x,lag,tot)',sqrt(bnrm2),sqrt(blagnrm2),sqrt(bnrm2+blagnrm2)
    endif
  end subroutine check_solution
 
  subroutine check_solution2(hecMESH, hecMAT, conMAT, hecLagMAT, conCOMM, slaves)
    implicit none
    type(hecmwst_local_mesh),       intent(in)    :: hecmesh
    type(hecmwst_matrix),           intent(inout) :: hecmat
    type(hecmwst_matrix),           intent(in)    :: conmat
    type(hecmwst_matrix_lagrange) , intent(in)    :: heclagmat
    type(hecmwst_contact_comm),     intent(in)    :: concomm
    integer(kind=kint),             intent(in)    :: slaves(:)
    !
    integer(kind=kint) :: ndof, ndof2, nndof, npndof, num_lagrange
    integer(kind=kint) :: i, idof, j, jdof, ls, le, l
    integer(kind=kint) :: irow, js, je, jcol
    real(kind=kreal), allocatable, target :: r(:)
    real(kind=kreal), allocatable :: r_con(:)
    real(kind=kreal), pointer :: rlag(:), blag(:), xlag(:)
    real(kind=kreal) :: rnrm2, rlagnrm2
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    ndof = hecmat%NDOF
    ndof2 = ndof*ndof
    nndof = hecmat%N * ndof
    npndof = hecmat%NP * ndof
    num_lagrange = heclagmat%num_lagrange
    !
    allocate(r(npndof + num_lagrange))
    r(:) = 0.0d0
    allocate(r_con(npndof))
    r_con(:) = 0.0d0
    !
    rlag => r(npndof+1:npndof+num_lagrange)
    blag => conmat%B(npndof+1:npndof+num_lagrange)
    xlag => hecmat%X(npndof+1:npndof+num_lagrange)
    !
    !! <SUMMARY>
    !! {r}    = {b} - [K] {x} - [Bt] {lag}
    !! {rlag} = {c} - [B] {x}
    !
    ! 1. {r} = {r_org} + {r_con}
    ! {r_org} = {b_org} - [hecMAT] {x}
    ! {r_con} = {b_con} - [conMAT] {x} - [Bt] {lag}
    !
    ! 1.1 {r_org}
    ! {r} = {b} - [K] {x}
    call hecmw_matresid(hecmesh, hecmat, hecmat%X, hecmat%B, r)
    !
    if (debug_vector) call debug_write_vector(r, 'Residual(original)', 'R', ndof, hecmat%N, &
        hecmat%NP, .false., num_lagrange, slaves)
    !
    ! 1.2 {r_con}
    ! 1.2.1 {r_con} = {b_con} - [conMAT]{x}
    ! Note: compute not only internal DOFs but also external DOFs, locally
    ! !call hecmw_update_3_R(hecMESH, hecMAT%X, hecMAT%NP)  ! X is already updated
    do i = 1, npndof
      r_con(i) = conmat%B(i)
    enddo
    do irow = 1,hecmat%NP
      ! lower
      js = conmat%indexL(irow-1)+1
      je = conmat%indexL(irow)
      do j = js, je
        jcol = conmat%itemL(j)
        do idof = 1, ndof
          i = ndof*(irow-1)+idof
          do jdof = 1, ndof
            r_con(i) = r_con(i) - conmat%AL(ndof2*(j-1)+ndof*(idof-1)+jdof) * hecmat%X(ndof*(jcol-1)+jdof)
          enddo
        enddo
      enddo
      ! diag
      do idof = 1, ndof
        i = ndof*(irow-1)+idof
        do jdof = 1, ndof
          r_con(i) = r_con(i) - conmat%D(ndof2*(irow-1)+ndof*(idof-1)+jdof) * hecmat%X(ndof*(irow-1)+jdof)
        enddo
      enddo
      ! upper
      js = conmat%indexU(irow-1)+1
      je = conmat%indexU(irow)
      do j = js, je
        jcol = conmat%itemU(j)
        do idof = 1, ndof
          i = ndof*(irow-1)+idof
          do jdof = 1, ndof
            r_con(i) = r_con(i) - conmat%AU(ndof2*(j-1)+ndof*(idof-1)+jdof) * hecmat%X(ndof*(jcol-1)+jdof)
          enddo
        enddo
      enddo
    end do
    !
    ! 1.2.2 {r_con} = {r_con} - [Bt] {lag}
    if (num_lagrange > 0) then
      do i = 1, hecmat%NP
        ls = heclagmat%indexU_lagrange(i-1)+1
        le = heclagmat%indexU_lagrange(i)
        do l = ls, le
          j = heclagmat%itemU_lagrange(l)
          do idof = 1, ndof
            r_con(ndof*(i-1)+idof) = r_con(ndof*(i-1)+idof) - heclagmat%AU_lagrange(ndof*(l-1)+idof) * xlag(j)
          enddo
        enddo
      enddo
    endif
    !
    if (debug_vector) call debug_write_vector(r, 'Residual(contact,local)', 'R_con', ndof, hecmat%N, &
        hecmat%NP, .true., num_lagrange, slaves)
    !
    ! 1.2.3 send external part of {r_con}
    call hecmw_contact_comm_reduce_r(concomm, r_con, hecmw_sum)
    !
    if (debug_vector) call debug_write_vector(r, 'Residual(contact,assembled)', 'R_con', ndof, hecmat%N, &
        hecmat%NP, .false., num_lagrange, slaves)
    !
    ! 1.3 {r} = {r_org} + {r_con}
    do i = 1,nndof
      r(i) = r(i) + r_con(i)
    enddo
    !
    if (debug_vector) call debug_write_vector(r, 'Residual(total)', 'R', ndof, hecmat%N, &
        hecmat%NP, .false., num_lagrange, slaves)
    !
    ! 2.  {rlag} = {c} - [B] {x}
    !call hecmw_update_3_R(hecMESH, hecMAT%X, hecMAT%NP)  ! X is already updated
    do i = 1, num_lagrange
      rlag(i) = blag(i)
      ls = heclagmat%indexL_lagrange(i-1)+1
      le = heclagmat%indexL_lagrange(i)
      do l = ls, le
        j = heclagmat%itemL_lagrange(l)
        do jdof = 1, ndof
          rlag(i) = rlag(i) - heclagmat%AL_lagrange(ndof*(l-1)+jdof) * hecmat%X(ndof*(j-1)+jdof)
        enddo
      enddo
    enddo
    !
    if (debug_vector) then
      write(1000+myrank,*) 'Residual(lagrange)-----------------------------------------------------'
      if (num_lagrange > 0) then
        write(1000+myrank,*) 'R(lag):',npndof+1,'-',npndof+num_lagrange
        write(1000+myrank,*) r(npndof+1:npndof+num_lagrange)
      endif
    endif
    !
    call hecmw_innerproduct_r(hecmesh, ndof, r, r, rnrm2)
    rlagnrm2 = dot_product(rlag, rlag)
    call hecmw_allreduce_r1(hecmesh, rlagnrm2, hecmw_sum)
    !
    if (myrank == 0) write(0,*) 'INFO: resid(x,lag,tot)',sqrt(rnrm2),sqrt(rlagnrm2),sqrt(rnrm2+rlagnrm2)
  end subroutine check_solution2
 
  subroutine debug_write_matrix(Mat, label)
    type(hecmwst_local_matrix), intent(in) :: mat
    character(len=*),           intent(in) :: label
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    write(1000+myrank,*) trim(label),'============================================================'
    call hecmw_localmat_write(mat, 1000+myrank)
  end subroutine debug_write_matrix
 
  subroutine debug_write_vector(Vec, label, name, ndof, N, &
      NP, write_ext, num_lagrange, slaves)
    real(kind=kreal),   intent(in) :: vec(:) 
    character(len=*),   intent(in) :: label
    character(len=*),   intent(in) :: name
    integer(kind=kint), intent(in) :: ndof
    integer(kind=kint), intent(in) :: n
    integer(kind=kint), intent(in), optional :: np
    logical,            intent(in), optional :: write_ext
    integer(kind=kint), intent(in), optional :: num_lagrange
    integer(kind=kint), intent(in), optional :: slaves(:)
    !
    integer(kind=kint) :: myrank
 
    myrank = hecmw_comm_get_rank()
    write(1000+myrank,*) trim(label),'------------------------------------------------------------'
    write(1000+myrank,*) 'size of ',trim(name),size(vec)
    write(1000+myrank,*) trim(name),': 1-',n*ndof
    write(1000+myrank,*) vec(1:n*ndof)
    if (present(write_ext) .and. present(np)) then
      if (write_ext) then
        write(1000+myrank,*) trim(name),'(external): ',n*ndof+1,'-',np*ndof
        write(1000+myrank,*) vec(n*ndof+1:np*ndof)
      endif
    endif
    if (present(num_lagrange) .and. present(np)) then
      if (num_lagrange > 0) then
        write(1000+myrank,*) trim(name),'(lag):',np*ndof+1,'-',np*ndof+num_lagrange
        write(1000+myrank,*) vec(np*ndof+1:np*ndof+num_lagrange)
      endif
    endif
    if (present(slaves)) then
      if (size(slaves) > 0) then
        write(1000+myrank,*) trim(name),'(slave):',slaves(:)
        write(1000+myrank,*) vec(slaves(:))
      endif
    endif
  end subroutine debug_write_vector
 
end module m_solve_lineq_contact_elim