!!-------------------------------------------------------------------------------------------------
!!-------------------------------------------------------------------------------------------------
!!
!!    
!!    ****************************************************************
!!    **   CONSTRAINT DYNAMICS OF A LINEAR CHAIN USING MILC SHAKE   **
!!    ****************************************************************
!!
!!    
!!
!!    If you use this code/algorithm, please cite the following reference:
!!
!!
!!    REFERENCE: A.G. Bailey, C.P. Lowe, A.P. Sutton, Efficient constraint dynamics using MILC SHAKE, J. Comput. Phys. 227 (2008) 8949-8959.
!!               DOI: 10.1016/j.jcp.2008.07.002
!!               Available Online: http://dx.doi.org/10.1016/j.jcp.2008.07.002
!!
!!
!!..................................................................................................
!!
!!
!!    The included FORTRAN90 code consists of two subroutines to calculate length and velocity 
!!    constraints between adjacent sites in a linear chain, following the algorithm MILC SHAKE.   
!!    The chain may be open or a ring. The constraint equations are linearized and solved iteratively 
!!    until each link length is satisfied to within a given desired tolerance.  Two subroutines are 
!!    included (MILCSHAKEa,MILCSHAKEb). The first (MILCSHAKEa) calculates the new, constrained positions 
!!    and updates the velocities a half step; the second subroutine (MILCSHAKEb) calculates the
!!    constrained velocities and the new kinetic energy. 
!!   
!!      SUBROUTINE MILCSHAKEa ( RX, RY, RZ, VX, VY, VZ, FX, FY, FZ, DT, DSQ, M, TOL, NA, NB, NM ) 
!!      SUBROUTINE MILCSHAKEb ( RX, RY, RZ, VX, VY, VZ, FX, FY, FZ, ENERGY, DT, M,  NA, NB, NM )
!!
!!
!!..................................................................................................
!!
!!
!!    NOTES: 
!!
!!    . The code uses a DGTSV( NB, 1, DL, D, DU, U, NB, INFO) function call from the LAPACK library
!!         SOURCE: Linear Algebra PACKage, http://www.netlib.org/lapack/
!!    . The algorithm for the cyclic tridiagonal matrix solver is from Numerical Recipes
!!         SOURCE: W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery, Numerical recipes in FORTRAN 
!!                 (2nd ed.): the art of scientific computing, Cambridge University Press, New York, NY, 1992. 
!!    . The code was modeled off of SHAKE/RATTLE code from CCPP5, so that those using CCP5 could implement
!!      MILC SHAKE quite easily.
!!         SOURCE: CCP5 Program Library, http://www.ccp5.ac.uk/librar.shtml
!!
!!
!!..................................................................................................
!!
!!    DISCLAIMER: 
!!  
!!    To our knowledge this code is fully functional; however, test it just in case.  If you find an error, 
!!    an e-mail notifying the authors would be much appreciated.  
!!
!!
!!-------------------------------------------------------------------------------------------------
!!
!!
!!  INPUT:
!!..................................................................................................
!!  NM             Number of molecules in the system.
!!  NA             Number of sites per molecule.
!!  NB             Number of bonds per molecule (NB = NA for a ring, NB = NA-1 for an open chain)
!!  TOL            The predefined error or tolerance. (Typically 10E-6 - 1E-14, depending on the system)
!!  M              Matrix (NM,NA) of masses of the sites.
!!  DSQ            Matrix (NM,NB) of squared link lengths.
!!  DT             Time step size.
!!  FX,..          Matrix (NM,NA) of x,y,z forces on the sites calculated at t=t0.
!!  VX,..          Matrix (NM,NA) of x,y,z velocities of the sites at t=t0.
!!  RX,..          Matrix (NM,NA) of x,y,z positions of the sites at t=t0.
!!
!!
!!  LOCAL SUBROUTINE VARIABLES:
!!..................................................................................................
!!
!!  DONE           Logical identifying whether the desired accuracy has been reached.  
!!  MAXIT          Maximum allowable iterations.
!!  DT2            DT/2.0
!!  DTSQ2          DT*DT/2
!!  ERROR          Error, as defined in reference.
!!  MAX            Maximum relative error.
!!  I,A,B,IT       Indices 
!!  
!!  RXAB,..        Vectors (NB) of x,y,z vectors between original sites A and B, defined r(A)-r(B), taken at t=t0 the starting time.  
!!  RNCXAB,..      Vectors (NB) of x,y,z vectors between the unconstrained sites A and B, taken at t+dt prior to applying constraint forces.  
!!  NX,..          Vectors (NA) of x,y,z positions calculated using the latest calculated constraint forces.  
!!  NXAB,..        Vectors (NB) of x,y,z vectors calculated using the best current estimate for the positions NX,NY,NZ.  
!!  AX,..          Acceleration of site   
!!  RXI,..         Vectors (NA) of x,y,z positions of the sites of molecule I
!!  PXI,..         Vectors (NA) of x,y,z positions of the unconstrained sites of molecule I, taken at t+dt prior to applying constraint forces
!!  VXI,..         Vectors (NA) of x,y,z velocities of molecule I, taken at t+dt prior to applying constraint forces  
!!
!!  INFO           Logical indicating whether DGSTV successfully solved the system of equations
!!  alpha,beta     Lower left and upper right corners respectively in a cyclic tridiagonal matrix
!!  gamma,fact     For the cyclic tridiagonal matrix algorithm
!!  CD,CDL,CDU     Multiplicative constant of the diagonal(/diagonal lower/diagonal upper) vectors
!!  U              For the cyclic tridiagonal matrix algorithm (NB)
!!  DL,DLi,DU,DUi  Subdiagonal vector (NB-1) (DL-diagonal lower) and a copy (DLi). The same for the superdiagonal vector (DU-diagonal upper).
!!  D,Di           Diagonal vector (NB) and a copy.  
!!  LAMBDA         Vector (NB) of the undetermined Lagrange multipliers
!!  RHS,RHSOLD     The 'RHS' is the vector (NB) of constraint conditions, Equation 1 from reference.  RHSOLD is the RHS vector from the preceding iteration.  
!!
!!
!!-------------------------------------------------------------------------------------------------
!!-------------------------------------------------------------------------------------------------



       SUBROUTINE MILCSHAKEa ( RX, RY, RZ, VX, VY, VZ, FX, FY, FZ, DT, DSQ, M, TOL, NA, NB, NM ) 

         INTEGER     NA, NB, NM                     
         REAL        DT, TOL             
         REAL        RX(NM,NA), RY(NM,NA), RZ(NM,NA)    
         REAL        VX(NM,NA), VY(NM,NA), VZ(NM,NA)    
         REAL        FX(NM, NA), FY(NM, NA), FZ(NM, NA) 
         REAL        DSQ(NM,NB), M(NM,NA)      

         LOGICAL     DONE
         INTEGER     MAXIT                           
         PARAMETER(MAXIT=1000)      
         REAL        DT2, DTSQ2, ERROR, MAX
         INTEGER     I, A, B, IT

         REAL        RXAB(NB), RYAB(NB), RZAB(NB), RAB2(NB)           
         REAL        RNCXAB(NB), RNCYAB(NB), RNCZAB(NB), RNCAB2(NB) 
         REAL        NX(NA), NY(NA), NZ(NA)                       
         REAL        NXAB(NB), NYAB(NB), NZAB(NB), NAB2(NB)           
         REAL        AX, AY, AZ                                    
         REAL        RXI(NA), RYI(NA), RZI(NA)                      
         REAL        PXI(NA), PYI(NA), PZI(NA)                    
         REAL        VXI(NA), VYI(NA), VZI(NA)      

         LOGICAL     INFO
         REAL        alpha, beta, gamma, fact
         REAL        CD, CDL, CDU
         REAL, DIMENSION(NB)   :: U
         REAL, DIMENSION(NB-1) :: DL, DLi, DU, DUi 
         REAL, DIMENSION(NB)   :: D, Di
         REAL, DIMENSION(NB)   :: LAMBDA, RHS, RHSOLD


 
         DT2    = DT / 2.0
         DTSQ2  = DT * DT2        
         IT = 1   

         DO I = 1,NM

            DO A = 1,NA
               RXI(A) = RX(I,A)
               RYI(A) = RY(I,A) 
               RZI(A) = RZ(I,A) 

               VXI(A) = VX(I,A)
               VYI(A) = VY(I,A) 
               VZI(A) = VZ(I,A)         
                   
               AX = FX(I,A) / M(I,A)
               AY = FY(I,A) / M(I,A)
               AZ = FZ(I,A) / M(I,A)            

               PXI(A) = RXI(A) + DT * VXI(A) + DTSQ2 * AX 
               PYI(A) = RYI(A) + DT * VYI(A) + DTSQ2 * AY 
               PZI(A) = RZI(A) + DT * VZI(A) + DTSQ2 * AZ 

               VXI(A) = VXI(A) + DT2 * AX
               VYI(A) = VYI(A) + DT2 * AY 
               VZI(A) = VZI(A) + DT2 * AZ 
            ENDDO

            MAX = 0.0     
            DO A = 1,NB
               B = A+1
               IF (A .EQ. NA) THEN
                  B = 1
               ENDIF
               RXAB(A) = RXI(A)-RXI(B)
               RYAB(A) = RYI(A)-RYI(B)
               RZAB(A) = RZI(A)-RZI(B)
               RAB2(A) = RXI(A)*RXI(A) + RYI(A)*RYI(A) + RZI(A)*RZI(A)
               RNCXAB(A) = PXI(A)-PXI(B)
               RNCYAB(A) = PYI(A)-PYI(B)
               RNCZAB(A) = PZI(A)-PZI(B)
               RNCAB2(A) = RNCXAB(A)*RNCXAB(A) + RNCYAB(A)*RNCYAB(A) + RNCZAB(A)*RNCZAB(A)
               ERROR = ABS( RNCAB2(A) - DSQ(I,A) ) /( 2.0*DSQ(I,A) )
               IF (ERROR .GT. MAX) THEN
                  MAX = ERROR
               ENDIF
            ENDDO
           
            CDL  = -2.0
            CD   =  2.0
            CDU  = -2.0          

            Di(1)  =  ( RXAB(1)*RNCXAB(1) + RYAB(1)*RNCYAB(1) + RZAB(1)*RNCZAB(1) )/( M(I,1)*M(I,1+1)/(M(I,1)+M(I,1+1)) )
            DUi(1) =  ( RXAB(2)*RNCXAB(1) + RYAB(2)*RNCYAB(1) + RZAB(2)*RNCZAB(1) )/M(I,1+1)
            DO A = 2,NB-1
               DLi(A-1) = ( RXAB(A-1)*RNCXAB(A) + RYAB(A-1)*RNCYAB(A) + RZAB(A-1)*RNCZAB(A) )/M(I,A)
               Di(A)    = ( RXAB(A)*RNCXAB(A) + RYAB(A)*RNCYAB(A) + RZAB(A)*RNCZAB(A)       )/( M(I,A)*M(I,A+1)/(M(I,A)+M(I,A+1)) )
               DUi(A)   = ( RXAB(A+1)*RNCXAB(A) + RYAB(A+1)*RNCYAB(A) + RZAB(A+1)*RNCZAB(A) )/M(I,A+1)
            ENDDO  
            IF (NB .EQ. NA) THEN               
               DLi(NB-1) = ( RXAB(NB-2)*RNCXAB(NB-1) + RYAB(NB-2)*RNCYAB(NB-1) + RZAB(NB-2)*RNCZAB(NB-1) )/M(I,NB)
               Di(NB)    = ( RXAB(NB)*RNCXAB(NB) + RYAB(NB)*RNCYAB(NB) + RZAB(NB)*RNCZAB(NB)             )/( M(I,NB)*M(I,1)/(M(I,NB)+M(I,1)) )
            ELSE
               DLi(NB-1) = ( RXAB(NB-2)*RNCXAB(NB-1) + RYAB(NB-2)*RNCYAB(NB-1) + RZAB(NB-2)*RNCZAB(NB-1) )/M(I,NB)
               Di(NB)    = ( RXAB(NB)*RNCXAB(NB) + RYAB(NB)*RNCYAB(NB) + RZAB(NB)*RNCZAB(NB)             )/( M(I,NB)*M(I,NB+1)/(M(I,NB)+M(I,NB+1)) )
            ENDIF

            DLi(:) = CDL*DLi(:)
            Di(:)  = CD*Di(:)  
            DUi(:) = CDU*DUi(:)

            RHS(:) = DSQ(I,:) - RNCAB2(:)
            RHSOLD(:) = RHS(:)

            DONE = .FALSE.

            IF (NB == NA) THEN

               beta  = CDU* ( RXAB(NA)*RNCXAB(1) + RYAB(NA)*RNCYAB(1) + RZAB(NA)*RNCZAB(1) )/M(I,1)    
               alpha = CDL* ( RXAB(1)*RNCXAB(NA) + RYAB(1)*RNCYAB(NA) + RZAB(1)*RNCZAB(NA) )/M(I,NA)  
            
1000           IF (.NOT. DONE) THEN

                  DL(:) = DLi(:)
                  DU(:) = DUi(:)                  
                  gamma = -Di(1) 
                  D(1) = Di(1) - gamma
                  D(NA) = Di(NA) - alpha*beta/gamma
                  DO A = 2,NA-1
                     D(A) = Di(A)
                  ENDDO
                  CALL DGTSV( NB, 1, DL, D, DU, RHS, NB, INFO)
                  DL(:) = DLi(:)
                  DU(:) = DUi(:)    
                  gamma = -Di(1) 
                  D(1) = Di(1) - gamma
                  D(NA) = Di(NA) - alpha*beta/gamma
                  DO A = 2,NA-1
                     D(A) = Di(A)
                  ENDDO
                  U(1) = gamma
                  U(NB) = alpha
                  DO A = 2,NB-1
                     U(A) = 0
                  ENDDO
                  CALL DGTSV( NB, 1, DL, D, DU, U, NB, INFO)
                  fact = (RHS(1) + beta*RHS(NA)/gamma)/(1.0+U(1)+beta*U(NA)/gamma)            
                  DO A = 1,NB
                     LAMBDA(A) = (RHS(A) - fact*U(A))
                  ENDDO

                  A = 1
                  NX(A)  = PXI(A) + LAMBDA(A)*RXAB(A)/M(I,A) - LAMBDA(NB)*RXAB(NB)/M(I,A)
                  NY(A)  = PYI(A) + LAMBDA(A)*RYAB(A)/M(I,A) - LAMBDA(NB)*RYAB(NB)/M(I,A)
                  NZ(A)  = PZI(A) + LAMBDA(A)*RZAB(A)/M(I,A) - LAMBDA(NB)*RZAB(NB)/M(I,A)   
                  DO A = 2,NA
                     NX(A)   = PXI(A) + LAMBDA(A)*RXAB(A)/M(I,A) - LAMBDA(A-1)*RXAB(A-1)/M(I,A)
                     NY(A)   = PYI(A) + LAMBDA(A)*RYAB(A)/M(I,A) - LAMBDA(A-1)*RYAB(A-1)/M(I,A)
                     NZ(A)   = PZI(A) + LAMBDA(A)*RZAB(A)/M(I,A) - LAMBDA(A-1)*RZAB(A-1)/M(I,A)                     
                  ENDDO

                  MAX = 0.0     
                  DO A = 1,NB
                     B = A+1
                     IF (A .EQ. NA) THEN
                        B = 1
                     ENDIF
                     NXAB(A) = NX(A)-NX(B)
                     NYAB(A) = NY(A)-NY(B)
                     NZAB(A) = NZ(A)-NZ(B)
                     NAB2(A) = NXAB(A)*NXAB(A) + NYAB(A)*NYAB(A) + NZAB(A)*NZAB(A)
                     ERROR = ABS( NAB2(A) - DSQ(I,A) ) /( 2.0*DSQ(I,A) )
                     IF (ERROR .GT. MAX) THEN
                        MAX = ERROR
                     ENDIF
                  ENDDO      

                  RHS(:) = DSQ(I,:) - NAB2(:) + RHSOLD(:)  
                  RHSOLD(:) = RHS(:)
                  
                  IF (MAX .le. TOL) THEN                                     
                     DONE = .TRUE.
                     VX(I,1) = VXI(1) + LAMBDA(1)*RXAB(1)/DT/M(I,1) - LAMBDA(NA)*RXAB(NA)/DT/M(I,1)
                     VY(I,1) = VYI(1) + LAMBDA(1)*RYAB(1)/DT/M(I,1) - LAMBDA(NA)*RYAB(NA)/DT/M(I,1)
                     VZ(I,1) = VZI(1) + LAMBDA(1)*RZAB(1)/DT/M(I,1) - LAMBDA(NA)*RZAB(NA)/DT/M(I,1)
                     DO A = 2,NA
                        VX(I,A) = VXI(A) + LAMBDA(A)*RXAB(A)/DT/M(I,A) - LAMBDA(A-1)*RXAB(A-1)/DT/M(I,A)
                        VY(I,A) = VYI(A) + LAMBDA(A)*RYAB(A)/DT/M(I,A) - LAMBDA(A-1)*RYAB(A-1)/DT/M(I,A)
                        VZ(I,A) = VZI(A) + LAMBDA(A)*RZAB(A)/DT/M(I,A) - LAMBDA(A-1)*RZAB(A-1)/DT/M(I,A)
                     ENDDO
                     RX(I,:) = NX(:)
                     RY(I,:) = NY(:)
                     RZ(I,:) = NZ(:)                         
                  ELSEIF (IT .GT. MAXIT) THEN                  
                     WRITE(*,'(''MAXIMUM NUMBER OF ITERATIONS EXCEEDED.'')') 
                  ELSE
                     IT = IT + 1
                     GOTO 1000                     
                  ENDIF
               ENDIF

            ELSEIF (NB == NA-1) THEN

2000           IF (.NOT. DONE) THEN
                  
               DL(:) = DLi(:)
               D(:) = Di(:)
               DU(:) = DUi(:)    
               
               CALL DGTSV( NB, 1, DL, D, DU, rhs, NB, INFO )              
               LAMBDA(:) = RHS(:)
                
               NX(1)  = PXI(1) + LAMBDA(1)*RXAB(1)/M(I,1)
               NY(1)  = PYI(1) + LAMBDA(1)*RYAB(1)/M(I,1)
               NZ(1)  = PZI(1) + LAMBDA(1)*RZAB(1)/M(I,1)            
               DO A = 2,NA-1
                  NX(A)  = PXI(A) - LAMBDA(A-1)*RXAB(A-1)/M(I,A) + LAMBDA(A)*RXAB(A)/M(I,A)
                  NY(A)  = PYI(A) - LAMBDA(A-1)*RYAB(A-1)/M(I,A) + LAMBDA(A)*RYAB(A)/M(I,A)
                  NZ(A)  = PZI(A) - LAMBDA(A-1)*RZAB(A-1)/M(I,A) + LAMBDA(A)*RZAB(A)/M(I,A)
               ENDDO
               NX(NA)  = PXI(NA) - LAMBDA(NA-1)*RXAB(NA-1)/M(I,NA)
               NY(NA)  = PYI(NA) - LAMBDA(NA-1)*RYAB(NA-1)/M(I,NA)
               NZ(NA)  = PZI(NA) - LAMBDA(NA-1)*RZAB(NA-1)/M(I,NA)               

               MAX = 0.0     
               DO A = 1,NB
                  B = A+1
                  NXAB(A) = NX(A)-NX(B)
                  NYAB(A) = NY(A)-NY(B)
                  NZAB(A) = NZ(A)-NZ(B)
                  NAB2(A) = NXAB(A)*NXAB(A) + NYAB(A)*NYAB(A) + NZAB(A)*NZAB(A)
                  ERROR = ABS( NAB2(A) - DSQ(I,A) ) /( 2.0*DSQ(I,A) )
                  IF (ERROR .GT. MAX) THEN
                     MAX = ERROR
                  ENDIF
               ENDDO
                     
               RHS(:) = DSQ(I,:) - NAB2(:) + RHSOLD(:)  
               RHSOLD(:) = RHS(:)               
 
               IF (MAX .LE. TOL) THEN                                  
                  DONE = .TRUE.                 
                  VX(I,1) = VXI(1) + LAMBDA(1)*RXAB(1)/DT/M(I,1)
                  VY(I,1) = VYI(1) + LAMBDA(1)*RYAB(1)/DT/M(I,1)
                  VZ(I,1) = VZI(1) + LAMBDA(1)*RZAB(1)/DT/M(I,1)
                  DO A = 2,NA-1
                     VX(I,A) = VXI(A) + LAMBDA(A)*RXAB(A)/DT/M(I,A) - LAMBDA(A-1)*RXAB(A-1)/DT/M(I,A)
                     VY(I,A) = VYI(A) + LAMBDA(A)*RYAB(A)/DT/M(I,A) - LAMBDA(A-1)*RYAB(A-1)/DT/M(I,A)
                     VZ(I,A) = VZI(A) + LAMBDA(A)*RZAB(A)/DT/M(I,A) - LAMBDA(A-1)*RZAB(A-1)/DT/M(I,A)
                  ENDDO
                  VX(I,NA) = VXI(NA) - LAMBDA(NA-1)*RXAB(NA-1)/DT/M(I,NA)
                  VY(I,NA) = VYI(NA) - LAMBDA(NA-1)*RYAB(NA-1)/DT/M(I,NA)
                  VZ(I,NA) = VZI(NA) - LAMBDA(NA-1)*RZAB(NA-1)/DT/M(I,NA)                                    
                  RX(I,:) = NX(:)
                  RY(I,:) = NY(:)
                  RZ(I,:) = NZ(:)                  
               ELSEIF (IT .GT. MAXIT) THEN                  
                  WRITE(*,'(''MAXIMUM NUMBER OF ITERATIONS EXCEEDED.'')') 
               ELSE                  
                  IT = IT + 1
                  GOTO 2000
               ENDIF
            ENDIF
            
         ENDIF

      ENDDO
      

    END SUBROUTINE MILCSHAKEa





       SUBROUTINE MILCSHAKEb ( RX, RY, RZ, VX, VY, VZ, FX, FY, FZ, ENERGY, DT, M,  NA, NB, NM )

         INTEGER     NM, NA, NB
         REAL        DT, M(NM,NA), ENERGY
         REAL        RX(NM,NA), RY(NM,NA), RZ(NM,NA)   
         REAL        VX(NM,NA), VY(NM,NA), VZ(NM,NA) 
         REAL        FX(NM,NA), FY(NM,NA), FZ(NM,NA)     

         REAL        AX, AY, AZ            
         REAL        VXI(NA), VYI(NA), VZI(NA)
         REAL        RXAB(NB), RYAB(NB), RZAB(NB) 
         REAL        VXAB(NB), VYAB(NB), VZAB(NB) 

         LOGICAL     DONE, INFO
         REAL, DIMENSION(NB-1) :: DL, DU, DLi, DUi
         REAL, DIMENSION(NB)   :: D, Di
         REAL, DIMENSION(NB)   :: LAMBDA, RHS
         REAL        DT2
         INTEGER     i, A, B

         REAL alpha, beta, gamma, fact, MAX, ERROR, timeBegin, timeEnd
         REAL, DIMENSION(NB) :: U

      

         DT2 = DT/2.0     

         DO I = 1,NM

            DO A = 1, NA                       
               AX = FX(I,A) / M(I,A)
               AY = FY(I,A) / M(I,A)
               AZ = FZ(I,A) / M(I,A)               

               VXI(A) = VX(I,A) + DT2 * AX
               VYI(A) = VY(I,A) + DT2 * AY
               VZI(A) = VZ(I,A) + DT2 * AZ
            ENDDO         

            DO A = 1, NB
               B = A+1
               IF (B .GT. NA) THEN
                  B = 1
               ENDIF
               VXAB(A) = VXI(A) - VXI(B)  
               VYAB(A) = VYI(A) - VYI(B)
               VZAB(A) = VZI(A) - VZI(B)
               RXAB(A) = RX(I,A) - RX(I,B)   
               RYAB(A) = RY(I,A) - RY(I,B)
               RZAB(A) = RZ(I,A) - RZ(I,B)
               RHS(A) = - (  VXAB(A)*RXAB(A) + VYAB(A)*RYAB(A) + VZAB(A)*RZAB(A)  )
            ENDDO
 
 
            D(1) =     (RXAB(1)*RXAB(1) + RYAB(1)*RYAB(1) + RZAB(1)*RZAB(1)) /( M(I,1)*M(I,2)/(M(I,1)+M(I,2)) )
            DU(1)= -   (RXAB(2)*RXAB(1) + RYAB(2)*RYAB(1) + RZAB(2)*RZAB(1)) /  M(I,2)
            DO A = 2,NB-1
               DL(A-1) = -   (RXAB(A-1)*RXAB(A) + RYAB(A-1)*RYAB(A) + RZAB(A-1)*RZAB(A)) /  M(I,A)
               D(A)    =     (RXAB(A)*RXAB(A)   + RYAB(A)*RYAB(A)   + RZAB(A)*RZAB(A))   /( M(I,A)*M(I,A+1)/(M(I,A)+M(I,A+1)) )
               DU(A)   = -   (RXAB(A+1)*RXAB(A) + RYAB(A+1)*RYAB(A) + RZAB(A+1)*RZAB(A)) /  M(I,A+1)
            ENDDO
            IF (NB == NA) THEN            
               DL(NB-1) = -   (RXAB(NB-1)*RXAB(NB) + RYAB(NB-1)*RYAB(NB) + RZAB(NB-1)*RZAB(NB)) /  M(I,NB)
               D(NB)    =     (RXAB(NB)*RXAB(NB)   + RYAB(NB)*RYAB(NB)   + RZAB(NB)*RZAB(NB))   /( M(I,NB)*M(I,1)/(M(I,NB)+M(I,1)) )
            ELSE
               DL(NB-1) = -   (RXAB(NB-1)*RXAB(NB) + RYAB(NB-1)*RYAB(NB) + RZAB(NB-1)*RZAB(NB)) /  M(I,NB)
               D(NB)    =     (RXAB(NB)*RXAB(NB)   + RYAB(NB)*RYAB(NB)   + RZAB(NB)*RZAB(NB))   /( M(I,NB)*M(I,NB+1)/(M(I,NB)+M(I,NB+1)) )
            ENDIF
  
            DUi(:) = DU(:)
            Di(:)  = D(:)
            DLi(:)  = DL(:)

            IF (NB == NA) THEN        
            

               alpha =  - (RXAB(1)*RXAB(NB) + RYAB(1)*RYAB(NB) + RZAB(1)*RZAB(NB)) /  M(I,1) 
               beta  =  - (RXAB(NB)*RXAB(1) + RYAB(NB)*RYAB(1) + RZAB(NB)*RZAB(1)) /  M(I,NB)
              
               gamma = -Di(1) 
               D(1) = Di(1) - gamma
               D(NB) = Di(NB) - alpha*beta/gamma
               DO A = 2,NB-1
                  D(A) = Di(A)
               ENDDO
               CALL DGTSV( NB, 1, DL, D, DU, RHS, NB, INFO)
               DL(:) = DLi(:)
               DU(:) = DUi(:)    
               gamma = -Di(1) 
               D(1) = Di(1) - gamma
               D(NB) = Di(NB) - alpha*beta/gamma
               DO A = 2,NB-1
                  D(A) = Di(A)
               ENDDO        
               U(1) = gamma
               U(NB) = alpha
               DO A = 2,NB-1
                  U(A) = 0
               ENDDO
               CALL DGTSV( NB, 1, DL, D, DU, U, NB, INFO)
               fact = (RHS(1) + beta*RHS(NB)/gamma)/(1.0+u(1)+beta*U(NB)/gamma)            
               DO A = 1,NB
                  LAMBDA(A) = (RHS(A) - fact*U(A))
               ENDDO

               VX(I,1) = VXI(1) + ( LAMBDA(1)*RXAB(1) - LAMBDA(NA)*RXAB(NA) )/M(I,1)
               VY(I,1) = VYI(1) + ( LAMBDA(1)*RYAB(1) - LAMBDA(NA)*RYAB(NA) )/M(I,1)
               VZ(I,1) = VZI(1) + ( LAMBDA(1)*RZAB(1) - LAMBDA(NA)*RZAB(NA) )/M(I,1)
               ENERGY = M(I,1)*( VX(I,1)*VX(I,1) + VY(I,1)*VY(I,1) + VZ(I,1)*VZ(I,1) )
               DO A = 2,NB
                  VX(I,A) = VXI(A) + ( LAMBDA(A)*RXAB(A) - LAMBDA(A-1)*RXAB(A-1) )/M(I,A) 
                  VY(I,A) = VYI(A) + ( LAMBDA(A)*RYAB(A) - LAMBDA(A-1)*RYAB(A-1) )/M(I,A)
                  VZ(I,A) = VZI(A) + ( LAMBDA(A)*RZAB(A) - LAMBDA(A-1)*RZAB(A-1) )/M(I,A)
                  ENERGY = ENERGY +  M(I,A)*( VX(I,A)*VX(I,A) + VY(I,A)*VY(I,A) + VZ(I,A)*VZ(I,A) )
               ENDDO
               ENERGY = 0.5*ENERGY


            ELSE

               CALL DGTSV( NB, 1, DL, D, DU, RHS, NB, INFO ) 
               LAMBDA(:) = RHS(:)
           
               VX(I,1) = VXI(1) + LAMBDA(1)*RXAB(1)/M(I,1)
               VY(I,1) = VYI(1) + LAMBDA(1)*RYAB(1)/M(I,1) 
               VZ(I,1) = VZI(1) + LAMBDA(1)*RZAB(1)/M(I,1) 
               ENERGY = M(I,1)*( VX(I,1)*VX(I,1) + VY(I,1)*VY(I,1) + VZ(I,1)*VZ(I,1) )
               DO A = 2,NA-1
                  VX(I,A) = VXI(A) + ( LAMBDA(A)*RXAB(A) - LAMBDA(A-1)*RXAB(A-1) )/M(I,A) 
                  VY(I,A) = VYI(A) + ( LAMBDA(A)*RYAB(A) - LAMBDA(A-1)*RYAB(A-1) )/M(I,A) 
                  VZ(I,A) = VZI(A) + ( LAMBDA(A)*RZAB(A) - LAMBDA(A-1)*RZAB(A-1) )/M(I,A) 
                  ENERGY = ENERGY +  M(I,A)*( VX(I,A)*VX(I,A) + VY(I,A)*VY(I,A) + VZ(I,A)*VZ(I,A) )
               ENDDO
               VX(I,NA) = VXI(NA) - LAMBDA(NA-1)*RXAB(NA-1)/M(I,NA)
               VY(I,NA) = VYI(NA) - LAMBDA(NA-1)*RYAB(NA-1)/M(I,NA)
               VZ(I,NA) = VZI(NA) - LAMBDA(NA-1)*RZAB(NA-1)/M(I,NA)         
               ENERGY = ENERGY + M(I,NA)*( VX(I,NA)*VX(I,NA) + VY(I,NA)*VY(I,NA) + VZ(I,NA)*VZ(I,NA) )
               ENERGY = 0.5*ENERGY

            ENDIF
            

         ENDDO

       END SUBROUTINE MILCSHAKEb



