module prec
   integer, parameter  :: dp = 16  ! by default quadruple precision
end module

module henon
   use prec
   character (len=*), parameter :: prob = "Henon-Heiles"
   integer, parameter  :: neqn = 4
   real(dp)            :: h = 0.01
   real(dp), parameter :: y0(neqn) = [sqrt(69._dp/500._dp), 0.2_dp, 0._dp, 0.3_dp]
   real(dp)            :: xend = 10.0_dp
   real(dp), parameter :: yref(neqn) = [ &
     & -0.40375699868280302602426982893256100678057940582957_dp , &
     & -0.1028198343316199700403780609395733843767328207596_dp ,  & 
     &  0.0093107903962697935789071025112142580722612949250_dp  , & 
     & -0.2554474874437120417906055767444824478015369886744_dp ]
   
   contains 

   function f(y) result (dy)
   real(dp) :: y(:), dy(size(y))
      dy = [ -(y(3) + 2*y(3)*y(4)),         &
           & -(y(3)**2 + y(4) - y(4)**2), &
           &   y(1), y(2) ]
   end function

   function jac(y) result (Jy) 
      real(dp) :: y(:), Jy(size(y), size(y))
      Jy(1,:) = [0.0_dp, 0.0_dp, -(1.0_dp+2._dp*y(4)), -2.0_dp*y(3)]
      Jy(2,:) = [0.0_dp, 0.0_dp, -2*y(3), -1._dp+2*y(4)]
      Jy(3,:) = [1.0_dp, 0.0_dp, 0.0_dp, 0.0_dp] 
      Jy(4,:) = [0.0_dp, 1.0_dp, 0.0_dp, 0.0_dp]

   end function

   function hamiltonian(y) result (energy)
      real(dp) :: y(:), energy
      energy =  (y(1)**2 + y(2)**2)/2._dp + &
             &  (y(3)**2 + y(4)**2)/2._dp + &
             &   y(3)**2*y(4) -  y(4)**3/3._dp
   end function

end module

module pendulum
   use prec
   implicit none
   character (len=*), parameter :: prob = "Pendulum"
   integer,  parameter  :: neqn = 2
   real(dp)             :: h = 0.01_dp
   real(dp), parameter  :: y0(neqn) = [3.14_dp, 0.0_dp]
   real(dp)             :: xend = 500.0_dp

!y0 = 3.14`50
!xend = 500
!tp = 2 Pi/ArithmeticGeometricMean[1, Cos[y0/2]]
!xend1 = Mod[xend, tp]
!sol = NDSolve[{y''[t] == -Sin[y[t]], y[0] == y0, y'[0] == 0}, {y}, {t,
!     0, xend1}, WorkingPrecision -> 50];
!{y[xend1], y'[xend1]} /. sol

   real(dp), parameter :: yref(neqn) = [ &
         &   -2.895231800487194246616970723492358331121464600_dp, &
             0.245733141797971843952941503115538448299432145_dp  ]

   contains

   function period() result (tp)
      real(dp) :: x(2), tp 
      x = [1.0_dp, cos(y0(1)/2)]
      do while (x(1) /= x(2))
         x = [(x(1)+x(2))/2, sqrt(x(1)*x(2))]
      end do
      tp = 2*4*atan(1.0_dp)/x(1)
   end function

   function f(y) result (dy)
      real(dp) :: y(:), dy(size(y))
      dy = [y(2), -sin(y(1))]
   end function

   function jac(y) result (Jy) 
      real(dp) :: y(:), Jy(size(y), size(y))
      Jy(1,:) = [0.0_dp, 1.0_dp]
      Jy(2,:) = [-cos(y(1)),0.0_dp]
   end function

   function hamiltonian(y) result (energy)
      real(dp) :: y(:), energy
      energy = y(2)**2/2.0_dp-cos(y(1))
   end function
end module

module beadonwire
   use prec
   implicit none
   character (len=*), parameter :: prob = "Bead on wire"
   integer,  parameter  :: neqn = 2
   real(dp)             :: h = 0.1
   real(dp), parameter  :: y0(neqn) = [0.49_dp, 0.0_dp]
   real(dp)             :: xend = 1000.0_dp

   real(dp), parameter :: yref(neqn) = [ &
         &   0.350213013591668350450482581191204431049689426982_dp, &
             1.91276852118768254731026725427295520404484299441_dp  ]
   contains
   function f(y) result (dy)
      real(dp) :: y(:), dy(size(y)), pd, qd, p, q
      p = y(1)
      q = y(2)
      pd = (q*(-25._dp*(-2._dp + q)**2 - 3._dp*q - 25._dp*(-2._dp + q)*q + & 
    & (3906250._dp*p**2*(50._dp + 3._dp*q*(-49._dp + 25._dp*q))* &
    & (100._dp + q*(-147._dp + 50._dp*q)))/(15625._dp + q**2*(100._dp + q*(-147._dp + 50._dp*q))**2)**2))/125._dp 
      qd = p/(1._dp + (q**2*(100._dp + q*(-147._dp + 50._dp*q))**2)/15625._dp)
      dy = [pd,qd]
   end function

   function jac(y) result (Jy) 
      real(dp) :: y(:), Jy(size(y), size(y))
      Jy(1,:) = [0.0_dp, 0.0_dp]
      Jy(2,:) = [0.0_dp, 0.0_dp]
   end function

   function hamiltonian(y) result (energy)
      real(dp) :: y(:), energy, p, q
      p = y(1)
      q = y(2)
      energy = ((-2 + q)**2*q**2)/10._dp + q**3/125._dp + p**2/(2._dp*(1 + (q**2*(100._dp - 147._dp*q + 50._dp*q**2)**2)/15625._dp))
   end function
end module


program gauss3
   use prec
#ifdef Xhenong
   use henon ! the problem is selected by conditional compilation
#endif
#ifdef Xpendulumg
   use pendulum
#endif
#ifdef Xbowg
   use beadonwire
#endif

   implicit none
   integer, parameter :: s = 3
   real(dp), parameter :: w35 = sqrt(3._dp/5._dp), w15 = sqrt(15._dp), b29 = 2._dp/9._dp
   real(dp), parameter ::  ark(s,s) = reshape(& 
         [5._dp/36._dp,(10._dp+3*w15)/72._dp, (5._dp/6._dp + w35)/6._dp, & 
         &2._dp/9._dp - 1._dp/w15, 2._dp/9._dp, 2._dp/9._dp + 1._dp/w15, &
         &5._dp/36._dp - 1._dp/(2._dp*w15), (10_dp-3._dp*w15)/72._dp, 5._dp/36._dp],[s,s])
   real(dp), parameter :: brk(s) = [5._dp/18._dp, 4._dp/9._dp, 5._dp/18._dp]
   real(dp) :: y(neqn), yy(neqn,s), yn(neqn,s), y1(neqn), hf(neqn,s), arkt(s,s), ham0, hamy
   real(dp)       :: tic, toc, xdelta, deltaham
   integer  :: m, i, nstep, iter, nfeval, nxout = 1, nout = 0
   integer  :: idid, every
   character(len=1000) :: line, word

   nstep = nint(xend / h)
   every = nstep/5 
   do 
      read (*, '(a)', iostat=idid) line
      if (idid == -1) exit
      read (line,*, iostat=idid) word
      if (idid == -1) exit
      line = line(len_trim(word)+1:)
      if (trim(word) == "nstep") read (line,*) nstep
      if (trim(word) == "every") read(line,*) every
   end do

   nfeval = 0
   y = y0
   arkt = transpose(ark)
   ham0 = hamiltonian(y0)
   h = xend/nstep

   print "(a,a)", "# ", prob
   print "(a)", "# Gauss-3"
   print "(a,i0)" ,"# nstep = ", nstep
   print "(a,e12.4)" ,"# h = ", h
   print "(a,e12.4)" ,"# xend = ", xend
   print "(a,i0)", "# dp =  ",  dp

   call cpu_time(tic)

   do m = 1, nstep
      yy = spread(y, 2, s)
      hf = spread(h*f(y), 2,s); nfeval = nfeval + 1
      do iter = 1, 20
         yn = spread(y, 2, s) + matmul(hf, arkt)         
         do i = 1, s
            hf(:,i) = h*f(yn(:,i)); nfeval = nfeval + 1
         end do
         if (maxval(abs(yy-yn))<10*epsilon(1.0_dp)) then
!            print *, "converged", iter
            exit
         end if
         yy = yn
      end do
      y = y + matmul(hf,brk)
      hamy = hamiltonian(y)
      if (abs(hamy-ham0) > deltaham) then
         deltaham = abs(hamy-ham0); xdelta = h*m
      end if
      if (every > 0) then
         if (mod (m,every) == 0) then 
            print "(f9.0,1x,e8.3)", h*m, abs(hamy-ham0)
         end if
      else
         if (m >= nxout) then 
            print "(f12.3,1x,e8.3)", h*m, abs(hamy-ham0)
            nout = nout + 1
            nxout = nint(dble(nstep)**(dble(nout)/dble(-every)))
         end if
      end if
   end do
   call cpu_time(toc)
   print "(a,e12.4)" ,"# cputime = ", toc-tic
   print "(a,e12.4)" ,"# xdelta = ", xdelta
   print "(a,e12.4)" ,"# deltaham = ", deltaham
   print "(a,e12.4)" ,"# finalerr = ", sqrt(sum(abs((y-yref)**2)))
   print "(a, i12)" , "# nfeval = ", nfeval


end program


