project_translation_rotation Subroutine

   subroutine project_translation_rotation(mw_hessian, coordinates, element_numbers)
      !! Project out translation and rotation modes from mass-weighted Hessian.
      !!
      !! Builds 6 vectors (3 translation + 3 rotation) in mass-weighted coordinates,
      !! orthonormalizes them using SVD, then projects them out:
      !!   H_proj = (I - D @ D^T) @ H @ (I - D @ D^T)
      !!
      !! This sets the 6 translation/rotation eigenvalues to exactly zero.
      real(dp), intent(inout) :: mw_hessian(:, :)
         !! Mass-weighted Hessian (modified in place)
      real(dp), intent(in) :: coordinates(:, :)
         !! Atomic coordinates in Bohr (3, N)
      integer, intent(in) :: element_numbers(:)
         !! Atomic numbers for each atom (N atoms)

      real(dp), allocatable :: D(:, :)        ! Translation/rotation vectors (3N, 6)
      real(dp), allocatable :: com(:)         ! Center of mass
      real(dp), allocatable :: r(:, :)        ! Coordinates relative to COM
      real(dp), allocatable :: sqrt_mass(:)   ! sqrt(mass) for each atom
      real(dp), allocatable :: S(:)           ! Singular values
      real(dp), allocatable :: U(:, :)        ! Left singular vectors
      real(dp), allocatable :: VT(:, :)       ! Right singular vectors (transposed)
      real(dp), allocatable :: D_orth(:, :)   ! Orthonormalized D vectors
      real(dp), allocatable :: proj(:, :)     ! Projector matrix
      real(dp), allocatable :: temp(:, :)     ! Temporary matrix
      real(dp) :: total_mass, mass, norm
      integer :: n_atoms, n_coords, iatom, i, j, k, n_modes, info
      integer :: idx

      n_atoms = size(element_numbers)
      n_coords = 3*n_atoms

      ! Allocate arrays
      allocate (D(n_coords, 6))
      allocate (com(3))
      allocate (r(3, n_atoms))
      allocate (sqrt_mass(n_atoms))

      ! Compute sqrt(mass) for each atom and total mass
      total_mass = 0.0_dp
      do iatom = 1, n_atoms
         mass = element_mass(element_numbers(iatom))
         sqrt_mass(iatom) = sqrt(mass)
         total_mass = total_mass + mass
      end do

      ! Compute center of mass
      com = 0.0_dp
      do iatom = 1, n_atoms
         mass = element_mass(element_numbers(iatom))
         com(:) = com(:) + mass*coordinates(:, iatom)
      end do
      com = com/total_mass

      ! Compute coordinates relative to center of mass
      do iatom = 1, n_atoms
         r(:, iatom) = coordinates(:, iatom) - com(:)
      end do

      ! Initialize D to zero
      D = 0.0_dp

      ! Build translation vectors (mass-weighted)
      ! D_trans_k: displacement along axis k, weighted by sqrt(mass)
      do iatom = 1, n_atoms
         idx = 3*(iatom - 1)
         ! Translation along x
         D(idx + 1, 1) = sqrt_mass(iatom)
         ! Translation along y
         D(idx + 2, 2) = sqrt_mass(iatom)
         ! Translation along z
         D(idx + 3, 3) = sqrt_mass(iatom)
      end do

      ! Build rotation vectors (mass-weighted)
      ! D_rot_k: rotation around axis k, proportional to r × e_k, weighted by sqrt(mass)
      do iatom = 1, n_atoms
         idx = 3*(iatom - 1)
         ! Rotation around x-axis: r × e_x = (0, r_z, -r_y)
         D(idx + 2, 4) = sqrt_mass(iatom)*r(3, iatom)
         D(idx + 3, 4) = -sqrt_mass(iatom)*r(2, iatom)
         ! Rotation around y-axis: r × e_y = (-r_z, 0, r_x)
         D(idx + 1, 5) = -sqrt_mass(iatom)*r(3, iatom)
         D(idx + 3, 5) = sqrt_mass(iatom)*r(1, iatom)
         ! Rotation around z-axis: r × e_z = (r_y, -r_x, 0)
         D(idx + 1, 6) = sqrt_mass(iatom)*r(2, iatom)
         D(idx + 2, 6) = -sqrt_mass(iatom)*r(1, iatom)
      end do

      ! Normalize each column of D
      do k = 1, 6
         norm = sqrt(sum(D(:, k)**2))
         if (norm > 1.0e-10_dp) then
            D(:, k) = D(:, k)/norm
         end if
      end do

      ! Orthonormalize D using SVD: D = U @ S @ VT
      ! The orthonormal basis is given by the columns of U corresponding to non-zero singular values
      allocate (S(6))
      allocate (U(n_coords, 6))
      allocate (VT(6, 6))

      ! pic_gesvd(A, S, U, VT, info) - A is input, U and VT are separate outputs
      call pic_gesvd(D, S, U, VT, info=info)

      ! Count non-zero singular values (determines number of modes to project)
      n_modes = 0
      do k = 1, 6
         if (S(k) > 1.0e-10_dp) n_modes = n_modes + 1
      end do

      ! Build orthonormalized D matrix from U (columns with non-zero singular values)
      allocate (D_orth(n_coords, n_modes))
      j = 0
      do k = 1, 6
         if (S(k) > 1.0e-10_dp) then
            j = j + 1
            D_orth(:, j) = U(:, k)
         end if
      end do

      ! Build projector: P = I - D_orth @ D_orth^T
      allocate (proj(n_coords, n_coords))
      proj = 0.0_dp
      do i = 1, n_coords
         proj(i, i) = 1.0_dp
      end do

      ! Subtract D_orth @ D_orth^T
      do i = 1, n_coords
         do j = 1, n_coords
            do k = 1, n_modes
               proj(i, j) = proj(i, j) - D_orth(i, k)*D_orth(j, k)
            end do
         end do
      end do

      ! Apply projection: H_proj = P @ H @ P
      allocate (temp(n_coords, n_coords))

      ! temp = H @ P
      do i = 1, n_coords
         do j = 1, n_coords
            temp(i, j) = 0.0_dp
            do k = 1, n_coords
               temp(i, j) = temp(i, j) + mw_hessian(i, k)*proj(k, j)
            end do
         end do
      end do

      ! H_proj = P @ temp
      do i = 1, n_coords
         do j = 1, n_coords
            mw_hessian(i, j) = 0.0_dp
            do k = 1, n_coords
               mw_hessian(i, j) = mw_hessian(i, j) + proj(i, k)*temp(k, j)
            end do
         end do
      end do

      ! Cleanup
      deallocate (D, com, r, sqrt_mass, S, U, VT, D_orth, proj, temp)

   end subroutine project_translation_rotation