MODULE Shared
  USE, INTRINSIC :: ISO_Fortran_env, dp=>REAL64 !modern DOUBLE PRECISION
  IMPLICIT NONE
  INTEGER :: i
  INTEGER, PARAMETER :: i_max = 12500, theta_dat = 10 !manage UNIT numbers as a resource
  REAL(dp) :: EOM1, EOM2, length1, length2, mass1, mass2, theta1_pass, theta2_pass, omega1_pass, omega2_pass
  REAL(dp) :: k11, k12, k13, k14, k21, k22, k23, k24, k31, k32, k33, k34, k41, k42, k43, k44
  REAL(dp), DIMENSION(:), ALLOCATABLE, ASYNCHRONOUS :: theta1, theta2, omega1, omega2, t
  REAL(dp), PARAMETER :: PI = ATAN2(0.0_dp, -1.0_dp), g = 9.80665, dt = 0.001, SIX = 6.0
  CHARACTER(*), PARAMETER :: my_format = '(2f7.3)'
  EXTERNAL EOM1, EOM2
END MODULE Shared
!1. dp?, D+00?
!2. i + 1?
!3. Stupid things with _plot?
PROGRAM Doppelt
  USE Shared
  IMPLICIT NONE
  ALLOCATE(theta1(i_max + 1), theta2(i_max + 1), omega1(i_max + 1), omega2(i_max + 1), t(i_max + 1))
  t(1) = 0.0_dp
  WRITE(6, '(a11)') "Pendulum #1"
  CALL Prompt(theta1_pass, omega1_pass, length1, mass1)
  theta1(1) = theta1_pass
  omega1(1) = omega1_pass
  WRITE(6, '(a11)') "Pendulum #2"
  CALL Prompt(theta2_pass, omega2_pass, length2, mass2)
  theta2(1) = theta2_pass
  omega2(1) = omega2_pass
  WRITE(6, '(a6)', advance='no') "Steps:"
  PRINT *, i_max
  i = 1
  OPEN(theta_dat, file='Theta.dat', access='sequential', status='unknown', action='write', asynchronous='yes')
  fourth: DO
    IF (i > i_max) EXIT
    WRITE(theta_dat, my_format) theta1(i), theta2(i)
    k11 = dt * omega1(i)
    k21 = dt * omega2(i)
    theta1_pass = theta1(i)
    theta2_pass = theta2(i)
    omega1_pass = omega1(i)
    omega2_pass = omega2(i)
    k31 = dt * EOM1(mass1, mass2, theta1_pass, theta2_pass, omega1_pass, omega2_pass, length1, length2, g)
    k41 = dt * EOM2(mass1, mass2, theta1_pass, theta2_pass, omega1_pass, omega2_pass, length1, length2, g)
    k12 = dt * (omega1(i) + k11 / 2.0_dp)
    k22 = dt * (omega2(i) + k21 / 2.0_dp)
    k32 = dt * EOM1(mass1, mass2, (theta1_pass + k11 / 2.0_dp), (theta2_pass + k21 / 2.0_dp), (omega1_pass + k31 / 2.0_dp),        &
      (omega2_pass + k41 / 2.0_dp), length1, length2, g)
    k42 = dt * EOM2(mass1, mass2, (theta1_pass + k11 / 2.0_dp), (theta2_pass + k21 / 2.0_dp), (omega1_pass + k31 / 2.0_dp),        &
      (omega2_pass + k41 / 2.0_dp), length1, length2, g)
    k13 = dt * (omega1(i) + k12 / 2.0_dp)
    k23 = dt * (omega1(i) + k22 / 2.0_dp)
    k33 = dt * EOM1(mass1, mass2, (theta1_pass + k12 / 2.0_dp), (theta2_pass + k22 / 2.0_dp), (omega1_pass + k32 / 2.0_dp),        &
      (omega2_pass + k42 / 2.0_dp), length1, length2, g)
    k43 = dt * EOM2(mass1, mass2, (theta1_pass + k12 / 2.0_dp), (theta2_pass + k22 / 2.0_dp), (omega1_pass + k32 / 2.0_dp),        &
      (omega2_pass + k42 / 2.0_dp), length1, length2, g)
    k14 = dt * (omega1(i) + k13)
    k24 = dt * (omega1(i) + k23)
    k34 = dt * EOM1(mass1, mass2, (theta1_pass + k13), (theta2_pass + k23), (omega1_pass + k33), (omega2_pass + k43), length1,     &
      length2, g)
    k44 = dt * EOM2(mass1, mass2, (theta1_pass + k13), (theta2_pass + k23), (omega1_pass + k33), (omega2_pass + k43), length1,     &
      length2, g)
    theta1(i + 1) = theta1(i) + ((k11 + (2.0_dp * (k12 + k13)) + k14) / SIX)
    theta2(i + 1) = theta2(i) + ((k21 + (2.0_dp * (k22 + k23)) + k24) / SIX)
    omega1(i + 1) = omega1(i) + ((k31 + (2.0_dp * (k32 + k33)) + k34) / SIX)
    omega2(i + 1) = omega2(i) + ((k41 + (2.0_dp * (k42 + k43)) + k44) / SIX)
    IF (theta1(i + 1) >  PI) theta1(i + 1) = theta1(i + 1) - 2.0_dp * PI
    IF (theta1(i + 1) < -PI) theta1(i + 1) = theta1(i + 1) + 2.0_dp * PI
    IF (theta2(i + 1) >  PI) theta2(i + 1) = theta2(i + 1) - 2.0_dp * PI
    IF (theta2(i + 1) < -PI) theta2(i + 1) = theta2(i + 1) + 2.0_dp * PI
    t(i + 1) = t(i) + dt
    i = i + 1
  END DO fourth
  CLOSE(theta_dat)
  DEALLOCATE(theta1, theta2, omega1, omega2, t)
END PROGRAM Doppelt

SUBROUTINE Prompt(theta, omega, length, mass)
  USE Shared, ONLY : dp
  IMPLICIT NONE
  REAL(dp), INTENT(IN OUT) :: theta, omega, length, mass
  REAL(dp) :: theta_read, omega_read, length_read, mass_read
  WRITE(6, '(a21)', advance='no') "Initial angle [rad]: "
  READ *, theta_read
  theta = theta_read
  WRITE(6, '(a36)', advance='no') "Initial angular velocity [rad/sec]: "
  READ *, omega_read
  omega = omega_read
  WRITE(6, '(a24)', advance='no') "Length of pendulum [m]: "
  READ *, length_read
  length = length_read
  WRITE(6, '(a27)', advance='no') "Mass of pendulum bob [kg]: "
  READ *, mass_read
  mass = mass_read
END SUBROUTINE Prompt

REAL(dp) FUNCTION EOM1(m1, m2, x1, x2, y1, y2, l1, l2, a)
  USE Shared, ONLY : dp
  IMPLICIT NONE
  REAL(dp) :: m1, m2, x1, x2, y1, y2, l1, l2, a
  EOM1 = ((-a * ((2.0_dp * m1) + m2) * SIN(x1)) - (m2 * a * SIN(x1 - (2.0_dp * x2))) - (2.0_dp * SIN(x1 - x2) * m2 * ((y2 * y2     &
    * l2) + (y1 * y1 * l1 * COS(x1 - x2))))) / (l1 * ((2.0_dp * m1) + m2 - (m2 * COS((2.0_dp * x1) - (2.0_dp * x2)))))
END FUNCTION EOM1

REAL(dp) FUNCTION EOM2(m1, m2, x1, x2, y1, y2, l1, l2, a)
  USE Shared, ONLY : dp
  IMPLICIT NONE
  REAL(dp) :: m1, m2, x1, x2, y1, y2, l1, l2, a
  EOM2 = (2.0_dp * SIN(x1 - x2) * ((y1 * y1 * l1 * (m1 + m2)) + (a * (m1 + m2) * COS(x1)) + (y2 * y2 * l2 * m2 * COS(x1 - x2))))   &
    / (l2 * ((2.0_dp * m1) + m2 - m2 * COS((2.0_dp * x1) - (2.0_dp * x2))))
END FUNCTION EOM2