      program pendel2r
! Double/Triple pendulum, using RATTLE:
      implicit double precision (a-h,o-z)
      common /par/pi,gg,dt,dth,xl,xlsq,omg,ek,ep,et,et0,tolsig,nr,nm
     1 ,nr1,ncr1,nr2,ncr2,it
      common /vct/x(4),y(4),xt(4),yt(4),vx(4),vy(4),vxt(4),vyt(4)
     1 ,qx(4),qy(4),xm(4),phi(3)
!
      pi=4.d0*datan(1.d0);
      nt=1000;
      dt=0.001d0; dth=0.5d0*dt; dtsq=dt*dt; dtsh=0.5d0*dtsq;
      nr=80;      ! max no of RATTLE iterations
      gg=-9.81d0;
      nm=3;      ! no of pendulum masses, *including pivot*
      phi(1)=0.5d0; phi(2)=phi(1); phi(3)=phi(2); ! init. angles at joints
      xl=1.d0; xlsq=xl*xl;  ! bond length
      omg=0.d0;
      tolsig=1.d-12;
!
      call setup
!
! nt loop:
      nr1=0; ncr1=0;
      nr2=0; ncr2=0;
      do 1 it=1,nt
!       Andersen's RATTLE:
        call rat;  ! RATTLE part1
!   Now the qx_y and x_y at t+dt are known, 
!   and the velocities will be repaired:
        do j=2,nm
          x(j)=xt(j);    y(j)=yt(j);
          vx(j)=qx(j);  vy(j)=qy(j)+dth*gg;
        enddo
        call energy;
        call tle;  ! RATTLE part2
!  
        call energy; 
        ndt=max(1,nt/20);
        if((it/ndt)*ndt.eq.it) then
          write(*,100) it,x(2),y(2),ek,ep,et
100       format(1x,'it,xy(2),ekpt= ',i6,6e14.5)
        endif
!
1     continue   ! end time step loop
!
      write(*,99) it,dfloat(nr1/ncr1),dfloat(nr2/ncr2)
99    format(1x,'it,rtle statistics: ',i8,2f10.5)
!
      stop
      end
!
!---------------------------------------
      subroutine setup
      implicit double precision (a-h,o-z)
      common /par/pi,gg,dt,dth,xl,xlsq,omg,ek,ep,et,et0,tolsig,nr,nm
     1 ,nr1,ncr1,nr2,ncr2,it
      common /vct/x(4),y(4),xt(4),yt(4),vx(4),vy(4),vxt(4),vyt(4)
     1 ,qx(4),qy(4),xm(4),phi(3)
!
      xm(1)=1.d0; phi(1)=phi(1)*pi;
      x(1)=0.d0;   y(1)=0.d0;
      vx(1)=0.d0; vy(1)=0.d0;
      ek=0.d0;
      do j=2,nm
        xm(j)=1.d0; ! masses
        if(j.lt.nm) phi(j)=phi(j)*pi;
        xj=x(j-1)+xl*dsin(phi(j-1)); 
        yj=y(j-1)-xl*dcos(phi(j-1)); 
        x(j)=xj; y(j)=yj;
        rr=dsqrt(xj*xj+yj*yj);
        vx(j)=yj*omg; vy(j)=-xj*omg;
        dek=0.5d0*xm(j)*(vx(j)*vx(j)+vy(j)*vy(j)); 
        ek=ek+dek; 
        ep=ep-xm(j)*gg*y(j);
      enddo
      et=ep+ek; et0=et;
!
      return
      end
!
!-------------------------------------------------
!
      subroutine rat
! First phase of RATTLE
      implicit double precision (a-h,o-z)
      common /par/pi,gg,dt,dth,xl,xlsq,omg,ek,ep,et,et0,tolsig,nr,nm
     1 ,nr1,ncr1,nr2,ncr2,it
      common /vct/x(4),y(4),xt(4),yt(4),vx(4),vy(4),vxt(4),vyt(4)
     1 ,qx(4),qy(4),xm(4),phi(3)
      logical lbreak;
!
      do j=1,nm
        qx(j)=vx(j); qy(j)=vy(j)+gg*dth;
      enddo
!
      do ir=1,nr   ! Loop over RATTLE-1 iterations
        lbreak=.true.;
        x(1)=0.; y(1)=0.;   ! 1st constraint fulfilled
        do j=2,nm-1
          do k=j-1,j+1,2
            xmu=xm(j)*xm(k)/(xm(j)+xm(k));
            xjk=x(j)-x(k); yjk=y(j)-y(k);          
            sx=xjk+dt*(qx(j)-qx(k));
            sy=yjk+dt*(qy(j)-qy(k));
            sig=sx*sx+sy*sy-xlsq;
            if(dabs(sig).gt.tolsig) lbreak=.false.;
            sr=sx*xjk+sy*yjk;
            cg=xmu*sig/sr/dt/2.d0;
            qx(j)=qx(j)-cg*xjk/xm(j);
            qy(j)=qy(j)-cg*yjk/xm(j);
            xt(j)=x(j)+dt*qx(j);
            yt(j)=y(j)+dt*qy(j);
            if(k.gt.1.and.k.le.nm) then  
              qx(k)=qx(k)+cg*xjk/xm(k);   
              qy(k)=qy(k)+cg*yjk/xm(k);
              xt(k)=x(k)+dt*qx(k);
              yt(k)=y(k)+dt*qy(k);
            endif
          enddo  ! k loop
        enddo  ! j loop
        if(lbreak) exit;
      enddo
      nr1=nr1+ir; ncr1=ncr1+1;
!
      return
      end
!
!-------------------------------------------------
      subroutine tle
! Second phase of RATTLE
      implicit double precision (a-h,o-z)
      common /par/pi,gg,dt,dth,xl,xlsq,omg,ek,ep,et,et0,tolsig,nr,nm
     1 ,nr1,ncr1,nr2,ncr2,it
      common /vct/x(4),y(4),xt(4),yt(4),vx(4),vy(4),vxt(4),vyt(4)
     1 ,qx(4),qy(4),xm(4),phi(3)
      logical lbreak;
!
      do ir=1,nr   ! Loop over RATTLE-2 iterations
        lbreak=.true.;
        do j=2,nm-1
          do k=j-1,j+1,2
            xmu=xm(j)*xm(k)/(xm(j)+xm(k));
            xjk=x(j)-x(k); yjk=y(j)-y(k);        
            vxjk=vx(j)-vx(k); vyjk=vy(j)-vy(k); 
            xvjk= xjk*vxjk+yjk*vyjk;
            if(dabs(xvjk).gt.tolsig/dt) lbreak=.false.
            ck=xmu*xvjk/xlsq;
            vx(j)=vx(j)-ck*xjk/xm(j);
            vy(j)=vy(j)-ck*yjk/xm(j);
            if(k.gt.1.and.k.le.nm) then
              vx(k)=vx(k)+ck*xjk/xm(k);
              vy(k)=vy(k)+ck*yjk/xm(k);
            endif
          enddo
        enddo
        if(lbreak) exit;
      enddo
      nr2=nr2+ir; ncr2=ncr2+1;
!
      return
      end
!
!-------------------------------------------------!---------------------------------------
      subroutine energy
      implicit double precision (a-h,o-z)
      common /par/pi,gg,dt,dth,xl,xlsq,omg,ek,ep,et,et0,tolsig,nr,nm
     1 ,nr1,ncr1,nr2,ncr2,it
      common /vct/x(4),y(4),xt(4),yt(4),vx(4),vy(4),vxt(4),vyt(4)
     1 ,qx(4),qy(4),xm(4),phi(3)
!
      ek=0.; ep=0.;
      do j=1,nm
        dek=0.5d0*xm(j)*(vx(j)*vx(j)+vy(j)*vy(j)); 
        ek=ek+dek; 
        ep=ep-xm(j)*gg*y(j); et=ek+ep;
      enddo
!
      return
      end
!
!-------------------------------------------------
