!---------------------------------------------------!
! Copyright (c) 2016 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
!-------10--------20--------30--------40--------50--------60--------70--------80--------90
subroutine hpsi
  use global_variables
  implicit none
! finite difference
  real(dp),parameter :: cN0=-205d0/72d0,cN1=8d0/5d0
  real(dp),parameter :: cN2=-1d0/5d0,cN3=8d0/315d0
  real(dp),parameter :: cN4=-1d0/560d0
  integer :: ix,iy
  real(dp) :: c0,c1,c2,c3,c4
  real(dp) :: c0e,c1e,c2e,c3e,c4e,c0n,c1n,c2n,c3n,c4n
! nine-points formula
  c0e=-0.5d0*cN0/(dx**2)/mu_e
  c1e=-0.5d0*cN1/(dx**2)/mu_e
  c2e=-0.5d0*cN2/(dx**2)/mu_e
  c3e=-0.5d0*cN3/(dx**2)/mu_e
  c4e=-0.5d0*cN4/(dx**2)/mu_e

  c0n=-0.5d0*cN0/(dr**2)/mu_n
  c1n=-0.5d0*cN1/(dr**2)/mu_n
  c2n=-0.5d0*cN2/(dr**2)/mu_n
  c3n=-0.5d0*cN3/(dr**2)/mu_n
  c4n=-0.5d0*cN4/(dr**2)/mu_n

  twfn_e_b(:)=0d0
  twfn_n_b(:)=0d0
!  tmp_wfn_b(0:Nx,0:NR) = tmp_wfn(0:Nx,0:NR)
  twfn_e_b(0:Nx)= twfn_e(0:Nx)
!  twfn_n_b(0:NR)= twfn_n(0:NR)
!  twfn_n_b(-1)= twfn_n(1)
!  twfn_n_b(-2)= twfn_n(2)
!  twfn_n_b(-3)= twfn_n(3)
!  twfn_n_b(-4)= twfn_n(4)

!  do iy=0,NR
!    hwfn_n(iy) = c0n*twfn_n_b(iy) &
!      + c1n*( twfn_n_b(iy+1) + twfn_n_b(iy-1) ) &
!      + c2n*( twfn_n_b(iy+2) + twfn_n_b(iy-2) ) &
!      + c3n*( twfn_n_b(iy+3) + twfn_n_b(iy-3) ) &
!      + c4n*( twfn_n_b(iy+4) + twfn_n_b(iy-4) )
!  end do

  do ix=0,Nx
    hwfn_e(ix) = c0e*twfn_e_b(ix) &
      + c1e*( twfn_e_b(ix+1) + twfn_e_b(ix-1) ) &
      + c2e*( twfn_e_b(ix+2) + twfn_e_b(ix-2) ) &
      + c3e*( twfn_e_b(ix+3) + twfn_e_b(ix-3) ) &
      + c4e*( twfn_e_b(ix+4) + twfn_e_b(ix-4) )

  end do

!  hwfn_n = hwfn_n + v_n*twfn_n
  hwfn_e = hwfn_e + v_e*twfn_e

  return
end subroutine hpsi


SUBROUTINE glob_to_loc(local,global,gamma,coord)
!
! This subroutine transforms the global end reactions and
! moments into the local system (2- or 3-d).
!
 IMPLICIT NONE
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 REAL(iwp),INTENT(IN)::global(:),gamma,coord(:,:)
 REAL(iwp),INTENT(OUT)::local(:)
 REAL(iwp)::t(12,12),r0(3,3),x1,x2,y1,y2,z1,z2,xl,yl,zl,pi,gamrad,cg,sg,  &
   den,ell,x,sum,zero=0.0_iwp,one=1.0_iwp,d180=180.0_iwp
 INTEGER::i,j,k,ndim
 ndim=UBOUND(coord,2)
 SELECT CASE(ndim)
 CASE(2)
   x1=coord(1,1)
   y1=coord(1,2)
   x2=coord(2,1)
   y2=coord(2,2)
   ell=SQRT((x2-x1)**2+(y2-y1)**2)
   cg=(x2-x1)/ell
   sg=(y2-y1)/ell
   local(1)=cg*global(1)+sg*global(2)
   local(2)=cg*global(2)-sg*global(1)
   local(3)=global(3)
   local(4)=cg*global(4)+sg*global(5)
   local(5)=cg*global(5)-sg*global(4)
   local(6)=global(6)
 CASE(3)
   x1=coord(1,1)
   y1=coord(1,2)
   z1=coord(1,3)
   x2=coord(2,1)
   y2=coord(2,2)
   z2=coord(2,3)
   xl=x2-x1
   yl=y2-y1
   zl=z2-z1
   ell=SQRT(xl*xl+yl*yl+zl*zl)
   t=zero
   pi=ACOS(-one)
   gamrad=gamma*pi/d180
   cg=COS(gamrad)
   sg=SIN(gamrad)
   den=ell*SQRT(xl*xl+zl*zl)
   IF(den/=zero)THEN
     r0(1,1)=xl/ell
     r0(1,2)=yl/ell
     r0(1,3)=zl/ell
     r0(2,1)=(-xl*yl*cg-ell*zl*sg)/den
     r0(2,2)=den*cg/(ell*ell)
     r0(2,3)=(-yl*zl*cg+ell*xl*sg)/den
     r0(3,1)=(xl*yl*sg-ell*zl*cg)/den
     r0(3,2)=-den*sg/(ell*ell)
     r0(3,3)=(yl*zl*sg+ell*xl*cg)/den
   ELSE
     r0(1,1)=zero
     r0(1,3)=zero
     r0(2,2)=zero
     r0(3,2)=zero
     r0(1,2)=one
     r0(2,1)=-cg
     r0(3,3)=cg
     r0(2,3)=sg
     r0(3,1)=sg
   END IF
   DO i=1,3
     DO j=1,3
       x=r0(i,j)
       DO k=0,9,3
         t(i+k,j+k)=x
       END DO
     END DO
   END DO
   DO i=1,12
     sum=zero
     DO j=1,12
       sum=sum+t(i,j)*global(j)
     END DO
     local(i)=sum
   END DO
 END SELECT
RETURN
END SUBROUTINE glob_to_loc


program main
    implicit none
    real :: frequency
    open(unit=1, file='out.txt')
    write (1,"(A, F0.6)") 'The resonant frequency of the circuit is ', frequency(real(100), 0.1E-3, 0.25E-9)
    close(1)
    stop
end program main

function frequency(r, l, c)
    implicit none
    real, parameter :: pi = 3.1415926535
    real :: r
    real :: l
    real :: c
    real :: frequency
    frequency = 1.0 / (2 * pi * (l * c) ** 0.5)
    return
end function


	SUBROUTINE SNDTWB ( ip, it, itd, itflg, thw, iplcl )
C************************************************************************
C* SNDTWB								*
C*									*
C* This routine will calculate:						*
C*	1) Wet-bulb potential temperature				*
C*	2) Saturation wet-bulb potential temperature			*
C*	3) Wet-bulb temperature						*
C*	4) Lifting condensation level					*
C*									*
C* SNDTWB ( IP, IT, ITD, ITFLG, THW, IPLCL )				*
C*									*
C* Input parameters:							*
C*	IP		INTEGER		Pressure			*
C*	IT		INTEGER		Temperature			*
C*	ITD		INTEGER		Dewpoint			*
C*	ITFLG		INTEGER		Saturation flag			*
C*									*
C* Output parameters:							*
C*	THW		REAL		Wet-bulb potential temperature	*
C*	IPLCL		INTEGER		Pressure of the LCL		*
C**									*
C* Log:									*
C* S. Jacobs/SSAI	 4/92						*
C* J. Whistler/SSAI	 4/93		Cleaned up header		*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	PARAMETER	( RD = .28704, CP = 1.0057,
     +			  RV = .46213, RKP = .28541 )
C------------------------------------------------------------------------
C*      Functions.
C
	XLEVAP(t) = ( 597.3 - .57 * (t - 273.17) ) * 4.186
	SATVAP(td) = 6.11 * EXP(XLEVAP(t) * (1./273. - 1./td) / RV )
	WFUN(e)   = .622 * e / ( p - e )
C
	dp = 160.
	p0 = 1000.
	p  = FLOAT ( ip  )
	t  = FLOAT ( it  )
	td = FLOAT ( itd )
C
C*	Calculate mixing ratios and potential temperature.
C
	esat  = SATVAP ( t )
	e     = SATVAP ( td )
	xmixs = WFUN ( esat )
	xmixr = WFUN ( e )
	thta  = t * ( p0/p ) ** RKP
C
C*	Decrement P by 80 mb. Calculate new TMP using constant THTA.
C*	Loop until XMIXS < XMIXR. This occurs only after P < PLCL.
C
	IF  ( itflg  .eq. 3     )  GOTO 260
	IF  ( it     .eq. itd   )  GOTO 260
230	IF  ( xmixs  .le. xmixr )  GOTO 250
	p = p - 80.
	t = thta / ( p0 / p ) ** RKP
	xmixs = WFUN ( SATVAP ( t ) )
	GOTO 230
C
C*	Now zero in on PLCL until criterion in first "IF" is met.
C
250	dp = dp / 2.
	IF  ( (ABS(xmixs-xmixr) .le. 5e-5) .or. (ABS(dp) .lt. 1.) )
     +		GOTO 260
	IF  ( (xmixs-xmixr) .lt. 0. ) p = p + dp
	IF  ( (xmixs-xmixr) .ge. 0. ) p = p - dp
	t = thta / ( p0 / p ) ** RKP
	xmixs = WFUN ( SATVAP ( t ) )
	GOTO 250
260	iplcl = INT ( p )
	IF  ( itflg .eq. 2 ) GOTO 280
C
C*	Go down moist adiabat to 1000 mb or surface pressure.
C*	Loop until criterion is met.
C*	See the Ceaseless Wind for TDP calculation (278,16)
C
	dp = 20.
	IF  ( itflg .eq. 0 ) p0 = ip
270	IF  ( p .gt. (p0-dp) )  dp = p0 - p
	p = p + 0.5 * dp
	xmixr = WFUN ( SATVAP ( t ) )
	pllv = XLEVAP ( t )
	pllvrs = pllv * xmixr
	dt = ( ( ( RD + pllvrs / t ) * ( dp / p ) ) /
     +		( CP + ( pllv * pllvrs ) / ( RV * t * t ) ) ) * t
	p = p + 0.5 * dp
	t = t + dt
	IF  ( ABS(p-p0) .gt. 0.5)  GOTO 270
	thw = t
C*
280	RETURN
	END


use FEMSolverClass
implicit none

type(FEMSolver_) :: solver
real(real64),allocatable :: eigen_value(:),eigen_vectors(:,:)
real(real64),allocatable :: DenseMatrix(:,:)

DenseMatrix = zeros(5,5)
DenseMatrix(1,1:5) = dble([1,2,0,0,0])
DenseMatrix(2,1:5) = dble([2,12,0,3,0])
DenseMatrix(3,1:5) = dble([0,0,5,3,1])
DenseMatrix(4,1:5) = dble([0,3,3,3,4])
DenseMatrix(5,1:5) = dble([0,0,1,4,6])
call to_CRS(DenseMatrix,&
    CRS_val = solver%CRS_val,&
    CRS_col = solver%CRS_Index_col,&
    CRS_rowptr = solver%CRS_Index_row)

call solver%keepThisMatrixAs("A")

DenseMatrix = eyes(5,5)
call to_CRS(DenseMatrix,&
    CRS_val = solver%CRS_val,&
    CRS_col = solver%CRS_Index_col,&
    CRS_rowptr = solver%CRS_Index_row)

call solver%keepThisMatrixAs("B")

call solver%eig(num_eigen=5,eigen_value=eigen_value,eigen_vectors=eigen_vectors)

call print(eigen_value)
call print(eigen_vectors)

end


module riemann_star_module

contains

subroutine riemann_star_state(rho_l, u_l, p_l, &
                              rho_r, u_r, p_r, &
                              xn_l, xn_r, &
                              ustar, pstar, W_l, W_r, &
                              verbose_in)

  use bl_types
  use bl_constants_module
  use eos_module
  use eos_type_module
  use network, only: nspec
  use riemann_support
  use probin_module, only: riemann_max_iter

  implicit none

  real (kind=dp_t), intent(in) :: rho_l, u_l, p_l
  real (kind=dp_t), intent(in) :: rho_r, u_r, p_r
  real (kind=dp_t), intent(in) :: xn_l(nspec), xn_r(nspec)
  real (kind=dp_t), intent(out) :: ustar, pstar, W_l, W_r
  logical, optional, intent(in) :: verbose_in

  real (kind=dp_t) :: cs_l, cs_r

  real (kind=dp_t) :: pstar_new, ustar_l, ustar_r

  real (kind=dp_t) :: rho_tmp, u_tmp, p_tmp
  real (kind=dp_t) :: rhostar

  real (kind=dp_t) :: csbar, rhobar
  real (kind=dp_t) :: cs_star

  real (kind=dp_t) :: gammaE_l, gammaC_l, gammaE_r, gammaC_r
  real (kind=dp_t) :: gammaE_bar, gammaC_bar

  real (kind=dp_t) :: Z_l, Z_r
  real (kind=dp_t) :: W_temp, Z_temp

  real (kind=dp_t) :: err1, err2
  real (kind=dp_t), parameter :: tol = 1.e-10_dp_t
  logical :: converged

  integer :: iter

  type (eos_t) :: eos_state

  character (len=16) :: lwave, rwave

  real (kind=dp_t), parameter :: smallp = 1.d-8
  real (kind=dp_t), parameter :: SMALL = 1.d-13

  logical, parameter :: debug = .false.

  logical :: verbose

  if (present(verbose_in)) then
     verbose = verbose_in
  else
     verbose = .false.
  endif

  ! get the initial sound speeds
  eos_state%rho = rho_l
  eos_state%p = p_l
  eos_state%xn(:) = xn_l(:)
  eos_state%T = initial_temp_guess

  call eos(eos_input_rp, eos_state)

  cs_l = sqrt(eos_state%gam1*p_l/rho_l)

  if (verbose) print *, 'T_l = ', eos_state%T

  gammaE_l = p_l/(rho_l*eos_state%e) + ONE
  gammaC_l = eos_state%gam1

  eos_state%rho = rho_r
  eos_state%p = p_r
  eos_state%xn(:) = xn_r(:)
  eos_state%T = initial_temp_guess

  call eos(eos_input_rp, eos_state)

  cs_r = sqrt(eos_state%gam1*p_r/rho_r)

  if (verbose) print *, 'T_r = ', eos_state%T

  gammaE_r = p_r/(rho_r*eos_state%e) + ONE
  gammaC_r = eos_state%gam1

  gammaE_bar = HALF*(gammaE_l + gammaE_r)
  gammaC_bar = HALF*(gammaC_l + gammaC_r)

  !---------------------------------------------------------------------------
  ! create an initial guess for pstar using a primitive variable
  ! Riemann solver
  ! ---------------------------------------------------------------------------

  ! We follow the PVRS solver from Toro (Chapter 9)

  ! alternative: two shock solver (see Toro 9.42)
  W_l = rho_l*cs_l
  W_r = rho_r*cs_r

  ! prevent roundoff errors from giving us a pstar that is unphysical
  ! if our input states are the same
  if (W_l == W_r) then
     pstar = 0.5d0*(p_l + p_r + W_l*(u_l - u_r))
  else
     pstar = ((W_r*p_l + W_l*p_r) + W_l*W_r*(u_l - u_r))/(W_l + W_r)
  endif

  ! if (abs(p_l - pstar) < SMALL*pstar .and. abs(p_r - pstar) < SMALL*pstar) then
  !    rhobar = HALF*(rho_l + rho_r)
  !    csbar = HALF*(cs_l + cs_r)

  !    pstar = HALF*(p_l + p_r) + HALF*(u_l - u_r)*rhobar*csbar
  ! endif

  pstar = max(pstar, smallp)

  !---------------------------------------------------------------------------
  ! find the exact pstar and ustar
  !---------------------------------------------------------------------------

  !if (verbose)
  print *, 'solving for star state: ', rho_l, u_l, p_l, rho_r, u_r, p_r
  if (verbose) print *, 'pstar: ', pstar, (pstar-p_l)/pstar, (pstar-p_r)/pstar

  ! this procedure follows directly from Colella & Glaz 1985, section 1

  converged = .false.
  iter = 1
  do while (.not. converged .and. iter < riemann_max_iter)

     ! compute Z_l and Z_r -- the form of these depend on whether the
     ! wave is a shock or a rarefaction

     ! left wave
     if (pstar - p_l > SMALL*p_l) then
        ! left shock
        call shock(pstar, rho_l, u_l, p_l, xn_l, gammaE_bar, gammaC_bar, Z_l, W_l)
        lwave = "shock"
     else
        ! left rarefaction
        call rarefaction(pstar, rho_l, u_l, p_l, xn_l, 1, Z_l, W_l)
        lwave = "rarefaction"
     endif

     ! right wave
     if (pstar - p_r > SMALL*p_r) then
        ! right shock
        call shock(pstar, rho_r, u_r, p_r, xn_r, gammaE_bar, gammaC_bar, Z_r, W_r)
        rwave = "shock"
     else
        ! right rarefaction
        call rarefaction(pstar, rho_r, u_r, p_r, xn_r, 3, Z_r, W_r)
        rwave = "rarefaction"
     endif

     if (verbose) print *, 'left wave: ', trim(lwave), ';   rightwave: ', trim(rwave)

     ustar_l = u_l - (pstar - p_l)/W_l
     ustar_r = u_r + (pstar - p_r)/W_r

     pstar_new = pstar - Z_l*Z_r*(ustar_r - ustar_l)/(Z_l + Z_r)

     if (verbose) print *, "done with iteration", iter
     if (verbose) print *, "ustar_l/r, pstar: ", &
          real(ustar_l), real(ustar_r), real(pstar_new)
     if (verbose) print * , " "

     ! estimate the error in the current star solution
     err1 = abs(ustar_r - ustar_l)
     err2 = pstar_new - pstar

     if (verbose) print *, "ERRORS: ", err1, err2
     if (verbose) print *, " "

     if (err1 < tol*max(abs(ustar_l),abs(ustar_r)) .and. err2 < tol*pstar) then
        converged = .true.
     endif

     ! get ready for the next iteration
     pstar = pstar_new

     iter = iter + 1

  enddo

  ustar = HALF*(ustar_l + ustar_r)

  if (verbose) print *, 'found pstar, ustar: ', pstar, ustar

  ! let's test if our integration across the rarefaction works, as expected
  if (lwave == "rarefaction" .and. debug) then
     call rarefaction(pstar, rho_l, u_l, p_l, xn_l, 1, Z_temp, W_temp, rhostar)

     ! get the soundspeed via the EOS (C&G suggest getting it from
     ! the jump conditions)
     eos_state%rho = rhostar
     eos_state%p = pstar
     eos_state%xn(:) = xn_l(:)
     eos_state%T = initial_temp_guess

     call eos(eos_input_rp, eos_state)

     cs_star = sqrt(eos_state%gam1*pstar/rhostar)

     call rarefaction_to_u(rho_l, u_l, p_l, xn_l, 1, ustar-cs_star, rho_tmp, p_tmp, u_tmp)

     print *, "left rarefaction integration test"
     print *, "(this should agree with pstar/ustar if the 1-wave is a rarefaction): ", p_tmp, u_tmp

  endif

  if (rwave == "rarefaction" .and. debug) then
     call rarefaction(pstar, rho_r, u_r, p_r, xn_r, 3, Z_temp, W_temp, rhostar)

     ! get the soundspeed via the EOS (C&G suggest getting it from
     ! the jump conditions)
     eos_state%rho = rhostar
     eos_state%p = pstar
     eos_state%xn(:) = xn_r(:)
     eos_state%T = initial_temp_guess

     call eos(eos_input_rp, eos_state)

     cs_star = sqrt(eos_state%gam1*pstar/rhostar)

     call rarefaction_to_u(rho_r, u_r, p_r, xn_r, 3, ustar+cs_star, rho_tmp, p_tmp, u_tmp)

     print *, "left rarefaction integration test"
     print *, "(this should agree with pstar/ustar if the 3-wave is a rarefaction): ", p_tmp, u_tmp

  endif

end subroutine riemann_star_state
end module riemann_star_module


!    Copyright (C) 2012 The SPEED FOUNDATION
!    Author: Ilario Mazzieri
!
!    This file is part of SPEED.
!
!    SPEED is free software; you can redistribute it and/or modify it
!    under the terms of the GNU Affero General Public License as
!    published by the Free Software Foundation, either version 3 of the
!    License, or (at your option) any later version.
!
!    SPEED is distributed in the hope that it will be useful, but
!    WITHOUT ANY WARRANTY; without even the implied warranty of
!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
!    Affero General Public License for more details.
!
!    You should have received a copy of the GNU Affero General Public License
!    along with SPEED.  If not, see <http://www.gnu.org/licenses/>.

!> @brief Compute the A0, A1 coefficient for the Rayleigh damping matrix C = A0 M + A1 K,
!! M = mass matrix, K= stiffness matrix
!! @author Ilario Mazzieri
!> @date November, 2014
!> @version 1.0
!> @param[in] namt number of material
!> @param[in] prop_mat material properties
!> @param[in] QS quality factors for the s-waves
!> @param[in] f_val value for the reference frequency
!> @param[in] mpi_id id of the mpi-process
!> @param[out] A0_ray Rayleigh coefficients
!> @param[out] A1_ray Rayleigh coefficients

      subroutine MAKE_RAYLEIGH_COEFFICIENTS(nmat, A0_ray, A1_ray, prop_mat, QS, f_val, mpi_id)

      implicit none

      integer*4 :: nmat, mpi_id, im, i, j, k

      real*8 :: f_val, fmin, fmax, esp1, esp2, deltax, &
                rho, lambda, mu, vp2, vs2, pi

      real*8 :: prop_mat(nmat,4), QS(nmat), csi(nmat),&
                A0_ray(nmat), A1_ray(nmat)

      pi = 4.d0*datan(1.d0);

      csi =1./(2*QS);

      if (f_val .le. 10) then
         fmin = 0.05d0; !fmax = 5.d0;
      elseif(f_val .le. 100) then
         fmin = 5.d0; !fmax = 100.d0;
      elseif(f_val .le. 1000) then
         fmin = 50.d0; !fmax = 1000.d0;
      elseif(f_val .le. 10000) then
         fmin = 500.d0; !fmax = 10000.d0;
      elseif(f_val .le. 100000) then
         fmin = 5000.d0; !fmax = 100000.d0;
      endif

      fmax=100*fmin;

      if(mpi_id .eq. 0) then
         write(*,*) 'Frequency range where Quality Factor is assumed to be constant is:'
         write(*,*) 'FMIN =' ,fmin, '  FMAX =', fmax;
      endif

      do im = 1, nmat

         A0_ray(im) = csi(im)* 4.*pi*fmin*fmax/(fmin+fmax);
         A1_ray(im) = csi(im)/(pi*(fmin+fmax));

         if(mpi_id .eq. 0) then
           write(*,*) 'RAYLEIGH COEFFICIENTS FOR MATERIAL ', im, '  ARE '
           write(*,*) 'A0 = ', A0_ray(im)
           write(*,*) 'A1 = ', A1_ray(im)
           write(*,*) '----------------------------------------------------'
         endif

       enddo

       return

       end subroutine MAKE_RAYLEIGH_COEFFICIENTS


program problem53
  implicit none
  integer :: n, r, count

  count=0
  do n=23,100
     do r=1,n
        if (predicate(n,r)) count=count+1
     end do
  end do

  print *, count

contains

  function predicate(n,r)
    implicit none
    integer, intent(in) :: n, r
    integer             :: i
    double precision    :: temp
    logical             :: predicate

    predicate=.false.
    temp=1.0
    do i=1,r
       temp=temp*(n-(r-i)) ! gets larger
       temp=temp/(r-i+1)   ! gets smaller
       if (temp>1000000.0) then
          predicate=.true.
          exit
       end if
    end do

  end function predicate

end program problem53


SUBROUTINE integr_rk4(mjdo, yo, step, mjdn, yn)

! ----------------------------------------------------------------------
! SUBROUTINE: integr_rk4.f03
! ----------------------------------------------------------------------
! Purpose:
!  Numerical integration method: Runge-Kutta 4th order by Runge and Kutta
!  RK4 method is implemented here for satellite orbit integration
! ----------------------------------------------------------------------
! Input arguments:
! - MJDo: 		Initial epoch's (to) Modified Julian Day (MJD) number (including the fraction of the day)
! - ro:			Position vector at initial epoch (m)
! - vo:			Velocity vector at initial epoch (m/sec)
! - step:		Numerical Integration stepsize (sec)
!
! Output arguments
! - MJD: 		Next epoch's (to+h) Modified Julian Day (MJD) number (including the fraction of the day)
! - r:			Position vector at next epoch (m)
! - v:			Velocity vector at next epoch (m/sec)
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou, Cooperative Research Centre for Spatial Information, Australia
! Created:	19 October 2017
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      REAL (KIND = prec_d), INTENT(IN), DIMENSION(6) :: yo
      REAL (KIND = prec_d), INTENT(IN) :: mjdo
      REAL (KIND = prec_d), INTENT(IN) :: step
! ----------------------------------------------------------------------
! OUT
      REAL (KIND = prec_d), INTENT(OUT), DIMENSION(6) :: yn
      REAL (KIND = prec_d), INTENT(OUT) :: mjdn
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      REAL (KIND = prec_d) :: h, to, mjd_to
      REAL (KIND = prec_d), DIMENSION(3) :: ro , vo
      REAL (KIND = prec_d) :: ti, mjd_t
      REAL (KIND = prec_d), DIMENSION(3) :: ri, vi
      REAL (KIND = prec_d), DIMENSION(6) :: yi
      REAL (KIND = prec_d) :: fx, fy, fz
      REAL (KIND = prec_d), DIMENSION(6) :: k1, k2, k3, k4, FRK4
!       , y
     INTEGER (KIND = prec_int4) :: i_stage
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Integration step
h = step
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Initial Conditions
! ----------------------------------------------------------------------
! Initial epoch
! MJD
mjd_to = mjdo
! Seconds at MJDo since 00h
to = (mjd_to - INT(mjd_to)) * (24.0D0 * 3600.D0)

! Initial State vector at epoch to
ro = yo(1:3)
vo = yo(4:6)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Computation of integration coefficients (k)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! k1 = f(to,yo)
! ----------------------------------------------------------------------
ri = ro
vi = vo

! MJD (in days) including the fraction of the day
mjd_t = mjd_to

! Force model acceleration component
i_stage=0
Call force_sum(mjd_to, to, ri, vi, fx, fy, fz, i_stage)

k1(1:3) = vi
k1(4:6) = (/ fx, fy, fz /)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! k2 = f(to+h/2,yo+h*k1/2)
! ----------------------------------------------------------------------
ti = to + h/2.0D0
yi = yo + (h/2.0D0) * k1

ri = yi(1:3)
vi = yi(4:6)

! MJD (in days) at ti including the fraction of the day
mjd_t = INT(mjd_to) + ti / (24.0D0 * 3600.D0)

! Force model acceleration components
i_stage = 2-1
Call force_sum(mjd_t, ti, ri, vi, fx, fy, fz, i_stage)

k2(1:3) = vi
k2(4:6) = (/ fx, fy, fz /)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! k3 = f(to+h/2,yo+h*k2/2)
! ----------------------------------------------------------------------
ti = to + h/2.0D0
yi = yo + (h/2.0D0) * k2

ri = yi(1:3)
vi = yi(4:6)

! MJD (in days) at ti including the fraction of the day
mjd_t = INT(mjd_to) + ti / (24.0D0 * 3600.D0)

! Force model acceleration components
i_stage = 3-1
Call force_sum(mjd_t, ti, ri, vi, fx, fy, fz, i_stage)

k3(1:3) = vi
k3(4:6) = (/ fx, fy, fz /)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! k4 = f(to+h,yo+h*k3)
! ----------------------------------------------------------------------
ti = to + h
yi = yo + h * k3

ri = yi(1:3)
vi = yi(4:6)

! MJD (in days) at ti including the fraction of the day
mjd_t = INT(mjd_to) + ti / (24.0D0 * 3600.D0)

! Force model acceleration components
i_stage = 4-1
Call force_sum(mjd_t, ti, ri, vi, fx, fy, fz, i_stage)

k4(1:3) = vi
k4(4:6) = (/ fx, fy, fz /)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Increment Function
FRK4 = (1.0D0/6.0D0) * (k1 + 2*k2 + 2*k3 + k4)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Solution of the numerical integration
! ----------------------------------------------------------------------
! State vector at the next epoch
yn = yo + h * FRK4
! ----------------------------------------------------------------------

! Next Epoch
! MJD (in days) including the fraction of the day
mjdn = mjdo + step / (24.0D0 * 3600.D0)

END SUBROUTINE