module m_ES_Mag_Constraint
! $Id: m_ES_Mag_Constraint.f90 409 2014-10-27 09:24:52Z jkoga $

  use m_Parallelization,      only : ista_kngp, iend_kngp, ierr, npes, mype, &
       &                             mpi_comm_group
  use m_PlaneWaveBasisSet,    only : ngabc, kgp

  use m_PseudoPotential,    only : ilmt, nlmt, dl2p, ltp, mtp, taup, prodphi

  use m_Control_Parameters,    only : neg, ndim_spinor, ndim_magmom, noncol, kimg, &
       &                              proj_attribute, num_proj_elems, &
       &                              proj_group

  use m_Const_Parameters,  only : DP, CMPLDP, yes, PAI4, &
       &                          MAG_MOMENT_VALS_GLOBAL, MAG_MOMENT_DIREC_GLOBAL, &
       &                          MAG_MOMENT_VALS_LOCAL,  MAG_MOMENT_DIREC_LOCAL, &
       &                          MAG_MOMENT_VALS_OCCMAT, &
       &                          MAG_MOMENT_DIREC_HARDPART

  use m_Charge_Density,        only : chgq_l, chgqo_l, hsr, hsi
  use m_Ionic_System,         only : natm, ityp,  mag_direction0_atomtyp, &
       &                             magmom_local_now, mag_moment0_atomtyp, &
       &                             pos, iproj_group

  use m_CD_Mag_Moment,       only : rad_cov, RhoMag_on_atom

  use m_Crystal_Structure,  only : univol, rltv, altv, &
       &                           mag_constraint_type, &
       &                           mag_moment0_global, &
       &                           mag_direc0_global, &
       &                           mag_constraint_lambda

  use m_IterationNumbers,  only : iteration_electronic
  use m_Electronic_Structure,  only : vlhxc_l, vlhxcQ

  use m_Orbital_Population,    only : om, ommix

! === KT_add === 2014/08/09
  use m_Crystal_Structure,   only : Global_Quantz_Axis_Fixed
! ============== 2014/08/09

  implicit none

  include 'mpif.h'
  integer istatus(mpi_status_size)

  real(kind=DP) :: MagField_constrain_global(3)
  real(kind=DP), allocatable :: MagField_constrain_local(:,:,:)
  real(kind=DP), allocatable :: MagField_constrain_hardpart(:,:,:,:)

contains

  subroutine m_ES_add_MagConstraintPot_chgql
    if ( ndim_magmom == 1 ) return

    select case ( mag_constraint_type )
    case (MAG_MOMENT_VALS_GLOBAL)
       call case_constraint_moment_global
    case (MAG_MOMENT_DIREC_GLOBAL)
       call case_constraint_direc_global
    case (MAG_MOMENT_VALS_LOCAL)
       call case_constraint_moment_local
    case (MAG_MOMENT_DIREC_LOCAL)
       call case_constraint_direc_local
    end select

  contains

    subroutine case_constraint_moment_global
      real(kind=DP) :: MagMom(3), c1
      integer :: is, ia, it, lmt1

      if ( noncol ) then
         if ( mype == 0 ) then
            Do is=2, ndim_magmom
               MagMom(is-1) = chgq_l( 1,1,is ) *univol
            End do

            MagField_constrain_global(:) = mag_constraint_lambda &
                 &                         *( MagMom(:) -mag_moment0_global(:) )
            
            Do is=2, ndim_magmom
               vlhxc_l(1,1,is) = vlhxc_l(1,1,is) + MagField_constrain_global(is-1)
            End do
         endif

      else
         if ( mype == 0 ) then
            MagMom(1) = ( chgq_l( 1,1,1 ) -chgq_l( 1,1,2 ) )*univol
            
            MagField_constrain_global(1) = mag_constraint_lambda &
                 &                        *( MagMom(1) -mag_moment0_global(1) )

            vlhxc_l(1,1,1) = vlhxc_l(1,1,1) +MagField_constrain_global(1)
            vlhxc_l(1,1,2) = vlhxc_l(1,1,2) -MagField_constrain_global(1)
         endif

      endif

    end subroutine case_constraint_moment_global

    subroutine case_constraint_direc_global            ! only for noncol
      real(kind=DP) :: MagMom(3), MagDirec(3)
      real(kind=DP) :: c1, c2, cnorm, cnorm2
      integer :: is, ia, it, ixyz, lmt1

      real(kind=DP), parameter :: cnorm_lower_limit = 1.0D-10

      if ( .not. noncol ) return

      if ( mype == 0 ) then
         Do is=2, ndim_magmom
            MagMom(is-1) = chgq_l( 1,1,is ) *univol
         End do

         cnorm2 = 0.0d0
         Do ixyz=1, 3
            cnorm2 = cnorm2 + MagMom(ixyz)**2
         End do
         cnorm = sqrt( cnorm2 )

         if ( cnorm < cnorm_lower_limit ) then
            MagField_constrain_global = 0.0d0
            return
         endif

         MagDirec = MagMom / cnorm

         Do ixyz=1, 3
            c1 = cnorm2 - MagMom(ixyz)**2
            c2 = cnorm**3
            MagField_constrain_global(ixyz) = &
                 &  mag_constraint_lambda *c1 /c2 &
                 & *( MagDirec(ixyz) - mag_direc0_global(ixyz) )
         End do

         Do is=2, ndim_magmom
            vlhxc_l(1,1,is) = vlhxc_l(1,1,is) + MagField_constrain_global(is-1)
         End do
      endif

    end subroutine case_constraint_direc_global

    subroutine case_constraint_moment_local
      real(kind=DP) :: MagMom(3), cfactor(3)
      real(kind=DP) :: rad1, fac1r, fac1i, fac2
      real(kind=DP) :: VecG(3), normG, normG3, gr, d1

      real(kind=DP), allocatable :: zfcos(:), zfsin(:)
      real(kind=DP), allocatable :: RhoMag_on_atom_mpi(:,:)

      integer :: i, j, ia, it, is, ixyz, ixyz_max, ist

      allocate(zfcos(ista_kngp:iend_kngp)); zfcos = 0.d0
      allocate(zfsin(ista_kngp:iend_kngp)); zfsin = 0.d0

      if ( noncol ) then
         ixyz_max = 3
      else
         ixyz_max = 1
      endif

      if ( allocated( MagField_constrain_local ) ) then
         deallocate( MagField_constrain_local )
      endif

      allocate( MagField_constrain_local(ista_kngp:iend_kngp, kimg, ixyz_max ))
      MagField_constrain_local = 0.0d0
!
      Do ia=1, natm
         it = ityp(ia)
         rad1 = rad_cov(ia)

         Do ixyz=1, ixyz_max
            if ( noncol ) then
               MagMom( ixyz ) = RhoMag_on_atom( ia, ixyz +1 )
            else
               MagMom( ixyz ) = RhoMag_on_atom( ia, 1 ) -RhoMag_on_atom( ia, 2 )
            endif
            
            cfactor( ixyz ) = mag_constraint_lambda /univol &
                 &               *( MagMom(ixyz) -mag_moment0_atomtyp(it,ixyz) )
               
            call calc_phase2( natm, pos, ia, kgp, ngabc, ista_kngp, iend_kngp, &
                 &            zfcos, zfsin )

            if ( ista_kngp == 1 ) then
               ist = 2
            else
               ist = ista_kngp
            endif

            Do i=ist, iend_kngp
               VecG(1) = rltv(1,1)*ngabc(i,1) +rltv(1,2)*ngabc(i,2) +rltv(1,3)*ngabc(i,3)
               VecG(2) = rltv(2,1)*ngabc(i,1) +rltv(2,2)*ngabc(i,2) +rltv(2,3)*ngabc(i,3)
               VecG(3) = rltv(3,1)*ngabc(i,1) +rltv(3,2)*ngabc(i,2) +rltv(3,3)*ngabc(i,3)
            
               normG = sqrt( VecG(1)**2 +VecG(2)**2 +VecG(3)**2 )
               normG3 = normG**3
               
               fac1r = zfcos(i); fac1i = zfsin(i)
!
               gr = normG *rad1
               fac2 = -gr *cos(gr) + sin(gr)
               fac2 = fac2 *PAI4 /normG3
!
               MagField_constrain_local( i, 1, ixyz ) &
                    & = MagField_constrain_local( i, 1, ixyz ) &
                    &   +fac1r *fac2 *cfactor( ixyz )
               if ( kimg == 2 ) then
                  MagField_constrain_local( i, 2, ixyz ) &
                       & = MagField_constrain_local( i, 2, ixyz ) &
                       &  -fac1i *fac2 *cfactor( ixyz )
               endif
            End Do

            if ( mype == 0 ) then
               fac2 = PAI4 /3.0d0 *rad1**3

               MagField_constrain_local( 1, 1, ixyz ) &
                    & = MagField_constrain_local( 1, 1, ixyz ) &
                    &   +fac2 *cfactor( ixyz )
            end if
         End Do
      End Do
!
      if ( noncol ) then
         Do i=ista_kngp, iend_kngp
            Do is=2, ndim_magmom
               vlhxc_l(i,1,is) = vlhxc_l(i,1,is) +MagField_constrain_local(i,1,is-1)
               vlhxc_l(i,2,is) = vlhxc_l(i,2,is) +MagField_constrain_local(i,2,is-1)
            End do
         End Do
      else
         Do i=ista_kngp, iend_kngp
            Do j=1, kimg
               vlhxc_l(i,j,1) = vlhxc_l(i,j,1) +MagField_constrain_local(i,j,1)
               vlhxc_l(i,j,2) = vlhxc_l(i,j,2) -MagField_constrain_local(i,j,1)
            End Do
         End Do
      endif

    end subroutine case_constraint_moment_local

    subroutine case_constraint_direc_local            ! only for noncol
      stop "Not supported"
    end subroutine case_constraint_direc_local

  end subroutine m_ES_add_MagConstraintPot_chgql

  subroutine m_ES_add_MagConstraintPot_hsr
    if ( ndim_magmom == 1 ) return

    select case ( mag_constraint_type )
    case (MAG_MOMENT_VALS_OCCMAT)
       call case_constraint_moment_occmat
    case (MAG_MOMENT_DIREC_HARDPART)
       call case_constraint_direc_hardpart
    end select

  contains

    subroutine case_constraint_moment_occmat
      integer :: ia, ig, it, i, ip, ilp
      integer :: ilmt1, ilmt2, l1, l2, m1, m2, t1, t2
      integer :: ixyz, ixyz_max, is

      real(kind=DP) :: MagMom(3), cfactor(3), c1

      if ( noncol ) then
         ixyz_max = 3
      else
         ixyz_max = 1
      endif

      if ( allocated( MagField_constrain_hardpart ) ) then
         deallocate( MagField_constrain_hardpart )
      endif

      allocate( MagField_constrain_hardpart(nlmt,nlmt,natm,ixyz_max) )
      MagField_constrain_hardpart = 0.0d0
!
      do ia=1,natm
         it = ityp(ia)

         ig = iproj_group(ia)
         if(ig<1) cycle
            
         Do ixyz=1, ixyz_max
            if ( noncol ) then
               c1 = 0.0d0
               do i=1,num_proj_elems(ig)
                  ip = proj_group(i,ig)
                  Do m1=1, 2*proj_attribute(ip)%l +1
                     c1 = c1 +ommix(m1,m1,i,ia,ixyz+1)
                  End Do
               End do
               MagMom(ixyz) = c1
            else
               c1 = 0.0d0
               do i=1,num_proj_elems(ig)
                  ip = proj_group(i,ig)
                  Do m1=1, 2*proj_attribute(ip)%l +1
                     c1 = c1 +ommix(m1,m1,i,ia,1) -ommix(m1,m1,i,ia,2)
                  End Do
               End do
               MagMom(ixyz) = c1
            endif

            cfactor(ixyz) = mag_constraint_lambda &
                 &               *( MagMom(ixyz) -mag_moment0_atomtyp(it,ixyz) )
         End do

         Do ixyz=1, ixyz_max

            do i=1,num_proj_elems(ig)
               ip = proj_group(i,ig)
               it = proj_attribute(ip)%ityp
               ilp = proj_attribute(ip)%l+1
               
               do ilmt1 = 1, ilmt(it)
                  l1 = ltp(ilmt1,it); m1 = mtp(ilmt1,it); t1 = taup(ilmt1,it)
                  if ( l1 /= ilp ) cycle
                  
                  do ilmt2 = 1, ilmt(it)
                     l2 = ltp(ilmt2,it); m2 = mtp(ilmt2,it); t2 = taup(ilmt2,it)
                     if( l2 /= ilp ) cycle
                     
                     if ( m1 == m2 ) then
                        MagField_constrain_hardpart(ilmt1,ilmt2,ia,ixyz) &
                             & = MagField_constrain_hardpart(ilmt1,ilmt2,ia,ixyz) &
                             & + cfactor(ixyz) *prodphi(ip,t1,t2)
                     endif
                  end do
               end do
            end do
         End Do
      End do

      if ( noncol ) then
         Do is=2, ndim_magmom
            vlhxcQ(:,:,:,is) = vlhxcQ(:,:,:,is) &
                 &               +MagField_constrain_hardpart(:,:,:,is-1)
         End Do
      else
         vlhxcQ(:,:,:,1) = vlhxcQ(:,:,:,1) +MagField_constrain_hardpart(:,:,:,1)
         vlhxcQ(:,:,:,2) = vlhxcQ(:,:,:,2) -MagField_constrain_hardpart(:,:,:,1)
      end if

    end subroutine case_constraint_moment_occmat

    subroutine case_constraint_direc_hardpart        ! noncol only
      real(kind=DP) :: MagMom(3), MagDirec(3)
      real(kind=DP) :: c1, c2, cnorm, cnorm2
      integer :: ia, it, lmt1, lmt2, is, ixyz

      real(kind=DP), parameter :: cnorm_lower_limit = 1.0D-10

      if ( .not. noncol ) return

      if ( .not. allocated( MagField_constrain_hardpart ) ) then
         allocate( MagField_constrain_hardpart(nlmt,nlmt,natm,3 ) );
      endif
!
      MagField_constrain_local = 0.0d0
!
      Do ia=1, natm
         it = ityp(ia)
         Do ixyz=1, 3
            MagMom( ixyz ) = magmom_local_now( ia,ixyz )
         End do

         cnorm2 = 0.0d0
         Do ixyz=1, 3
            cnorm2 = cnorm2 + MagMom(ixyz)**2
         End do
         cnorm = sqrt( cnorm2 )

         if ( cnorm < cnorm_lower_limit ) then
            MagField_constrain_hardpart(:,:,ia,:) = 0.0d0
            cycle
         endif

         MagDirec = MagMom / cnorm

         Do lmt1=1, ilmt(it)
            Do lmt2=1, ilmt(it)

               Do ixyz=1, 3
                  c1 = cnorm2 - MagMom(ixyz)**2
                  c2 = cnorm**3
                  MagField_constrain_hardpart(lmt1,lmt2,ia,ixyz) = &
                       &  mag_constraint_lambda *c1 /c2 &
                       & *( MagDirec(ixyz) -mag_direction0_atomtyp(it,ixyz) ) &
                       & *dl2p(lmt1,lmt2,1,it)             ! approx
               End do
            End do
         End do

         Do lmt1=1, ilmt(it)
            Do lmt2=1, ilmt(it)
               Do is=2, ndim_magmom
                  vlhxcQ(lmt1,lmt2,ia,is) = vlhxcQ(lmt1,lmt2,ia,is) &
                       &       + MagField_constrain_hardpart(lmt1,lmt2,ia,is-1)
               End do
            End do
         End Do

      end Do

    end subroutine case_constraint_direc_hardpart

  end subroutine m_ES_add_MagConstraintPot_hsr

  subroutine m_ES_calc_MagConstraint_Energy( ene_double_counting, ene_mag_constraint )
    real(kind=DP), intent(out) :: ene_double_counting
    real(kind=DP), intent(out) :: ene_mag_constraint

    ene_double_counting = 0.0d0;      ene_mag_constraint = 0.0d0

    select case ( mag_constraint_type )

    case (MAG_MOMENT_VALS_GLOBAL)
       call case_constraint_moment_global
    case (MAG_MOMENT_DIREC_GLOBAL)
       call case_constraint_direc_global
    case (MAG_MOMENT_VALS_LOCAL)
       call case_constraint_moment_local
    case (MAG_MOMENT_DIREC_LOCAL)
       call case_constraint_direc_local

    case (MAG_MOMENT_VALS_OCCMAT)
       call case_constraint_moment_occmat
    case (MAG_MOMENT_DIREC_HARDPART)
       call case_constraint_direc_hardpart

    end select

  contains

    subroutine case_constraint_moment_global
      real(kind=DP) :: MagMom(3), c1, c2
      real(kind=DP) :: MagMomOld(3)
      integer :: is, ixyz

      if ( mype == 0 ) then

         if ( noncol ) then
            Do is=2, ndim_magmom
               MagMom(is-1) = chgq_l(  1,1,is ) *univol
!               MagMomOld(is-1) = chgqo_l(  1,1,is ) *univol
            End do
            
            c1 = 0.0d0;  c2 = 0.0d0
            Do ixyz=1, 3
               c1 = c1 + MagField_constrain_global(ixyz) *MagMom(ixyz)
               c2 = c2 + ( MagMom(ixyz) -mag_moment0_global(ixyz) )**2
            End do
         else
            MagMom(1) = ( chgq_l( 1,1,1 ) -chgq_l( 1,1,2 ) )*univol
            
            c1 = MagField_constrain_global(1) *MagMom(1)
            c2 = ( MagMom(1) -mag_moment0_global(1) )**2
         endif

         ene_double_counting = c1
         ene_mag_constraint = c2 *mag_constraint_lambda / 2.0d0
      endif

      if (npes > 1) then
         call mpi_bcast( ene_double_counting, 1, mpi_double_precision, & 
              &          0, mpi_comm_group, ierr )
         call mpi_bcast( ene_mag_constraint,  1, mpi_double_precision, &
              &          0, mpi_comm_group, ierr )
      end if

    end subroutine case_constraint_moment_global

    subroutine case_constraint_direc_global
      real(kind=DP) :: MagMom(3), MagDirec(3), c1, c2
      integer :: is, ixyz

      real(kind=DP) :: cnorm, cnorm2
      real(kind=DP), parameter :: cnorm_lower_limit = 1.0D-10

      if ( mype == 0 ) then
         Do is=2, ndim_magmom
            MagMom(is-1) = chgq_l(  1,1,is ) *univol
         End do

         cnorm2 = 0.0d0
         Do ixyz=1, 3
            cnorm2 = cnorm2 + MagMom(ixyz)**2
         End do
         cnorm = sqrt( cnorm2 )

         if ( cnorm > cnorm_lower_limit ) then
            MagDirec = MagMom / cnorm

            c1 = 0.0d0;  c2 = 0.0d0
            Do ixyz=1, 3
               c1 = c1 + MagField_constrain_global(ixyz) *MagMom(ixyz)
               c2 = c2 + ( MagDirec(ixyz) -mag_direc0_global(ixyz) )**2
            End do

            ene_double_counting = c1
            ene_mag_constraint = c2 *mag_constraint_lambda /2.0d0
         endif
      endif

      if (npes > 1) then
         call mpi_bcast( ene_double_counting, 1, mpi_double_precision, & 
              &          0, mpi_comm_group, ierr )
         call mpi_bcast( ene_mag_constraint,  1, mpi_double_precision, &
              &          0, mpi_comm_group, ierr )
      end if

    end subroutine case_constraint_direc_global

    subroutine case_constraint_direc_local
      stop "kt: Not supported"
    end subroutine case_constraint_direc_local

    subroutine case_constraint_moment_local
      real(kind=DP) :: MagMom(3), MagDirec(3), c1, c2
      integer :: i, j, ia, it, is, ixyz

      real(kind=DP) :: csum1, csum2

      csum1 = 0.0d0; csum2 = 0.0d0

      if ( noncol ) then
         Do i=ista_kngp, iend_kngp
            Do j=1, kimg
               Do ixyz=1, 3
                  csum1 = csum1 + MagField_constrain_local(i,j,ixyz)*chgq_l(i,j,ixyz+1)
               End Do
            End Do
         End Do

         Do ia=1, natm
            it = ityp(ia)
            Do ixyz=1, 3
               MagMom(ixyz) = RhoMag_on_Atom( ia,ixyz+1 )
            End do
            csum2 = csum2 + ( MagMom(ixyz) -mag_moment0_atomtyp(it,ixyz) )**2
         End do

      else
         Do i=ista_kngp, iend_kngp
            Do j=1, kimg
               csum1 = csum1 + MagField_constrain_local(i,j,1) &
                    &         *( chgq_l(i,j,1) -chgq_l(i,j,2) )
            End Do
         End Do

         Do ia=1, natm
            it = ityp(ia)
            MagMom(1) = RhoMag_on_Atom( ia,1 ) -RhoMag_on_Atom( ia,2 )
            csum2 = ( MagMom(1) -mag_moment0_atomtyp(it,1) )**2
         End do
      endif

      if ( npes > 1 ) then
         call mpi_allreduce( csum1, c1, 1, mpi_double_precision, &
              &              mpi_sum, mpi_comm_group, ierr )
         csum1 = c1
      endif

      ene_double_counting = csum1 *univol
      ene_mag_constraint = csum2 *mag_constraint_lambda/ 2.0d0

    end subroutine case_constraint_moment_local

    subroutine case_constraint_moment_occmat
      integer :: ia, it, ilmt1, ilmt2, m1
      integer :: i, ig, ip, ixyz, ixyz_max
      real(kind=DP) :: MagMom(3), c1, csum1, csum2

      if ( noncol ) then
         ixyz_max = 3
      else
         ixyz_max = 1
      endif

      csum1 = 0.0d0; csum2 = 0.0d0

      if ( noncol ) then
         Do ia=1, natm
            it = ityp(ia)
            Do ixyz=1, ixyz_max
               Do ilmt1=1, ilmt(it)
                  Do ilmt2=1, ilmt(it)
                     csum1 = csum1 +MagField_constrain_hardpart(ilmt1,ilmt2,ia,ixyz) &
                          &          *hsr(ia,ilmt1,ilmt2,ixyz+1) 
                  End do
               End do
            End do
         End Do
      else
         Do ia=1, natm
            it = ityp(ia)
            Do ilmt1=1, ilmt(it)
               Do ilmt2=1, ilmt(it)
                  csum1 = csum1 +MagField_constrain_hardpart(ilmt1,ilmt2,ia,1) &
                       &          *( hsr(ia,ilmt1,ilmt2,1) -hsr(ia,ilmt1,ilmt2,2) ) 
               End Do
            End do
         End do
      endif

      Do ia=1, natm
         it = ityp(ia)
            
         ig = iproj_group(ia)
         if(ig<1) cycle
            
         if ( noncol ) then
            Do ixyz=1, ixyz_max
               c1 = 0.0d0
               do i=1,num_proj_elems(ig)
                  ip = proj_group(i,ig)
                  Do m1=1, 2*proj_attribute(ip)%l +1
                     c1 = c1 +om(m1,m1,i,ia,ixyz+1)
                  End Do
               End do
               MagMom(ixyz) = c1
            End do
         else
            c1 = 0.0d0
            do i=1,num_proj_elems(ig)
               ip = proj_group(i,ig)
               Do m1=1, 2*proj_attribute(ip)%l +1
                  c1 = c1 +om(m1,m1,i,ia,1) -om(m1,m1,i,ia,2)
               End Do
            End do
            MagMom(1) = c1
         endif

         Do ixyz=1, ixyz_max
            csum2 = csum2 +( MagMom(ixyz) -mag_moment0_atomtyp(it,ixyz) )**2
         End do
      End Do

      ene_double_counting = csum1
      ene_mag_constraint = csum2 *mag_constraint_lambda/ 2.0d0
      
    end subroutine case_constraint_moment_occmat

    subroutine case_constraint_direc_hardpart
      real(kind=DP) :: MagMom(3), MagDirec(3), c1, c2
      integer :: ia, it, lmt1, lmt2, is, ixyz

      real(kind=DP) :: csum1, csum2
      real(kind=DP) :: cnorm, cnorm2
      real(kind=DP), parameter :: cnorm_lower_limit = 1.0D-10

      csum1 = 0.0d0; csum2 = 0.0d0

      Do ia=1, natm
         it = ityp(ia)
         Do ixyz=1, 3
            MagMom(ixyz) = magmom_local_now( ia,ixyz )
         End do

         cnorm2 = 0.0d0
         Do ixyz=1, 3
            cnorm2 = cnorm2 + MagMom(ixyz)**2
         End do
         cnorm = sqrt( cnorm2 )

         if ( cnorm > cnorm_lower_limit ) then
            MagDirec = MagMom / cnorm

            Do lmt1=1, ilmt(it)
               Do lmt2=1, ilmt(it)

                  Do ixyz=1, 3
                     csum1 = csum1 + MagField_constrain_hardpart(lmt1,lmt2,ia,ixyz) &
                          &          *hsr(ia,lmt1,lmt2,ixyz +1)
                  End do
               End do
            End do

            Do ixyz=1, 3
               csum2 = csum2 + ( MagDirec(ixyz)-mag_direction0_atomtyp(it,ixyz) )**2
            End do
         endif

      End do

      ene_double_counting = csum1
      ene_mag_constraint = csum2 *mag_constraint_lambda/ 2.0d0

    end subroutine case_constraint_direc_hardpart

  end subroutine m_ES_calc_MagConstraint_Energy

! === KT_add === 2014/08/09
  subroutine m_ES_proj_magmom_G_quantz_axis
    integer :: i, j, k, is, ia
    real(kind=DP) :: ctmp1, ctmp2

! soft part
    Do k=1, kimg
       Do i=ista_kngp, iend_kngp
          ctmp1 = 0.0d0
          Do is=1, 3
             ctmp1 = ctmp1 +chgq_l(i,k,is+1) *Global_Quantz_Axis_Fixed(is)
          End do
          Do is=1, 3
             chgq_l(i,k,is+1) = ctmp1 *Global_Quantz_Axis_Fixed(is)
          End do
       End do
    End do
!
! hard part
    Do ia=1, natm
       Do i=1, nlmt
          Do j=1, nlmt
             ctmp1 = 0.0d0
             ctmp2 = 0.0d0
             Do is=1, 3
                ctmp1 = ctmp1 +hsr(ia,i,j,is+1) *Global_Quantz_Axis_Fixed(is)
                ctmp2 = ctmp2 +hsi(ia,i,j,is+1) *Global_Quantz_Axis_Fixed(is)
             End do
             Do is=1, 3
                hsr(ia,i,j,is+1) = ctmp1 *Global_Quantz_Axis_Fixed(is)
                hsi(ia,i,j,is+1) = ctmp2 *Global_Quantz_Axis_Fixed(is)
             End do
          End do
       End do
    End Do

  end subroutine m_ES_proj_magmom_G_quantz_axis
! ============== 2014/08/09

! === KT_add === 2014/09/26
  subroutine m_ES_set_magmom_zero
    integer :: is

    Do is=2, ndim_magmom
       chgq_l(:,:,is) = 0.0d0
       hsr(:,:,:,is) = 0.0d0;    hsi(:,:,:,is) = 0.0d0
    End do
  end subroutine m_ES_set_magmom_zero
! ============== 2014/09/26

end module m_ES_Mag_Constraint
