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

!================================================================
!       vecteur
!================================================================

         SUBROUTINE vecteur(n1,n2,x,y,z,vx,vy,vz,norm)

        use Path_module, only : NAt, KINT, KREAL


        integer(KINT) :: n1,n2
        real(KREAL) ::  x(Nat),y(Nat),z(Nat)
        real(KREAL) ::  vx,vy,vz,norm

        vx=x(n2)-x(n1)
        vy=y(n2)-y(n1)
        vz=z(n2)-z(n1)

        !norm=dsqrt( vx*vx + vy*vy + vz*vz )
            norm=sqrt( vx*vx + vy*vy + vz*vz )

!       write(6,*)
!       write(6,*) vx,vy,vz,norm
!       write(6,*)

      RETURN
      END

!================================================================
!       angle
!================================================================

         FUNCTION angle(v1x,v1y,v1z,norm1,v2x,v2y,v2z,norm2)


        use Path_module, only :  Pi,KREAL

        real(KREAL) ::  v1x,v1y,v1z,norm1
        real(KREAL) ::  v2x,v2y,v2z,norm2
        real(KREAL) ::  angle,ps

!       write(6,*)
!       write(6,*) v1x,v1y,v1z,norm1
!       write(6,*) v2x,v2y,v2z,norm2
!       write(6,*)
        IF (abs(norm1) .LT. 1.D-4) THEN
            STOP 'Angle: norm1 egale 0'
        ELSE IF (abs(norm2) .LT. 1.D-4) THEN
            STOP 'Angle: norm2 egale 0'
        ELSE
          ps=v1x*v2x+v1y*v2y+v1z*v2z
          angle=dacos(ps/(norm1*norm2))*180./Pi
        ENDIF

      RETURN
      END


!================================================================
!       angle oriente (dihedre)
!================================================================

         FUNCTION angle_d(v1x,v1y,v1z,norm1, &
                          v2x,v2y,v2z,norm2, &
                          v3x,v3y,v3z,norm3)

        use Path_module, only :  Pi, KREAL

        real(KREAL) ::  v1x,v1y,v1z,norm1
        real(KREAL) ::  v2x,v2y,v2z,norm2
        real(KREAL) ::  v3x,v3y,v3z,norm3
        real(KREAL) ::  angle_d,ca,sa

!       write(6,*)
!       write(6,*) v1x,v1y,v1z,norm1
!       write(6,*) v2x,v2y,v2z,norm2
!       write(6,*) v3x,v3y,v3z,norm3
!       write(6,*)
        !Print *, 'Inside angle_d'
        IF (abs(norm1) .LT. 1.D-4) THEN
            STOP 'Angle_d: norm1 egale 0'
         ELSE IF (abs(norm2) .LT. 1.D-4) THEN
            STOP 'Angle_d: norm2 egale 0'
        ELSE IF (abs(norm3) .LT. 1.D-4) THEN
            STOP 'Angle_d: norm3 egale 0'
        ELSE
          ca=(v1x*v2x+v1y*v2y+v1z*v2z)/(norm1*norm2)
          sa=(v1x*(v2y*v3z-v2z*v3y) &
             -v1y*(v2x*v3z-v2z*v3x) &
             +v1z*(v2x*v3y-v2y*v3x)) &
             /(norm1*norm2*norm3)
           angle_d=datan2(sa,ca)*180./Pi
!         write(*,*) sa,ca,angle_d,norm1,norm2,norm3
        ENDIF
        !Print *, 'End of angle_d'
      RETURN
      END

!================================================================
!       produit vectoriel
!================================================================

       SUBROUTINE produit_vect(v1x,v1y,v1z, &
                               v2x,v2y,v2z, &
                               v3x,v3y,v3z,norm3)

        use Path_module, only :   KREAL

        real(KREAL) ::  v1x,v1y,v1z ! what do you do with norm1, norm2???
        real(KREAL) ::  v2x,v2y,v2z
        real(KREAL) ::  v3x,v3y,v3z,norm3

        v3x= v1y*v2z-v1z*v2y
        v3y=-v1x*v2z+v1z*v2x
            v3z= v1x*v2y-v1y*v2x

        norm3=dsqrt( v3x*v3x + v3y*v3y + v3z*v3z )


      RETURN
      END

!================================================================
!       rotation suivant l axe x de phi tq a11=cos(phi) a12=sin(phi)
!================================================================

       SUBROUTINE rota_x(v1x,v1y,v1z,a11,a12)

        use Path_module, only :  KREAL

        real(KREAL) ::  v1x,v1y,v1z
        real(KREAL) ::  v2x,v2y,v2z
        real(KREAL) ::  a11,a12

        v2x=  v1x
        v2y=  a11*v1y - a12*v1z
        v2z=  a12*v1y + a11*v1z


        v1x=v2x
        v1y=v2y
        v1z=v2z


      RETURN
      END


!================================================================
!       rotation suivant l axe y de phi tq a11=cos(phi) a12=sin(phi)
!================================================================

       SUBROUTINE rota_y(v1x,v1y,v1z,a11,a12)

        use Path_module, only :   KREAL

        real(KREAL) ::  v1x,v1y,v1z
        real(KREAL) ::  v2x,v2y,v2z
        real(KREAL) ::  a11,a12

        v2x=  a11*v1x - a12*v1z
        v2y=  v1y
        v2z=  a12*v1x + a11*v1z


        v1x=v2x
        v1y=v2y
        v1z=v2z


        RETURN
        END


!================================================================
!       rotation suivant l axe z de phi tq a11=cos(phi) a12=sin(phi)
!================================================================

       SUBROUTINE rota_z(v1x,v1y,v1z,a11,a12)

        use Path_module, only :   KREAL

        real(KREAL) ::  v1x,v1y,v1z
        real(KREAL) ::  v2x,v2y,v2z
        real(KREAL) ::  a11,a12

        v2x=  a11*v1x - a12*v1y
        v2y=  a12*v1x + a11*v1y
        v2z=  v1z


        v1x=v2x
        v1y=v2y
        v1z=v2z

        RETURN
        END
