!
! The following vector routines are adapted from Construct v84
! by Sam French, Dan Wilson and Alexey Sokol DFRL RI 2008
!

!  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 lattice_parameters_2(vect,area,a,b,gamma)
!
! 2-dim case
!
! Calculates lattice parameters and UC area from lattice vectors.
!
      implicit          none
      real*8            vect(2,2),a,b,gamma,area
      area=dabs(vect(1,1)*vect(2,2)-vect(1,2)*vect(2,1))
      a=dsqrt(vect(1,1)**2+vect(1,2)**2)
      b=dsqrt(vect(2,1)**2+vect(2,2)**2)
      gamma=dacos( &
           (vect(1,1)*vect(2,1)+vect(1,2)*vect(2,2))/(a*b))
      return
      end


      subroutine transform_2(device,f,r)
      implicit          none
      real*8            device(2,2),f(2),r(2)
      r(1)=device(1,1)*f(1)+device(2,1)*f(2)
      r(2)=device(1,2)*f(1)+device(2,2)*f(2)
      return
      end


      real*8 function dist_mod_2(device,f1,z1,f2,z2)
!
! 2-dim case
!
! Calculates closest distance between two points f1 and f2 defined by their
! fractional coordinates.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(2,2),                &
                        f1(2),f2(2),d(2),r(2),p(2), &
                        dist2o,dist2n,z1,z2,z3
      integer*4         i,j,k
      do k=1,2
         d(k)=dmod(f1(k)-f2(k),1.d0)
         if(d(k).lt.0.d0) d(k)=d(k)+1.d0
      enddo
      dist2o=1.d32
      do i=-2,2
         p(1)=d(1)+real(i)
         do j=-2,2
            p(2)=d(2)+real(j)
            call transform_2(device,p,r)
            dist2n=r(1)**2+r(2)**2
            if(dist2n.lt.dist2o) then
               dist2o=dist2n
            endif
         enddo
      enddo
      z3=z1-z2
      dist_mod_2=dsqrt( dist2o+z3*z3 )
      return
      end

      real*8 function distance_2(device,f1,z1,f2,z2)
!
! 2-dim case
!
! Calculates absolute distance between two points f1 and f2 defined by their
! fractional and cartesian z1 and z2 coordinates.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(2,2),f1(2),z1,f2(2),z2,d(2),z3,r(2)
      integer*4         i
      d(1)=f2(1)-f1(1)
      d(2)=f2(2)-f1(2)
      z3=z2-z1
      call transform_2(device,d,r)
      distance_2=dsqrt( r(1)*r(1)+r(2)*r(2)+z3*z3 )
      return
      end


      subroutine translate_2(device,f1,f2,r2)
!
! 2-dim case
!
! Translates point f2 to the closest positon to f1 for f1 and f2 defined
! by their fractional coordinates. r2 are revised Cartesian coordinates of f2.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(2,2),f1(2),f2(2),r2(2), &
                        d(2),p(2),r(2),dist2n,dist2o,search_tol
      integer*4         i,j,k
! Threshold prevents arbitrary switching between equivalent coordinates
      search_tol = 1.0d-4
      do k=1,2
         d(k)=dmod(f2(k)-f1(k),1.d0)
      enddo
! Calculate the unchanged position first, again to avoid switching for 
! no reason
      p(1) = d(1)
      p(2) = d(2)
      call transform_2(device,p,r)
      dist2n = r(1)**2 + r(2)**2
      dist2o = dist2n - search_tol
      do k=1,2
         f2(k)=f1(k)+p(k)
      enddo      
! Now search over other positions
      do i=-2,2
         p(1)=d(1)+real(i)
         do j=-2,2
            p(2)=d(2)+real(j)
            call transform_2(device,p,r)
            dist2n=r(1)**2+r(2)**2
            if(dist2n.lt.dist2o) then
               dist2o = dist2n - search_tol
               do k=1,2
                  f2(k)=f1(k)+p(k)
               enddo
            endif
         enddo
      enddo
      call transform_2(device,f2,r2)
      return
      end

! Note on construction of cell vectors
!
! Due to limits of floating precision, gamma=90deg (for example) 
! results in a very small non-zero value for one of the resulting 
! cell vectors and therefore some of the resulting coordinates
!
! The old construct got clean coordinates by printing them out
! in f format instead of the standard e format. As we pass
! data directly to ChemShell this option is not available so
! instead we introduce a threshold (zthresh) for cell vectors below which
! they are reset to 0.
!
! (nb: the 'tiny' threshold in the 3d case is unrelated to this change)
!

      subroutine  lattice_cryst_2(a,b,gamma,vect,area)
!
! 2-dim case
!
! Builds matrix of Cartesian coordinates of surface lattice vectors
! from linear and angular lattice parameters according to CRYSTALL's
! convention:
!      First index counts vectors 1=a, 2=b
!      Second index counts coordinates 1=x, 2=y.
!      Vector b looks along axis Y.
!      Vector a is in XY plane under angle gamma to vector b.
!
      implicit          none
      real*8            vect(2,2),a,b,gamma,area,sin_gamma
      integer           i, j
      real*8            zthresh
      data              zthresh/1.0d-14/
      sin_gamma = dsin(gamma)
      vect(1,1) = a*sin_gamma
      vect(1,2) = a*dcos(gamma)
      vect(2,1) = 0.0d0
      vect(2,2) = b
      do i = 1, 2
         do j = 1, 2
            if (dabs(vect(i,j)) < zthresh) vect(i,j) = 0.0d0
         end do
      end do
      area = a*b*dabs(sin_gamma)
      return
      end


      subroutine  lattice_conv_2(a,b,gamma,vect,area)
!
! 2-dim case
!
! Builds matrix of Cartesian coordinates of surface lattice vectors
! from linear and angular lattice parameters according to CRYSTALL's
! convention:
!      First index counts vectors 1=a, 2=b
!      Second index counts coordinates 1=x, 2=y.
!      Vector a looks along axis X.
!      Vector b is in XY plane under angle gamma to vector a.
!
      implicit          none
      real*8            vect(2,2),a,b,gamma,area,sin_gamma
      integer           i, j
      real*8            zthresh
      data              zthresh/1.0d-14/
      sin_gamma = dsin(gamma)
      vect(1,1) = a
      vect(1,2) = 0.0d0
      vect(2,1) = b*dcos(gamma)
      vect(2,2) = b*sin_gamma
      do i = 1, 2
         do j = 1, 2
            if (dabs(vect(i,j)) < zthresh) vect(i,j) = 0.0d0
         end do
      end do
      area = a*b*dabs(sin_gamma)
      return
      end


      subroutine  lattice_cryst_3(a,b,c,alpha,beta,gamma,vect,volume)
!
! 3-dim case
!
! Builds matrix of Cartesian coordinates of lattice vectors
! from linear and angular lattice parameters according to CRYSTALL's
! convention:
!      First index counts vectors 1=a, 2=b, 3=c.
!      Second index counts coordinates 1=x, 2=y, 3=z.
!      Vector b looks along axis Y.
!      Vector a is in XY plane under angle gamma to vector b.
!      Vector c is over XY plane under angles alpha and beta
!      to vectors a and b, respectively.
! Input lenghths in atomic units.
! Input angles in radians.
!
      implicit          none
      real*8            vect(3,3),a,b,c,alpha,beta,gamma,volume, &
                        tiny,cos_gamma,sin_gamma,x3,y3,square
      integer           i, j
      real*8            zthresh
      data              zthresh/1.0d-14/
      data              tiny/0.1e-20/
      cos_gamma = dcos(gamma)
      sin_gamma = dsin(gamma)
      y3 = dcos(alpha)
      x3 = (dcos(beta)-y3*cos_gamma)/sin_gamma
      square = 1.0d0-x3*x3-y3*y3
      vect(1,1) = a*sin_gamma
      vect(1,2) = a*cos_gamma
      vect(1,3) = 0.0d0
      vect(2,1) = 0.0d0
      vect(2,2) = b
      vect(2,3) = 0.0d0
      vect(3,1) = c*x3
      vect(3,2) = c*y3
      if(square.lt.tiny) then
         vect(3,3) = 0.0d0
      else
         vect(3,3) = c*dsqrt(square)
      endif
      do i = 1, 3
         do j = 1, 3
            if (dabs(vect(i,j)) < zthresh) vect(i,j) = 0.0d0
         end do
      end do
      volume=dabs(vect(1,1)*(vect(2,2)*vect(3,3)-vect(2,3)*vect(3,2))- &
                  vect(2,1)*(vect(1,2)*vect(3,3)-vect(1,3)*vect(3,2))+ &
                  vect(3,1)*(vect(1,2)*vect(2,3)-vect(1,3)*vect(2,2)) )
      return
      end


      subroutine  lattice_conv_3(a,b,c,alpha,beta,gamma,vect,volume)
!
! 3-dim case
!
! Builds matrix of Cartesian coordinates of lattice vectors
! from linear and angular lattice parameters.
!      First index counts vectors 1=a, 2=b, 3=c.
!      Second index counts coordinates 1=x, 2=y, 3=z.
!      Vector a looks along axis X.
!      Vector b is in XY plane under angle gamma to vector a.
!      Vector c is over XY plane under angles alpha and beta
!      to vectors b and a, respectively.
!      The orientation of vectors corresponds to the right trio.
! Input lenghths in atomic units.
! Input angles in radians.
!
      implicit          none
      real*8            vect(3,3),a,b,c,alpha,beta,gamma,volume, &
                        tiny,cos_gamma,sin_gamma,x3,y3,square
      integer           i, j
      real*8            zthresh
      data              zthresh/1.0d-14/
      data              tiny/0.1e-20/
      cos_gamma = dcos(gamma)
      sin_gamma = dsin(gamma)
      x3 = dcos(beta)
      y3 = (dcos(alpha)-x3*cos_gamma)/sin_gamma
      square = 1.0d0-x3*x3-y3*y3
      vect(1,1) = a
      vect(1,2) = 0.0d0
      vect(1,3) = 0.0d0
      vect(2,1) = b*cos_gamma
      vect(2,2) = b*sin_gamma
      vect(2,3) = 0.0d0
      vect(3,1) = c*x3
      vect(3,2) = c*y3
      if(square.lt.tiny) then
         vect(3,3) = 0.0d0
      else
         vect(3,3) = c*dsqrt(square)
      endif
      do i = 1, 3
         do j = 1, 3
            if (dabs(vect(i,j)) < zthresh) vect(i,j) = 0.0d0
         end do
      end do
      volume=dabs(vect(1,1)*(vect(2,2)*vect(3,3)-vect(2,3)*vect(3,2))- &
                  vect(2,1)*(vect(1,2)*vect(3,3)-vect(1,3)*vect(3,2))+ &
                  vect(3,1)*(vect(1,2)*vect(2,3)-vect(1,3)*vect(2,2)) )
      return
      end


      subroutine inverse_3(ai,bi)
!
! 3-dim case
!
! Builds reciprocal vectors, bi, from direct, ai.
! Physical convention is used for reciprocal lattice vectors:
!      exp(i*ai*bi)=1
!
      implicit           none
      real*8                      &
                         two_pi,  &
                         det,     &
                         ai(3,3), &
                         bi(3,3)
       two_pi=2.d0*dacos(-1.d0)
      det= ai(1,1)*(ai(2,2)*ai(3,3)-ai(2,3)*ai(3,2)) &
          -ai(2,1)*(ai(1,2)*ai(3,3)-ai(1,3)*ai(3,2)) &
          +ai(3,1)*(ai(1,2)*ai(2,3)-ai(1,3)*ai(2,2))
      det=two_pi/det
      bi(1,1)= (ai(2,2)*ai(3,3)-ai(2,3)*ai(3,2))*det
      bi(1,2)=-(ai(2,1)*ai(3,3)-ai(2,3)*ai(3,1))*det
      bi(1,3)= (ai(2,1)*ai(3,2)-ai(2,2)*ai(3,1))*det
      bi(2,1)=-(ai(1,2)*ai(3,3)-ai(1,3)*ai(3,2))*det
      bi(2,2)= (ai(1,1)*ai(3,3)-ai(1,3)*ai(3,1))*det
      bi(2,3)=-(ai(1,1)*ai(3,2)-ai(1,2)*ai(3,1))*det
      bi(3,1)= (ai(1,2)*ai(2,3)-ai(2,2)*ai(1,3))*det
      bi(3,2)=-(ai(1,1)*ai(2,3)-ai(1,3)*ai(2,1))*det
      bi(3,3)= (ai(1,1)*ai(2,2)-ai(1,2)*ai(2,1))*det
      return
      end


      subroutine lattice_parameters_3(         &
           vect,volume,a,b,c,alpha,beta,gamma)

!
! 3-dim case
!
! Calculates lattice parameters and UC volume from lattice vectors.
!
      implicit          none
      real*8            vect(3,3),a,b,c,alpha,beta,gamma,volume
      volume=dabs(                                              &
           vect(1,1)*(vect(2,2)*vect(3,3)-vect(2,3)*vect(3,2))- &
           vect(2,1)*(vect(1,2)*vect(3,3)-vect(1,3)*vect(3,2))+ &
           vect(3,1)*(vect(1,2)*vect(2,3)-vect(1,3)*vect(2,2)) )
      a=dsqrt(vect(1,1)**2+vect(1,2)**2+vect(1,3)**2)
      b=dsqrt(vect(2,1)**2+vect(2,2)**2+vect(2,3)**2)
      c=dsqrt(vect(3,1)**2+vect(3,2)**2+vect(3,3)**2)
      alpha=dacos(                                                       &
           (vect(2,1)*vect(3,1)+vect(2,2)*vect(3,2)+vect(2,3)*vect(3,3)) &
          /(b*c) )
      beta=dacos(                                                        &
           (vect(1,1)*vect(3,1)+vect(1,2)*vect(3,2)+vect(1,3)*vect(3,3)) &
          /(a*c) )
      gamma=dacos(                                                       &
           (vect(1,1)*vect(2,1)+vect(1,2)*vect(2,2)+vect(1,3)*vect(2,3)) &
          /(a*b) )
      return
      end


      subroutine transform_3(device,ri,rf)
!
! 3-dim case
!
! Transforms coordinates of a vector from fractional into Cartesian
! and from Cartesian into fractional.
! device, transformation matrix of first rank, calculated beforehand
! as Cartesian coordinates of lattice vectors and reciprocal lattice
! vectors respectively.
!
      implicit          none
      real*8            device(3,3),ri(3),rf(3)
      integer*4         i
      do i=1,3
         rf(i)=ri(1)*device(1,i)+ri(2)*device(2,i)+ri(3)*device(3,i)
      enddo
      return
      end


      real*8 function dist_mod_3(device,f1,f2)
!
! 3-dim case
!
! Calculates closest distance between two points f1 and f2 defined by their
! fractional coordinates.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(3,3)
      real*8            f1(3),f2(3),d(3),r(3),p(3),dist2o,dist2n
      integer*4         i,j,k
      do k=1,3
         d(k)=dmod(f1(k)-f2(k),1.d0)
         if(d(k).lt.0.d0) d(k)=d(k)+1.d0
      enddo
      dist2o=1.d32
      do i=-2,2
         p(1)=d(1)+real(i)
         do j=-2,2
            p(2)=d(2)+real(j)
            do k=-2,2
               p(3)=d(3)+real(k)
               call transform_3(device,p,r)
               dist2n=r(1)**2+r(2)**2+r(3)**2
               if(dist2n.lt.dist2o) then
                  dist2o=dist2n
               endif
            enddo
         enddo
      enddo
      dist_mod_3=dsqrt( dist2o )
      return
      end


      real*8 function distance_3(device,f1,f2)
!
! 3-dim case
!
! Calculates absolute distance between two points f1 and f2 defined by their
! fractional coordinates.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(3,3),f1(3),f2(3),d(3),r(3)
      integer*4         i
      do i=1,3
         d(i)=f2(i)-f1(i)
      enddo
      call transform_3(device,d,r)
      distance_3=dsqrt( r(1)*r(1)+r(2)*r(2)+r(3)*r(3) )
      return
      end


      subroutine translate_3(device,f1,f2,r2)
!
! 3-dim case
!
! Translates point f2 to the closest positon to f1 for f1 and f2 defined
! by their fractional coordinates. r2 are revised Cartesian coordinates of f2.
! device, transformation matrix of first rank, calculated beforehand as
! Cartesian coordinates of lattice vectors.
!
      implicit          none
      real*8            device(3,3),f1(3),f2(3),r2(3),          &
                        d(3),p(3),r(3),dist2n,dist2o,search_tol
      integer*4         i,j,k,l
! Threshold prevents arbitrary switching between equivalent coordinates
      search_tol = 1.0d-4
      do k=1,3
         d(k)=dmod(f2(k)-f1(k),1.0d0)
!         if(d(k).lt.0.d0) d(k)=d(k)+1.d0
      enddo
! Calculate the unchanged position first, again to avoid switching for 
! no reason
      p(1) = d(1)
      p(2) = d(2)
      p(3) = d(3)
      call transform_3(device,p,r)
      dist2n = r(1)**2 + r(2)**2 + r(3)**2
      dist2o = dist2n - search_tol
      do l=1,3
         f2(l)=f1(l)+p(l)
      enddo      
! Now search over other positions
      do i=-2,2
         p(1)=d(1)+real(i)
         do j=-2,2
            p(2)=d(2)+real(j)
            do k=-2,2
               p(3)=d(3)+real(k)
               call transform_3(device,p,r)
               dist2n=r(1)**2+r(2)**2+r(3)**2
               if(dist2n.lt.dist2o) then
                  dist2o = dist2n - search_tol
                  do l=1,3
                     f2(l)=f1(l)+p(l)
                  enddo
               endif
            enddo
         enddo
      enddo
      call transform_3(device,f2,r2)
      return
      end


      subroutine inverse_2(ai,bi,area)
!
! 2-dim case
!
! Builds reciprocal vectors bi from direct vectors ai.
! Factor 2*pi is dropped.
! Calculates volume of UC in direct space.
! ai and bi are transformation matrices of first rank.
!
      implicit           none
      real*8             ai(2,2),bi(2,2)
      real*8             area,det
      real*8                    &
                         two_pi
      two_pi=2.d0*dacos(-1.d0)
      det= ai(1,1)*ai(2,2)-ai(1,2)*ai(2,1)
      area = abs(det)
      det=two_pi/det
      bi(1,1)= ai(2,2)*det
      bi(1,2)=-ai(2,1)*det
      bi(2,1)=-ai(1,2)*det
      bi(2,2)= ai(1,1)*det
      return
      end

