!  Copyright (C) 2017 The authors of Py-ChemShell
!
!  This file is part of Py-ChemShell.
!
!  Py-ChemShell is free software: you can redistribute it and/or modify
!  it under the terms of the GNU Lesser General Public License as
!  published by the Free Software Foundation, either version 3 of the
!  License, or (at your option) any later version.
!
!  Py-ChemShell is distributed in the hope that it will be useful,
!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!  GNU Lesser General Public License for more details.
!
!  You should have received a copy of the GNU Lesser General Public
!  License along with Py-ChemShell.  If not, see
!  <http://www.gnu.org/licenses/>.


! cut out a cluster from a periodic crystal structure
integer function cluster_cut(clustersize, clustersize_shells) result(ierr)
  
  use ChemShellClusterModule

  implicit none

  integer, intent(inout)    :: clustersize, clustersize_shells

  integer :: ndim, ncores, nshells
  integer, parameter :: nbqs = 0
  character(len=:), pointer :: crystal_type, coordinated_mode, dimensionality
  double precision :: radius_cluster
  integer :: set_cluster_origin
  double precision :: frac_origin_point(3)

  integer :: i, j, k, l, m, p, ip, l1, sign, jj, ishel, pshel, isite

  ! Arrays for crystal information
  double precision :: a, b, c, alpha, beta, gamma, area, volume
  double precision, dimension(3, 3) :: vect, vecinv, cart_to_frac
  double precision, allocatable :: cart_coords(:,:)
  double precision, allocatable :: f_core(:,:), f_shel(:,:), r_core(:,:), r_shel(:,:)
  double precision, allocatable :: cart_charges(:)
  double precision, allocatable :: charge_core(:), charge_shel(:)
  integer, allocatable :: point_core(:)
  integer, allocatable :: point_shel(:)
  integer, parameter :: max_coord = 14
  integer, allocatable :: coordination(:)
  integer, allocatable :: point_neighbour(:,:)
  double precision, allocatable :: v_neighbour(:,:,:)
  integer, allocatable :: mol_point(:)
  integer, allocatable :: mol_size(:)
  integer, allocatable :: mol_core_point(:,:)
  double precision, allocatable :: mol_vec(:,:)

  double precision, dimension(3) :: f_centre, r_centre
  double precision, dimension(3) :: f, f1, f2, r, r2
  double precision :: romb_side, r_v, radius, radius1, buffer, charge1
  integer :: i_max, j_max, k_max
  integer :: nmol
  double precision, dimension(3) :: sum_f, av_f

  double precision, allocatable :: f_cluster_core(:,:)
  double precision, allocatable :: f_cluster_shel(:,:)
  double precision, allocatable :: r_cluster_core(:,:)
  double precision, allocatable :: r_cluster_shel(:,:)
  double precision, allocatable :: charge_cluster_core(:)
  double precision, allocatable :: charge_cluster_shel(:)
  integer, allocatable :: point_cluster_core(:)
  integer, allocatable :: point_uc_c(:)
  integer, allocatable :: point_uc_s(:)
  integer, allocatable :: point_added(:)
  double precision, allocatable :: r_cluster_mol(:,:)
  integer, allocatable :: point_cluster_mol(:)
  double precision :: charge_cluster
  integer :: tot_uc, max_core, max_shel, max_mol
  integer :: n_cluster_core, n_cluster_shel, n_cluster_mol
  integer :: n_added
  logical :: add_neighbour

  logical :: shell_model

  double precision :: two_pi
  integer, parameter :: periodic_frag = 1, cluster_frag = 2
  double precision, parameter :: d_min = 0.1d0

  double precision :: distance_2, distance_3
  double precision :: dum

  integer :: ncores_new_cluster, nshells_new_cluster

  ierr = 1

  ! get the parameters for the routine from Python
  call PyFrag%get('npts'   , ncores)
  call PyFrag%get('nshells', nshells)
  call PyOptions%get('radius_cluster', radius_cluster)
  call PyOptions%get('crystal_type'  , crystal_type)
  call PyOptions%get('coordinated'   , coordinated_mode)
  call PyOptions%get('origin'        , frac_origin_point)

  ndim = get_ndims()

  allocate(cart_coords(3,ncores+nshells))
  allocate(f_core(3,ncores))
  allocate(f_shel(3,nshells))
  allocate(r_core(3,ncores))
  allocate(r_shel(3,nshells))
  allocate(cart_charges(ncores+nshells))
  allocate(charge_core(ncores))
  allocate(charge_shel(nshells))
  allocate(point_core(nshells))
  allocate(point_shel(ncores))
  allocate(coordination(ncores))
  allocate(point_neighbour(max_coord,ncores))
  allocate(v_neighbour(3,max_coord,ncores))
  allocate(mol_point(ncores))
  allocate(mol_size(ncores))
  allocate(mol_core_point(ncores,ncores))
  allocate(mol_vec(3,ncores))

  if (debug > 0) then
     write(6,'(a)')    "-----------------------------"
     write(6,'(a,i4)') "cluster_cut debug level:", debug
     write(6,'(a)')    "-----------------------------"
  end if

  ! PGI does not like acos in parameter initialisation so initialise here
  two_pi = 2.d0*dacos(-1.d0)

  ! Check there are no point charges
  if (nbqs .ne. 0) then
     write(6,*) "cluster_cut: number of point charges must be zero"
     ierr = 1
     return
  end if

  ! coordinated_mode should only be specified for bulk systems
  if(ndim /= 3 .and. (trim(coordinated_mode).eq.'cations'.or.trim(coordinated_mode).eq.'anions')) then
     write(6,*) "cluster_cut: coordinated_mode only valid for bulk systems"
     ierr = 1
     return     
  end if
  ! molecular mode only for bulk systems
  if (ndim /= 3 .and. trim(crystal_type) .eq. 'molecular') then
     write(6,*) "cluster_cut: molecular crystal type only supported for bulk systems"
     ierr = 1
     return     
  end if
  ! coordinated_mode not compatible with molecular mode
  if((trim(coordinated_mode).eq.'cations'.or.trim(coordinated_mode).eq.'anions').and.trim(crystal_type).eq.'molecular') then
     write(6,*) "cluster_cut: coordinated_mode not valid for molecular crystals"
     ierr = 1
     return     
  end if

  if (nshells .eq. 0) then
     shell_model = .false.
  else
     shell_model = .true.
  end if

  ! Get unit cell vectors from fragment
  vect = 0.0d0
  call get_cell_vectors(periodic_frag, vect, ierr)
  if (ierr .ne. 0) return

  if (debug > 0) then
      write(6,*) "Cell vector:"
      write(6,'(3f12.7)') vect(1,:)
      write(6,'(3f12.7)') vect(2,:)
      write(6,'(3f12.7)') vect(3,:)
  end if 

  ! Call construct routines to generate unit cell parameters
  ! as they will not necessarily be present in the fragment object
  select case (ndim)
  case (2)
     call lattice_parameters_2(vect(1:2, 1:2), area, a, b, gamma)
  case (3)
     call lattice_parameters_3(vect, volume, a, b, c, alpha, beta, gamma)
  case default
     write(6,*) "cluster_cut: error, ndim=", ndim
     ierr = 1
     return
  end select

  ! Calculate inverse cell vector
  vecinv = 0.0d0
  select case (ndim)
  case (2)
     call inverse_2(vect(1:2, 1:2), vecinv(1:2, 1:2), dum)
  case (3)
     call inverse_3(vect, vecinv)
  case default
     write(6,*) "cluster_cut: error, ndim=", ndim
     ierr = 1
     return
  end select

  ! The construct routine takes fractional coordinates of the unit cell
  ! as an input. ChemShell fractional coordinates refer only to the asymmetric
  ! unit. They also are not 'the closest to the origin' (see frac_to_cart).
  ! Therefore we take the final cartesian coordinates and convert back
  ! to fractional coordinates.

  ! Get cartesian coordinates from fragment
  ! Split into core and shell arrays
  call get_cart_coords(periodic_frag, cart_coords, ncores, nshells, ierr)
  if (ierr .ne. 0) return

  ! Convert from cartesians to fractional coordinates
  select case (ndim)
  case (2)
     do i = 1, 2
        do j = 1, 2
           cart_to_frac(i,j) = vecinv(j,i)/two_pi
        end do
     end do
     do isite = 1, ncores
        f_core(1, isite) = cart_to_frac(1,1) * cart_coords(1, isite) &
             + cart_to_frac(2,1) * cart_coords(2, isite)
        f_core(2, isite) = cart_to_frac(1,2) * cart_coords(1, isite) &
             + cart_to_frac(2,2) * cart_coords(2, isite)
        ! Just store the z component
        f_core(3, isite) = cart_coords(3, isite)
     end do
     do isite = 1, nshells
        ishel = ncores + isite
        f_shel(1, isite) = cart_to_frac(1,1) * cart_coords(1, ishel) &
             + cart_to_frac(2,1) * cart_coords(2, ishel)
        f_shel(2, isite) = cart_to_frac(1,2) * cart_coords(1, ishel) &
             + cart_to_frac(2,2) * cart_coords(2, ishel)
        ! Just store the z component
        f_shel(3, isite) = cart_coords(3, ishel)
     end do
  case (3)
     do i = 1, 3
        do j = 1, 3
           cart_to_frac(i,j) = vecinv(j,i)/two_pi
        end do
     end do
     do isite = 1, ncores
        f_core(1, isite) = cart_to_frac(1,1) * cart_coords(1, isite) &
             + cart_to_frac(2,1) * cart_coords(2, isite) + cart_to_frac(3,1) * cart_coords(3, isite)
        f_core(2, isite) = cart_to_frac(1,2) * cart_coords(1, isite) &
             + cart_to_frac(2,2) * cart_coords(2, isite) + cart_to_frac(3,2) * cart_coords(3, isite)
        f_core(3, isite) = cart_to_frac(1,3) * cart_coords(1, isite) &
             + cart_to_frac(2,3) * cart_coords(2, isite) + cart_to_frac(3,3) * cart_coords(3, isite)
     end do
     do isite = 1, nshells
        ishel = ncores + isite
        f_shel(1, isite) = cart_to_frac(1,1) * cart_coords(1, ishel) &
             + cart_to_frac(2,1) * cart_coords(2, ishel) + cart_to_frac(3,1) * cart_coords(3, ishel)
        f_shel(2, isite) = cart_to_frac(1,2) * cart_coords(1, ishel) &
             + cart_to_frac(2,2) * cart_coords(2, ishel) + cart_to_frac(3,2) * cart_coords(3, ishel)
        f_shel(3, isite) = cart_to_frac(1,3) * cart_coords(1, ishel) &
             + cart_to_frac(2,3) * cart_coords(2, ishel) + cart_to_frac(3,3) * cart_coords(3, ishel)
     end do
  case default
     write(6,*) "cluster_cut: error, ndim=", ndim
     ierr = 1
     return
  end select
  if (debug > 3) then
     write(6,'(/a)') "Initial fractional coordinates"
     write(6,'(/a)') "CORES"
     do i = 1, ncores
        write(6,'(i6, 3f12.7)') i, f_core(1:3,i)
     end do
     write(6,'(/a)') "SHELLS"
     do i = 1, nshells
        write(6,'(i6, 3f12.7)') i, f_shel(1:3,i)
     end do
     write(6,*)
  end if

  ! Get charges of cartesian sites from fragment
  ! Split into core and shell arrays
  call get_cart_charges(periodic_frag, cart_charges, ncores+nshells, ierr)
  if (ierr .ne. 0) return
  charge_core = cart_charges(1:ncores)
  if (shell_model) charge_shel = cart_charges(ncores+1:ncores+nshells)

  ! Get pointers from shells to cores
  ! Then setup pointers in the other direction
  point_shel = 0
  if (shell_model) then
     call get_point_core(periodic_frag, point_core, nshells, ierr)
     if (ierr .ne. 0) return
     do i = 1, nshells
        point_shel(point_core(i)) = i
     end do
  end if

  ! YL 15/11/2019: user input of origin is only processed in Python
  ! f_centre is fractional and r_centre is cartesian
  ! YL 15/11/2019: frac_origin_point is always fractional on the Fortran side
  f_centre = frac_origin_point

  ! Translate cores towards the nominated centre and track shells
  select case (ndim)
  case (2)
     call transform_2(vect(1:2,1:2), f_centre(1:2), r_centre(1:2))
     r_centre(3) = f_centre(3)
     do i = 1, ncores
        f(1:2) = f_core(1:2,i)
        call translate_2(vect(1:2,1:2), f_centre(1:2), f(1:2), r(1:2))
        f_core(1:2,i) = f(1:2)
        r_core(1:2,i) = r(1:2)
        r_core(3,i) = f_core(3,i)
        if (shell_model) then
           j=point_shel(i)
           if(j.ne.0) then
              f1(1:2) = f_core(1:2,i)
              f2(1:2) = f_shel(1:2,j)
              call translate_2(vect(1:2,1:2), f1(1:2), f2(1:2), r(1:2))
              f_shel(1:2,j) = f2(1:2)
              r_shel(1:2,j) = r(1:2)
              r_shel(3,j) = f_shel(3,j)
           endif
        endif
     enddo
  case (3)
     call transform_3(vect, f_centre, r_centre)
     do i=1,ncores
        f(1:3) = f_core(1:3,i)
        call translate_3(vect,f_centre,f,r)
        f_core(1:3,i) = f(1:3)
        r_core(1:3,i) = r(1:3)
        if(shell_model) then
           j=point_shel(i)
           if(j.ne.0) then
              f1(1:3)=f_core(1:3,i)
              f2(1:3)=f_shel(1:3,j)
              call translate_3(vect,f1,f2,r)
              f_shel(1:3,j) = f2(1:3)
              r_shel(1:3,j) = r(1:3)
           endif
        endif
     enddo
  end select

  ! YL 17/11/2019: save to module
  cluster_origin = r_centre

  if (debug > 0) then
     write(6,'(/a)') "Cluster origin"
     write(6,'(/a)') "Fractional"
     write(6,'(4x, 3f12.7)') f_centre(1:3)
     write(6,'(/a)') "Cartesian"
     write(6,'(4x, 3f12.7)') r_centre(1:3)
     write(6,*)
  endif
  if (debug > 3) then
     write(6,'(/a)') "Fractional coords after translation to centre"
     write(6,'(/a)') "CORES"
     do i = 1, ncores
        write(6,'(i6, 3f12.7)') i, f_core(1:3,i)
     end do
     write(6,'(/a)') "SHELLS"
     do i = 1, nshells
        write(6,'(i6, 3f12.7)') i, f_shel(1:3,i)
     end do
     write(6,*)
  end if
  if (debug > 3) then
     write(6,'(/a)') "Cartesian coords after translation to centre"
     write(6,'(/a)') "CORES"
     do i = 1, ncores
        write(6,'(i6, 3f12.7)') i, r_core(1:3,i)
     end do
     write(6,'(/a)') "SHELLS"
     do i = 1, nshells
        write(6,'(i6, 3f12.7)') i, r_shel(1:3,i)
     end do
     write(6,*)
  end if

  ! Determine coordination number and neighbours for all atoms in the unit cell
  coordination(:) = 0
  call periodic_neighbour_list(ncores, coordination, point_neighbour, v_neighbour, ierr)
  if (ierr .ne. 0) return
  if (debug > 0) then
     write(6,'(/a)') "Crystal neighbour list"
     do i = 1, ncores
        write(6,'(i4,a,i4)') i, ":", coordination(i)
        do j = 1, coordination(i)
           write(6,'(2x,i4,2x,3f12.7)') point_neighbour(j,i), v_neighbour(1:3,j,i)
        end do
        write(6,*)
     end do
     write(6,*)
  end if

  ! Generate list of molecules
  nmol = 0
  mol_point = 0
  mol_size = 0
  mol_core_point = 0
  if(trim(crystal_type).eq.'molecular') then
     do i = 1, ncores
        ! Ignore atoms already assigned to a molecule
        if (mol_point(i) /= 0) cycle 
        ! New molecule
        nmol = nmol + 1
        mol_point(i) = nmol
        mol_vec(:,i) = 0.0d0
        ! Find all atoms in the molecule by a recursive search
        call traverse_molecule(i)
     end do
     do i = 1, ncores
        j = mol_point(i)
        mol_size(j) = mol_size(j) + 1
        mol_core_point(mol_size(j), j) = i
     end do
     if (debug > 0) then
        write(6,'(/a,i4)') "Number of molecules: ", nmol
        do i = 1, nmol
           write(6,'(i4,a,i4)') i, ':', mol_size(i)
           do j = 1, mol_size(i)
              write(6,'(i6,2x,3f12.7)') mol_core_point(j,i), mol_vec(1:3,mol_core_point(j,i))
           end do
        end do
     end if
  end if

  ! Determine extent of cluster, allocate memory
  select case (ndim)
  case (2)
     ! Calculate extensions of UC which includes the cluster hemisphere
     romb_side = radius_cluster / dabs(dsin(gamma))
     i_max = romb_side / a + 1
     j_max = romb_side / b + 1
     k_max = 0
     tot_uc = (2 * i_max + 1) * (2 * j_max + 1)
  case (3)
     ! Calculate extensions of UC which includes the cluster sphere 
     r_v = radius_cluster / volume
     i_max = r_v * b * c * dabs(dsin(alpha)) + 1
     j_max = r_v * a * c * dabs(dsin( beta)) + 1
     k_max = r_v * a * b * dabs(dsin(gamma)) + 1
     tot_uc = (2 * i_max + 1) * (2 * j_max + 1) * (2 * k_max + 1)
  case default
     write(6,*) "cluster_cut: error, ndim=", ndim
     ierr = 1
     return
  end select
  max_core = tot_uc * ncores
  max_shel = tot_uc * nshells
  max_mol = tot_uc * nmol
  allocate(f_cluster_core(3,max_core))
  allocate(f_cluster_shel(3,max_shel))
  allocate(r_cluster_core(3,max_core))
  allocate(r_cluster_shel(3,max_shel))
  allocate(charge_cluster_core(max_core))
  allocate(charge_cluster_shel(max_shel))
  allocate(point_cluster_core(max_shel))
  allocate(point_uc_c(max_core))
  allocate(point_uc_s(max_shel))
  allocate(point_added(max_core))
  allocate(r_cluster_mol(3,max_mol))
  allocate(point_cluster_mol(max_core))
  if (debug > 0) then
     write(6,'(/a)') "Unit cell extensions"
     write(6,'(/a,4i8)') "i_max, j_max, k_max, tot_uc: ", i_max, j_max, k_max, tot_uc
     write(6,'(a,3i8)') "max_core, max_shel, max_mol: ", max_core, max_shel, max_mol
     write(6,*)
  end if

  ! Build the cluster
  n_cluster_core = 0
  n_cluster_shel = 0
  ! n_added refers to atoms that are outside the cluster boundary
  ! but have been added to ensure full coordination of cations/anions
  n_added = 0
  point_added(:) = 0
  ! Molecular information for sorting molecular crystals
  n_cluster_mol = 0
  r_cluster_mol(1:3,:) = 0.0d0
  point_cluster_mol(:) = 0
  select case (ndim)
  case (2)
     if (trim(crystal_type).eq.'molecular') then
        write(6,*) "Surface cutting for molecular crystals not supported."
        ierr = 1
        return
     end if
     do i=-i_max,i_max
        do j=-j_max,j_max
           do p=1,ncores
              f(1)=f_core(1,p)+i
              f(2)=f_core(2,p)+j
              f(3)=f_core(3,p)
              radius=distance_2(vect(1:2,1:2),f(1:2),r_core(3,p), &
                   f_centre(1:2),f_centre(3))
              if(radius < radius_cluster) then
                 n_cluster_core = n_cluster_core + 1
                 f_cluster_core(1:3, n_cluster_core) = f(1:3)
                 point_uc_c(n_cluster_core) = p
              endif
           enddo !p
        enddo !j
     enddo !i
     charge_cluster=0.d0
     do i = 1, n_cluster_core
        ! Core coordinates and charges
        f(1:2) = f_cluster_core(1:2,i)        
        call transform_2(vect(1:2,1:2), f(1:2), r(1:2))
        r_cluster_core(1:2,i) = r(1:2)
        r_cluster_core(3,i) = f_cluster_core(3,i)
        j = point_uc_c(i)
        charge_cluster_core(i) = charge_core(j)
        charge_cluster = charge_cluster + charge_cluster_core(i)
     end do
  case(3)
     selectcase(trim(crystal_type))
     case('ionic', 'covalent')
        ! Extended crystals (ionic or covalent)
        do i=-i_max,i_max
           do j=-j_max,j_max
              do k=-k_max,k_max
                 do p=1,ncores
                    f(1)=f_core(1,p)+i
                    f(2)=f_core(2,p)+j
                    f(3)=f_core(3,p)+k
                    radius=distance_3(vect,f,f_centre)
                    if (radius < radius_cluster) then
                       ! All atoms within cutoff are added to cluster
                       n_cluster_core = n_cluster_core + 1
                       f_cluster_core(1:3, n_cluster_core) = f(1:3)
                       point_uc_c(n_cluster_core) = p
                       ! Atoms outside cutoff may also be added if
                       ! they neighbour a cluster atom and they are 
                       ! required to fully coordinate that atom
                       ! (depending on the coordinated_mode mode)
                       if(trim(coordinated_mode).eq.'cations'.or.trim(coordinated_mode).eq.'anions') then
                          charge1 = charge_core(p)
                          if (shell_model .and. point_shel(p) /= 0) then
                             charge1 = charge1 + charge_shel(point_shel(p))
                          endif
                          do l = 1, coordination(p)
                             l1 = point_neighbour(l,p)
                             ! Vector to the neighbour
                             call transform_3(cart_to_frac, v_neighbour(:,l,p), f2)
                             ! Absolute position of the neighbour
                             f2(1:3) = f2(1:3) + f(1:3)
                             radius1 = distance_3(vect, f2, f_centre)
                             ! Fully coordinate cations or anions depending on mode
                             sign = 1
                             if (trim(coordinated_mode).eq.'anions') sign = -1
                             if (radius1 > radius_cluster .and. &
                                  (sign * charge1) > 0) then
                                add_neighbour = .true.
                                do jj = 1, n_cluster_core
                                   if(point_added(jj).eq.1) then
                                      f1(1:3) = f_cluster_core(1:3, jj)
                                      radius = distance_3(vect, f1, f2)
                                      if(radius.lt.d_min) then
                                         add_neighbour = .false.
                                         exit
                                      end if
                                   end if
                                end do
                                if (add_neighbour) then
                                   n_cluster_core = n_cluster_core + 1
                                   f_cluster_core(1:3, n_cluster_core) = f2(1:3)
                                   point_added(n_cluster_core) = 1
                                   n_added = n_added + 1
                                   point_uc_c(n_cluster_core) = l1
                                endif !add_neighbour
                             endif !radius1>radius_cluster
                          enddo !coordination
                       endif !trim(coordinated_mode)
                    endif !radius<radius_cluster
                 enddo !p
              enddo !k
           enddo !j
        enddo !i
     case('molecular')
        ! Molecular crystals
        ! If the average distance of the molecule from the centre is below the 
        ! cluster radius the whole molecule is added to the cluster 
        if (ndim /= 3) then
           write(6,*) "cluster_cut: error, 3D molecular crystals only"
           ierr = 1
           return
        end if
        do i=-i_max,i_max
           do j=-j_max,j_max
              do k=-k_max,k_max
                 do m=1, nmol
                    ! Calculate distance of molecule centroid from cluster centre 
                    sum_f(1:3) = 0.0d0
                    ! Anchor atom
                    p = mol_core_point(1, m)
                    f(1)=f_core(1,p)+i
                    f(2)=f_core(2,p)+j
                    f(3)=f_core(3,p)+k                   
                    do ip = 1, mol_size(m)
                       p = mol_core_point(ip, m)
                       call transform_3(cart_to_frac, mol_vec(1:3,p), f2)
                       f2(1:3)=f2(1:3)+f(1:3)
                       sum_f(1:3) = sum_f(1:3) + f2(1:3)
                    end do
                    av_f(1:3) = sum_f(1:3) / mol_size(m)
                    radius = distance_3(vect,av_f,f_centre)
                    if (radius < radius_cluster) then
                       n_cluster_mol = n_cluster_mol + 1
                       ! Centroid stored for sorting cores later
                       call transform_3(vect, av_f(1:3), r_cluster_mol(1:3, n_cluster_mol))
                       ! All atoms in molecule are added to cluster
                       do ip = 1, mol_size(m)
                          p = mol_core_point(ip, m)
                          call transform_3(cart_to_frac, mol_vec(1:3,p), f2)
                          f2(1:3)=f2(1:3)+f(1:3)
                          n_cluster_core = n_cluster_core + 1
                          f_cluster_core(1:3, n_cluster_core) = f2(1:3)
                          point_uc_c(n_cluster_core) = p
                          point_cluster_mol(n_cluster_core) = n_cluster_mol
                       end do !ip
                    end if !radius < radius_cluster
                 end do !m
              end do !k
           end do !j
        end do !i
     case default
        write(6,*) "cluster_cut ERROR: crystal_type =", crystal_type
        ierr = 1
        return
     end select
     ! Cluster coordinates and charges
     charge_cluster = 0.0d0
     do i = 1, n_cluster_core
        ! Core coordinates and charges
        f(1:3) = f_cluster_core(1:3,i)
        call transform_3(vect,f,r)
        r_cluster_core(1:3,i) = r(1:3)
        j = point_uc_c(i)
        charge_cluster_core(i) = charge_core(j)
        charge_cluster = charge_cluster + charge_cluster_core(i)
     end do
  case default
     write(6,*) "cluster_cut ERROR: ndim=", ndim
     ierr = 1
     return
  end select
  ! This should never happen and would probably cause a crash but just in case
  if (n_cluster_core > max_core) then
     write(6,'(a,i6,a,i6)') "cluster_cut: error, n_cluster_core=", n_cluster_core, &
          " > max_core=", max_core
     ierr = 1
     return
  end if
  if (n_cluster_mol > max_mol) then
     write(6,'(a,i6,a,i6)') "cluster_cut: error, n_cluster_mol=", n_cluster_mol, &
          " > max_mol=", max_mol
     ierr = 1
     return
  end if
  if (debug > 4) then
     write(6,'(/a)') "Unsorted cluster cartesian coords and map back to UC (+point_added)"
     write(6,'(/a)') "CORES"
     do i = 1, n_cluster_core
        write(6,'(i6, 3f12.7,i6,i6)') i, r_cluster_core(1:3,i), point_uc_c(i), point_added(i)
     end do
  end if
  
  ! Sort cores into a more convenient order
  call sort_cores(crystal_type, 1, n_cluster_core)
 
  if (debug > 3) then
     write(6,'(/a)') "Sorted cluster cartesian coords and map back to UC (+point_added for cores)"
     write(6,'(/a)') "CORES"
     do i = 1, n_cluster_core
        write(6,'(i6, 3f12.7,i6,i6)') i, r_cluster_core(1:3,i), point_uc_c(i), point_added(i)
     end do
  end if

  ! Assign cluster shell coordinates and charges
  if (shell_model) then 
     do i = 1, n_cluster_core
        j = point_uc_c(i)
        k = point_shel(j)
        if (k /= 0) then
           n_cluster_shel = n_cluster_shel + 1
           point_uc_s(n_cluster_shel) = k
           point_cluster_core(n_cluster_shel) = i
           select case (ndim)
           case (2)
              f(1:2) = f_cluster_core(1:2,i)
              f2(1:2) = f_shel(1:2,k)
              call translate_2(vect(1:2,1:2),f(1:2),f2(1:2),r2(1:2))
              f_cluster_shel(1:2,n_cluster_shel) = f2(1:2)
              f_cluster_shel(3,n_cluster_shel) = f_shel(3,k)
              r_cluster_shel(1:2,n_cluster_shel) = r2(1:2)
              r_cluster_shel(3,n_cluster_shel) = f_shel(3,k)
           case (3)
              f(1:3) = f_cluster_core(1:3,i)
              f2(1:3) = f_shel(1:3,k)
              call translate_3(vect,f,f2,r2)
              f_cluster_shel(1:3,n_cluster_shel)=f2(1:3)
              r_cluster_shel(1:3,n_cluster_shel)=r2(1:3)
           end select
           charge_cluster_shel(n_cluster_shel) = charge_shel(k)  
           charge_cluster = charge_cluster + charge_cluster_shel(n_cluster_shel)
        end if
     end do
     if (n_cluster_shel > max_shel) then
        write(6,'(a,i6,a,i6)') "cluster_cut: error, n_cluster_shel=", n_cluster_shel, &
             " > max_shel=", max_shel
        ierr = 1
        return
     end if
     if (debug > 3) then
        write(6,'(/a)') "SHELLS"
        do i = 1, n_cluster_shel
           write(6,'(i6, 3f12.7,i6)') i, r_cluster_shel(1:3,i), point_uc_s(i)
        enddo
        write(6,*)
     end if
  end if

  if(clustersize.eq.0) then
      clustersize        = n_cluster_core
      clustersize_shells = n_cluster_shel
      return
  endif

  ! Write the cluster to the fragment object
  call setCluster(r_cluster_core, r_cluster_shel,           &
                  charge_cluster_core, charge_cluster_shel, &
                  point_uc_c, point_uc_s,                   &
                  point_cluster_core, ierr)

! Old call:
!  call set_cart_cluster(cluster_frag, periodic_frag, &
!       ncores, nshells, f_core, f_shel, &
!       ncores_cluster, nshels_cluster, r_cluster_core, r_cluster_shel, &
!       charge_cluster, charge_cluster_core, charge_cluster_shel, &
!       point_cluster_core, point_uc_c, point_uc_s, ierr)

  ! Write out origin in cartesians
  ierr = set_cluster_origin(r_centre)
  if (ierr .ne. 0) return

  ! Deallocate
  deallocate(f_cluster_core)
  deallocate(f_cluster_shel)
  deallocate(r_cluster_core)
  deallocate(r_cluster_shel)
  deallocate(charge_cluster_core)
  deallocate(charge_cluster_shel)
  deallocate(point_cluster_core)
  deallocate(point_uc_c)
  deallocate(point_uc_s)
  deallocate(point_added)
  deallocate(r_cluster_mol)
  deallocate(point_cluster_mol)

  deallocate(cart_coords)
  deallocate(f_core)
  deallocate(f_shel)
  deallocate(r_core)
  deallocate(r_shel)
  deallocate(cart_charges)
  deallocate(charge_core)
  deallocate(charge_shel)
  deallocate(point_core)
  deallocate(point_shel)
  deallocate(coordination)
  deallocate(point_neighbour)
  deallocate(v_neighbour)
  deallocate(mol_point)
  deallocate(mol_size)
  deallocate(mol_core_point)
  deallocate(mol_vec)

  ierr = 0

  call flush(6)

contains

  recursive subroutine traverse_molecule(icore)
    integer :: icore, icoord, pn
    do icoord = 1, coordination(icore)
       pn = point_neighbour(icoord, icore)
       if (mol_point(pn) == 0) then
          mol_point(pn) = nmol
          mol_vec(:,pn) = mol_vec(:,icore) + v_neighbour(:,icoord,icore) 
          call traverse_molecule(pn)
       end if
    end do
  end subroutine traverse_molecule
  
  recursive subroutine sort_cores(crystal_type, left, right)
    ! Quicksort
    integer :: left, right, pivot, newpivot
    character(*) :: crystal_type

    if (right > left) then
       pivot = left+((right-left)/2)
       if (trim(crystal_type).eq.'molecular') then
          newpivot = partition_mols(left, right, pivot)
       else
          newpivot = partition_cores(left, right, pivot)
       end if
       call sort_cores(crystal_type, left, newpivot - 1)
       call sort_cores(crystal_type, newpivot + 1, right)
    end if
  end subroutine sort_cores

  integer function partition_cores(left, right, pivot)
    integer :: left, right, pivot, store, spi
    double precision, dimension(3) :: rspi, rpivot
    rpivot = r_cluster_core(:, pivot)
    call swap_cores(pivot, right)
    store = left
    do spi = left, right-1
       rspi = r_cluster_core(:, spi)
       if (cmp_points(rspi, rpivot) == 1) then
          call swap_cores(spi, store)
          store = store + 1
       end if
    end do
    call swap_cores(store, right)
    partition_cores = store
  end function partition_cores

  integer function partition_mols(left, right, pivot)
    integer :: left, right, pivot, pivotmol, pivotuc, store, spi, spimol
    double precision, dimension(3) :: rspi, rpivot
    ! Cores in molecular crystals are sorted using molecule centroids
    pivotmol = point_cluster_mol(pivot)
    rpivot = r_cluster_mol(:, pivotmol)
    pivotuc = point_uc_c(pivot)
    call swap_cores(pivot, right)
    store = left
    do spi = left, right-1
       spimol = point_cluster_mol(spi)
       if (pivotmol == spimol) then
          ! Cores are in the same molecule
          ! Retain order from the input cell
          if (point_uc_c(spi) < pivotuc) then
             call swap_cores(spi, store)
             store = store + 1
          end if
       else
          ! Cores are in different molecules
          ! Sort by molecule centroid
          rspi = r_cluster_mol(:, spimol)
          if (cmp_points(rspi, rpivot) == 1) then
             call swap_cores(spi, store)
             store = store + 1
          end if
       end if
    end do
    call swap_cores(store, right)
    partition_mols = store
  end function partition_mols

  integer function cmp_points(ri, rj)
    ! Points (cores or molecules) are sorted according to:
    ! 1. Shortest distance to the origin
    ! 2. Highest Z
    ! 3. Angle on XY plane (atan2)
    !
    ! Returns:
    !  1 if ri before rj
    ! -1 if ri after rj
    !  0 if ri = rj
    !
    double precision, dimension(3) :: ri, rj, vi, vj
    double precision, parameter :: cmp_tol = 1.0d-4
    double precision :: disti, distj, ati, atj
    vi = ri - r_centre
    vj = rj - r_centre
    ! 1st test: shortest distance to origin
    disti = sqrt(dot_product(vi,vi))
    distj = sqrt(dot_product(vj,vj))
    if (disti < (distj - cmp_tol)) then
       cmp_points = 1
       return
    else if (distj < (disti - cmp_tol)) then
       cmp_points = -1
       return
    endif
    ! 2nd test: highest Z coordinate
    if (ri(3) > (rj(3) + cmp_tol)) then
       cmp_points = 1
       return
    else if (rj(3) > (ri(3) + cmp_tol)) then
       cmp_points = -1
       return
    endif
    ! 3rd test: angle on XY plane
    ati = atan2(ri(2), ri(1))
    atj = atan2(rj(2), rj(1))
    if (ati > atj) then
       cmp_points = 1
    else if (ati < atj) then
       cmp_points = -1
    else 
       cmp_points = 0
    end if
    end function cmp_points

    subroutine swap_cores(pi, pj)
      ! Swap two cores in cluster 
      ! Do not call after shells have been assigned
      integer :: pi, pj, si, sj
      integer :: idum
      double precision :: ddum
      double precision, dimension(3) :: rdum

      rdum = f_cluster_core(:,pi)
      f_cluster_core(:,pi) = f_cluster_core(:,pj)
      f_cluster_core(:,pj) = rdum

      rdum = r_cluster_core(:,pi)
      r_cluster_core(:,pi) = r_cluster_core(:,pj)
      r_cluster_core(:,pj) = rdum

      ddum = charge_cluster_core(pi)
      charge_cluster_core(pi) = charge_cluster_core(pj)
      charge_cluster_core(pj) = ddum

      idum = point_uc_c(pi)
      point_uc_c(pi) = point_uc_c(pj)
      point_uc_c(pj) = idum

      idum = point_added(pi)
      point_added(pi) = point_added(pj)
      point_added(pj) = idum

      idum = point_cluster_mol(pi)
      point_cluster_mol(pi) = point_cluster_mol(pj)
      point_cluster_mol(pj) = idum

    end subroutine swap_cores
 
end function cluster_cut
