      program MDHarmMult
      implicit none
      include 'MD.cm'

      integer istep,nstep,ndump

      real*8 r,p,xt,vt
      real*8 r1,v1,om
      real*8 avgr, avgv
      real*8 epot, ekin, etot
      real*8 dt, time, derpot

      open(170,file='EnergyMult',status='unknown')
      open(171,file='TrajMult',status='unknown')
      open(172,file='AveragesMult',status='unknown')
      open(273,file='ETrajMult',status='unknown')

      mass=1.
      omega1=1.
      omega2=7.

      dt=0.05
      nstep=10000
      ndump=100

      om=omega2*dt

      avgr=0.d0
      avgv=0.d0

      call SetInCon(r,p)
        ekin=0.5*p**2/mass
        epot=0.5*mass*(omega1**2+omega2**2)*r**2
        etot=ekin+epot
        write(170,555) 0.,epot,ekin,etot
        write(171,555) 0.,r,p

!====================================================
! Use Modified Algorithm
!====================================================
      call GetForce(r,derpot)

      avgr=avgr+r
      avgv=avgv+p

      do istep=1,nstep

        p=p-0.5*dt*derpot
        r1=r*cos(om)+p*sin(om)/(mass*omega2)
        v1=p*cos(om)-mass*omega2*r*sin(om)
        r=r1
        p=v1
        call GetForce(r,derpot)
        p=p-0.5*dt*derpot/mass

        avgr=avgr+r
        avgv=avgv+p

        if(mod(istep,ndump).eq.1) then
          ekin=0.5*p**2/mass
          epot=0.5*mass*(omega1**2+omega2**2)*r**2
          etot=ekin+epot
          write(170,555) istep*dt,epot,ekin,etot
          write(171,555) istep*dt,r,p
          write(172,555) istep*dt,avgr/real(istep),avgv/real(istep)
        end if
      enddo

555   format(7E20.10)

      close(170)
      close(171)
      close(172)
      close(273)

      end program MDHarmMult
!==================================================================
      subroutine SetInCon(r0,v0)
!~~~~~~~~~~~~~~~~~~~~~~~~~~
! Set initial conditions
!~~~~~~~~~~~~~~~~~~~~~~~~~~
      implicit none
      real*8 r0,v0

        r0=0.
        v0=1.

      return
      end subroutine SetInCon

!=================================================================
      subroutine GetForce(x,derp)
      implicit none
      include 'MD.cm'

      real*8 x,derp

      derp=x*mass*omega1**2

      return
      end subroutine GetForce