module const ! declarations of system parameters
  implicit none
  public
  real*16, parameter :: &
       m1 = 1.0d0, m2 = 1.25d0, &
       l1 = 1.0d0, l2 = 1.0d0, &
       g = 9.8d0,&
       h = 0.02d0
  integer, parameter :: &
       n = 10e3, m = 4
  integer :: i, j
  real*16 :: &
       ! parameters of decart representation
       x1 = 0.0, y1 = 0.0, x2 = 0.0, y2 = 0.0
end module const
subroutine incep (q1, q2, q3, q4)
  implicit none
  real*16 :: q1, q2, q3, q4
  q1 = 1.0
  q2 = 3.0
  q3 = 0.0
  q4 = 0.0
end subroutine incep
!====================================================

function F1(phi1, phi2, w1, w2)
  use const
  implicit none
  real*16 :: F1, phi1, phi2, w1, w2
  F1 = w1
end function F1

function F2(phi1, phi2, w1, w2)
  use const
  implicit none
  real*16 :: F2, phi1, phi2, w1, w2
  F2=w2
end function F2

function F3(phi1, phi2, w1, w2)
  use const
  implicit none
  real*16 :: F3, phi1, phi2, w1, w2, add1
  add1 = l1*(2*m1+m2-m2*cos(2*phi2-2*phi1))
  F3 = (-g*(2*m1+m2)*sin(phi1)+m2*g*sin(phi1-2*phi2)-2*sin(phi1-phi2)*m2*(w2*w2*l2+w1*w1*l1*cos(phi1-phi2)))/add1
end function F3

function F4(phi1, phi2, w1, w2)
  use const
  implicit none
  real*16 :: F4, phi1, phi2, w1, w2, add2
  add2 = (l2*(2*m1+m2-m2*cos(2*phi2-2*phi1)))
  F4 = (2*sin(phi1-phi2)*(w1*w1*l1*(m1+m2)+g*(m1+m2)*cos(phi1)+w2*w2*l2*m2*cos(phi1-phi2)))/add2
end function F4

subroutine runge_kutt
  use const
  implicit none
  real*16, external :: F1, F2, F3, F4
  real*16 :: q1, q2, q3, q4
  ! asign matrix with coefficiants
  real*16, allocatable :: k1(:), k2(:), k3(:), k4(:)
  ! matrix (n * m) with solution
  real*16, allocatable :: y(:,:)

  allocate(k1(m), k2(m), k3(m), k4(m))
  if (m==0) stop 'Allocation error, m = 0'

  allocate(y(n,m))
  if (n==0) stop 'Allocation error, n = 0'
  call incep (q1, q2, q3, q4)

   y(1,1) = q1 !  alpha1(0) = angle1 in rad
   y(1,2) = q2 !  alpha2(0) = angle2 in rad
   y(1,3) = q3 !  w1(0) = 0
   y(1,4) = q4 !  w2(0) = 0

  open(unit = 1, file = 'xypendulum.txt')
  open(unit = 2, file = 'angle.txt')
  ! Algorithm Runge-Kutta
  do i = 1, n-1, 1
     ! K_{ij} = (k11 & k12 & k13 & k14\\ ...)
     k1(1) = F1(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(2) = F2(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(3) = F3(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(4) = F4(y(i,1), y(i,2), y(i,3), y(i,4))*h

     k2(1) = F1(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(2) = F2(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(3) = F3(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(4) = F4(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h

     k3(1) = F1(y(i,1) + k2(1)/2, y(i,2) + k2(2)/2, y(i,3) + k2(3)/2, y(i,4) + k2(4)/2)*h
     k3(2) = F2(y(i,1) + k2(1)/2, y(i,2) + k2(2)/2, y(i,3) + k2(3)/2, y(i,4) + k2(4)/2)*h
     k3(3) = F3(y(i,1) + k2(1)/2, y(i,2) + k2(2)/2, y(i,3) + k2(3)/2, y(i,4) + k2(4)/2)*h
     k3(4) = F4(y(i,1) + k2(1)/2, y(i,2) + k2(2)/2, y(i,3) + k2(3)/2, y(i,4) + k2(4)/2)*h

     k4(1) = F1(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(2) = F2(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(3) = F3(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(4) = F4(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     do j = 1, m
        y(i + 1,j) = y(i,j) + (k1(j) + 2*k2(j) + 2*k3(j) + k4(j))/6
     end do
  end do
  do i = 1, n
     x1 = sin(y(i,1))
     y1 = -cos(y(i,1))
     x2 = x1 + sin(y(i,2))
     y2 = y1 - cos(y(i,2))
     write(1,'(1x, 4e20.12)') x2, y2, x1, y1 ! output
     write(2,'(1x, 3e20.12)') (i-1)*h, y(i,2), y(i,1)
  end do
  deallocate(k1, k2, k3, k4, y)
  close(1)
  close(2)
end subroutine runge_kutt

subroutine inter()
  use const
  implicit none
  write(*,101)
101 format(1x,'System of differential equation of first order is solved'/&
              ' due runge-kutta method of 4-th order')
  write(*,'(a36, i8)') 'Number of rows (sampling rate) is  ', n
  write(*,100) h
100 format(1x,'Step:  ', e10.4)
  write(*,10)
10 format(1x,'Solution:  '/&
         ' +----------------+-----------------+'/&
         ' |    analitic    |  interpolation  |'/&
         ' +----------------+-----------------+'/&
         ' |    not exist   |  xypendulum.txt |'/&
         ' +----------------+-----------------+'/&
         ' |    ---------   |    angle.txt    |'/&
         ' +----------------+-----------------+'/)
end subroutine inter

program pendulum
  implicit none
  call runge_kutt
  call inter()
end program pendulum