
! This subroutine calculates RMSD (Root mean square deviation) using quaternions.
! It is baNatsed on the F90 routine bu E. Coutsias
! http://www.math.unm.edu/~vageli/homepage.html
! I (PFL) have just translated it, and I have changed the diagonalization
! subroutine.
! I also made some changes to make it suitable for Cart package.
!----------------------------------------------------------------------
!----------------------------------------------------------------------
! Copyright (C) 2004, 2005 Chaok Seok, Evangelos Coutsias and Ken Dill
!      UCSF, Univeristy of New Mexico, Seoul National University
! Witten by Chaok Seok and Evangelos Coutsias 2004.
                                                                                
! This library 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 2.1 of the License, or (at your option) any later version.
!
                                                                                
! This library 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 this library; if not, write to the Free Software
! Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
!----------------------------------------------------------------------------
!----------------------------------------------------------------------
!  Copyright 2003-2014 Ecole Normale Supérieure de Lyon, 
!  Centre National de la Recherche Scientifique,
!  Université Claude Bernard Lyon 1. All rights reserved.
!
!  This work is registered with the Agency for the Protection of Programs 
!  as IDDN.FR.001.100009.000.S.P.2014.000.30625
!
!  Authors: P. Fleurat-Lessard, P. Dayal
!  Contact: optnpath@gmail.com
!
! This file is part of "Opt'n Path".
!
!  "Opt'n Path" is free software: you can redistribute it and/or modify
!  it under the terms of the GNU Affero General Public License as
!  published by the Free Software Foundation, either version 3 of the License,
!  or (at your option) any later version.
!
!  "Opt'n Path" 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 Affero General Public License for more details.
!
!  You should have received a copy of the GNU Affero General Public License
!  along with "Opt'n Path". If not, see <http://www.gnu.org/licenses/>.
!
! Contact The Office of Technology Licensing, valorisation@ens-lyon.fr,
! for commercial licensing opportunities.
!----------------------------------------------------------------------

      subroutine CalcRmsd(Nat,x0,y0,z0, x2,y2,z2,U,rmsd,FRot,FAlign)
!-----------------------------------------------------------------------
!  This subroutine calculates the least square rmsd of two coordinate
!  sets coord1(3,n) and coord2(3,n) using a method based on quaternion.
!  It then calculate the rotation matrix U and the centers of coord, and uses
! them to align the two molecules.
! x0(Nat), y0(Nat), z0(Nat) are the coordinates of the reference geometry.
! x2(Nat), y2(Nat), z2(Nat) are the coordinates of the geometry which is to
! be aligned with the reference geometry. 
! The rotation matrix U has INTENT(OUT) in subroutine rotation_matrix(...),
! which is called in this CalcRmsd(...).
! rmsd: the root mean square deviation and calculated in this subroutine
! CalcRmsd(...).
! FRot: if .TRUE. the rotation matrix is calculated. This is the matrix that rotates
! the first molecule onto the second one.
! FAlgin: if .TRUE. then the second molecule is aligned on the first one.
!-----------------------------------------------------------------------

        use VarTypes

      IMPLICIT NONE

      INTEGER(KINT) :: Nat
      real(KREAL) ::  x0(Nat),y0(Nat),z0(Nat)
      real(KREAL) ::  x2(Nat),y2(Nat),z2(Nat)
      real(KREAL) ::  U(3,3), rmsd
      LOGICAL  FRot,FAlign,Debug

      REAL(KREAL),ALLOCATABLE :: Coord1(:,:), Coord2(:,:) ! 3,Nat
      real(KREAL) ::  x0c1,y0c1,z0c1, xc2,yc2,zc2
      

      INTEGER(KINT) :: i, j, ia
      real(KREAL) :: x_norm, y_norm, lambda
      real(KREAL) :: Rmatrix(3,3)
      real(KREAL) :: S(4, 4)
      real(KREAL) :: EigVec(4, 4), EigVal(4)

      INTERFACE
       function valid(string) result (isValid)
         CHARACTER(*), intent(in) :: string
         logical                  :: isValid
         END function VALID
      END INTERFACE


      debug=valid('CalcRmsd').OR.valid('align').OR.valid('alignpartial')

      if (debug) Call Header("Entering CalcRmsd")

      if (debug) THEN
         WRITE(*,*) "NAt=",NAt
         WRITE(*,*) x0
         WRITE(*,*) y0
         WRITE(*,*) z0
      END IF

      ALLOCATE(Coord1(3,Nat),Coord2(3,Nat))
  ! calculate the barycenters, centroidal coordinates, and the norms
      x_norm = 0.0d0
      y_norm = 0.0d0
      x0c1=0.
      y0c1=0.
      z0c1=0.
      xc2=0.
      yc2=0.
      zc2=0.
      do ia=1,Nat
         if (debug) WRITE(*,'(A,I5,4(1X,F10.4))') 'ia...',ia,x0(ia), &
                             x2(ia),x0c1,xc2
         x0c1=x0c1+x0(ia)
         xc2=xc2+x2(ia)
         y0c1=y0c1+y0(ia)
         yc2=yc2+y2(ia)
         z0c1=z0c1+z0(ia)
         zc2=zc2+z2(ia)
         if (debug) WRITE(*,'(A,I5,4(1X,F10.4))') 'ia...',ia,x0(ia), &
                             x2(ia),x0c1,xc2
      END DO
      x0c1=x0c1/dble(Nat)
      y0c1=y0c1/dble(Nat)
      z0c1=z0c1/dble(Nat)
      xc2=xc2/dble(Nat)
      yc2=yc2/dble(Nat)
      zc2=zc2/dble(Nat)

      IF (debug) WRITE(*,'(1X,A,3(1X,F10.4))') 'Center1',x0c1,y0c1,z0c1
      IF (debug) WRITE(*,'(1X,A,3(1X,F10.4))') 'Center2',xc2,yc2,zc2
      do i=1,Nat
         Coord1(1,i)=x0(i)-x0c1
         Coord1(2,i)=y0(i)-y0c1
         Coord1(3,i)=z0(i)-z0c1
         Coord2(1,i)=x2(i)-xc2
         Coord2(2,i)=y2(i)-yc2
         Coord2(3,i)=z2(i)-zc2
         x_norm=x_norm+Coord1(1,i)**2+Coord1(2,i)**2+Coord1(3,i)**2
         y_norm=y_norm+Coord2(1,i)**2+Coord2(2,i)**2+Coord2(3,i)**2
      end do

      IF (debug) THEN
         WRITE(*,*) "R matrix"
         DO I=1,3
            WRITE(*,*) (RMatrix(I,j),j=1,3)
         END DO
      END IF

  ! calculate the R matrix
      do i = 1, 3
         do j = 1, 3
            Rmatrix(i,j)=0.
            do ia=1,Nat
               Rmatrix(i,j) = Rmatrix(i,j)+Coord1(i,ia)*Coord2(j,ia)
            END DO
         end do
      end do

      IF (debug) THEN
         WRITE(*,*) "R matrix"
         DO I=1,3
            WRITE(*,*) (RMatrix(I,j),j=1,3)
         END DO
      END IF


  ! S matrix
      S(1, 1) = Rmatrix(1, 1) + Rmatrix(2, 2) + Rmatrix(3, 3)
      S(2, 1) = Rmatrix(2, 3) - Rmatrix(3, 2)
      S(3, 1) = Rmatrix(3, 1) - Rmatrix(1, 3)
      S(4, 1) = Rmatrix(1, 2) - Rmatrix(2, 1)

      S(1, 2) = S(2, 1)
      S(2, 2) = Rmatrix(1, 1) - Rmatrix(2, 2) - Rmatrix(3, 3)
      S(3, 2) = Rmatrix(1, 2) + Rmatrix(2, 1)
      S(4, 2) = Rmatrix(1, 3) + Rmatrix(3, 1)

      S(1, 3) = S(3, 1)
      S(2, 3) = S(3, 2)
      S(3, 3) =-Rmatrix(1, 1) + Rmatrix(2, 2) - Rmatrix(3, 3)
      S(4, 3) = Rmatrix(2, 3) + Rmatrix(3, 2)

      S(1, 4) = S(4, 1)
      S(2, 4) = S(4, 2)
      S(3, 4) = S(4, 3)
      S(4, 4) =-Rmatrix(1, 1) - Rmatrix(2, 2) + Rmatrix(3, 3) 


! PFL : I use my usual Jacobi diagonalisation
  ! Calculate eigenvalues and eigenvectors, and 
  ! take the maximum eigenvalue lambda and the corresponding eigenvector q.

      IF (debug) THEN
         WRITE(*,*) "S matrix"
         DO I=1,4
            WRITE(*,*) (S(I,j),j=1,4)
         END DO
      END IF

      Call Jacobi(S,4,EigVal,EigVec,4)

      Call Trie(4,EigVal,EigVec,4)

      Lambda=EigVal(4)

! RMS Deviation
       rmsd=sqrt(max(0.0d0,((x_norm+y_norm)-2.0d0*lambda))/dble(Nat))
! The subroutine rotation_matrix(...) constructs rotation matrix U as output
! from the input quaternion EigVec(1,4).
      if (FRot.OR.FAlign) Call rotation_matrix(EigVec(1,4),U)
      IF (FAlign) THEN
         DO I=1,Nat
            x2(i)=Coord2(1,i)*U(1,1)+Coord2(2,i)*U(2,1) &
                +Coord2(3,i)*U(3,1) +x0c1
            y2(i)=Coord2(1,i)*U(1,2)+Coord2(2,i)*U(2,2) &
                +Coord2(3,i)*U(3,2) +y0c1
            z2(i)=Coord2(1,i)*U(1,3)+Coord2(2,i)*U(2,3) &
                +Coord2(3,i)*U(3,3) +z0c1
         END DO
      END IF

      DEALLOCATE(Coord1,Coord2)

    END subroutine CalcRmsd
