program SimplePend
*     Program to integrate the single pendulum equations
*     Ross Bannister, June/August 2001

*     implicit none
      integer Nt,t,Nout
      real l,g,dt,y(2),pi

*     Parameters (SI units)
      g=10.
      dt=0.002
      pi=4.*atan(1.)
*     Number of time steps
      Nt=10000
*     Output every how many timesteps?
      Nout=15
      print*,'Please enter length l'
      read*,l

*     Initial conditions
*     Angle
      y(1)=120.*pi/180.
      print*,'Please enter initial r.o.c. angle'
      print*,'(degrees per second)'
      read*,y(2)
*     Convert to radians
      y(2)=y(2)*pi/180.


      open (12,file='Spend')
      write (12,100)'# Output from simple pendulum'
      write (12,102)'# Length : ',l

      do t=1,Nt
        print*,t,Nt
        call RungeKutt4 (y,l,g,dt,pi)
        if (mod(t,Nout).eq.0)
     :    write (12,101) real(t)*dt,180.*y(1)/pi, 180.*y(2)/pi
      enddo

      close (12)
100   format ((A))
101   format (5F12.2)
102   format ((A),3F8.4)
      end



      subroutine operate (f,dt,y,kk)
*     To perform operation k=Ty+g where T(A,Binv) and g(f)
*     (see notes)
*     implicit none
      integer i,j,k
      real f,dt,y(2),kk(2)

      kk(1)=dt*y(2)
      kk(2)=dt*f

      return
      end


      subroutine sum (a,b,c,size,flag)
*     Vector sum c=a+b   (flag=1)
*     Vector sum c=a+b/2 (flag=2)
*     implicit none
      integer size,i,flag
      real a(size),b(size),c(size)
      if (flag.eq.1) then
        do i=1,size
          c(i)=a(i)+b(i)
        enddo
        else
        do i=1,size
          c(i)=a(i)+b(i)/2.
        enddo
      endif
      return
      end


      subroutine RungeKutt4 (y,l,g,dt,pi)
*     Fourth order Runge-Kutta for double pendulum
*     implicit none
      integer i
      real y(2),l,g,dt,pi
      real k1(2),k2(2),k3(2),k4(2),f,ytemp(2),mymod

*     Stage 1
*     -------
      f=-1.*g*sin(y(1))/l
      call operate (f,dt,y,k1)

*     Stage 2
*     -------
      call sum (y,k1,ytemp,2,2)
      f=-1.*g*sin(ytemp(1))/l
      call operate (f,dt,ytemp,k2)

*     Stage 3
*     -------
      call sum (y,k2,ytemp,2,2)
      f=-1.*g*sin(ytemp(1))/l
      call operate (f,dt,ytemp,k3)

*     Stage 4
*     -------
      call sum (y,k3,ytemp,2,1)
      f=-1.*g*sin(ytemp(1))/l
      call operate (f,dt,ytemp,k4)

*     Add-up for incremented y-vector
      do i=1,2
        y(i)=y(i) + k1(i)/6. + k2(i)/3. + k3(i)/3. + k4(i)/6.
      enddo

*     Put angle back into range (-pi to pi)
      y(1)=mymod(y(1),2.*pi)
      if (y(1).gt.pi) y(1)=y(1)-2.*pi

      return
      end


      real function mymod (x,interval)
*     This function puts x into the interval 0<=x<interval
*     implicit none
      real x,interval,sgn
      sgn=sign(1.0,x)
      x=abs(x)
      mymod=sgn*(x-interval*real(int(x/interval)))
      return
      end