      program pendel2s
! Double Pendulum using SHAKE 
      implicit double precision (a-h,o-z)
      common /par/gg,dt,xl,xlsq,sig,stol,xm,ns,nm
      common /vec/x(3),y(3),xo(3),yo(3),xn(3),yn(3),vx(3),vy(3)
!
      nt=1000; nm=3;
      dt=0.001d0; dtsq=dt*dt; dtsh=0.5d0*dtsq;
      stol=1.d-12;
      ns=80;      ! no of SHAKE iterations
      xm=1.;
      gg=-9.81d0;     
      xl=1.; xlsq=xl*xl;   ! bond length (squared)
!
      call setup;
      call shake;
! for zero initial velocities, r(-1) = r(1):
      do im=2,nm
        xo(im)=xn(im); yo(im)=yn(im);
      enddo
      call energy(ek,ep,et);
!
      write(*,100) it,ek,ep,et,sig;
100   format(1x,'it,xy2,ekpt,sig = ',i6,6e14.5)
!
      do 1 it=1,nt 
!  Verlet move:
        do im=2,nm
          xn(im)=2.d0*x(im)-xo(im);
          yn(im)=2.d0*y(im)-yo(im)+gg*dtsq;
        enddo
!  Improve xn,yn by SHAKE:
        call shake;
        call energy(ek,ep,et);
!  Check:
        ndt=max(1,nt/20);  
        if((it/ndt)*ndt.eq.it) then
          write(*,100) it,x(2),y(2),ek,ep,et,sig
        endif
!
1     continue   ! end time step loop
!
      stop
      end
!
!---------------------------------------
      subroutine setup
      implicit double precision (a-h,o-z)
      common /par/gg,dt,xl,xlsq,sig,stol,xm,ns,nm
      common /vec/x(3),y(3),xo(3),yo(3),xn(3),yn(3),vx(3),vy(3)
!
! For later extensions, r(1) is treated as a bead (but kept stationary)
      x(1)=0.d0; y(1)=0.d0;
      xn(1)=0.d0; yn(1)=0.d0;
      xo(1)=0.d0; yo(1)=0.d0;
      do im=2,nm
        x(im)=x(im-1)+xl; y(im)=0.d0;
        xn(im)=x(im); yn(im)=y(im)+gg*dtsh;
      enddo
!
      return
      end
!
!---------------------------------------
      subroutine shake
      implicit double precision (a-h,o-z)
      common /par/gg,dt,xl,xlsq,sig,stol,xm,ns,nm
      common /vec/x(3),y(3),xo(3),yo(3),xn(3),yn(3),vx(3),vy(3)
!
      do is=1,ns   ! Loop over SHAKE iterations
        isw=1   ! =1 if exit loop
        j=2;      
        do k=j-1,j+1,2  ! go through all constraints of particle 2 ???? ****
          xmu=xm/2.d0;
          xjk= x(j) -x(k);   yjk= y(j)-y(k);
          xnjk=xn(j)-xn(k);  ynjk=yn(j)-yn(k); 
          sr=xnjk*xjk+ynjk*yjk;
          sig=xnjk*xnjk+ynjk*ynjk-xlsq;
          if(dabs(sig).gt.stol) isw=0;
          cg=sig/sr/2.d0;
          xn(j)  =xn(j)  -cg*xjk;
          yn(j)  =yn(j)  -cg*yjk;
          if(k.gt.1.and.k.le.nm) then
            xn(k)  =xn(k)  +cg*xjk;
            yn(k)  =yn(k)  +cg*yjk;
          endif
 !         print *,sig,is,k,isw,ns
        enddo     ! end k loop
        if(isw.eq.1) exit;
      enddo       ! end SHAKE loop 
!
      return
      end   
!
!-----------------------------------------
      subroutine energy(ek,ep,et)
      implicit double precision (a-h,o-z)
      common /par/gg,dt,xl,xlsq,sig,stol,xm,ns,nm
      common /vec/x(3),y(3),xo(3),yo(3),xn(3),yn(3),vx(3),vy(3)
!
      ek=0.d0; ep=0.d0;
      do im=2,nm
        vx(im)=0.5d0*(xn(im)-xo(im))/dt;
        vy(im)=0.5d0*(yn(im)-yo(im))/dt; 
        ek=ek+0.5d0*xm*(vx(im)*vx(im)+vy(im)*vy(im)); ! ek at time t
        ep=ep-xm*gg*y(im);                            ! ep at time t
        xo(im)=x(im); yo(im)=y(im);
        x(im)=xn(im); y(im)=yn(im);
      enddo
      et=ek+ep;
!
      return
      end 
!------------------------------------------------------
