!> @file
!! Pulay correction calculation for linear version
!! @author
!!    Copyright (C) 2007-2013 BigDFT group
!!    This file is distributed under the terms of the
!!    GNU General Public License, see ~/COPYING file
!!    or http://www.gnu.org/copyleft/gpl.txt .
!!    For the list of contributors, see ~/AUTHORS


subroutine pulay_correction_new(iproc, nproc, tmb, orbs, at, fpulay)
  use module_base
  use module_types
  !use module_interfaces, only: small_to_large_locreg
  use yaml_output
  use communications_base, only: TRANSPOSE_FULL
  use communications, only: transpose_localized
  use sparsematrix_base, only: sparsematrix_malloc_ptr, DENSE_FULL, assignment(=), &
                               sparsematrix_malloc, SPARSE_FULL
  use sparsematrix, only: compress_matrix, uncompress_matrix, gather_matrix_from_taskgroups_inplace, &
                          uncompress_matrix2
  use transposed_operations, only: calculate_overlap_transposed, build_linear_combination_transposed
  use locregs_init, only: small_to_large_locreg
  implicit none

  ! Calling arguments
  integer,intent(in) :: iproc, nproc
  type(DFT_wavefunction),intent(inout) :: tmb
  type(orbitals_data),intent(in) :: orbs
  type(atoms_data),intent(in) :: at
  real(kind=8),dimension(3,at%astruct%nat),intent(out) :: fpulay

  ! Local variables
  integer :: iat, isize, iorb, jorb, korb, idir, iiorb, ierr, num_points, num_points_tot
  real(kind=8),dimension(:,:),allocatable :: phi_delta, energykernel, tempmat, phi_delta_large
  real(kind=8),dimension(:),allocatable :: hphit_c, hphit_f, denskern_tmp, delta_phit_c, delta_phit_f, tmparr
  real(kind=8) :: tt

  call timing(iproc,'new_pulay_corr','ON') 

  call f_routine(id='pulay_correction_new')

  phi_delta=f_malloc0((/tmb%npsidim_orbs,3/),id='phi_delta')
  ! Get the values of the support functions on the boundary of the localization region
  call extract_boundary(tmb, phi_delta, num_points, num_points_tot)


  ! calculate the "energy kernel"
  energykernel=f_malloc0((/tmb%orbs%norb,tmb%orbs%norb/),id='energykernel')
  do iorb=1,tmb%orbs%norbp
      iiorb=tmb%orbs%isorb+iorb
      do jorb=1,tmb%orbs%norb
          do korb=1,orbs%norb
              energykernel(jorb,iiorb) = energykernel(jorb,iiorb) &
                                      + tmb%coeff(jorb,korb)*tmb%coeff(iiorb,korb)*tmb%orbs%eval(korb)
          end do
      end do
  end do

  if (nproc > 1) then
     call mpiallred(energykernel, mpi_sum, comm=bigdft_mpi%mpi_comm)
  end if

  ! calculate the overlap matrix
  !!if(.not.associated(tmb%psit_c)) then
  !!    isize=sum(tmb%collcom%nrecvcounts_c)
  !!    tmb%psit_c=f_malloc_ptr(isize,id='tmb%psit_c')
  !!end if
  !!if(.not.associated(tmb%psit_f)) then
  !!    isize=7*sum(tmb%collcom%nrecvcounts_f)
  !!    tmb%psit_f=f_malloc_ptr(isize,id=' tmb%psit_f')
  !!end if
  call transpose_localized(iproc, nproc, tmb%npsidim_orbs, tmb%orbs, tmb%collcom, &
       TRANSPOSE_FULL, tmb%psi, tmb%psit_c, tmb%psit_f, tmb%lzd)
  call calculate_overlap_transposed(iproc, nproc, tmb%orbs, tmb%collcom, tmb%psit_c, &
       tmb%psit_c, tmb%psit_f, tmb%psit_f, tmb%linmat%smat(1), tmb%linmat%ovrlp_)
  !!call gather_matrix_from_taskgroups_inplace(iproc, nproc, tmb%linmat%smat(1), tmb%linmat%ovrlp_)
  ! This can then be deleted if the transition to the new type has been completed.
  !tmb%linmat%ovrlp%matrix_compr=tmb%linmat%ovrlp_%matrix_compr


  !!call f_free_ptr(tmb%psit_c)
  !!call f_free_ptr(tmb%psit_f)


  ! Construct the array chi
  call construct_chi()
  call f_free(energykernel)


  ! transform phi_delta to the shamop region
  phi_delta_large=f_malloc((/tmb%ham_descr%npsidim_orbs,3/),id='phi_delta_large')
  call small_to_large_locreg(iproc, tmb%npsidim_orbs, tmb%ham_descr%npsidim_orbs, tmb%lzd, tmb%ham_descr%lzd, &
                      tmb%orbs, phi_delta(1,1), phi_delta_large(1,1))
  call small_to_large_locreg(iproc, tmb%npsidim_orbs, tmb%ham_descr%npsidim_orbs, tmb%lzd, tmb%ham_descr%lzd, &
                      tmb%orbs, phi_delta(1,2), phi_delta_large(1,2))
  call small_to_large_locreg(iproc, tmb%npsidim_orbs, tmb%ham_descr%npsidim_orbs, tmb%lzd, tmb%ham_descr%lzd, &
                      tmb%orbs, phi_delta(1,3), phi_delta_large(1,3))

  isize=sum(tmb%ham_descr%collcom%nrecvcounts_c)
  delta_phit_c=f_malloc(isize,id='delta_phit_c')
  isize=7*sum(tmb%ham_descr%collcom%nrecvcounts_f)
  delta_phit_f=f_malloc(isize,id='delta_phit_f')
  !fpulay=f_malloc((/3,at%astruct%nat/),id='fpulay')
  tmb%linmat%kernel_%matrix = sparsematrix_malloc_ptr(tmb%linmat%smat(3), iaction=DENSE_FULL, id='tmb%linmat%kernel_%matrix')
  call uncompress_matrix2(iproc, nproc, bigdft_mpi%mpi_comm, &
       tmb%linmat%smat(3), tmb%linmat%kernel_%matrix_compr, tmb%linmat%kernel_%matrix)
  call f_zero(fpulay)
  do idir=1,3
      ! calculate the overlap matrix among hphi and phi_delta_large
      call transpose_localized(iproc, nproc, tmb%ham_descr%npsidim_orbs, tmb%orbs, tmb%ham_descr%collcom, &
           TRANSPOSE_FULL, phi_delta_large(1,idir), delta_phit_c, delta_phit_f, tmb%ham_descr%lzd)
      call calculate_overlap_transposed(iproc, nproc, tmb%orbs, tmb%ham_descr%collcom, &
           hphit_c, delta_phit_c, hphit_f, delta_phit_f, tmb%linmat%smat(2), tmb%linmat%ham_)
      !!call gather_matrix_from_taskgroups_inplace(iproc, nproc, tmb%linmat%smat(2), tmb%linmat%ham_)
      ! This can then be deleted if the transition to the new type has been completed.
      !tmb%linmat%ham%matrix_compr=tmb%linmat%ham_%matrix_compr

      tmb%linmat%ham_%matrix = sparsematrix_malloc_ptr(tmb%linmat%smat(2), iaction=DENSE_FULL, id='tmb%linmat%ham_%matrix')
      call uncompress_matrix2(iproc, nproc, bigdft_mpi%mpi_comm, &
           tmb%linmat%smat(2), tmb%linmat%ham_%matrix_compr, tmb%linmat%ham_%matrix)

      do iorb=1,tmb%orbs%norbp
          iiorb=tmb%orbs%isorb+iorb
          iat=tmb%orbs%onwhichatom(iiorb)
          tt=0.d0
          do jorb=1,tmb%orbs%norb
              tt = tt -2.d0*tmb%linmat%kernel_%matrix(jorb,iiorb,1)*tmb%linmat%ham_%matrix(jorb,iiorb,1)
              !if (iproc==0) write(*,*) 'kern, ovrlp', tmb%linmat%denskern%matrix(jorb,iiorb), tmb%linmat%ham%matrix(iiorb,jorb)
          end do  
          fpulay(idir,iat)=fpulay(idir,iat)+tt
      end do
      call f_free_ptr(tmb%linmat%ham_%matrix)
  end do

  if (nproc > 1) then
     call mpiallred(fpulay, mpi_sum, comm=bigdft_mpi%mpi_comm)
  end if
  call f_free_ptr(tmb%linmat%kernel_%matrix)


  if(iproc==0) then
       call yaml_comment('new Pulay correction',hfill='-')
       call yaml_sequence_open('Pulay forces (Ha/Bohr)')
          do iat=1,at%astruct%nat
             call yaml_sequence(advance='no')
             call yaml_mapping_open(flow=.true.)
             call yaml_map(trim(at%astruct%atomnames(at%astruct%iatype(iat))),fpulay(1:3,iat),fmt='(1es20.12)')
             call yaml_mapping_close(advance='no')
             call yaml_comment(trim(yaml_toa(iat,fmt='(i4.4)')))
          end do
          call yaml_sequence_close()
  end if


    
  call f_free(phi_delta)
  call f_free(phi_delta_large)
  call f_free(hphit_c)
  call f_free(hphit_f)
  call f_free(delta_phit_c)
  call f_free(delta_phit_f)
  
  call f_release_routine()

  call timing(iproc,'new_pulay_corr','OF') 

  contains

    subroutine construct_chi()

      tempmat=f_malloc((/tmb%orbs%norb,tmb%orbs%norb/),id='tempmat')
      tmb%linmat%ovrlp_%matrix = sparsematrix_malloc_ptr(tmb%linmat%smat(1), iaction=DENSE_FULL, id='tmb%linmat%ovrlp_%matrix')
      call uncompress_matrix2(iproc, nproc, bigdft_mpi%mpi_comm, &
           tmb%linmat%smat(1), tmb%linmat%ovrlp_%matrix_compr, tmb%linmat%ovrlp_%matrix)
      call dgemm('n', 'n', tmb%orbs%norb, tmb%orbs%norb, tmb%orbs%norb, 1.d0, &
                 tmb%linmat%ovrlp_%matrix, tmb%orbs%norb, energykernel, tmb%orbs%norb, &
                 0.d0, tempmat, tmb%orbs%norb)
      call f_free_ptr(tmb%linmat%ovrlp_%matrix)
      isize=sum(tmb%ham_descr%collcom%nrecvcounts_c)
      hphit_c=f_malloc(isize,id='hphit_c')
      isize=7*sum(tmb%ham_descr%collcom%nrecvcounts_f)
      hphit_f=f_malloc(isize,id='hphit_f')
      call transpose_localized(iproc, nproc, tmb%ham_descr%npsidim_orbs, tmb%orbs, tmb%ham_descr%collcom, &
                               TRANSPOSE_FULL, tmb%hpsi, hphit_c, hphit_f, tmb%ham_descr%lzd)
      denskern_tmp=f_malloc(tmb%linmat%smat(3)%nvctr,id='denskern_tmp')
      denskern_tmp=tmb%linmat%kernel_%matrix_compr
      tmb%linmat%kernel_%matrix = sparsematrix_malloc_ptr(tmb%linmat%smat(3), iaction=DENSE_FULL, id='tmb%linmat%kernel_%matrix')
      tmb%linmat%kernel_%matrix(:,:,1)=tempmat
      call compress_matrix(iproc, nproc, tmb%linmat%smat(3), inmat=tmb%linmat%kernel_%matrix, outmat=tmb%linmat%kernel_%matrix_compr)
      call f_free_ptr(tmb%linmat%kernel_%matrix)

      tmparr = sparsematrix_malloc(tmb%linmat%smat(3),iaction=SPARSE_FULL,id='tmparr')
      call vcopy(tmb%linmat%smat(3)%nvctr, tmb%linmat%kernel_%matrix_compr(1), 1, tmparr(1), 1)
      !!call gather_matrix_from_taskgroups_inplace(iproc, nproc, tmb%linmat%smat(3), tmb%linmat%kernel_)
      call build_linear_combination_transposed(tmb%ham_descr%collcom, &
           tmb%linmat%smat(3), tmb%linmat%kernel_, tmb%ham_descr%psit_c, tmb%ham_descr%psit_f, &
           .false., hphit_c, hphit_f, iproc)
      call vcopy(tmb%linmat%smat(3)%nvctr, tmparr(1), 1, tmb%linmat%kernel_%matrix_compr(1), 1)
      call f_free(tmparr)

      tmb%linmat%kernel_%matrix_compr=denskern_tmp
    
      call f_free(tempmat)
      call f_free(denskern_tmp)
    
    end subroutine construct_chi
 
end subroutine pulay_correction_new




subroutine extract_boundary(tmb, phi_delta, numpoints, numpoints_tot)
  use module_base
  use module_types
  use yaml_output
  implicit none

  ! Calling arguments
  type(DFT_wavefunction),intent(in) :: tmb
  real(kind=8),dimension(tmb%npsidim_orbs,3),intent(out) :: phi_delta
  integer, intent(out) :: numpoints, numpoints_tot

  ! Local variables
  integer :: ishift, iorb, iiorb, ilr, iseg, jj_prev, j0_prev, j1_prev, ii_prev, i3_prev, i2_prev, i1_prev, i0_prev
  integer :: jj, j0, j1, ii, i3, i2, i1, i0, jorb, korb, istat, idir, iat, i, isize
  logical,dimension(:,:,:),allocatable :: boundaryarray
  real(kind=8) :: dist, crit, xsign, ysign, zsign

  call f_routine(id='extract_boundary')
  numpoints=0
  ! First copy the boundary elements of the first array to a temporary array,
  ! filling the remaining part with zeros.
  call f_zero(phi_delta)
  ishift=0
  do iorb=1,tmb%orbs%norbp
      iiorb=tmb%orbs%isorb+iorb
      ilr=tmb%orbs%inwhichlocreg(iiorb)
      boundaryarray=f_malloc((/0.to.tmb%lzd%llr(ilr)%d%n1,0.to.tmb%lzd%llr(ilr)%d%n2,0.to.tmb%lzd%llr(ilr)%d%n3/), &
                             id='boundaryarray')
      boundaryarray=.false.


      ! coarse part
      do iseg=1,tmb%lzd%llr(ilr)%wfd%nseg_c

          ! indizes of the last element of the previous segment
          if (iseg>1) then
              jj_prev=tmb%lzd%llr(ilr)%wfd%keyvloc(iseg-1)
              j0_prev=tmb%lzd%llr(ilr)%wfd%keygloc(1,iseg-1)
              j1_prev=tmb%lzd%llr(ilr)%wfd%keygloc(2,iseg-1)
              ii_prev=j0_prev-1
              i3_prev=ii_prev/((tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1))
              ii_prev=ii_prev-i3_prev*(tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1)
              i2_prev=ii_prev/(tmb%lzd%llr(ilr)%d%n1+1)
              i0_prev=ii_prev-i2_prev*(tmb%lzd%llr(ilr)%d%n1+1)
              i1_prev=i0_prev+j1_prev-j0_prev
          else
              !just some large values
              i1_prev=-99999
              i2_prev=-99999
              i3_prev=-99999
          end if

          ! indizes of the first element of the current segment
          jj=tmb%lzd%llr(ilr)%wfd%keyvloc(iseg)
          j0=tmb%lzd%llr(ilr)%wfd%keygloc(1,iseg)
          j1=tmb%lzd%llr(ilr)%wfd%keygloc(2,iseg)
          ii=j0-1
          i3=ii/((tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1))
          ii=ii-i3*(tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1)
          i2=ii/(tmb%lzd%llr(ilr)%d%n1+1)
          i0=ii-i2*(tmb%lzd%llr(ilr)%d%n1+1)
          i1=i0

          if (i2/=i2_prev .or. i3/=i3_prev) then
              ! Segment starts on a new line, i.e. this is a boundary element.
              ! Furthermore the last element of the previous segment must be a
              ! boundary element as well. However this is only true if the
              ! distance from the locreg center corresponds to the cutoff
              ! radius, otherwise it is only a boundary element of the global
              ! grid and not of the localization region.
              if (iseg>1) then
                  ! this makes only sense if we are not in the first segment
                  dist= sqrt(((tmb%lzd%llr(ilr)%ns1+i1_prev)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))**2 &
                            +((tmb%lzd%llr(ilr)%ns2+i2_prev)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))**2 &
                            +((tmb%lzd%llr(ilr)%ns3+i3_prev)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))**2)
                  crit=tmb%lzd%llr(ilr)%locrad-sqrt(tmb%lzd%hgrids(1)**2+tmb%lzd%hgrids(2)**2+tmb%lzd%hgrids(3)**2)
                  if (dist>=crit) then
                      ! boundary element of the locreg
                      xsign=((tmb%lzd%llr(ilr)%ns1+i1_prev)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))/dist
                      ysign=((tmb%lzd%llr(ilr)%ns2+i2_prev)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))/dist
                      zsign=((tmb%lzd%llr(ilr)%ns3+i3_prev)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))/dist
                      !xsign=1.d0 ; ysign=1.d0 ; zsign=1.d0
                      phi_delta(ishift+jj-1,1)=xsign*tmb%psi(ishift+jj-1)
                      phi_delta(ishift+jj-1,2)=ysign*tmb%psi(ishift+jj-1)
                      phi_delta(ishift+jj-1,3)=zsign*tmb%psi(ishift+jj-1)
                      boundaryarray(i1_prev,i2_prev,i3_prev)=.true.
                      numpoints=numpoints+1
                  end if
                  numpoints_tot=numpoints_tot+1
              end if
              dist= sqrt(((tmb%lzd%llr(ilr)%ns1+i1)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))**2 &
                        +((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))**2 &
                        +((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))**2)
              crit=tmb%lzd%llr(ilr)%locrad-sqrt(tmb%lzd%hgrids(1)**2+tmb%lzd%hgrids(2)**2+tmb%lzd%hgrids(3)**2)
              if (dist>=crit) then
                  xsign=((tmb%lzd%llr(ilr)%ns1+i1)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))/dist
                  ysign=((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))/dist
                  zsign=((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))/dist
                  !xsign=1.d0 ; ysign=1.d0 ; zsign=1.d0
                  phi_delta(ishift+jj,1)=xsign*tmb%psi(ishift+jj)
                  phi_delta(ishift+jj,2)=ysign*tmb%psi(ishift+jj)
                  phi_delta(ishift+jj,3)=zsign*tmb%psi(ishift+jj)
                  boundaryarray(i1,i2,i3)=.true.
                  numpoints=numpoints+1
              end if
              numpoints_tot=numpoints_tot+1
          end if
      end do

      ! fine part
      do iseg=tmb%lzd%llr(ilr)%wfd%nseg_c+1,tmb%lzd%llr(ilr)%wfd%nseg_c+tmb%lzd%llr(ilr)%wfd%nseg_f
          jj=tmb%lzd%llr(ilr)%wfd%keyvloc(iseg)
          j0=tmb%lzd%llr(ilr)%wfd%keygloc(1,iseg)
          j1=tmb%lzd%llr(ilr)%wfd%keygloc(2,iseg)
          ii=j0-1
          i3=ii/((tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1))
          ii=ii-i3*(tmb%lzd%llr(ilr)%d%n1+1)*(tmb%lzd%llr(ilr)%d%n2+1)
          i2=ii/(tmb%lzd%llr(ilr)%d%n1+1)
          i0=ii-i2*(tmb%lzd%llr(ilr)%d%n1+1)
          i1=i0+j1-j0
          ! The segments goes now from i0 to i1
          ! Check the beginnig of the segment. If it was a boundary element of
          ! the coarse grid, copy its content also for the fine part.
          if (boundaryarray(i0,i2,i3)) then
              dist= sqrt(((tmb%lzd%llr(ilr)%ns1+i0)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))**2 &
                        +((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))**2 &
                        +((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))**2)
              xsign=((tmb%lzd%llr(ilr)%ns1+i0)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))/dist
              ysign=((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))/dist
              zsign=((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))/dist
              !xsign=1.d0 ; ysign=1.d0 ; zsign=1.d0
              ! x direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7)
              ! y direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7)
              ! z direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(jj-1)+7)
              numpoints=numpoints+7
          end if
          numpoints_tot=numpoints_tot+1
          ! Check the end of the segment. If it was a boundary element of
          ! the coarse grid, copy its content also for the fine part.
          if (boundaryarray(i1,i2,i3)) then
              dist= sqrt(((tmb%lzd%llr(ilr)%ns1+i1)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))**2 &
                        +((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))**2 &
                        +((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))**2)
              xsign=((tmb%lzd%llr(ilr)%ns1+i1)*tmb%lzd%hgrids(1)-tmb%lzd%llr(ilr)%locregcenter(1))/dist
              ysign=((tmb%lzd%llr(ilr)%ns2+i2)*tmb%lzd%hgrids(2)-tmb%lzd%llr(ilr)%locregcenter(2))/dist
              zsign=((tmb%lzd%llr(ilr)%ns3+i3)*tmb%lzd%hgrids(3)-tmb%lzd%llr(ilr)%locregcenter(3))/dist
              !xsign=1.d0 ; ysign=1.d0 ; zsign=1.d0
              ! x direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7,1) = &
                  xsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7)
              ! y direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7,2) = &
                  ysign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7)
              ! z direction
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+1)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+2)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+3)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+4)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+5)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+6)
              phi_delta(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7,3) = &
                  zsign*tmb%psi(ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*(i1-i0+jj-1)+7)
              numpoints=numpoints+7
          end if
          numpoints_tot=numpoints_tot+1
      end do
      
      ishift=ishift+tmb%lzd%llr(ilr)%wfd%nvctr_c+7*tmb%lzd%llr(ilr)%wfd%nvctr_f
      call f_free(boundaryarray)
  end do

call f_release_routine()

end subroutine extract_boundary


subroutine pulay_correction(iproc, nproc, orbs, at, rxyz, nlpsp, SIC, denspot, GPU, tmb, fpulay)
  use module_base
  use module_types
  use module_interfaces, only: LocalHamiltonianApplication, SynchronizeHamiltonianApplication
  use yaml_output
  use communications_base, only: TRANSPOSE_FULL
  use communications, only: transpose_localized, start_onesided_communication
  use rhopotential, only: full_local_potential
  use sparsematrix_base
  use sparsematrix, only: gather_matrix_from_taskgroups_inplace
  use transposed_operations, only: calculate_overlap_transposed
  use locreg_operations, only: confpot_data
  implicit none

  ! Calling arguments
  integer,intent(in) :: iproc, nproc
  type(orbitals_data),intent(in) :: orbs
  type(atoms_data),intent(in) :: at
  real(kind=8),dimension(3,at%astruct%nat),intent(in) :: rxyz
  type(DFT_PSP_projectors),intent(inout) :: nlpsp
  type(SIC_data),intent(in) :: SIC
  type(DFT_local_fields), intent(inout) :: denspot
  type(GPU_pointers),intent(inout) :: GPU
  type(DFT_wavefunction),intent(inout) :: tmb
  real(kind=8),dimension(3,at%astruct%nat),intent(out) :: fpulay

  ! Local variables
  integer:: istat, iall, ierr, iialpha, jorb
  integer:: iorb, ii, iseg, isegstart, isegend, is, ie, ishift, ispin
  integer:: jat, jdir, ibeta
  !!integer :: ialpha, iat, iiorb
  real(kind=8) :: kernel, ekernel
  real(kind=8),dimension(:),allocatable :: lhphilarge, psit_c, psit_f, hpsit_c, hpsit_f, lpsit_c, lpsit_f
  type(sparse_matrix) :: dovrlp(3), dham(3)
  type(matrices) :: dovrlp_(3), dham_(3)
  type(energy_terms) :: energs
  type(confpot_data),dimension(:),allocatable :: confdatarrtmp
  character(len=*),parameter :: subname='pulay_correction'
  type(matrices) :: ham_

  call f_routine(id='pulay_correction')
  energs=energy_terms_null()
  
  ! Begin by updating the Hpsi
  call local_potential_dimensions(iproc,tmb%ham_descr%lzd,tmb%orbs,denspot%xc,denspot%dpbox%ngatherarr(0,1))

  lhphilarge = f_malloc0(tmb%ham_descr%npsidim_orbs,id='lhphilarge')

  !!call post_p2p_communication(iproc, nproc, denspot%dpbox%ndimpot, denspot%rhov, &
  !!     tmb%ham_descr%comgp%nrecvbuf, tmb%ham_descr%comgp%recvbuf, tmb%ham_descr%comgp, tmb%ham_descr%lzd)
  call start_onesided_communication(iproc, nproc, denspot%dpbox%ndims(1), denspot%dpbox%ndims(2), &
       max(denspot%dpbox%nscatterarr(:,2),1), denspot%rhov, &
       tmb%ham_descr%comgp%nspin*tmb%ham_descr%comgp%nrecvbuf, tmb%ham_descr%comgp%recvbuf, tmb%ham_descr%comgp, tmb%ham_descr%lzd)

  allocate(confdatarrtmp(tmb%orbs%norbp))
  call default_confinement_data(confdatarrtmp,tmb%orbs%norbp)


  call NonLocalHamiltonianApplication(iproc,at,tmb%ham_descr%npsidim_orbs,tmb%orbs,&
       tmb%ham_descr%lzd,nlpsp,tmb%ham_descr%psi,lhphilarge,energs%eproj,tmb%paw)

  ! only kinetic because waiting for communications
  call LocalHamiltonianApplication(iproc,nproc,at,tmb%ham_descr%npsidim_orbs,tmb%orbs,&
       tmb%ham_descr%lzd,confdatarrtmp,denspot%dpbox%ngatherarr,denspot%pot_work,tmb%ham_descr%psi,lhphilarge,&
       energs,SIC,GPU,3,denspot%xc,pkernel=denspot%pkernelseq,dpbox=denspot%dpbox,&
       & potential=denspot%rhov,comgp=tmb%ham_descr%comgp)
  call full_local_potential(iproc,nproc,tmb%orbs,tmb%ham_descr%lzd,2,denspot%dpbox,&
       & denspot%xc,denspot%rhov,denspot%pot_work,tmb%ham_descr%comgp)
  ! only potential
  call LocalHamiltonianApplication(iproc,nproc,at,tmb%ham_descr%npsidim_orbs,tmb%orbs,&
       tmb%ham_descr%lzd,confdatarrtmp,denspot%dpbox%ngatherarr,denspot%pot_work,tmb%ham_descr%psi,lhphilarge,&
       energs,SIC,GPU,2,denspot%xc,pkernel=denspot%pkernelseq,dpbox=denspot%dpbox,&
       & potential=denspot%rhov,comgp=tmb%ham_descr%comgp)

  call timing(iproc,'glsynchham1','ON')
  call SynchronizeHamiltonianApplication(nproc,tmb%ham_descr%npsidim_orbs,&
       & tmb%orbs,tmb%ham_descr%lzd,GPU,denspot%xc,lhphilarge,&
       energs)!%ekin,energs%epot,energs%eproj,energs%evsic,energs%eexctX)
  call timing(iproc,'glsynchham1','OF')
  deallocate(confdatarrtmp)
  

  ! Now transpose the psi and hpsi
  lpsit_c = f_malloc(tmb%ham_descr%collcom%ndimind_c,id='lpsit_c')
  lpsit_f = f_malloc(7*tmb%ham_descr%collcom%ndimind_f,id='lpsit_f')
  hpsit_c = f_malloc(tmb%ham_descr%collcom%ndimind_c,id='hpsit_c')
  hpsit_f = f_malloc(7*tmb%ham_descr%collcom%ndimind_f,id='hpsit_f')
  psit_c = f_malloc(tmb%ham_descr%collcom%ndimind_c,id='psit_c')
  psit_f = f_malloc(7*tmb%ham_descr%collcom%ndimind_f,id='psit_f')


  call transpose_localized(iproc, nproc, tmb%ham_descr%npsidim_orbs, tmb%orbs, tmb%ham_descr%collcom, &
       TRANSPOSE_FULL, tmb%ham_descr%psi, lpsit_c, lpsit_f, tmb%ham_descr%lzd)

  call transpose_localized(iproc, nproc, tmb%ham_descr%npsidim_orbs, tmb%orbs, tmb%ham_descr%collcom, &
       TRANSPOSE_FULL, lhphilarge, hpsit_c, hpsit_f, tmb%ham_descr%lzd)

  !now build the derivative and related matrices <dPhi_a | H | Phi_b> and <dPhi_a | Phi_b>

  ! DOVRLP AND DHAM SHOULD HAVE DIFFERENT SPARSITIES, BUT TO MAKE LIFE EASIER KEEPING THEM THE SAME FOR NOW
  ! also array of structure a bit inelegant at the moment

  ham_ = matrices_null()
  call allocate_matrices(tmb%linmat%smat(2), allocate_full=.false., &
       matname='ham_', mat=ham_)
  do jdir = 1, 3
    !call nullify_sparse_matrix(dovrlp(jdir))
    !call nullify_sparse_matrix(dham(jdir))
    dovrlp(jdir)=sparse_matrix_null()
    dovrlp_(jdir)=matrices_null()
    dham(jdir)=sparse_matrix_null()
    dham_(jdir)=matrices_null()
    !call sparse_copy_pattern(tmb%linmat%smat(2),dovrlp(jdir),iproc,subname) 
    !call sparse_copy_pattern(tmb%linmat%smat(2),dham(jdir),iproc,subname)
    call copy_sparse_matrix(tmb%linmat%smat(2),dovrlp(jdir))
    call copy_sparse_matrix(tmb%linmat%smat(2),dham(jdir))
    !dham_(jdir)%matrix_compr=f_malloc_ptr(dham(jdir)%nvctr,id='dham(jdir)%matrix_compr')
    !dovrlp_(jdir)%matrix_compr=f_malloc_ptr(dovrlp(jdir)%nvctr,id='dovrlp(jdir)%matrix_compr')
    dham_(jdir)%matrix_compr=sparsematrix_malloc_ptr(dham(jdir),iaction=SPARSE_FULL,id='dham(jdir)%matrix_compr')
    dovrlp_(jdir)%matrix_compr=sparsematrix_malloc_ptr(dovrlp(jdir),iaction=SPARSE_FULL,id='dovrlp(jdir)%matrix_compr')

    call get_derivative(jdir, tmb%ham_descr%npsidim_orbs, tmb%ham_descr%lzd%hgrids(1), tmb%orbs, &
         tmb%ham_descr%lzd, tmb%ham_descr%psi, lhphilarge)

    call transpose_localized(iproc, nproc, tmb%ham_descr%npsidim_orbs, tmb%orbs, tmb%ham_descr%collcom, &
         TRANSPOSE_FULL, lhphilarge, psit_c, psit_f, tmb%ham_descr%lzd)

    call calculate_overlap_transposed(iproc, nproc, tmb%orbs, tmb%ham_descr%collcom,&
         psit_c, lpsit_c, psit_f, lpsit_f, tmb%linmat%smat(2), ham_)
    !!call gather_matrix_from_taskgroups_inplace(iproc, nproc, tmb%linmat%smat(2), tmb%linmat%ham_)
    ! This can then be deleted if the transition to the new type has been completed.
    dovrlp_(jdir)%matrix_compr=ham_%matrix_compr

    call calculate_overlap_transposed(iproc, nproc, tmb%orbs, tmb%ham_descr%collcom,&
         psit_c, hpsit_c, psit_f, hpsit_f, tmb%linmat%smat(2), ham_)
    !!call gather_matrix_from_taskgroups_inplace(iproc, nproc, tmb%linmat%smat(2), tmb%linmat%ham_)
    ! This can then be deleted if the transition to the new type has been completed.
    dham_(jdir)%matrix_compr=ham_%matrix_compr
  end do
  call deallocate_matrices(ham_)


  !DEBUG
  !!print *,'iproc,tmb%orbs%norbp',iproc,tmb%orbs%norbp
  !!if(iproc==0)then
  !!do iorb = 1, tmb%orbs%norb
  !!   do iiorb=1,tmb%orbs%norb
  !!      !print *,'Hamiltonian of derivative: ',iorb, iiorb, (matrix(iorb,iiorb,jdir),jdir=1,3)
  !!      print *,'Overlap of derivative: ',iorb, iiorb, (dovrlp(iorb,iiorb,jdir),jdir=1,3)
  !!   end do
  !!end do
  !!end if
  !!!Check if derivatives are orthogonal to functions
  !!if(iproc==0)then
  !!  do iorb = 1, tmbder%orbs%norb
  !!     !print *,'overlap of derivative: ',iorb, (dovrlp(iorb,iiorb),iiorb=1,tmb%orbs%norb)
  !!     do iiorb=1,tmbder%orbs%norb
  !!         write(*,*) iorb, iiorb, dovrlp(iorb,iiorb)
  !!     end do
  !!  end do
  !!end if
  !END DEBUG

   ! needs generalizing if dovrlp and dham are to have different structures
   call f_zero(fpulay)
   do jdir=1,3
     !do ialpha=1,tmb%orbs%norb
     do ispin=1,tmb%linmat%smat(2)%nspin
         ishift=(ispin-1)*tmb%linmat%smat(2)%nvctr
         if (tmb%linmat%smat(2)%nfvctrp>0) then
             isegstart=dham(jdir)%istsegline(tmb%linmat%smat(2)%isfvctr_par(iproc)+1)
             if (tmb%linmat%smat(2)%isfvctr+tmb%linmat%smat(2)%nfvctrp<tmb%linmat%smat(2)%nfvctr) then
                 isegend=dham(jdir)%istsegline(tmb%linmat%smat(2)%isfvctr_par(iproc+1)+1)-1
             else
                 isegend=dham(jdir)%nseg
             end if
             do iseg=isegstart,isegend
                  ii=dham(jdir)%keyv(iseg)-1
                  do jorb=dham(jdir)%keyg(1,1,iseg),dham(jdir)%keyg(2,1,iseg)
                      ii=ii+1
                      iialpha = dham(jdir)%keyg(1,2,iseg)
                      ibeta = jorb
                      jat=tmb%orbs%onwhichatom(iialpha)
                      kernel = 0.d0
                      ekernel= 0.d0
                      if (ispin==1) then
                          is=1
                          ie=orbs%norbu
                      else
                          is=orbs%norbu+1
                          ie=orbs%norb
                      end if
                      do iorb=is,ie
                          kernel  = kernel+orbs%occup(iorb)*tmb%coeff(iialpha,iorb)*tmb%coeff(ibeta,iorb)
                          !!ekernel = ekernel+tmb%orbs%eval(iorb)*orbs%occup(iorb) &
                          !!     *tmb%coeff(iialpha,iorb)*tmb%coeff(ibeta,iorb) 
                          ekernel = ekernel+orbs%eval(iorb)*orbs%occup(iorb) &
                               *tmb%coeff(iialpha,iorb)*tmb%coeff(ibeta,iorb) 
                      end do
                      fpulay(jdir,jat)=fpulay(jdir,jat)+&
                             2.0_gp*(kernel*dham_(jdir)%matrix_compr(ishift+ii)-ekernel*dovrlp_(jdir)%matrix_compr(ishift+ii))
                  end do
             end do
         end if
      end do
   end do 

   if (nproc > 1) then
      call mpiallred(fpulay, mpi_sum, comm=bigdft_mpi%mpi_comm)
   end if

  if(iproc==0) then
       !!do jat=1,at%astruct%nat
       !!    write(*,'(a,i5,3es16.6)') 'iat, fpulay', jat, fpulay(1:3,jat)
       !!end do
       call yaml_comment('Pulay Correction',hfill='-')
       call yaml_sequence_open('Pulay Forces (Ha/Bohr)')
          do jat=1,at%astruct%nat
             call yaml_sequence(advance='no')
             call yaml_mapping_open(flow=.true.)
             call yaml_map(trim(at%astruct%atomnames(at%astruct%iatype(jat))),fpulay(1:3,jat),fmt='(1es20.12)')
             call yaml_mapping_close(advance='no')
             call yaml_comment(trim(yaml_toa(jat,fmt='(i4.4)')))
          end do
          call yaml_sequence_close()
  end if


  call f_free(psit_c)
  call f_free(psit_f)
  call f_free(hpsit_c)
  call f_free(hpsit_f)
  call f_free(lpsit_c)
  call f_free(lpsit_f)
  call f_free(lhphilarge)

  call f_free_ptr(denspot%pot_work)


  do jdir=1,3
     call deallocate_sparse_matrix(dovrlp(jdir))
     call deallocate_sparse_matrix(dham(jdir))
     call deallocate_matrices(dovrlp_(jdir))
     call deallocate_matrices(dham_(jdir))
  end do

  !!if(iproc==0) write(*,'(1x,a)') 'done.'

  call f_release_routine()

end subroutine pulay_correction

subroutine get_derivative(idir, ndim, hgrid, orbs, lzd, phi, phider)
  use module_bigdft_arrays
  use module_types
  implicit none
  
  ! Calling arguments
  integer,intent(in):: ndim, idir
  real(kind=8),intent(in) :: hgrid
  type(orbitals_data),intent(in) :: orbs
  type(local_zone_descriptors),intent(in) :: lzd
  real(kind=8),dimension(ndim),intent(in) :: phi !< Basis functions
  real(kind=8),dimension(ndim),intent(inout) :: phider  !< Derivative of basis functions
  
  ! Local variables
  integer :: nf, istat, iall, iorb, iiorb, ilr, istrt
  real(kind=8),dimension(0:3),parameter :: scal=1.d0
  real(kind=8),dimension(:),allocatable :: w_f1, w_f2, w_f3
  real(kind=8),dimension(:,:,:),allocatable :: w_c, phider_c
  real(kind=8),dimension(:,:,:,:),allocatable :: w_f, phider_f
  character(len=*),parameter :: subname='get_derivative'

   call f_zero(phider)

   istrt = 1
   do iorb=1, orbs%norbp
     iiorb = iorb+orbs%isorb 
     ilr = orbs%inwhichlocreg(iiorb)

     call allocateWorkarrays()
 
     ! Uncompress the wavefunction.
     call uncompress_forstandard(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
          lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, & 
          lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3,  &
          lzd%llr(ilr)%wfd%nseg_c, lzd%llr(ilr)%wfd%nvctr_c, lzd%llr(ilr)%wfd%keygloc, lzd%llr(ilr)%wfd%keyvloc,  &
          lzd%llr(ilr)%wfd%nseg_f, lzd%llr(ilr)%wfd%nvctr_f, &
          lzd%llr(ilr)%wfd%keygloc(1,lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)), &
          lzd%llr(ilr)%wfd%keyvloc(lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)),  &
          scal, phi(istrt), phi(istrt+lzd%llr(ilr)%wfd%nvctr_c), w_c, w_f, w_f1, w_f2, w_f3)
  
     if(idir==1) then
         call createDerivativeX(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
              lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, &
              lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3,  &
              hgrid, lzd%llr(ilr)%bounds%kb%ibyz_c, lzd%llr(ilr)%bounds%kb%ibyz_f, &
              w_c, w_f, w_f1, phider_c, phider_f)
         ! Compress the x wavefunction.
         call compress_forstandard(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
              lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, &
              lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3, &
              lzd%llr(ilr)%wfd%nseg_c, lzd%llr(ilr)%wfd%nvctr_c, lzd%llr(ilr)%wfd%keygloc, lzd%llr(ilr)%wfd%keyvloc, &
              lzd%llr(ilr)%wfd%nseg_f, lzd%llr(ilr)%wfd%nvctr_f, &
              lzd%llr(ilr)%wfd%keygloc(1,lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)), &
              lzd%llr(ilr)%wfd%keyvloc(lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)),  &
              scal, phider_c, phider_f, phider(istrt), phider(istrt+lzd%llr(ilr)%wfd%nvctr_c))
     else if (idir==2) then
        call createDerivativeY(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
              lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, &
              lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3,  &
              hgrid, lzd%llr(ilr)%bounds%kb%ibxz_c, lzd%llr(ilr)%bounds%kb%ibxz_f, &
              w_c, w_f, w_f2, phider_c, phider_f)
        ! Compress the y wavefunction.
        call compress_forstandard(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
             lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, &
             lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3, &
             lzd%llr(ilr)%wfd%nseg_c, lzd%llr(ilr)%wfd%nvctr_c, lzd%llr(ilr)%wfd%keygloc, lzd%llr(ilr)%wfd%keyvloc, &
             lzd%llr(ilr)%wfd%nseg_f, lzd%llr(ilr)%wfd%nvctr_f, &
             lzd%llr(ilr)%wfd%keygloc(1,lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)), &
             lzd%llr(ilr)%wfd%keyvloc(lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)),  &
             scal, phider_c, phider_f, phider(istrt), phider(istrt+lzd%llr(ilr)%wfd%nvctr_c))
     else
        call createDerivativeZ(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
              lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, &
              lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3,  &
              hgrid, lzd%llr(ilr)%bounds%kb%ibxy_c, lzd%llr(ilr)%bounds%kb%ibxy_f, &
              w_c, w_f, w_f3, phider_c, phider_f)
        ! Compress the z wavefunction.
        call compress_forstandard(lzd%llr(ilr)%d%n1, lzd%llr(ilr)%d%n2, lzd%llr(ilr)%d%n3, &
             lzd%llr(ilr)%d%nfl1, lzd%llr(ilr)%d%nfu1, &
             lzd%llr(ilr)%d%nfl2, lzd%llr(ilr)%d%nfu2, lzd%llr(ilr)%d%nfl3, lzd%llr(ilr)%d%nfu3, &
             lzd%llr(ilr)%wfd%nseg_c, lzd%llr(ilr)%wfd%nvctr_c, lzd%llr(ilr)%wfd%keygloc, lzd%llr(ilr)%wfd%keyvloc, &
             lzd%llr(ilr)%wfd%nseg_f, lzd%llr(ilr)%wfd%nvctr_f, &
             lzd%llr(ilr)%wfd%keygloc(1,lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)), &
             lzd%llr(ilr)%wfd%keyvloc(lzd%llr(ilr)%wfd%nseg_c+min(1,lzd%llr(ilr)%wfd%nseg_f)),  &
             scal, phider_c, phider_f, phider(istrt),phider(istrt+lzd%llr(ilr)%wfd%nvctr_c ))
     end if

     istrt = istrt + lzd%llr(ilr)%wfd%nvctr_c + 7*lzd%llr(ilr)%wfd%nvctr_f
  
     call deallocateWorkarrays()                                

   end do

contains
  subroutine allocateWorkarrays()

    ! THIS IS COPIED FROM allocate_work_arrays. Works only for free boundary.
    nf=(lzd%llr(ilr)%d%nfu1-lzd%llr(ilr)%d%nfl1+1)*(lzd%llr(ilr)%d%nfu2-lzd%llr(ilr)%d%nfl2+1)* &
       (lzd%llr(ilr)%d%nfu3-lzd%llr(ilr)%d%nfl3+1)

    ! Allocate work arrays
    w_c = f_malloc0((/ 0.to.lzd%llr(ilr)%d%n1, 0.to.lzd%llr(ilr)%d%n2, 0.to.lzd%llr(ilr)%d%n3 /),id='w_c')

    w_f = f_malloc0((/ 1.to.7, lzd%llr(ilr)%d%nfl1.to.lzd%llr(ilr)%d%nfu1, lzd%llr(ilr)%d%nfl2.to.lzd%llr(ilr)%d%nfu2, &
                           lzd%llr(ilr)%d%nfl3.to.lzd%llr(ilr)%d%nfu3 /),id='w_f')


    w_f1 = f_malloc0(nf,id='w_f1')

    w_f2 = f_malloc0(nf,id='w_f2')

    w_f3 = f_malloc0(nf,id='w_f3')


    phider_f = f_malloc0((/ 1.to.7, lzd%llr(ilr)%d%nfl1.to.lzd%llr(ilr)%d%nfu1, lzd%llr(ilr)%d%nfl2.to.lzd%llr(ilr)%d%nfu2, &
                                lzd%llr(ilr)%d%nfl3.to.lzd%llr(ilr)%d%nfu3 /),id='phider_f')

    phider_c = f_malloc0((/ 0.to.lzd%llr(ilr)%d%n1, 0.to.lzd%llr(ilr)%d%n2, 0.to.lzd%llr(ilr)%d%n3 /),id='phider_c')

  end subroutine allocateWorkarrays


  subroutine deallocateWorkarrays


    call f_free(w_c)
    call f_free(w_f)
    call f_free(w_f1)
    call f_free(w_f2)
    call f_free(w_f3)
    call f_free(phider_f)
    call f_free(phider_c)

  end subroutine deallocateWorkarrays
end subroutine get_derivative

subroutine createDerivativeX(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3,  &
     hgrid,ibyz_c,ibyz_f,w_c, w_f, w_f1, x_c, x_f)

  use liborbs_precisions
  use filterModule
  implicit none

  ! Calling arguments
  integer, intent(in) :: n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3
  real(gp), intent(in) :: hgrid
  integer, dimension(2,0:n2,0:n3), intent(in) :: ibyz_c,ibyz_f
  real(wp), dimension(0:n1,0:n2,0:n3), intent(in) :: w_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(in) :: w_f
  real(wp), dimension(nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(in) :: w_f1
  real(wp), dimension(0:n1,0:n2,0:n3), intent(out) :: x_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(out) :: x_f
  !local variables
  integer, parameter :: lowfil=-14,lupfil=14
  integer :: i,t,i1,i2,i3
  integer :: icur,istart,iend,l
  real(wp) :: dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211
  real(wp), dimension(-3+lowfil:lupfil+3) :: ad1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: bd1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: cd1_ext
  real(wp), dimension(lowfil:lupfil) :: ed1_ext


! Copy the filters to the 'extended filters', i.e. add some zeros.
! This seems to be required since we use loop unrolling.
ad1_ext=0.d0
bd1_ext=0.d0
cd1_ext=0.d0

do i=lowfil,lupfil
    ad1_ext(i)=ad1(i)
    bd1_ext(i)=bd1(i)
    cd1_ext(i)=cd1(i)
    ed1_ext(i)=ed1(i) !this is only needed due to OpenMP, since it seems I cannot declare ed1 (which is in filterModule) as shared
end do


!$omp parallel default(none) &
!$omp shared(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3) &
!$omp shared(ibyz_c,ibyz_f,w_c,w_f,x_c,x_f)& 
!$omp shared(w_f1,ad1_ext,bd1_ext,cd1_ext,ed1_ext)&
!$omp private(i,t,i1,i2,i3,icur,istart,iend,l)&
!$omp private(dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211)

  ! x direction
  !$omp do
  do i3=0,n3
     do i2=0,n2
        if (ibyz_c(2,i2,i3)-ibyz_c(1,i2,i3).ge.4) then
           do i1=ibyz_c(1,i2,i3),ibyz_c(2,i2,i3)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibyz_c(1,i2,i3),lowfil+i1),min(lupfil+i1+3,ibyz_c(2,i2,i3))
                 dyi0=dyi0 + w_c(t,i2,i3)*ad1_ext(t-i1-0)
                 dyi1=dyi1 + w_c(t,i2,i3)*ad1_ext(t-i1-1)
                 dyi2=dyi2 + w_c(t,i2,i3)*ad1_ext(t-i1-2)
                 dyi3=dyi3 + w_c(t,i2,i3)*ad1_ext(t-i1-3)
              enddo
              x_c(i1+0,i2,i3)=dyi0
              x_c(i1+1,i2,i3)=dyi1
              x_c(i1+2,i2,i3)=dyi2
              x_c(i1+3,i2,i3)=dyi3
           enddo
           icur=i1
        else
           icur=ibyz_c(1,i2,i3)
        endif

        do i1=icur,ibyz_c(2,i2,i3)
           dyi=0.0_wp
           !! Get the effective a-filters for the x dimension
           do t=max(ibyz_c(1,i2,i3),lowfil+i1),min(lupfil+i1,ibyz_c(2,i2,i3))
              dyi=dyi + w_c(t,i2,i3)*ad1_ext(t-i1)
           enddo
           x_c(i1,i2,i3)=dyi
        enddo

        istart=max(ibyz_c(1,i2,i3),ibyz_f(1,i2,i3)-lupfil)
        iend=min(ibyz_c(2,i2,i3),ibyz_f(2,i2,i3)-lowfil)

        if (iend-istart.ge.4) then
           do i1=istart,iend-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibyz_f(1,i2,i3),lowfil+i1),min(lupfil+i1+3,ibyz_f(2,i2,i3))
                 dyi0=dyi0 + w_f1(t,i2,i3)*bd1_ext(t-i1-0)
                 dyi1=dyi1 + w_f1(t,i2,i3)*bd1_ext(t-i1-1)
                 dyi2=dyi2 + w_f1(t,i2,i3)*bd1_ext(t-i1-2)
                 dyi3=dyi3 + w_f1(t,i2,i3)*bd1_ext(t-i1-3)
              enddo
              x_c(i1+0,i2,i3)=x_c(i1+0,i2,i3)+dyi0
              x_c(i1+1,i2,i3)=x_c(i1+1,i2,i3)+dyi1
              x_c(i1+2,i2,i3)=x_c(i1+2,i2,i3)+dyi2
              x_c(i1+3,i2,i3)=x_c(i1+3,i2,i3)+dyi3
           enddo
           istart=i1
        endif

        do i1=istart,iend
           dyi=0.0_wp
           do t=max(ibyz_f(1,i2,i3),lowfil+i1),min(lupfil+i1,ibyz_f(2,i2,i3))
              dyi=dyi + w_f1(t,i2,i3)*bd1_ext(t-i1)
           enddo
           x_c(i1,i2,i3)=x_c(i1,i2,i3)+dyi
        enddo

         if (ibyz_c(2,i2,i3)-ibyz_c(1,i2,i3).ge.4) then
           do i1=ibyz_f(1,i2,i3),ibyz_f(2,i2,i3)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibyz_c(1,i2,i3),lowfil+i1),min(lupfil+i1+3,ibyz_c(2,i2,i3))
                 dyi0=dyi0 + w_c(t,i2,i3)*cd1_ext(t-i1-0)
                 dyi1=dyi1 + w_c(t,i2,i3)*cd1_ext(t-i1-1)
                 dyi2=dyi2 + w_c(t,i2,i3)*cd1_ext(t-i1-2)
                 dyi3=dyi3 + w_c(t,i2,i3)*cd1_ext(t-i1-3)
              enddo
              x_f(1,i1+0,i2,i3)=dyi0
              x_f(1,i1+1,i2,i3)=dyi1
              x_f(1,i1+2,i2,i3)=dyi2
              x_f(1,i1+3,i2,i3)=dyi3
           enddo
           icur=i1
        else
           icur=ibyz_f(1,i2,i3)
        endif
        do i1=icur,ibyz_f(2,i2,i3)
           dyi=0.0_wp
           do t=max(ibyz_c(1,i2,i3),lowfil+i1),min(lupfil+i1,ibyz_c(2,i2,i3))
              dyi=dyi + w_c(t,i2,i3)*cd1_ext(t-i1)
           enddo
           x_f(1,i1,i2,i3)=dyi
        enddo
     enddo
  enddo
  !$omp enddo

   ! wavelet part

  ! x direction
  !$omp do
  do i3=nfl3,nfu3
     do i2=nfl2,nfu2
        do i1=ibyz_f(1,i2,i3),ibyz_f(2,i2,i3)
           t112=0.0_wp;t121=0.0_wp;t122=0.0_wp;t212=0.0_wp;t221=0.0_wp;t222=0.0_wp;t211=0.0_wp
           do l=max(nfl1-i1,lowfil),min(lupfil,nfu1-i1)
              t121=t121 + w_f(2,i1+l,i2,i3)*ad1_ext(l) + w_f(3,i1+l,i2,i3)*bd1_ext(l)
              t221=t221 + w_f(2,i1+l,i2,i3)*cd1_ext(l) + w_f(3,i1+l,i2,i3)*ed1_ext(l)
           enddo
           x_f(4,i1,i2,i3)=t112
           x_f(2,i1,i2,i3)=t121
           x_f(1,i1,i2,i3)=x_f(1,i1,i2,i3)+t211
           x_f(6,i1,i2,i3)=t122
           x_f(5,i1,i2,i3)=t212
           x_f(3,i1,i2,i3)=t221
           x_f(7,i1,i2,i3)=t222
        enddo
     enddo
  enddo
  !$omp enddo

!$omp end parallel

END SUBROUTINE createDerivativeX


subroutine createDerivativeY(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3,  &
     hgrid,ibxz_c,ibxz_f,w_c, w_f, w_f2, y_c, y_f)

  use liborbs_precisions
  use filterModule
  implicit none

  ! Calling arguments
  integer, intent(in) :: n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3
  real(gp), intent(in) :: hgrid
  integer, dimension(2,0:n1,0:n3), intent(in) :: ibxz_c,ibxz_f
  real(wp), dimension(0:n1,0:n2,0:n3), intent(in) :: w_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(in) :: w_f
  real(wp), dimension(nfl2:nfu2,nfl1:nfu1,nfl3:nfu3), intent(in) :: w_f2
  real(wp), dimension(0:n1,0:n2,0:n3), intent(out) :: y_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(out) :: y_f
  !local variables
  integer, parameter :: lowfil=-14,lupfil=14
  integer :: i,t,i1,i2,i3
  integer :: icur,istart,iend,l
  real(wp) :: dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211
  real(wp), dimension(-3+lowfil:lupfil+3) :: ad1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: bd1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: cd1_ext
  real(wp), dimension(lowfil:lupfil) :: ed1_ext


! Copy the filters to the 'extended filters', i.e. add some zeros.
! This seems to be required since we use loop unrolling.
ad1_ext=0.d0
bd1_ext=0.d0
cd1_ext=0.d0

do i=lowfil,lupfil
    ad1_ext(i)=ad1(i)
    bd1_ext(i)=bd1(i)
    cd1_ext(i)=cd1(i)
    ed1_ext(i)=ed1(i) !this is only needed due to OpenMP, since it seems I cannot declare ed1 (which is in filterModule) as shared
end do


!$omp parallel default(none) &
!$omp shared(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3) &
!$omp shared(ibxz_c,ibxz_f,w_c,w_f,y_c,y_f)& 
!$omp shared(w_f2,ad1_ext,bd1_ext,cd1_ext,ed1_ext)&
!$omp private(i,t,i1,i2,i3,icur,istart,iend,l)&
!$omp private(dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211)

  ! y direction
  !$omp do
  do i3=0,n3
     do i1=0,n1
        if (ibxz_c(2,i1,i3)-ibxz_c(1,i1,i3).ge.4) then
           do i2=ibxz_c(1,i1,i3),ibxz_c(2,i1,i3)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxz_c(1,i1,i3),lowfil+i2),min(lupfil+i2+3,ibxz_c(2,i1,i3))
                 dyi0=dyi0 + w_c(i1,t,i3)*ad1_ext(t-i2-0)
                 dyi1=dyi1 + w_c(i1,t,i3)*ad1_ext(t-i2-1)
                 dyi2=dyi2 + w_c(i1,t,i3)*ad1_ext(t-i2-2)
                 dyi3=dyi3 + w_c(i1,t,i3)*ad1_ext(t-i2-3)
              enddo
              y_c(i1,i2+0,i3)=dyi0
              y_c(i1,i2+1,i3)=dyi1
              y_c(i1,i2+2,i3)=dyi2
              y_c(i1,i2+3,i3)=dyi3
           enddo
           icur=i2
        else
           icur=ibxz_c(1,i1,i3)
        endif

        do i2=icur,ibxz_c(2,i1,i3)
           dyi=0.0_wp
           do t=max(ibxz_c(1,i1,i3),lowfil+i2),min(lupfil+i2,ibxz_c(2,i1,i3))
              dyi=dyi + w_c(i1,t,i3)*ad1_ext(t-i2)
           enddo
           y_c(i1,i2,i3)=dyi
        enddo
        istart=max(ibxz_c(1,i1,i3),ibxz_f(1,i1,i3)-lupfil)
        iend= min(ibxz_c(2,i1,i3),ibxz_f(2,i1,i3)-lowfil)

        if (iend-istart.ge.4) then
           do i2=istart,iend-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxz_f(1,i1,i3),lowfil+i2),min(lupfil+i2+3,ibxz_f(2,i1,i3))
                 dyi0=dyi0 + w_f2(t,i1,i3)*bd1_ext(t-i2-0)
                 dyi1=dyi1 + w_f2(t,i1,i3)*bd1_ext(t-i2-1)
                 dyi2=dyi2 + w_f2(t,i1,i3)*bd1_ext(t-i2-2)
                 dyi3=dyi3 + w_f2(t,i1,i3)*bd1_ext(t-i2-3)
              enddo
              y_c(i1,i2+0,i3)=y_c(i1,i2+0,i3)+dyi0
              y_c(i1,i2+1,i3)=y_c(i1,i2+1,i3)+dyi1
              y_c(i1,i2+2,i3)=y_c(i1,i2+2,i3)+dyi2
              y_c(i1,i2+3,i3)=y_c(i1,i2+3,i3)+dyi3
           enddo
           istart=i2
        endif

        do i2=istart,iend
           dyi=0.0_wp
           do t=max(ibxz_f(1,i1,i3),lowfil+i2),min(lupfil+i2,ibxz_f(2,i1,i3))
              dyi=dyi + w_f2(t,i1,i3)*bd1_ext(t-i2)
           enddo
           y_c(i1,i2,i3)=y_c(i1,i2,i3)+dyi
        enddo

         if (ibxz_f(2,i1,i3)-ibxz_f(1,i1,i3).ge.4) then
           do i2=ibxz_f(1,i1,i3),ibxz_f(2,i1,i3)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxz_c(1,i1,i3),lowfil+i2),min(lupfil+i2+3,ibxz_c(2,i1,i3))
                 dyi0=dyi0 + w_c(i1,t,i3)*cd1_ext(t-i2-0)
                 dyi1=dyi1 + w_c(i1,t,i3)*cd1_ext(t-i2-1)
                 dyi2=dyi2 + w_c(i1,t,i3)*cd1_ext(t-i2-2)
                 dyi3=dyi3 + w_c(i1,t,i3)*cd1_ext(t-i2-3)
              enddo
              y_f(2,i1,i2+0,i3)=dyi0
              y_f(2,i1,i2+1,i3)=dyi1
              y_f(2,i1,i2+2,i3)=dyi2
              y_f(2,i1,i2+3,i3)=dyi3
           enddo
           icur=i2
        else
           icur=ibxz_f(1,i1,i3)
        endif

        do i2=icur,ibxz_f(2,i1,i3)
           dyi=0.0_wp
           do t=max(ibxz_c(1,i1,i3),lowfil+i2),min(lupfil+i2,ibxz_c(2,i1,i3))
              dyi=dyi + w_c(i1,t,i3)*cd1_ext(t-i2)
           enddo
           y_f(2,i1,i2,i3)=dyi
        enddo
     enddo
  enddo
  !$omp enddo

  ! y direction
  !$omp do
  do i3=nfl3,nfu3
     do i1=nfl1,nfu1
        do i2=ibxz_f(1,i1,i3),ibxz_f(2,i1,i3)
           t112=0.0_wp;t121=0.0_wp;t122=0.0_wp;t212=0.0_wp;t221=0.0_wp;t222=0.0_wp;t211=0.0_wp
           do l=max(nfl2-i2,lowfil),min(lupfil,nfu2-i2)
              t112=t112 + w_f(4,i1,i2+l,i3)*ad1_ext(l) + w_f(6,i1,i2+l,i3)*bd1_ext(l)
              t211=t211 + w_f(1,i1,i2+l,i3)*ad1_ext(l) + w_f(3,i1,i2+l,i3)*bd1_ext(l)
              t122=t122 + w_f(4,i1,i2+l,i3)*cd1_ext(l) + w_f(6,i1,i2+l,i3)*ed1_ext(l)
              t212=t212 + w_f(5,i1,i2+l,i3)*ad1_ext(l) + w_f(7,i1,i2+l,i3)*bd1_ext(l)
              t221=t221 + w_f(1,i1,i2+l,i3)*cd1_ext(l) + w_f(3,i1,i2+l,i3)*ed1_ext(l)
              t222=t222 + w_f(5,i1,i2+l,i3)*cd1_ext(l) + w_f(7,i1,i2+l,i3)*ed1_ext(l)
              t121=t121 + w_f(2,i1,i2+l,i3)*ed1_ext(l)
           enddo
           y_f(4,i1,i2,i3)=t112
           y_f(2,i1,i2,i3)=y_f(2,i1,i2,i3)+t121
           y_f(1,i1,i2,i3)=t211
           y_f(6,i1,i2,i3)=t122
           y_f(5,i1,i2,i3)=t212
           y_f(3,i1,i2,i3)=t221
           y_f(7,i1,i2,i3)=t222
        enddo
     enddo
  enddo
  !$omp enddo
!$omp end parallel

END SUBROUTINE createDerivativeY


subroutine createDerivativeZ(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3,  &
     hgrid,ibxy_c,ibxy_f,w_c, w_f, w_f3, z_c, z_f)

  use liborbs_precisions
  use filterModule
  implicit none

  ! Calling arguments
  integer, intent(in) :: n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3
  real(gp), intent(in) :: hgrid
  integer, dimension(2,0:n1,0:n2), intent(in) :: ibxy_c,ibxy_f
  real(wp), dimension(0:n1,0:n2,0:n3), intent(in) :: w_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(in) :: w_f
  real(wp), dimension(nfl3:nfu3,nfl1:nfu1,nfl2:nfu2), intent(in) :: w_f3
  real(wp), dimension(0:n1,0:n2,0:n3), intent(out) :: z_c
  real(wp), dimension(7,nfl1:nfu1,nfl2:nfu2,nfl3:nfu3), intent(out) :: z_f
  !local variables
  integer, parameter :: lowfil=-14,lupfil=14
  integer :: i,t,i1,i2,i3
  integer :: icur,istart,iend,l
  real(wp) :: dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211
  real(wp), dimension(-3+lowfil:lupfil+3) :: ad1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: bd1_ext
  real(wp), dimension(-3+lowfil:lupfil+3) :: cd1_ext
  real(wp), dimension(lowfil:lupfil) :: ed1_ext


! Copy the filters to the 'extended filters', i.e. add some zeros.
! This seems to be required since we use loop unrolling.
ad1_ext=0.d0
bd1_ext=0.d0
cd1_ext=0.d0

do i=lowfil,lupfil
    ad1_ext(i)=ad1(i)
    bd1_ext(i)=bd1(i)
    cd1_ext(i)=cd1(i)
    ed1_ext(i)=ed1(i) !this is only needed due to OpenMP, since it seems I cannot declare ed1 (which is in filterModule) as shared
end do

!$omp parallel default(none) &
!$omp shared(n1,n2,n3,nfl1,nfu1,nfl2,nfu2,nfl3,nfu3) &
!$omp shared(ibxy_c,ibxy_f,w_c,w_f,z_c,z_f)& 
!$omp shared(w_f3,ad1_ext,bd1_ext,cd1_ext,ed1_ext)&
!$omp private(i,t,i1,i2,i3,icur,istart,iend,l)&
!$omp private(dyi,dyi0,dyi1,dyi2,dyi3,t112,t121,t122,t212,t221,t222,t211)

  ! z direction
  !$omp do
  do i2=0,n2
     do i1=0,n1
        if (ibxy_c(2,i1,i2)-ibxy_c(1,i1,i2).ge.4) then
           do i3=ibxy_c(1,i1,i2),ibxy_c(2,i1,i2)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxy_c(1,i1,i2),lowfil+i3),min(lupfil+i3+3,ibxy_c(2,i1,i2))
                 dyi0=dyi0 + w_c(i1,i2,t)*ad1_ext(t-i3-0)
                 dyi1=dyi1 + w_c(i1,i2,t)*ad1_ext(t-i3-1)
                 dyi2=dyi2 + w_c(i1,i2,t)*ad1_ext(t-i3-2)
                 dyi3=dyi3 + w_c(i1,i2,t)*ad1_ext(t-i3-3)
              enddo
              z_c(i1,i2,i3+0)=dyi0
              z_c(i1,i2,i3+1)=dyi1
              z_c(i1,i2,i3+2)=dyi2
              z_c(i1,i2,i3+3)=dyi3
           enddo
           icur=i3
        else
           icur=ibxy_c(1,i1,i2)
        endif

        do i3=icur,ibxy_c(2,i1,i2)
           dyi=0.0_wp
           do t=max(ibxy_c(1,i1,i2),lowfil+i3),min(lupfil+i3,ibxy_c(2,i1,i2))
              dyi=dyi + w_c(i1,i2,t)*ad1_ext(t-i3)
           enddo
           z_c(i1,i2,i3)=dyi
        enddo
        istart=max(ibxy_c(1,i1,i2),ibxy_f(1,i1,i2)-lupfil)
        iend=min(ibxy_c(2,i1,i2),ibxy_f(2,i1,i2)-lowfil)

        if (iend-istart.ge.4) then
           do i3=istart,iend-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxy_f(1,i1,i2),lowfil+i3),min(lupfil+i3+3,ibxy_f(2,i1,i2))
                 dyi0=dyi0 + w_f3(t,i1,i2)*bd1_ext(t-i3-0)
                 dyi1=dyi1 + w_f3(t,i1,i2)*bd1_ext(t-i3-1)
                 dyi2=dyi2 + w_f3(t,i1,i2)*bd1_ext(t-i3-2)
                 dyi3=dyi3 + w_f3(t,i1,i2)*bd1_ext(t-i3-3)
              enddo
              z_c(i1,i2,i3+0)=z_c(i1,i2,i3+0)+dyi0
              z_c(i1,i2,i3+1)=z_c(i1,i2,i3+1)+dyi1
              z_c(i1,i2,i3+2)=z_c(i1,i2,i3+2)+dyi2
              z_c(i1,i2,i3+3)=z_c(i1,i2,i3+3)+dyi3
           enddo
           istart=i3
        endif

        do i3=istart,iend
           dyi=0.0_wp
           do t=max(ibxy_f(1,i1,i2),lowfil+i3),min(lupfil+i3,ibxy_f(2,i1,i2))
              dyi=dyi + w_f3(t,i1,i2)*bd1_ext(t-i3)
           enddo
           z_c(i1,i2,i3)=z_c(i1,i2,i3)+dyi
        enddo

         if (ibxy_f(2,i1,i2)-ibxy_f(1,i1,i2).ge.4) then
           do i3=ibxy_f(1,i1,i2),ibxy_f(2,i1,i2)-4,4
              dyi0=0.0_wp
              dyi1=0.0_wp
              dyi2=0.0_wp
              dyi3=0.0_wp
              do t=max(ibxy_c(1,i1,i2),lowfil+i3),min(lupfil+i3+3,ibxy_c(2,i1,i2))
                 dyi0=dyi0 + w_c(i1,i2,t)*cd1_ext(t-i3-0)
                 dyi1=dyi1 + w_c(i1,i2,t)*cd1_ext(t-i3-1)
                 dyi2=dyi2 + w_c(i1,i2,t)*cd1_ext(t-i3-2)
                 dyi3=dyi3 + w_c(i1,i2,t)*cd1_ext(t-i3-3)
              enddo
              z_f(4,i1,i2,i3+0)=dyi0
              z_f(4,i1,i2,i3+1)=dyi1
              z_f(4,i1,i2,i3+2)=dyi2
              z_f(4,i1,i2,i3+3)=dyi3
           enddo
           icur=i3
        else
           icur=ibxy_f(1,i1,i2)
        endif

        do i3=icur,ibxy_f(2,i1,i2)
           dyi=0.0_wp
           do t=max(ibxy_c(1,i1,i2),lowfil+i3),min(lupfil+i3,ibxy_c(2,i1,i2))
              dyi=dyi + w_c(i1,i2,t)*cd1_ext(t-i3)
           enddo
           z_f(4,i1,i2,i3)=dyi
        enddo
     enddo
  enddo
  !$omp enddo

  ! wavelet part

  ! z direction
  !$omp do
  do i2=nfl2,nfu2
     do i1=nfl1,nfu1
        do i3=ibxy_f(1,i1,i2),ibxy_f(2,i1,i2)
           t112=0.0_wp;t121=0.0_wp;t122=0.0_wp;t212=0.0_wp;t221=0.0_wp;t222=0.0_wp;t211=0.0_wp
           do l=max(nfl3-i3,lowfil),min(lupfil,nfu3-i3)
              t121=t121 + w_f(2,i1,i2,i3+l)*ad1_ext(l) + w_f(6,i1,i2,i3+l)*bd1_ext(l)
              t211=t211 + w_f(1,i1,i2,i3+l)*ad1_ext(l) + w_f(5,i1,i2,i3+l)*bd1_ext(l)
              t122=t122 + w_f(2,i1,i2,i3+l)*cd1_ext(l) + w_f(6,i1,i2,i3+l)*ed1_ext(l)
              t212=t212 + w_f(1,i1,i2,i3+l)*cd1_ext(l) + w_f(5,i1,i2,i3+l)*ed1_ext(l)
              t221=t221 + w_f(3,i1,i2,i3+l)*ad1_ext(l) + w_f(7,i1,i2,i3+l)*bd1_ext(l)
              t222=t222 + w_f(3,i1,i2,i3+l)*cd1_ext(l) + w_f(7,i1,i2,i3+l)*ed1_ext(l)
              t112=t112 + w_f(4,i1,i2,i3+l)*ed1_ext(l)
           enddo
           z_f(4,i1,i2,i3)=z_f(4,i1,i2,i3)+t112
           z_f(2,i1,i2,i3)=t121
           z_f(1,i1,i2,i3)=t211
           z_f(6,i1,i2,i3)=t122
           z_f(5,i1,i2,i3)=t212
           z_f(3,i1,i2,i3)=t221
           z_f(7,i1,i2,i3)=t222

        enddo
     enddo
  enddo
  !$omp enddo

  !$omp end parallel


END SUBROUTINE createDerivativeZ

