!  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/>.

subroutine fit_bqs(nccore, ncbq, ncshell, &
                   npcore, npbq, npshell, &
                   radius_cluster, radius_active, &
                   bq_margin, bq_density, bq_layer, bq_fit_weight, ierr)

    use ChemShellClusterModule, only : PyOptions, get_ndims, cluster_origin, debug

  ! Fit point charges around a cluster to reproduce electrostatics
  ! of the periodic system it was cut from
  !
  ! The fit is to the active atoms only, so a radius for this 
  ! region must be specified
  !
  ! bq_margin specifies the distance between the outer edge of 
  ! the cluster and the shell of bqs
  !
  ! bq_density is related to the number of bqs added
  ! If bq_density=n, the number of bqs in the surrounding shell 
  ! is 4n^2 + 2 (3d case). 
  ! In the 2d case number of bqs determined by bq_density and bq_layer
  ! (extra rings of charges to accommodate the adsorption layer). 
  ! Extra fitting sites are added above the surface to ensure a good
  ! fit for the bq_layer charges.
  !
  ! In addition there are 6 bqs placed far away from the cluster
  ! to account for the 'floating zero' of the potential and electric
  ! field
  !
  implicit none

  integer, intent(in) :: nccore, ncbq, ncshell
  integer, intent(in) :: npcore, npbq, npshell
  double precision, intent(in) :: radius_cluster
  double precision, intent(in) :: radius_active, bq_margin, bq_layer
  integer, intent(in) :: bq_density
  integer, intent(out) :: ierr
  ! AJL, 12/06/18. 
  real(kind=8), intent(in) :: bq_fit_weight
  
  double precision, dimension(3) :: r
  double precision :: v
  double precision, dimension(3) :: e
  double precision :: distance, t, z
  integer :: i, j, k, l, m, n, p

!  double precision, dimension(3) :: cluster_origin

  ! Arrays for periodic information
  double precision, dimension(npcore) :: pot_core
  double precision, dimension(npshell) :: pot_shel
  double precision, dimension(3, npcore) :: efield_core
  double precision, dimension(3, npshell) :: efield_shel
  integer, dimension(npshell) :: point_core
  integer, dimension(npcore) :: point_shel  

  ! Arrays for cluster information
  double precision, dimension(3, nccore+ncshell) :: cart_coords
  double precision, dimension(3, nccore) :: r_cluster_core
  double precision, dimension(3, ncshell) :: r_cluster_shel
  double precision, dimension(nccore+ncshell) :: cart_charges
  double precision, dimension(nccore) :: charge_cluster_core
  double precision, dimension(ncshell) :: charge_cluster_shel  
  integer, dimension(ncshell) :: point_cluster_core
  integer, dimension(nccore+ncshell) :: xtal_map
  integer, dimension(nccore) :: point_uc_c
  integer, dimension(ncshell) :: point_uc_s
  logical, dimension(nccore) :: core_active
  integer :: n_active_core, n_active_shel, n_fitting_sites
  
  ! Potential, field and fitting
  double precision :: pot_zero, charge_zero
  double precision, dimension(3) :: e_zero, charge_delta
  double precision :: mean_error, abs_error, mean_square
  double precision, dimension(nccore) :: pot_error_c
  double precision, dimension(ncshell) :: pot_error_s
  double precision, dimension(3, nccore) :: e_error_c
  double precision, dimension(3, ncshell) :: e_error_s
  double precision :: pole_big, pole_small
  double precision :: pi_half_n, theta, pi_half_i, fi, sin_theta
  integer :: i4

  ! Extra Fitting Sites for 2d case
  integer :: maxefs, nefs
  double precision :: sgrid, maxvol
  integer :: xygrid, zgrid
  double precision, allocatable :: r_efs(:,:)
  double precision, allocatable :: pot_efs(:)
  double precision, allocatable :: efield_efs(:,:)
  double precision, allocatable :: pot_error_efs(:)
  double precision, allocatable :: e_error_efs(:,:)

  ! Arrays for addded point charges
  double precision, allocatable :: r_polar_bq(:,:)
  double precision, allocatable :: r_lsq_bq(:,:)  
  double precision, allocatable :: charge_polar_bq(:)
  double precision, allocatable :: charge_lsq_bq(:)
  integer :: n_polar_bq, n_lsq_bq, count_bq
  double precision :: radius_bq
  double precision :: step_z
  integer :: n_layer
  double precision :: zmin, zmax, sign_z
  double precision :: pole_huge, height, z0, midheight, width

  ! Least squares
  double precision, allocatable :: a(:,:), b(:), v_kernel(:), x_bq(:,:)
  double precision, allocatable :: work(:)
  double precision, dimension(1) :: qwork
  integer :: lwork
  double precision :: lsq_constraint, charge_weight, charge_calc, q_polar_tot

  logical :: shell_model

  double precision :: pi, pi_half

  integer, parameter :: periodic_frag = 1, cluster_frag = 2

  double precision :: distance_3, totalcharge

  integer :: ndim

  ierr = 0

  ndim = get_ndims()

  efield_core = 0.0d0
  pot_error_c = 0.0d0
  pot_error_s = 0.0d0
  e_error_c   = 0.0d0
  e_error_s   = 0.0d0

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

  ! totalcharge is the target charge to fit
  call PyOptions%get('totalcharge', totalcharge)

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

  ! Check there are no point charges already in the cluster
  if (ncbq .ne. 0 .or. npbq .ne. 0) then
     write(6,*) "fit_bqs: number of point charges must be zero"
     ierr = 1
     return
  end if

  if (ncshell .eq. 0) then
     shell_model = .false.
  else
     shell_model = .true.
  end if
 
  ! Get cluster cartesian coordinates from fragment
  ! Split into core and shell arrays
  call get_cart_coords(cluster_frag, cart_coords, nccore, ncshell, ierr)
  r_cluster_core = cart_coords(:, 1:nccore)
  if (shell_model) r_cluster_shel = cart_coords(:, nccore+1:nccore+ncshell)
  if (ierr .ne. 0) return

  ! Get charges of cartesian sites from fragment
  ! Split into core and shell arrays
  call get_cart_charges(cluster_frag, cart_charges, nccore+ncshell, ierr)
  if (ierr .ne. 0) return
  charge_cluster_core = cart_charges(1:nccore)
  if (shell_model) charge_cluster_shel = cart_charges(nccore+1:nccore+ncshell)

  ! Get pointers from shells to cores
  ! Then setup pointers in the other direction
  if (shell_model) then
     call get_point_core(cluster_frag, point_cluster_core, ncshell, ierr)
     if (ierr .ne. 0) return
  end if

  ! Get pointer from cores/shells to the unit cell (xtal map) 
  call get_xtal_map(cluster_frag, xtal_map, nccore, ncshell, ierr)
  if (ierr .ne. 0) return
  point_uc_c = xtal_map(1:nccore)
  if (shell_model) point_uc_s = xtal_map(nccore+1:nccore+ncshell)

  ! Get origin of cluster in cartesians
!  call get_cluster_origin(cluster_origin, ierr)
!  if(ierr .ne. 0) return

  ! Get electrostatic potential and electric field for periodic system
  call get_electrostatics(periodic_frag, pot_core, pot_shel, &
                          efield_core, efield_shel, & 
                          npcore, npshell, ierr)
  if (ierr .ne. 0) return
  if (debug > 3) then
     write(6,'(/a)') "Periodic electrostatic potential and electric field (x y z)"
     write(6,'(/a)') "CORES"
     do i = 1, npcore
        write(6,'(i6, 4f20.10)') i, pot_core(i), efield_core(1:3,i)
     end do
     if (shell_model) then
        write(6,'(/a)') "SHELLS"
        do i = 1, npshell
           write(6,'(i6, 4f20.10)') i, pot_shel(i), efield_shel(1:3,i)
        end do
     end if
  end if

  ! DETERMINE POSITION OF ADDED BQS

  ! Determine the number of added bqs and allocate memory
  select case (ndim)
  case (2)
     ! bqs at the poles
     n_polar_bq = 6
     ! No extra layers above the hemisphere to accommodate
     ! adsorption layer
     step_z = pi * (radius_cluster + bq_margin) * 2.d0 / (4 * bq_density - 1)
     n_layer = (bq_layer + bq_margin) / step_z + 1
     !n_lsq_bq = 16 + &
     n_lsq_bq =  &
          (2 * bq_density * (bq_density + 1) + 1) + &
          (4 * bq_density * n_layer)
  case (3)
     ! bqs at the poles
     n_polar_bq = 6
     ! bqs to be fitted by least squares 
     n_lsq_bq = 4 * (bq_density**2) + 2
  case default
     write(6,*) "fit_bqs: error, ndim=", ndim
     ierr = 1
     return
  end select
  allocate(r_polar_bq(3, n_polar_bq))
  allocate(r_lsq_bq(3, n_lsq_bq))
  allocate(charge_polar_bq(n_polar_bq))
  allocate(charge_lsq_bq(n_lsq_bq))
  allocate(a(n_lsq_bq,n_lsq_bq))
  allocate(b(n_lsq_bq))
  allocate(v_kernel(n_lsq_bq))
  allocate(x_bq(3,n_lsq_bq))
  count_bq = 0
  r_polar_bq = 0.0d0
  r_lsq_bq = 0.0d0
  charge_polar_bq = 0.0d0
  charge_lsq_bq = 0.0d0

  ! Polar bqs
  ! These charges will account for floating zero potential
  select case (ndim)
  case (2,3)
     ! Six charges at remote poles on the three cartesian axes
     pole_big = 10.d0 * radius_cluster
     do i = 1, 3
        count_bq = count_bq + 1
        r_polar_bq(i,count_bq) = pole_big
        count_bq = count_bq + 1
        r_polar_bq(i,count_bq) = -pole_big
     end do
  end select
  if (count_bq /= n_polar_bq) then
     write(6, '(a)') "fit_bqs: count_bq /= n_polar_bq"
     ierr = -1
     return
  end if

  ! bqs for least squares fit
  count_bq = 0
  radius_bq = radius_cluster + bq_margin
  pi_half_n = pi_half / bq_density  
  select case (ndim)
  case (2)
     ! Determine direction of z axis with respect to surface normal
     zmin = +1.d70
     zmax = -1.d70
     do i = 1, nccore
        if (r_cluster_core(3,i) < zmin) zmin = r_cluster_core(3,i)
        if (r_cluster_core(3,i) > zmax) zmax = r_cluster_core(3,i)
     enddo
     if (dabs(cluster_origin(3) - zmin) > dabs(zmax - cluster_origin(3))) then
        sign_z=+1.d0
     else
        sign_z=-1.d0
     endif
     ! Parameters
     pole_huge = radius_bq * 100.d0
     height = bq_layer + bq_margin
     z0=(radius_bq + height)/2
     midheight=-radius_bq + z0
     width=1.5*radius_bq
     ! Two charges on far axial poles
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=-pole_huge
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=pole_huge   
     ! Eight charges in the corners of a cube to get higher poles
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=radius_bq
!!$     r_lsq_bq(2,count_bq)=radius_bq
!!$     r_lsq_bq(3,count_bq)=-radius_bq
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=-radius_bq
!!$     r_lsq_bq(2,count_bq)=radius_bq
!!$     r_lsq_bq(3,count_bq)=-radius_bq
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=radius_bq
!!$     r_lsq_bq(2,count_bq)=-radius_bq
!!$     r_lsq_bq(3,count_bq)=-radius_bq
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=-radius_bq
!!$     r_lsq_bq(2,count_bq)=-radius_bq
!!$     r_lsq_bq(3,count_bq)=-radius_bq
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=radius_bq
!!$     r_lsq_bq(2,count_bq)=radius_bq
!!$     r_lsq_bq(3,count_bq)=height
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=-radius_bq
!!$     r_lsq_bq(2,count_bq)=radius_bq
!!$     r_lsq_bq(3,count_bq)=height
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=radius_bq
!!$     r_lsq_bq(2,count_bq)=-radius_bq
!!$     r_lsq_bq(3,count_bq)=height
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=-radius_bq
!!$     r_lsq_bq(2,count_bq)=-radius_bq
!!$     r_lsq_bq(3,count_bq)=height
!    Put extra 4 charges on horizontal axes and 2 extra charges on z axis
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=width
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=midheight
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=-width
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=midheight
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=width
!!$     r_lsq_bq(3,count_bq)=midheight
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=-width
!!$     r_lsq_bq(3,count_bq)=midheight
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=-radius_bq-z0
!!$     count_bq=count_bq+1
!!$     r_lsq_bq(1,count_bq)=0.d0
!!$     r_lsq_bq(2,count_bq)=0.d0
!!$     r_lsq_bq(3,count_bq)=height+z0
     ! Build hemisphere of point charges around cluster
     ! Put one charge on the pole of the hemi_sphere
     count_bq=count_bq+1
     r_lsq_bq(1,count_bq)=0.d0
     r_lsq_bq(2,count_bq)=0.d0
     r_lsq_bq(3,count_bq)=-radius_bq
     ! Build hemi-spherical grid by mapping triangles on 4 octants
     do i = 1, bq_density
        theta = -pi + i*pi_half_n
        pi_half_i = pi_half / i
        i4 = 4*i - 1
        do j=0, i4
           count_bq = count_bq + 1
           fi = pi_half_i*j
           sin_theta = dsin(theta)
           r_lsq_bq(1,count_bq)=radius_bq*dcos(fi)*sin_theta
           r_lsq_bq(2,count_bq)=radius_bq*dsin(fi)*sin_theta
           r_lsq_bq(3,count_bq)=radius_bq*dcos(theta)
        enddo
     enddo
     ! Build cylinder on top of hemi-sphere to accomodate adsorption layer
     do i=1,n_layer
        z=step_z*i
        do j=0,i4
           count_bq=count_bq+1
           fi=pi_half_i*j
           r_lsq_bq(1,count_bq)=radius_bq*dcos(fi)
           r_lsq_bq(2,count_bq)=radius_bq*dsin(fi)
           r_lsq_bq(3,count_bq)=z
        enddo
     enddo
     ! Turn upside down if necessary
     do i = 1, count_bq
        r_lsq_bq(3,i) = sign_z * r_lsq_bq(3,i)
     enddo
  case (3)
     ! Build sphere of point charges around the cluster
     pole_small = 1.5 * radius_bq
     ! Build hemi-spherical grid by mapping triangles on 4 octants
     ! and reflect on the other hemi-sphere
     count_bq = 1
     r_lsq_bq(1,count_bq)=0.d0
     r_lsq_bq(2,count_bq)=0.d0
     r_lsq_bq(3,count_bq)=-radius_bq
     count_bq = 2
     r_lsq_bq(1,count_bq)=0.d0
     r_lsq_bq(2,count_bq)=0.d0
     r_lsq_bq(3,count_bq)=radius_bq
     do i = 1, bq_density
        theta = -pi + i*pi_half_n
        pi_half_i = pi_half / i
        i4 = 4*i - 1
        do j=0, i4
           count_bq = count_bq + 1
           fi = pi_half_i*j
           sin_theta = dsin(theta)
           r_lsq_bq(1,count_bq)=radius_bq*dcos(fi)*sin_theta
           r_lsq_bq(2,count_bq)=radius_bq*dsin(fi)*sin_theta
           r_lsq_bq(3,count_bq)=radius_bq*dcos(theta)
           if(i .ne. bq_density) then
              count_bq=count_bq+1
              r_lsq_bq(1,count_bq)=r_lsq_bq(1,count_bq-1)
              r_lsq_bq(2,count_bq)=r_lsq_bq(2,count_bq-1)
              r_lsq_bq(3,count_bq)=-r_lsq_bq(3,count_bq-1)
           endif
        enddo
     enddo
  end select
  if (count_bq /= n_lsq_bq) then
     write(6, '(a)') "fit_bqs: count_bq /= n_lsq_bq"
     ierr = -1
     return
  end if  

  ! Position bqs around given centre
  do i = 1, n_polar_bq
     r_polar_bq(1:3,i) = r_polar_bq(1:3,i) + cluster_origin(1:3)
  end do
  do i = 1, n_lsq_bq
     r_lsq_bq(1:3, i) = r_lsq_bq(1:3, i) + cluster_origin(1:3)
  enddo
  if (debug > 3) then
     write(6,'(/a)') "BQ POSITIONS"    
     write(6,'(/a)') "Polar bqs"
     write(6,'(/10X,2(A,20X),A)') "X", "Y", "Z"
     do i = 1, n_polar_bq
        write(6,'(3f20.10)') r_polar_bq(1:3, i)
     end do
     write(6,'(/a)') "Least squares bqs"
     write(6,'(/10X,2(A,20X),A)') "X", "Y", "Z"
     do i = 1, n_lsq_bq
        write(6,'(3f20.10)') r_lsq_bq(1:3, i)
     end do
  end if


  ! FIT BQS TO PERIODIC ELECTROSTATIC POTENTIAL

  ! Determine which atoms are active
  ! TODO - will need to be modified in the molecular case
  core_active = .false.
  n_active_core = 0
  n_active_shel = 0
  do i = 1, nccore
     r(1:3) = r_cluster_core(1:3, i) - cluster_origin(1:3)
     distance = sqrt(r(1)*r(1) + r(2)*r(2) + r(3)*r(3))
     if (distance < radius_active) then
        core_active(i) = .true.
        n_active_core = n_active_core + 1
     end if
  end do
  if (shell_model) then
     do i = 1, ncshell
        if (core_active(point_cluster_core(i))) then
           n_active_shel = n_active_shel + 1
        end if
     end do
  end if
  if (debug > 0) then
     write(6,'(/a)') "Origin (x y z) and active radius"
     write(6,'(/4f20.10)') cluster_origin, radius_active
!     write(6,'(/a)') "Active cores"
!     do i = 1, nccore
!        if(core_active(i)) write(6,'(i4)') i
!     enddo
  endif


  ! 2d case: add extra fitting sites above the surface 
  nefs = 0
  if (ndim == 2) then
     ! Cylinder with radius=radius_active and height=(bq_layer+bq_margin)
     ! The 2.5 bohr grid size is arbitrary but chosen to be roughly 
     ! the same density as atomic fitting sites.
     sgrid = 2.5d0
     xygrid = int(radius_active/sgrid)+1
     ! z starts from 2 bohr above the surface, not from 0
     zgrid = int((bq_layer+bq_margin)/sgrid)
     maxefs = ((xygrid*2)+1) * ((xygrid*2)+1) * zgrid
     allocate(r_efs(3,maxefs))
     allocate(pot_efs(maxefs))
     allocate(efield_efs(3,maxefs))
     allocate(pot_error_efs(maxefs))
     allocate(e_error_efs(3,maxefs))
     do k = 1, zgrid
        do j = -xygrid, xygrid
           do i = -xygrid, xygrid
              r(1) = i * sgrid
              r(2) = j * sgrid
              r(3) = k * sgrid
              distance = sqrt(r(1)*r(1) + r(2)*r(2))
              if (distance < radius_active) then
                 nefs = nefs + 1
                 r_efs(1:3, nefs) = r(1:3)
              end if
           end do
        end do
     end do
     ! Turn upside down if necessary 
     ! and position around given centre
     do i = 1, nefs
        r_efs(3, i) = sign_z * r_efs(3, i)
        r_efs(1:3, i) = r_efs(1:3, i) + cluster_origin(1:3)
     enddo
     ! Now find the periodic potential/efield to fit to at these locations
     call periodic_electrostatics_f(1, 2, npcore, npbq, npshell, nefs, &
          r_efs, nefs, pot_efs, efield_efs, ierr)
     if (ierr .ne. 0) then
        write(6,*) 'fit_bqs: unable to get extra fitting site electrostatics'
        return
     end if
     if(debug > 3) then
        write(6,'(/a)') "Extra fitting sites locations"
        ! TWKDEBUG
        write(6,*) nefs
        write(6,*) nefs
        do i = 1, nefs
           write(6,'(a,3f12.5)') &
                "H ", r_efs(1:3, i)*0.529177249d0
        end do
        write(6,'(/a)') "Extra fitting sites pot, efield"
        do i = 1, nefs
           write(6,'(i5,f12.5,2x,3f12.5)') &
                i, pot_efs(i), efield_efs(1:3, i)
        end do
     end if
  else
     ! 0-sized arrays
     allocate(r_efs(3,nefs))
     allocate(pot_efs(nefs))
     allocate(efield_efs(3,nefs))
     allocate(pot_error_efs(nefs))
     allocate(e_error_efs(3,nefs))
  end if

  n_fitting_sites = n_active_core + n_active_shel + nefs

  ! Set up ESP and electric field error arrays with periodic values
  do i=1,nccore
     if(core_active(i)) then
        n = point_uc_c(i)
        pot_error_c(i) = -pot_core(n)
        e_error_c(1:3,i) = -efield_core(1:3,n)
     endif
  enddo
  if (shell_model) then
     do i=1,ncshell
        if(core_active(point_cluster_core(i))) then
           m = point_uc_s(i)
           pot_error_s(i) = -pot_shel(m)
           e_error_s(1:3,i) = -efield_shel(1:3,m)
        endif
     enddo
  endif
  do i=1,nefs
     pot_error_efs(i) = -pot_efs(i)
     e_error_efs(1:3, i) = -efield_efs(1:3, i)
  end do

  ! ESP and electric field contributions from active atoms
  ! Values at core sites
  do i=1,nccore
     if(core_active(i)) then
        ! Contribution from other cores
        do j=1,nccore
           if(j.ne.i) then
              call cluster_add_ve(r_cluster_core(1:3,i), r_cluster_core(1:3,j), &
                   charge_cluster_core(j), pot_error_c(i), e_error_c(1:3,i))
           endif
        enddo
        ! Contribution from shells (except for attached shell)
        if(shell_model) then
           do j=1,ncshell
              if(point_cluster_core(j).ne.i) then
                 call cluster_add_ve(r_cluster_core(1:3,i), r_cluster_shel(1:3,j), &
                      charge_cluster_shel(j), pot_error_c(i), e_error_c(1:3,i))
              endif
           enddo
        endif
     endif
  enddo
  ! Values at shell sites
  if (shell_model) then
     do i=1,ncshell
        if(core_active(point_cluster_core(i))) then
           ! Contribution from cores (except for attached core)
           do j=1,nccore
              if(j.ne.point_cluster_core(i)) then
                 call cluster_add_ve(r_cluster_shel(1:3,i), r_cluster_core(1:3,j), &
                      charge_cluster_core(j), pot_error_s(i), e_error_s(1:3,i))
              endif
           enddo
           ! Contribution from other shells
           do j=1,ncshell
              if(j.ne.i) then
                 call cluster_add_ve(r_cluster_shel(1:3,i), r_cluster_shel(1:3,j), &
                      charge_cluster_shel(j), pot_error_s(i), e_error_s(1:3,i))
              endif
           enddo
        endif
     enddo
  endif
  ! Values at extra fitting sites
  do i = 1, nefs
     ! Contribution from cores
     do j=1,nccore
        call cluster_add_ve(r_efs(1:3,i), r_cluster_core(1:3,j), &
                   charge_cluster_core(j), pot_error_efs(i), e_error_efs(1:3,i))
     enddo
     ! Contribution from shells
     if (shell_model) then
        do j=1,ncshell
           call cluster_add_ve(r_efs(1:3,i), r_cluster_shel(1:3,j), &
                charge_cluster_shel(j), pot_error_efs(i), e_error_efs(1:3,i))
        enddo
     endif
  enddo
  call cluster_fit_errors(nccore, ncshell, n_fitting_sites, nefs, core_active, &
       point_cluster_core, pot_error_c, pot_error_s, pot_error_efs, &
       e_error_c, e_error_s, e_error_efs, &
       pot_zero, e_zero, mean_error, abs_error, mean_square)
  write(*,'(/a/a,i12)') &
          ' Fitting...', &
          ' Number of sites to fit     : ', n_fitting_sites
  write(*,'(/a)') ' Before fitting'
  write(*,'(a,g20.12)') &
          ' Mean error                 : ', mean_error,  &
          ' Absolute error             : ', abs_error,   &
          ' RMS                        : ', mean_square, & 
          ' Mean potential             : ', pot_zero,    &
          ' Mean field Ex              : ', e_zero(1),   &
          ' Mean field Ey              : ', e_zero(2),   &
          ' Mean field Ez              : ', e_zero(3) 

  ! Calculate error for on-site electrostatic potential with floating zero

  ! Calculate polar bq charges
  !     these charges should create potential and field of opposite sign
  !     to cancel mean potential and field errors.
  select case (ndim)
  case (2,3)
     charge_zero = -pot_zero*pole_big/6.d0
     charge_delta(1:3) = -e_zero(1:3)*pole_big*pole_big/2.d0
     count_bq = 0
     do i = 1, 3
        count_bq = count_bq + 1
        charge_polar_bq(count_bq) = charge_zero - charge_delta(i)
        count_bq = count_bq + 1
        charge_polar_bq(count_bq) = charge_zero + charge_delta(i)
     end do
  end select

  !   Recalculate potential and field errors
  !   in the presence of added charges
  do i=1,nccore
     if(core_active(i)) then
        do j=1, n_polar_bq
           call cluster_add_ve(r_cluster_core(1:3,i), r_polar_bq(1:3,j), &
                charge_polar_bq(j), pot_error_c(i), e_error_c(1:3,i))
        enddo
     endif
  enddo
  if (shell_model) then
     do i=1,ncshell
        if(core_active(point_cluster_core(i))) then
           do j=1, n_polar_bq
              call cluster_add_ve(r_cluster_shel(1:3,i), r_polar_bq(1:3,j), &
                   charge_polar_bq(j), pot_error_s(i), e_error_s(1:3,i))
           enddo
        endif
     enddo
  endif
  do i = 1, nefs
     do j = 1, n_polar_bq
        call cluster_add_ve(r_efs(1:3,i), r_polar_bq(1:3,j), &
             charge_polar_bq(j), pot_error_efs(i), e_error_efs(1:3,i))        
     end do
  end do

  if (n_polar_bq > 0) then
     call cluster_fit_errors(nccore, ncshell, n_fitting_sites, nefs, core_active, &
          point_cluster_core, pot_error_c, pot_error_s, pot_error_efs, &
          e_error_c, e_error_s, e_error_efs, &
          pot_zero, e_zero, mean_error, abs_error, mean_square)
     write(*,'(/a)') ' Excluding mean potential and field'
     write(*,'(a,g20.12)') &
          ' Mean error                 : ', mean_error,  &
          ' Absolute error             : ', abs_error,   &
          ' RMS                        : ', mean_square, &
          ' Mean potential             : ', pot_zero,    &
          ' Mean field Ex              : ', e_zero(1),   &
          ' Mean field Ey              : ', e_zero(2),   &
          ' Mean field Ez              : ', e_zero(3) 
  endif
  
  ! This section is marked "INSANE" in construct
  ! According to Alexey, in principle just the first run evaluation 
  ! based on the raw average potential and field should be sufficient
  ! as we are working with double precision, but in practice reiterating
  ! this calc helps to get much better values.
  select case (ndim)
  case(2,3)
     charge_zero=-pot_zero*pole_big/6.d0
     do i=1, n_polar_bq
        charge_polar_bq(i)=charge_polar_bq(i)+charge_zero
     enddo
  end select
  !   Recalculate potential and field errors
  !   in the presence of added charges
  do i=1,nccore
     if(core_active(i)) then
        do j = 1, n_polar_bq
           ! Note charge_zero
           call cluster_add_ve(r_cluster_core(1:3,i), r_polar_bq(1:3,j), &
                charge_zero, pot_error_c(i), e_error_c(1:3,i))
        enddo
     endif
  enddo
  if(shell_model) then
     do i=1,ncshell
        if(core_active(point_cluster_core(i))) then
           do j = 1, n_polar_bq
              ! Note charge_zero
              call cluster_add_ve(r_cluster_shel(1:3,i), r_polar_bq(1:3,j), &
                   charge_zero, pot_error_s(i), e_error_s(1:3,i))
           enddo
        endif
     enddo
  endif
  do i = 1, nefs
     do j = 1, n_polar_bq
        call cluster_add_ve(r_efs(1:3,i), r_polar_bq(1:3,j), &
             charge_zero, pot_error_efs(i), e_error_efs(1:3,i))        
     end do
  end do
  if (n_polar_bq > 0) then
     call cluster_fit_errors(nccore, ncshell, n_fitting_sites, nefs, core_active, &
          point_cluster_core, pot_error_c, pot_error_s, pot_error_efs, &
          e_error_c, e_error_s, e_error_efs, &
          pot_zero, e_zero, mean_error, abs_error, mean_square)
     write(*,'(/a)') ' Excluding mean potential and field, reiterated'
     write(*,'(a,g20.12)') &
          ' Mean error                 : ', mean_error,  &
          ' Absolute error             : ', abs_error,   &
          ' RMS                        : ', mean_square, &
          ' Mean potential             : ', pot_zero,    &
          ' Mean field Ex              : ', e_zero(1),   &
          ' Mean field Ey              : ', e_zero(2),   &
          ' Mean field Ez              : ', e_zero(3) 
  endif

  ! Build the least squares equations that find the bq charges that 
  ! minimize the error in the potential and electric field
  b = 0.d0
  a = 0.d0
  !AJL, 12/06/18: Commented out as now defined in python
  !charge_weight=1.0d-10
  charge_weight = bq_fit_weight
  lsq_constraint = -totalcharge + sum(charge_polar_bq)
  do l=1, nccore
     if(core_active(l)) then
        do i = 1, n_lsq_bq
           x_bq(1:3,i) = r_cluster_core(1:3,l)- r_lsq_bq(1:3,i)
           v_kernel(i)=1.d0/ &
                sqrt(x_bq(1,i)**2+x_bq(2,i)**2+x_bq(3,i)**2)
           t=v_kernel(i)*v_kernel(i)
           a(i,i)=a(i,i)+t*(1.d0+t)
           b(i)=b(i)+v_kernel(i)* &
                (pot_error_c(l)+t* &
                ( e_error_c(1,l)*x_bq(1,i)+ &
                e_error_c(2,l)*x_bq(2,i)+ &
                e_error_c(3,l)*x_bq(3,i) ) )
        enddo
        do i = 1, n_lsq_bq
           do j= i+1, n_lsq_bq
              t=v_kernel(i)*v_kernel(j)
              a(i,j)=a(i,j)+t*(1.d0+t*t* &
                   ( x_bq(1,i)*x_bq(1,j)+ &
                   x_bq(2,i)*x_bq(2,j)+ &
                   x_bq(3,i)*x_bq(3,j) ) )
           enddo
        enddo
     endif
  enddo
  do l=1, ncshell
     if(core_active(point_cluster_core(l))) then
        do i = 1, n_lsq_bq 
           x_bq(1:3,i) = r_cluster_shel(1:3,l) - r_lsq_bq(1:3,i)
           v_kernel(i)=1.d0/ &
                sqrt(x_bq(1,i)**2+x_bq(2,i)**2+x_bq(3,i)**2)
           t=v_kernel(i)*v_kernel(i)
           a(i,i)=a(i,i)+t*(1.d0+t)
           b(i)=b(i)+v_kernel(i)* &
                (pot_error_s(l)+t* &
                ( e_error_s(1,l)*x_bq(1,i)+ &
                e_error_s(2,l)*x_bq(2,i)+ &
                e_error_s(3,l)*x_bq(3,i) ) )
        enddo
        do i = 1, n_lsq_bq
           do j = i+1, n_lsq_bq
              t=v_kernel(i)*v_kernel(j)
              a(i,j)=a(i,j)+t*(1.d0+t*t* &
                   ( x_bq(1,i)*x_bq(1,j)+ &
                   x_bq(2,i)*x_bq(2,j)+ &
                   x_bq(3,i)*x_bq(3,j) ) )
           enddo
        enddo
     endif
  enddo
  do l = 1, nefs
     do i = 1, n_lsq_bq 
        x_bq(1:3,i) = r_efs(1:3,l) - r_lsq_bq(1:3,i)
        v_kernel(i)=1.d0/ &
             sqrt(x_bq(1,i)**2+x_bq(2,i)**2+x_bq(3,i)**2)
        t=v_kernel(i)*v_kernel(i)
        a(i,i)=a(i,i)+t*(1.d0+t)
        b(i)=b(i)+v_kernel(i)* &
             (pot_error_efs(l)+t* &
             ( e_error_efs(1,l)*x_bq(1,i)+ &
             e_error_efs(2,l)*x_bq(2,i)+ &
             e_error_efs(3,l)*x_bq(3,i) ) )
     enddo
     do i = 1, n_lsq_bq
        do j = i+1, n_lsq_bq
           t=v_kernel(i)*v_kernel(j)
           a(i,j)=a(i,j)+t*(1.d0+t*t* &
                ( x_bq(1,i)*x_bq(1,j)+ &
                x_bq(2,i)*x_bq(2,j)+ &
                x_bq(3,i)*x_bq(3,j) ) )
        enddo
     enddo
  enddo
  ! Full a matrix required for lapack call
  do i = 1, n_lsq_bq
     do j=1,i-1
        a(i,j) = a(j,i)
     end do
  end do
  ! Charge constraint added in least squares procedure
  ! lsq_constraint is the target total least squares charge of the bqs
  ! charge_weight is the weight given to the constraint in the penalty function
  ! AJL, 12/06/18. Commented out as now incoming from python
  !charge_weight = 1.0d-10
  charge_weight=bq_fit_weight
  lsq_constraint = -totalcharge + sum(charge_polar_bq)
  b = b + charge_weight*lsq_constraint
  a = a + charge_weight
  ! Solve the resulting system of linear equations using call to lapack
  ! First query workspace size 
  call dgels('N', n_lsq_bq, n_lsq_bq, 1, &
       a, n_lsq_bq, b, n_lsq_bq, &
       qwork, -1, ierr)
  lwork = int(qwork(1))
  ! Solve the equations
  ! Solution is returned in b
  allocate(work(lwork))
  call dgels('N', n_lsq_bq, n_lsq_bq, 1, &
       a, n_lsq_bq, b, n_lsq_bq, &
       work, lwork, ierr)
  deallocate(work)
  if (ierr .ne. 0) then
     write(6,*) 'fit_bqs: lapack least squares solver failed'
     return
  end if
  
  ! Output is actually the negative of the true charge
  charge_lsq_bq(1:n_lsq_bq) = -b(1:n_lsq_bq)

  write(6,'(/a)')    ' BQ fitting'
  write(6,'(a,i12)') ' Number of polar BQs        : ', n_polar_bq
  write(6,'(a,i12)') ' Number of least squares BQs: ', n_lsq_bq

  if (debug > 3) then
     write(*,'(/a/)') 'BQ CHARGES'
     write(*,'(a)') 'Polar bqs'
     do i = 1, n_polar_bq
        write(*,'(i4,f30.18)') i, charge_polar_bq(i)
     end do
     write(*,'(/a)') 'Least squares bqs'
     do i = 1, n_lsq_bq
        write(*,'(i4,f30.18)') i, charge_lsq_bq(i)
     enddo
  end if

  ! Calculate errors after fitting
  ! Note there is a sign error in the corresponding construct version
  ! Here the code is consistent with what is used for the polar charges
  do i=1, nccore
     if(core_active(i)) then
        ! Only the sphere bqs
        do j = 1, n_lsq_bq
           call cluster_add_ve(r_cluster_core(1:3,i), r_lsq_bq(1:3,j), &
                charge_lsq_bq(j), pot_error_c(i), e_error_c(1:3,i))
        enddo
     endif
  enddo
  if(shell_model) then
     do i=1, ncshell
        if(core_active(point_cluster_core(i))) then
           ! Only the sphere bqs
           do j = 1, n_lsq_bq
              call cluster_add_ve(r_cluster_shel(1:3,i), r_lsq_bq(1:3,j), &
                   charge_lsq_bq(j), pot_error_s(i), e_error_s(1:3,i))
           enddo
        endif
     enddo
  endif
  do i=1, nefs
     ! Only the sphere bqs
     do j = 1, n_lsq_bq
        call cluster_add_ve(r_efs(1:3,i), r_lsq_bq(1:3,j), &
             charge_lsq_bq(j), pot_error_efs(i), e_error_efs(1:3,i))
     end do
  end do
  call cluster_fit_errors(nccore, ncshell, n_fitting_sites, nefs, core_active, &
       point_cluster_core, pot_error_c, pot_error_s, pot_error_efs, &
       e_error_c, e_error_s, e_error_efs, &
       pot_zero, e_zero, mean_error, abs_error, mean_square)
  write(6,'(/a)') ' After fitting'
  write(6,'(a,g20.12)') &
       ' Mean error                 : ', mean_error, &
       ' Absolute error             : ', abs_error,  &
       ' RMS                        : ', mean_square
  ! Fitted charge does not include the polar charges
  charge_calc = sum(charge_lsq_bq)
  ! Added charge is the sum of the polar charges
  q_polar_tot = sum(charge_polar_bq)
  write(6,'(a,g20.12)') &
       ' Target total cluster charge: ', totalcharge, &
       ' Total least squares charge : ', charge_calc, &
       ' Total polar charge         : ', q_polar_tot, &
       ' Total BQs                  : ', charge_calc+q_polar_tot
  
  ! Write the cluster to the fragment object
  call set_cluster_bqs(n_polar_bq, n_lsq_bq, &
       r_polar_bq, r_lsq_bq, charge_polar_bq, charge_lsq_bq, &
       ierr)
  if (ierr .ne. 0) return

  ! Deallocate
  deallocate(r_polar_bq)
  deallocate(r_lsq_bq)
  deallocate(charge_polar_bq)
  deallocate(charge_lsq_bq)
  deallocate(a)
  deallocate(b)
  deallocate(v_kernel)
  deallocate(x_bq)
  deallocate(r_efs)
  deallocate(pot_efs)
  deallocate(efield_efs)
  deallocate(pot_error_efs)
  deallocate(e_error_efs)

end subroutine fit_bqs


subroutine cluster_add_ve(r0, ri, qi, vtot, etot)
  
  implicit none

  double precision, dimension(3), intent(in) :: r0, ri
  double precision, intent(in) :: qi

  double precision, intent(inout) :: vtot
  double precision, dimension(3), intent(inout) :: etot

  double precision, dimension(3) :: ri0
  double precision :: d2, d, v

  ri0(1:3) = r0(1:3) - ri(1:3)
  d2 = ri0(1)*ri0(1) + ri0(2)*ri0(2) + ri0(3)*ri0(3)
  d = sqrt(d2)
  v = qi / d
  vtot = vtot + v
  etot(1:3) = etot(1:3) + (v * ri0(1:3) / d2)

end subroutine cluster_add_ve


subroutine cluster_fit_errors(nccore, ncshell, n_fitting_sites, nefs, core_active, &
     point_cluster_core, pot_error_c, pot_error_s, pot_error_efs, &
     e_error_c, e_error_s, e_error_efs, &
     pot_zero, e_zero, mean_error, abs_error, mean_square)

  implicit none

  integer, intent(in) :: nccore, ncshell, n_fitting_sites, nefs
  logical, dimension(nccore), intent(in) :: core_active
  integer, dimension(ncshell), intent(in) :: point_cluster_core
  double precision, dimension(nccore), intent(in) :: pot_error_c
  double precision, dimension(ncshell), intent(in) :: pot_error_s
  double precision, dimension(nefs), intent(in) :: pot_error_efs
  double precision, dimension(3, nccore), intent(in) :: e_error_c
  double precision, dimension(3, ncshell), intent(in) :: e_error_s
  double precision, dimension(3, nefs), intent(in) :: e_error_efs

  double precision, intent(out) :: pot_zero
  double precision, dimension(3), intent(out) :: e_zero
  double precision, intent(out) :: mean_error, abs_error, mean_square

  integer :: i
  double precision :: d_active_tot

  ! Mean error of electrostatic potential
  pot_zero = 0.0d0
  ! Mean error of electric field components
  e_zero(1:3) = 0.0d0
  ! ME, MAE, RMSE of ESP and electric field together
  mean_error = 0.0d0
  abs_error = 0.0d0
  mean_square = 0.0d0
  
  do i = 1, nccore
     if (core_active(i)) then
        pot_zero = pot_zero + pot_error_c(i)
        e_zero(1:3) = e_zero(1:3) + e_error_c(1:3,i)
        mean_error = mean_error + pot_error_c(i) + &
             e_error_c(1,i) + e_error_c(2,i) + e_error_c(3,i) 
        abs_error = abs_error + abs(pot_error_c(i)) + &
             abs(e_error_c(1,i)) + abs(e_error_c(2,i)) + abs(e_error_c(3,i))
        mean_square = mean_square + pot_error_c(i)**2.0d0 +  &
             e_error_c(1,i)**2.0d0 + e_error_c(2,i)**2.0d0 + e_error_c(3,i)**2.0d0
     end if
  end do
  do i = 1, ncshell
     if (core_active(point_cluster_core(i))) then
        pot_zero = pot_zero + pot_error_s(i)
        e_zero(1:3) = e_zero(1:3) + e_error_s(1:3,i)
        mean_error = mean_error + pot_error_s(i) + &
             e_error_s(1,i) + e_error_s(2,i) + e_error_s(3,i) 
        abs_error = abs_error + abs(pot_error_s(i)) + &
             abs(e_error_s(1,i)) + abs(e_error_s(2,i)) + abs(e_error_s(3,i))
        mean_square = mean_square + pot_error_s(i)**2.0d0 +  &
             e_error_s(1,i)**2.0d0 + e_error_s(2,i)**2.0d0 + e_error_s(3,i)**2.0d0
     end if
  end do
  do i = 1, nefs
     pot_zero = pot_zero + pot_error_efs(i)
     e_zero(1:3) = e_zero(1:3) + e_error_efs(1:3,i)
     mean_error = mean_error + pot_error_efs(i) + &
          e_error_efs(1,i) + e_error_efs(2,i) + e_error_efs(3,i) 
     abs_error = abs_error + abs(pot_error_efs(i)) + &
             abs(e_error_efs(1,i)) + abs(e_error_efs(2,i)) + abs(e_error_efs(3,i))
     mean_square = mean_square + pot_error_efs(i)**2.0d0 +  &
          e_error_efs(1,i)**2.0d0 + e_error_efs(2,i)**2.0d0 + e_error_efs(3,i)**2.0d0    
  end do

  if(n_fitting_sites /= 0) then
     d_active_tot = dble(n_fitting_sites)
     pot_zero = pot_zero / d_active_tot
     e_zero(1:3) = e_zero(1:3) / d_active_tot
     mean_error = mean_error / d_active_tot
     abs_error = abs_error / d_active_tot
     mean_square = sqrt(mean_square / d_active_tot)
  endif

end subroutine cluster_fit_errors
