! This fortran code has been created to carry out the numerical experiments 
! for the manuscript
!
! A G-Symplectic method with order 6
! by J. C. Butcher, G. Imran and H. Podhaisky,
! submitted for publication
! 
! The program reads its options from standard input, see 
! argument processing in subroutine integ for details.
!
! compile and run with : 
!
!   gfortran -O3 -cpp -fdefault-integer-8  -DHENON glm.f90 -o ./henon
!   printf "nstep 50 \nxend 10\nevery 10\n" | ./henon
! 
! ------------------------------------------------- License: (CC BY-SA 3.0)  

module prec
#ifndef DP
   integer, parameter  :: dp = 16  ! by default quadruple precision
#else 
   integer, parameter  :: dp = DP
#endif
end module prec

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 
   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

module glm
   use prec
#ifdef Xhenon 
   use henon ! the problem is selected by conditional compilation
#endif
#ifdef Xpendulum
   use pendulum
#endif
#ifdef Xbow
   use beadonwire
#endif

   implicit none

   logical        ::  fixedpoint = .true.

   integer, parameter :: s = 5, r = 4
   real(dp)       :: A(s,s), U(s,r), B(r,s), V(r,r)
   integer        :: nstep
   contains

   function linsol(a0,b0) result (x)
      real(dp)                :: a0(:,:), b0(:)
      real(dp)                :: x(size(b0)), s
      real(dp), allocatable   :: a(:,:), b(:)
      integer                 :: n, ip(size(b0)), i, j, ix

      n = size(b0); a = a0; b = b0; x = 0.0_dp
 
      forall (i=1:n)
         ip(i) = i
      end forall

      do i = 1, n-1
         ix = i-1+maxloc(abs(a(ip(i:n), i)),1)
         if (i /= ix) then
            ip([i,ix]) = ip([ix,i])
         end if
         do j = i+1, n
            s = a(ip(j),i)/a(ip(i),i)
            a(ip(j),i+1:n) = a(ip(j),i+1:n) - s * a(ip(i),i+1:n) 
            b(ip(j)) = b(ip(j)) - s * b(ip(i))
         end do
      end do

      do i = n,1,-1
         s = 0.0_dp
         do j = i+1,n
            s = s + a(ip(i),j)*x(j)
         end do
         x(i) = (b(ip(i)) - s)/a(ip(i),i)
      end do 
    end function

    subroutine checklinsol()
      integer, parameter :: n = 500
      real (dp) ::  a(n,n), b(n), x(n)
      call random_number(a)
      call random_number(b)
      x = linsol(a,b)
      print *, "linsol error = ", sum(abs(matmul(a,x)-b))
    end subroutine

   subroutine compute_aubv()
      real(dp)            ::  bt(s), alf(s), bet(s), gam(s)
      real(dp), parameter :: w5 = sqrt(5.0_dp), w3 = sqrt(330.0_dp)
      real(dp)            :: t1, t2
      integer             :: i, j

      bt = [11.0_dp/120.0_dp, 27.0_dp/40.0_dp, &
          & -(8.0_dp/15.0_dp), 27.0_dp/40.0_dp, 11.0_dp/120.0_dp]
      
      alf = [4.0_dp*w5/11.0_dp, -4.0_dp*w5/27.0_dp, & 
          & 0.0_dp, 4.0_dp*w5/27.0_dp, -4.0*w5/11.0_dp]
      t1 =  sqrt(1265902.0_dp/(65274.0_dp*w3-347009.0_dp))    
      t2 =  sqrt(115082.0_dp /(5934.0_dp*w3 - 70541.0_dp))
      bet = t1 * [(65274.0_dp*w3 - 347009.0_dp)/1265902.0_dp,  & 
          &  -(70518.0_dp*w3 + 318613.0_dp)/3107214.0_dp,      &
          & -(18285.0_dp*w3 + 162856.0_dp)/460328.0_dp,        &
          & -(70518.0_dp*w3 + 318613.0_dp)/3107214.0_dp,       &
          &  (65274.0_dp*w3 - 347009.0_dp)/1265902.0_dp]

      gam = t2* [(5934.0_dp*w3 - 70541.0_dp)/115082.0_dp,      &
          &  -(23506.0_dp*w3 - 462231.0_dp)/1035738.0_dp,      &
          &  -(18285.0_dp*w3 - 423016.0_dp)/460328.0_dp,       &
          &  -(23506.0_dp*w3 - 462231.0_dp)/1035738.0_dp,      &
          &  (5934.0_dp*w3 - 70541.0_dp)/115082.0_dp ]
      a = 0.0_dp
      do i = 1, s
         do j = 1, i-1
            a(i,j) = bt(j)*(1.0_dp-alf(i)*alf(j)-bet(i)*bet(j)+gam(i)*gam(j))
         end do
         a(i,i) = bt(i)*(1.0_dp-alf(i)*alf(i)-bet(i)*bet(i)+gam(i)*gam(i))/2.0_dp
      end do
      b(1,:) = bt
      b(2,:) = bt*alf
      b(3,:) = bt*bet
      b(4,:) = bt*gam
      u(:,1) = 1.0_dp
      u(:,2) = -bet
      u(:,3) = alf
      u(:,4) = -gam
      v = 0.0_dp
      v(1,1) = 1.0_dp; v(4,4) = -1.0_dp; v(2,3) = -1.0_dp; v(3,2) = 1.0_dp

   end subroutine

   function onestep(ym1, nfeval) result(ym)
      real(dp), intent(in)  :: ym1(neqn,r)
      real(dp)       :: ym(neqn,r)
      real(dp)       ::  eye(neqn,neqn)
      real(dp)       :: hf(neqn, s), y(neqn), w(neqn), dy(neqn), inc(neqn,r)
      integer, intent(inout) :: nfeval
      integer        :: i, j, iter

      eye = 0.0_dp
      forall (i=1:neqn)
         eye(i,i) = 1.0_dp
      end forall

      do i = 1, s
         w =  matmul(ym1, u(i,:)) + matmul(hF(:,1:i-1), A(i,1:i-1))
         y = w
         do iter = 1,20
            hf(:,i) = h * f(y); nfeval = nfeval + 1 
            if ((i>1) .and. (i<5)) then
               if (fixedpoint) then
                  dy = w-y+a(i,i)*hf(:,i) 
               else
                  dy = linsol(eye-h*a(i,i)*jac(y),w-y+a(i,i)*hf(:,i)) 
               end if
               y = y + dy
               if (maxval(abs(dy)) < 10*epsilon(1.0_dp)) then
!                     if (fixedpoint) print *, "converged", i, iter, sum(abs(dy))
                  exit
               end if
            else
               exit
            end if
         end do
      end do
      ym =  matmul(hF, transpose(b)) + matmul(ym1, transpose(v))
   end function

   subroutine integ
      real(dp)       :: ym(neqn,r)
      real(dp)       :: k1(neqn), k2(neqn), k3(neqn), k4(neqn), k5(neqn), uu(neqn)
      real(dp)       :: ham0, hamy, deltaham, xdelta, tmp, tmp1, tmp2
      real(dp)       :: tic, toc
      integer        :: i,j,m, iter, idid, every, nout = 0, nxout = 1, nfeval = 0
      character(len=1000) :: line, word, starter = "trivial"

      call compute_aubv()

      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) == "fixedpoint") fixedpoint = .true.
         if (trim(word) == "xend") read(line,*) xend
         if (trim(word) == "every") read(line,*) every
         if (trim(word) == "starter") read(line,*) starter
         if (trim(word) == "vhat0") V(2:,2:) = 0.0_dp 
      end do
      
      h = xend/nstep
      print "(a,a)", "# ", prob
      print "(a,i0)" ,"# nstep = ", nstep
      print "(a,e12.4)" ,"# h = ", h
      print "(a,e12.4)" ,"# xend = ", xend
      print "(a,i0)", "# dp =  ",  dp
      print "(a,a)", "# starter = ", trim(starter)
      if (fixedpoint) then
         print "(a)", "# fixedpoint "
      else 
         print "(a)", "# newton"
      endif

      if (every<0) then ! use negative value for every for logarithmically spaced output
         nout = 0
         nxout = 1
      end if

!begin{bmatrix}
!\frac{69\sqrt{330}-3395}{29522299320}\sqrt{-3630408158+682896588\sqrt{330}}\,\delta\\
!-\frac{69\sqrt{330}-3395}{29522299320}\sqrt{-3630408158+682896588\sqrt{330}}\,\delta\\
!\frac{\sqrt{330}+55}{338341080}\sqrt{-8117999362+682896588\sqrt{330}}\,\delta
!\end{bmatrix}
      call cpu_time(tic)
      ym = 0
      ym(:,1) = y0
      tmp1 = (69._dp * sqrt(330._dp)-3395._dp)/29522299320._dp * & 
                & sqrt(-3630408158._dp+682896588._dp*sqrt(330._dp))
      tmp2 = (sqrt(330._dp)+55)/338341080._dp* & 
                & sqrt(-8117999362._dp+682896588._dp* sqrt(330._dp)) 
      if (trim(starter) == "trivial") then
         ym(:,2:4) = 0
      elseif (trim(starter) == "starter1") then
         k1 = f(y0)
         k2 = f(y0+h*k1/2)
         k3 = f(y0 + h*(k1+k2)/2)
         uu = h*(2*k1-4*k2+2*k3)
         ym(:,2) = tmp1 * uu
         ym(:,3) = -ym(:,2)
         ym(:,4) = tmp2 * uu
         nfeval = nfeval + 3
      elseif (trim(starter) == "starter2") then
         k1 = f(y0)
         k2 = f(y0 + h*k1/2)
         k3 = f(y0 + h*(k1 + k2)/2)
         k4 = f(y0 - h*k1/2)
         k5 = f(y0 - h*(k1+k4)/2)
         uu = h*(2*k1-2*k2+k3-2*k4+k5)
         ym(:,2) = tmp1 * uu
         ym(:,3) = -ym(:,2)
         ym(:,4) = tmp2 * uu
         nfeval = nfeval + 5
      else 
         stop "unknown option for starter"
      end if
      ham0 = hamiltonian(y0)
      deltaham = 0.0_dp; xdelta = 0.0_dp
      do m = 1, nstep
         ym = onestep(ym, nfeval)
         hamy = hamiltonian(ym(:,1))
         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)
            endif         
         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,1x,e12.4)", "# finalerr = ", sqrt(sum(abs((ym(:,1)-yref)**2)))
      print "(a,i12)" , "# nfeval = ", nfeval
   end subroutine
end module

program main
   use glm
!   call checklinsol()
   call integ()
end program
