fstr_solve_NonLinear.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_fstr_nonlinearmethod
 
  use m_fstr
  use m_static_lib
  use m_static_output
 
  use m_fstr_spring
  use m_fstr_creatematrix_and_dampingforce
  use m_fstr_update
  use m_fstr_nodalkinematics, only: fstr_apply_solution_increment, fstr_commit_solution_increment
  use m_fstr_ass_load
  use m_fstr_addbc
  use m_fstr_residual
  use m_fstr_restart
  use m_fstr_elemact
  use m_fstr_iterationcontrol
  use fstr_matrix_con_contact
  use mcontact
  use m_fstr_contact_assembly
  use m_solve_lineq_contact
  use hecmw_ebc_defer
 
  implicit none
 
contains
 
  subroutine fstr_init_newton(hecMESH, hecMAT, fstrSOLID, &
      &  ctime, tincr, iter, cstep, dtime, fstrPARAM, hecLagMAT, ndof, ctAlgo, conMAT)
    use m_fstr_update
    implicit none
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    real(kind=kreal), intent(inout)       :: tincr
    integer(kind=kint)                    :: iter
    integer, intent(in)                   :: cstep
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
    integer(kind=kint), intent(inout)     :: ndof
    integer(kind=kint), intent(in)        :: ctAlgo
    type (hecmwST_matrix)                 :: conMAT
 
    hecmat%NDOF = hecmesh%n_dof
    ndof = hecmat%ndof
 
    ! direct solvers choose their factorization mode from this flag (default .true.),
    ! which nothing on the non-contact path would otherwise set; iterative solvers
    ! overwrite it from the solution method
    hecmat%symmetric = fstr_is_tangent_symmetric(fstrsolid, hecmesh)
 
    tincr = dtime
    if( fstrsolid%step_ctrl(cstep)%solution == stepstatic ) tincr = 0.d0
 
    fstrsolid%dunode(:) = 0.0d0
    fstrsolid%NRstat_i(:) = 0 ! logging newton iteration(init)
 
    call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
 
    if( fstrsolid%elemact%ELEMACT_egrp_tot > 0 ) then
      call fstr_updatenewton(hecmesh, hecmat, fstrsolid, fstrparam, ctime, tincr, 0)
      call fstr_update_ndforce(cstep, hecmesh, hecmat, fstrsolid)
    endif
 
    ! conMAT is hecMAT itself for the contact-free callers (fstr_Newton, fstr_Quasi_Newton);
    ! clearing it there would wipe the external load just assembled by fstr_ass_load.
    if( associated(conmat%B) .and. .not. associated(conmat%B, hecmat%B) ) call hecmw_mat_clear_b(conmat)
    if( fstr_is_contact_active() ) then
      call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
      !    Consider SPC condition
      call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
      call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
    endif
 
  end subroutine fstr_init_newton
 
  subroutine fstr_newton( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM, &
      restrt_step_num, sub_step, ctime, dtime )
 
    integer, intent(in)                   :: cstep
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    integer, intent(in)                   :: sub_step
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
 
    type (hecmwST_local_mesh), pointer :: hecMESHmpc
    type (hecmwST_matrix), pointer :: hecMATmpc
    type (hecmwST_ebc) :: hecEBC
    integer(kind=kint) :: ndof
    integer(kind=kint) :: iter
    integer(kind=kint) :: stepcnt
    integer(kind=kint) :: restrt_step_num
    real(kind=kreal)   :: tincr
    real(kind=kreal), allocatable :: coord(:), p(:)
    logical :: isLinear = .false.
    integer(kind=kint) :: iterStatus
    type(fstr_convergence_state) :: cnvstat
 
    call hecmw_mpc_mat_init(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
 
    if(.not. fstrparam%nlgeom)then
      islinear = .true.
    endif
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof, 0, hecmat)
 
    allocate(p(hecmesh%n_node*ndof))
    allocate(coord(hecmesh%n_node*ndof))
    p = 0.0d0
    stepcnt = 0
 
    ! ----- Inner Iteration, lagrange multiplier constant
    do iter=1,fstrsolid%step_ctrl(cstep)%max_iter
      stepcnt = stepcnt+1
 
      call fstr_creatematrix_and_dampingforce( hecmesh, hecmat, fstrsolid, ctime, tincr )
      call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
      ! ----- Set Boundary condition
      call hecmw_ebc_init(hecmat, hecebc)
      call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt, hecebc)
      call hecmw_mpc_mat_ass(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
      call hecmw_mpc_trans_rhs(hecmesh, hecmat, hecmatmpc)
      call hecmw_ebc_apply(hecmeshmpc, hecmatmpc, hecebc)
 
      !----- SOLVE [Kt]{du}={R}
      if( sub_step == restrt_step_num .and. iter == 1 ) hecmatmpc%Iarray(98) = 1
      if( iter == 1 ) then
        hecmatmpc%Iarray(97) = 2   !Force numerical factorization
      else
        hecmatmpc%Iarray(97) = 1   !Need numerical factorization
      endif
      hecmatmpc%X = 0.0d0
      call fstr_set_current_config_to_mesh(hecmeshmpc,fstrsolid,coord)
      call solve_lineq(hecmeshmpc,hecmatmpc)
      call fstr_recover_initial_config_to_mesh(hecmeshmpc,fstrsolid,coord)
      ! ----- check matrix solver error
      call fstr_check_linear_solver(hecmesh, hecmatmpc, fstrsolid, cstep, sub_step, iterstatus)
      if( iterstatus /= kitrcontinue ) then
        call hecmw_ebc_finalize(hecebc)
        call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
        return
      endif
      call hecmw_mpc_tback_sol(hecmesh, hecmat, hecmatmpc)
 
      ! ----- update the small displacement and the displacement for 1step
      !       \delta u^k => solver's solution
      !       \Delta u_{n+1}^{k} = \Delta u_{n+1}^{k-1} + \delta u^k
      call fstr_apply_solution_increment( hecmesh, fstrsolid, ndof, hecmat%X )
 
      call fstr_calc_residual_vector(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam)
 
      if( islinear ) exit
 
      ! ----- check convergence
      call fstr_check_convergence(hecmesh, hecmat, fstrsolid, fstrparam, &
          ndof, iter, sub_step, cstep, &
          hecmat%B, cnvstat, iterstatus)
      if (iterstatus == kitrconverged) exit
      if (iterstatus == kitrdiverged .or. iterstatus==kitrfloatingerror) then
        call hecmw_ebc_finalize(hecebc)
        call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
        return
      endif
    enddo
    ! ----- end of inner loop
 
    fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
    fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
    ! ----- update the total displacement
    ! u_{n+1} = u_{n} + \Delta u_{n+1}
    call fstr_commit_solution_increment( hecmesh, fstrsolid, ndof )
 
    call fstr_updatestate( hecmesh, fstrsolid, tincr )
    !    Update REACTION using current QFORCE
    call fstr_update_reaction_spc( cstep, hecmesh, fstrsolid )
 
    fstrsolid%CutBack_stat = 0
    deallocate(coord)
    deallocate(p)
    call hecmw_ebc_finalize(hecebc)
    call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
  end subroutine fstr_newton
 
  subroutine fstr_newton_contactalag( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM,                   &
      restart_step_num, restart_substep_num, sub_step, ctime, dtime, infoCTChange, conMAT )
 
    integer, intent(in)                   :: cstep
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    integer, intent(in)                   :: sub_step
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    type (fstr_info_contactChange)        :: infoCTChange
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
    type (hecmwST_matrix)                 :: conMAT
 
    type (hecmwST_ebc) :: hecEBC
    integer(kind=kint) :: ndof
    integer(kind=kint) :: ctAlgo
    integer(kind=kint) :: iter
    integer(kind=kint) :: al_step, n_al_step, stepcnt, count_step
    real(kind=kreal)   :: tincr
    integer(kind=kint) :: restart_step_num, restart_substep_num
    logical            :: ctchange
    integer(kind=kint) :: contact_changed_global
    logical            :: need_prof_refresh
    real(kind=kreal), allocatable :: coord(:)
    integer(kind=kint)  :: istat
    logical            :: is_first_Stiffmatrixcall
    integer(kind=kint)  :: iterStatus, nresid
    real(kind=kreal), allocatable :: resid_work(:)
    type(fstr_convergence_state) :: cnvstat
 
    ctalgo = fstrparam%contact_algo
 
    ! ----- BEGIN lambda transaction: clear working before fstr_init_Newton reads the multipliers -----
    if( ctalgo == kcaalagrange ) call fstr_begin_lambda_txn( fstrsolid, cstep )
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof, ctalgo, conmat)
 
    if( cstep == 1 .and. sub_step == restart_substep_num ) then
      call fstr_save_originalmatrixstructure(hecmat)
      if( restart_step_num > 1 .or. restart_substep_num > 1 ) then
        call fstr_set_contact_active_restart( cstep, fstrsolid, infoctchange )
        infoctchange%contactNode_current = infoctchange%contactNode_previous
      else
        if(hecmesh%my_rank==0) write(*,*) "---Scanning initial contact state---"
        call fstr_scan_contact_state( cstep, sub_step, 0, dtime, ctalgo, hecmesh, fstrsolid, infoctchange )
      endif
      call hecmw_mat_copy_profile( hecmat, conmat )
      if ( fstr_is_contact_active() ) then
        call fstr_mat_con_contact(cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
      elseif( hecmat%Iarray(99)==4 ) then
        write(*, *) ' This type of direct solver is not yet available in such case ! '
        write(*, *) ' Please change the solver type to intel MKL direct solver !'
        call hecmw_abort(hecmw_comm_get_comm())
      endif
      ! the ALag contact terms are treated as symmetric here, unless a contact pair
      ! asks for the friction cone to follow the normal force, which adds the unsymmetric
      ! coupling block of getContactStiffness_Alag; material-induced asymmetry is propagated
      ! by the same predicate
      call solve_lineq_contact_init(hecmesh, hecmat, heclagmat, fstr_is_contactalag_symmetric(fstrsolid, hecmesh))
    endif
 
    hecmat%X = 0.0d0
 
    stepcnt = 0
    allocate(coord(hecmesh%n_node*ndof))
    allocate(resid_work(hecmesh%n_node*ndof + conmat%NP*ndof))
 
    ! ----- Augmentation loop. In case of no contact, it is inactive
    n_al_step = fstrparam%augiter
    count_step = 0
    is_first_stiffmatrixcall = .true.
 
    loopforcontactanalysis: do while( .true. )
      count_step = count_step + 1
 
      do al_step = 1, n_al_step
 
        if( hecmesh%my_rank == 0) then
          write(*,*) "Contact iter: ", count_step, " Augmentation iter: ", al_step
        end if
 
        ! ----- Inner Iteration, lagrange multiplier constant
        do iter = 1,fstrsolid%step_ctrl(cstep)%max_iter
          stepcnt = stepcnt+1
 
          call fstr_creatematrix_and_dampingforce( hecmesh, hecmat, fstrsolid, ctime, tincr )
          call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
          call hecmw_mat_clear( conmat )
          conmat%X = 0.0d0
 
          ! ----- Contact
          if( is_first_stiffmatrixcall ) then
            call fstr_calc_contact_refstiff(cstep, hecmesh, hecmat, fstrsolid)
            is_first_stiffmatrixcall = .false.
          endif
          if( fstr_is_contact_active() ) then
            call fstr_addcontactstiffness(cstep,ctalgo,iter,hecmesh,conmat,heclagmat,fstrsolid)
          endif
 
          ! ----- Set Boundary condition
          call hecmw_ebc_init(hecmat, hecebc)
          call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt, hecebc, conmat)
 
          !----- SOLVE [Kt]{du}={R}
          ! ----  For Parallel Contact with Multi-Partition Domains
          hecmat%X = 0.0d0
          call fstr_set_current_config_to_mesh(hecmesh,fstrsolid,coord)
          call solve_lineq_contact(hecmesh, hecmat, heclagmat, conmat, hecebc, istat, 1.0d0, fstr_is_contact_active())
          call fstr_recover_initial_config_to_mesh(hecmesh,fstrsolid,coord)
          call hecmw_ebc_finalize(hecebc)
          ! ----- check matrix solver error
          call fstr_check_linear_solver(hecmesh, hecmat, fstrsolid, cstep, sub_step, iterstatus, istat)
          if( iterstatus /= kitrcontinue ) then
            fstrsolid%NRstat_i(knstciter) = count_step
            return
          endif
 
          call hecmw_update_r (hecmesh, hecmat%X, hecmat%NP, hecmesh%n_dof)
 
          ! ----- update the small displacement and the displacement for 1step
          !       \delta u^k => solver's solution
          !       \Delta u_{n+1}^{k} = \Delta u_{n+1}^{k-1} + \delta u^k
          call fstr_apply_solution_increment( hecmesh, fstrsolid, ndof, hecmat%X )
 
          ! ----- update the strain, stress, and internal force
          call fstr_updatenewton(hecmesh, hecmat, fstrsolid, fstrparam, ctime, tincr, iter)
 
          if( fstrsolid%elemact%ELEMACT_egrp_tot > 0 ) then
            call fstr_update_elemact_solid_by_value( hecmesh, fstrsolid, cstep, ctime+dtime )
          endif
 
          ! ----- Set residual
          if( fstrsolid%DLOAD_follow /= 0 .or. fstrsolid%CLOAD_ngrp_rot /= 0 )                                 &
            call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
 
          call fstr_update_ndforce(cstep, hecmesh, hecmat, fstrsolid, conmat)
          call hecmw_mat_clear_b( conmat )
          call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
 
          !    Consider SPC condition
          call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
          call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
 
          call fstr_assemble_residual_contact(hecmat, heclagmat, conmat, hecmesh, resid_work, nresid)
          call fstr_check_convergence(hecmesh, hecmat, fstrsolid, fstrparam, &
              ndof, iter, sub_step, cstep, &
              resid_work, cnvstat, iterstatus, heclagmat)
          if (iterstatus == kitrconverged) exit
          if (iterstatus == kitrdiverged .or. iterstatus == kitrfloatingerror) then
            fstrsolid%NRstat_i(knstciter) = count_step
            return
          endif
 
        enddo
        ! ----- end of inner loop
 
        fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
        fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
        call fstr_update_contact_multiplier( cstep, ctalgo, hecmesh, heclagmat, fstrsolid, ctchange )
 
        ! ----- Set residual for next newton iteration
        call fstr_update_ndforce(cstep, hecmesh, hecmat, fstrsolid, conmat)
        ! ----- deal with contact boundary
        call hecmw_mat_clear_b( conmat )
        call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
        !    Consider SPC condition
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
 
      enddo
      ! ----- end of augmentation loop
 
      ! ----- compute CONT_NFORCE/CONT_FRIC for output
      if( fstr_is_contact_active() ) &
        call fstr_calc_contact_output_force(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
 
      ! ----- deal with contact boundary
      call fstr_scan_contact_state( cstep, sub_step, count_step, dtime, ctalgo, hecmesh, fstrsolid, infoctchange )
 
      contact_changed_global = 0
      ! Mortar safety net: force a rebuild when an active pair's coupling is no longer in the
      ! matrix profile, even if the structural-change counters stayed silent.
      need_prof_refresh = .false.
      if( .not. fstr_is_matrixstructure_changed(infoctchange) ) &
        need_prof_refresh = fstr_s2s_profile_needs_refresh( cstep, ctalgo, fstrsolid, conmat )
      if( fstr_is_matrixstructure_changed(infoctchange) .or. need_prof_refresh ) then
        call fstr_mat_con_contact( cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
        contact_changed_global = 1
      endif
      call hecmw_allreduce_i1(hecmesh, contact_changed_global, hecmw_max)
      if (contact_changed_global > 0) then
        call hecmw_mat_clear_b( hecmat )
        call hecmw_mat_clear_b( conmat )
        call solve_lineq_contact_init(hecmesh, hecmat, heclagmat, fstr_is_contactalag_symmetric(fstrsolid, hecmesh))
      endif
 
      if( fstr_is_contact_conv(ctalgo,infoctchange,hecmesh) .and. .not. ctchange ) exit loopforcontactanalysis
 
      ! ----- check divergence
      if( count_step >= fstrsolid%step_ctrl(cstep)%max_contiter ) then
        if( hecmesh%my_rank == 0) then
          write(   *,'(a,i5,a,i5)') '     ### Contact failed to Converge  : at total_step=', cstep, '  sub_step=', sub_step
        end if
        fstrsolid%NRstat_i(knstciter) = count_step                              ! logging contact iteration
        fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
        fstrsolid%NRstat_i(knstdresn) = 3
        return
      end if
 
      ! ----- Set residual for next newton iteration
      call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
      if( fstr_is_contact_active() )  then
        call hecmw_mat_clear_b( conmat )
        call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
        !    Consider SPC condition
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
      endif
 
    enddo loopforcontactanalysis
 
    ! ----- update the total displacement
    ! u_{n+1} = u_{n} + \Delta u_{n+1}
    call fstr_commit_solution_increment( hecmesh, fstrsolid, ndof )
 
    fstrsolid%NRstat_i(knstciter) = count_step ! logging contact iteration
 
    call fstr_updatestate( hecmesh, fstrsolid, tincr )
 
    !    Update REACTION using current QFORCE
    call fstr_update_reaction_spc( cstep, hecmesh, fstrsolid )
 
    ! ----- COMMIT lambda transaction (converged exit only; failure returns skip this) -----
    if( ctalgo == kcaalagrange ) call fstr_commit_lambda_txn( fstrsolid, cstep )
 
    deallocate(coord)
    deallocate(resid_work)
    fstrsolid%CutBack_stat = 0
  end subroutine fstr_newton_contactalag
 
  subroutine fstr_newton_contactslag( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM, hecLagMAT,                  &
      restart_step_num, restart_substep_num, sub_step, ctime, dtime, infoCTChange, conMAT )
 
    integer, intent(in)                    :: cstep
    type (hecmwst_local_mesh)              :: hecMESH
    type (hecmwST_matrix)                  :: hecMAT
    type (fstr_solid)                      :: fstrSOLID
    integer, intent(in)                    :: sub_step
    real(kind=kreal), intent(in)           :: ctime     
    real(kind=kreal), intent(in)           :: dtime     
    type (fstr_param)                      :: fstrPARAM
    type (fstr_info_contactChange)         :: infoCTChange
    type (hecmwST_matrix_lagrange)         :: hecLagMAT
    type (hecmwST_matrix)                  :: conMAT
 
    type (hecmwST_ebc) :: hecEBC
    integer(kind=kint) :: ndof
    integer(kind=kint) :: ctAlgo
    integer(kind=kint) :: i, iter
    integer(kind=kint) :: stepcnt, count_step
    real(kind=kreal)   :: tincr
    integer(kind=kint) :: restart_step_num, restart_substep_num
    logical            :: is_mat_symmetric
    integer(kind=kint) :: contact_changed_global
    real(kind=kreal), allocatable :: coord(:)
    integer(kind=kint)  :: istat
    integer(kind=kint)  :: iterStatus, nresid
    real(kind=kreal), allocatable :: resid_work(:)
    type(fstr_convergence_state) :: cnvstat
 
    ctalgo = fstrparam%contact_algo
 
    if( hecmat%Iarray(99) == 4 .and. .not. fstr_is_matrixstruct_symmetric(fstrsolid, hecmesh) ) then
      write(*, *) ' This type of direct solver is not yet available in such case ! '
      write(*, *) ' Please use intel MKL direct solver !'
      call  hecmw_abort( hecmw_comm_get_comm() )
    endif
 
    do i=1,fstrsolid%n_contacts
      fstrsolid%contacts(i)%ctime = ctime + dtime
    enddo
 
    if( cstep==1 .and. sub_step==restart_substep_num  ) then
      call fstr_save_originalmatrixstructure(hecmat)
      if( restart_step_num > 1 .or. restart_substep_num > 1 ) then
        call fstr_set_contact_active_restart( cstep, fstrsolid, infoctchange )
        infoctchange%contactNode_current = infoctchange%contactNode_previous
      else
        call fstr_scan_contact_state( cstep, sub_step, 0, dtime, ctalgo, hecmesh, fstrsolid, infoctchange )
      endif
        call hecmw_mat_copy_profile( hecmat, conmat )
      if ( fstr_is_contact_active() ) then
        call fstr_mat_con_contact(cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
      elseif( hecmat%Iarray(99)==4 ) then
        write(*, *) ' This type of direct solver is not yet available in such case ! '
        write(*, *) ' Please change the solver type to intel MKL direct solver !'
        call hecmw_abort(hecmw_comm_get_comm())
      endif
      is_mat_symmetric = fstr_is_matrixstruct_symmetric(fstrsolid, hecmesh)
      call solve_lineq_contact_init(hecmesh, hecmat, heclagmat, is_mat_symmetric)
    endif
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof, ctalgo, conmat)
 
    stepcnt = 0
    count_step = 0
    allocate(coord(hecmesh%n_node*ndof))
    allocate(resid_work(hecmesh%n_node*ndof + conmat%NP*ndof))
 
    loopforcontactanalysis: do while( .true. )
      count_step = count_step+1
 
      ! ----- Inner Iteration
      do iter = 1, fstrsolid%step_ctrl(cstep)%max_iter
        call hecmw_barrier(hecmesh)
        if( myrank == 0 ) print *,'-------------------------------------------------'
        call hecmw_barrier(hecmesh)
        stepcnt = stepcnt+1
 
        call fstr_creatematrix_and_dampingforce(hecmesh, hecmat, fstrsolid, ctime, tincr)
        call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
          call hecmw_mat_clear( conmat )
          conmat%X = 0.0d0
 
        if( fstr_is_contact_active() ) then
          call fstr_addcontactstiffness(cstep,ctalgo,iter,hecmesh,conmat,heclagmat,fstrsolid)
        endif
 
        ! ----- Set Boundary condition
        call hecmw_ebc_init(hecmat, hecebc)
        call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt, hecebc, conmat)
 
        !----- SOLVE [Kt]{du}={R}
        ! ----  For Parallel Contact with Multi-Partition Domains
        hecmat%X = 0.0d0
        call fstr_set_current_config_to_mesh(hecmesh,fstrsolid,coord)
        call solve_lineq_contact(hecmesh, hecmat, heclagmat, conmat, hecebc, istat, 1.0d0, fstr_is_contact_active())
        call fstr_recover_initial_config_to_mesh(hecmesh,fstrsolid,coord)
        call hecmw_ebc_finalize(hecebc)
        ! ----- check matrix solver error
        call fstr_check_linear_solver(hecmesh, hecmat, fstrsolid, cstep, sub_step, iterstatus, istat)
        if( iterstatus /= kitrcontinue ) then
          fstrsolid%NRstat_i(knstciter) = count_step
          return
        endif
 
        ! ----- update the small displacement and the displacement for 1step
        call fstr_apply_solution_increment( hecmesh, fstrsolid, ndof, hecmat%X )
 
        ! ----- update the Lagrange multipliers
        if( fstr_is_contact_active() ) then
          do i = 1, heclagmat%num_lagrange
            heclagmat%lagrange(i) = heclagmat%lagrange(i)+hecmat%X(hecmesh%n_node*ndof+i)
          enddo
        endif
 
        ! ----- update the strain, stress, and internal force (only QFORCE)
        call fstr_updatenewton(hecmesh, hecmat, fstrsolid, fstrparam, ctime, tincr, iter)
 
        if( fstrsolid%elemact%ELEMACT_egrp_tot > 0 ) then
          call fstr_update_elemact_solid_by_value( hecmesh, fstrsolid, cstep, ctime+dtime )
        endif
 
        ! ----- Set residual
        if( fstrsolid%DLOAD_follow /= 0 .or. fstrsolid%CLOAD_ngrp_rot /= 0 ) &
          call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
 
          call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
        if( fstr_is_contact_active() )  then
          call hecmw_mat_clear_b( conmat )
          call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
          !    Consider SPC condition
          call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
          call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
        endif
 
        call fstr_assemble_residual_contact(hecmat, heclagmat, conmat, hecmesh, resid_work, nresid)
        call fstr_check_convergence(hecmesh, hecmat, fstrsolid, fstrparam, &
            ndof, iter, sub_step, cstep, &
            resid_work, cnvstat, iterstatus, heclagmat)
        if (iterstatus == kitrconverged) then
          exit
        endif
        if (iterstatus == kitrdiverged .or. iterstatus == kitrfloatingerror) then
          fstrsolid%NRstat_i(knstciter) = count_step
          return
        endif
 
      enddo
      ! ----- end of inner loop
 
      ! ----- compute CONT_NFORCE/CONT_FRIC for output
      if( fstr_is_contact_active() ) &
        call fstr_calc_contact_output_force(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
 
      fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
      fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
      call fstr_scan_contact_state( cstep, sub_step, count_step, dtime, ctalgo, hecmesh, fstrsolid, infoctchange )
 
      if( hecmat%Iarray(99) == 4 .and. .not. fstr_is_contact_active() ) then
        write(*, *) ' This type of direct solver is not yet available in such case ! '
        write(*, *) ' Please use intel MKL direct solver !'
        call  hecmw_abort( hecmw_comm_get_comm() )
      endif
 
      is_mat_symmetric = fstr_is_matrixstruct_symmetric(fstrsolid, hecmesh)
      contact_changed_global = 0
      if( fstr_is_matrixstructure_changed(infoctchange) ) then
        call fstr_mat_con_contact( cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
        contact_changed_global = 1
      endif
 
      if( fstr_is_contact_conv(ctalgo,infoctchange,hecmesh) ) exit loopforcontactanalysis
 
      call hecmw_allreduce_i1(hecmesh, contact_changed_global, hecmw_max)
      if (contact_changed_global > 0) then
        call hecmw_mat_clear_b( hecmat )
          call hecmw_mat_clear_b( conmat )
        call solve_lineq_contact_init(hecmesh, hecmat, heclagmat, is_mat_symmetric)
      endif
 
      ! ----- check divergence
      if( count_step >= fstrsolid%step_ctrl(cstep)%max_contiter ) then
        if( hecmesh%my_rank == 0) then
          write(   *,'(a,i5,a,i5)') '     ### Contact failed to Converge  : at total_step=', cstep, '  sub_step=', sub_step
        end if
        fstrsolid%NRstat_i(knstciter) = count_step                              ! logging contact iteration
        fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
        fstrsolid%NRstat_i(knstdresn) = 3
        return
      end if
 
      ! ----- Set residual for next newton iteration
      call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
      if( fstr_is_contact_active() )  then
          call hecmw_mat_clear_b( conmat )
        call fstr_update_ndforce_contact(cstep,ctalgo,hecmesh,heclagmat,fstrsolid,conmat)
        !    Consider SPC condition
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
      endif
 
    enddo loopforcontactanalysis
 
    fstrsolid%NRstat_i(knstciter) = count_step ! logging contact iteration
 
    ! ----- update the total displacement
    !       u_{n+1} = u_{n} + \Delta u_{n+1}
    call fstr_commit_solution_increment( hecmesh, fstrsolid, ndof )
 
    call fstr_updatestate(hecmesh, fstrsolid, tincr)
    call fstr_update_contact_tangentforce( cstep, fstrsolid )
    if( fstrsolid%n_embeds > 0 .and. paracontactflag ) then
      call fstr_setup_parancon_contactvalue(hecmesh,ndof,fstrsolid%EMBED_NFORCE,1)
      call fstr_update_ndforce_spc( cstep, hecmesh, fstrsolid, hecmat%B )
    endif
    !    Update REACTION using current QFORCE
    call fstr_update_reaction_spc( cstep, hecmesh, fstrsolid )
 
    deallocate(coord)
    deallocate(resid_work)
    fstrsolid%CutBack_stat = 0
  end subroutine fstr_newton_contactslag
 
end module m_fstr_nonlinearmethod