! MIT License
!
! Copyright (c) 2020 SHEMAT-Suite
!
! Permission is hereby granted, free of charge, to any person obtaining a copy
! of this software and associated documentation files (the "Software"), to deal
! in the Software without restriction, including without limitation the rights
! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
! copies of the Software, and to permit persons to whom the Software is
! furnished to do so, subject to the following conditions:
!
! The above copyright notice and this permission notice shall be included in all
! copies or substantial portions of the Software.
!
! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
! SOFTWARE.

!>    @brief calculate parameter weighting matrix wp^2
!>    @param[in] i matrix index, first dimension
!>    @param[in] j matrix index, second dimension
!>    @param[in] ismpl local sample index
!>    @return weighting value
!>    @details
!>in this case wp^2  = Cpp^-1 is assumed to\n
!>be diagonal and the inverse parameter covariance \n
!>apriori in bayesian estimation.\n
!>    C_pp == I .* d?unit(:)\n
!>    C_pp^-1 == I ./ d?unit(:) = wp2_ps =wp2_ps\n
!>               [g_p_^2]\n
      DOUBLE PRECISION FUNCTION wp2_ps(i,j,ismpl)
        use arrays
        use mod_inverse
        use mod_genrl
        use mod_data
        IMPLICIT NONE
        integer :: ismpl
        integer :: i, j
        INTEGER s_k, s_u
        DOUBLE PRECISION wgt, get_optid
        EXTERNAL get_optid

        IF (para_weight==1) THEN
          wp2_ps = covar_prior_p(i,j)
        ELSE IF (i==j) THEN
          s_k = seed_para(1,i)
          s_u = seed_para(2,i)
!         wgt = 0.0d0
!         do k = 1, ndata
!            wgt = wgt +jac(k,i)**2
!         enddo
          wgt = 1.0D0
          IF ((s_k<=lastidx) .AND. (s_k>=1)) THEN
            wgt = propwgt(s_k-firstidx+1)
          ELSE IF ((s_k<=bc_lastidx) .AND. (s_k>=1)) THEN
            wgt = bcwgt(s_k-bc_firstidx+1)
          ELSE IF (s_k==-1) THEN
            wgt = tpwgt(opti_tp(1,s_u),opti_tp(2,s_u),opti_tp(3,s_u))
          END IF
          wp2_ps = wgt/(get_optid(i,ismpl)*get_optid(i,ismpl))
        ELSE
          wp2_ps = 0.0D0
        END IF
!
        RETURN
      END

!>    @brief calculate data weighing matrix wd^2
!>    @param[in] i matrix index, first dimension
!>    @param[in] j matrix index, second dimension
!>    @param[in] ismpl local sample index
!>    @return weighting value
!>    @details
!>in this case wd^2  = Cdd^-1 is assumed to\n
!>be diagonal and the inverse data covariance \n
!>apriori in bayesian estimation.\n
!>    C_dd == I .* data(:,2)\n
!>    C_dd^-1 == I ./ data(:,2) = Cddm1 = wd2_ps\n
!>               [ndata^2]\n
      DOUBLE PRECISION FUNCTION wd2_ps(i,j,ismpl)
        use arrays
        use mod_inverse
        use mod_data
        IMPLICIT NONE
        INTEGER i, j, k, ismpl
        DOUBLE PRECISION wgt

        IF (data_weight==1) THEN
          wd2_ps = covar_prior_d(i,j)
        ELSE IF (i==j) THEN
!        wgt=0.0d0
!        do k=1,mpara
!          wgt = wgt +jac(i,k)**2
!        enddo
          wgt = 1.0D0
!    wgt = wdt(type(i))
          wd2_ps = 1.0D0/(wgt*ddata(i,cdd_w)*ddata(i,cdd_w))
        ELSE
          wd2_ps = 0.0D0
        END IF
!
        RETURN
      END


Subroutine boosti
  Implicit Double Precision (A-H, O-Z)
  Parameter (maxptn=400001)
  Common /para1/mul
  Common /para6/centy
  Common /prec1/gx0(maxptn), gy0(maxptn), gz0(maxptn), ft0(maxptn), px0(maxptn), py0(maxptn), pz0(maxptn), e0(maxptn), xmass0(maxptn), ityp0(maxptn)
  Common /lor/enenew, pxnew, pynew, pznew
  Save
  External lorenz
  bex = 0D0
  bey = 0D0
  bez = -tanh(centy)
  Do i = 1, mul
     px1 = gx0(i)
     py1 = gy0(i)
     pz1 = gz0(i)
     e1 = ft0(i)
     Call lorenz(e1, px1, py1, pz1, bex, bey, bez)
     gx0(i) = pxnew
     gy0(i) = pynew
     gz0(i) = pznew
     ft0(i) = enenew
     px1 = px0(i)
     py1 = py0(i)
     pz1 = pz0(i)
     e1 = e0(i)
     Call lorenz(e1, px1, py1, pz1, bex, bey, bez)
     px0(i) = pxnew
     py0(i) = pynew
     pz0(i) = pznew
     e0(i) = enenew
  End Do
  Return
End Subroutine boosti


SUBROUTINE DRESSEDBASIS_SP(D,ID,NM,MODES_NUM,FIELDS,U_FD,E_DRESSED,INFO)

!!$THIS SUBROUTINES CALCULATES THE TRANSFORMATION BETWEEN THE BARE BASIS TO THE DRESSED BASIS DEFINDED BY THE FULL SET OF DRIVING FIELDS.
!!$ D                            : DIMENSION OF THE MULTIMODE EXTENDED HILBERT SPACE
!!$ ID (IN)                      : TYPE OF QUATUM SYSTEM
!!$ NM (IN)                      : NUMBER OF MODES == NUMBER OF DRIVING FIELDS
!!$ MODES_NUM                    : VECTOR INDICATING THE NUMBER OF HARMONICS OF EACH DRESSING FIELD
!!$ FIELDS (IN)                  : AMPLITUDE, FREQUENCY AND PHASES OF ALL DRIVING FIELDS
!!$ U_FD (OUT)                   : THIS IS THE TRANSFORMATION WE ARE LOOKING FOR
!!$ E_DRESSED (OUT)              : DRESSED ENERGIES
!!$ INFO (INOUT)                 : INFO = 0 MEANS SUCESS

  USE ATOMIC_PROPERTIES
  USE TYPES
  USE SPARSE_INTERFACE
  USE SUBINTERFACE
  USE SUBINTERFACE_LAPACK
!  USE FLOQUETINIT_
  USE ARRAYS

  IMPLICIT NONE
  TYPE(MODE), DIMENSION(NM),      INTENT(INOUT)    :: FIELDS
  TYPE(ATOM),                     INTENT(IN)    :: ID
  INTEGER,    DIMENSION(NM),      INTENT(IN)    :: MODES_NUM
  COMPLEX*16, DIMENSION(D,D),     INTENT(OUT)   :: U_FD
  DOUBLE PRECISION, DIMENSION(D), INTENT(OUT)   :: E_DRESSED
  INTEGER,                        INTENT(IN)    :: NM,D
  INTEGER,                        INTENT(INOUT) :: INFO

  INTEGER r,m,FIELD_INDEX
  !INTEGER ND_,NM_
  INTEGER TOTAL_FREQUENCIES

    !PARAMETERS NEEDED TO DEFINE THE SPARSE MATRIX
  INTEGER,    DIMENSION(:), ALLOCATABLE :: ROW_INDEX,COLUMN
  COMPLEX*16, DIMENSION(:), ALLOCATABLE :: VALUES
  !PARAMETERS TO DIAGONALIZE THE SPARSE MATRIX WITH MKL
  DOUBLE PRECISION :: E_L,E_R

  TOTAL_FREQUENCIES = SUM(MODES_NUM,1)

  CALL SETHAMILTONIANCOMPONENTS(ID,NM,TOTAL_FREQUENCIES,MODES_NUM,FIELDS,INFO)

  !--- FIND THE MULTIMODE FLOQUET SPECTRUM
  CALL MULTIMODEFLOQUETMATRIX_SP(ID,NM,TOTAL_FREQUENCIES,MODES_NUM,FIELDS,VALUES,ROW_INDEX,COLUMN,INFO)

  E_L =  -2.0*MAXVAL(ABS(VALUES))
  E_R =   2.0*MAXVAL(ABS(VALUES))
  E_DRESSED = 0.0
  U_FD = 0.0
  CALL MKLSPARSE_FULLEIGENVALUES(D,SIZE(VALUES,1),VALUES,ROW_INDEX,COLUMN,E_L,E_R,E_DRESSED,U_FD,INFO)

END SUBROUTINE DRESSEDBASIS_SP

SUBROUTINE DRESSEDBASIS_SUBSET_SP(ID,DRESSINGFLOQUETDIMENSION,DRESSINGFIELDS,NM,&
        & DRESSINGFIELDS_INDICES,MODES_NUM,FIELDS,U_FD,E_DRESSED,INFO)

!!$THIS SUBROUTINES CALCULATES THE TRANSFORMATION BETWEEN THE BARE BASIS TO THE DRESSED BASIS DEFINDED BY A SUBSET OF THE DRIVING FIELDS.
!!$
!!$ ID (IN)                      : TYPE OF QUATUM SYSTEM
!!$ DRESSINGFLOQUETDIMENSION(IN) : DIMENSION OF THE DRESSED FLOQUET HILBERT SPACE
!!$ DRESSINGFIELDS (IN)          : NUMBER OF DRESSING FIELDS
!!$ NM (IN)                      : NUMBER OF MODES == NUMBER OF DRIVING FIELDS
!!$ DRESSINGFIELDS_INDICES       : OUT OF ALL THE DRIVING FIELDS, THIS VECTOR INDICATES THE INDEX OF THE DRESSING FIELD
!!$ MODES_NUM                    : VECTOR INDICATING THE NUMBER OF HARMONICS OF EACH DRESSING FIELD
!!$ FIELDS (IN)                  : AMPLITUDE, FREQUENCY AND PHASES OF ALL DRIVING FIELDS
!!$ U_FD (OUT)                   : THIS IS THE TRANSFORMATION WE ARE LOOKING FOR
!!$ E_DRESSED (OUT)              : DRESSED ENERGIES
!!$ INFO (INOUT)                 : INFO = 0 MEANS SUCESS

  USE ATOMIC_PROPERTIES
  USE TYPES
  USE SPARSE_INTERFACE
  USE SUBINTERFACE
  USE SUBINTERFACE_LAPACK
!  USE FLOQUETINIT_
  USE ARRAYS

  IMPLICIT NONE
  INTEGER,                                               INTENT(IN)    :: NM
  INTEGER,                                               INTENT(IN)    :: DRESSINGFLOQUETDIMENSION,DRESSINGFIELDS
  TYPE(MODE), DIMENSION(NM),                             INTENT(INOUT)    :: FIELDS
  TYPE(ATOM),                                            INTENT(IN)    :: ID
  INTEGER,    DIMENSION(DRESSINGFIELDS),                 INTENT(IN)    :: DRESSINGFIELDS_INDICES
  INTEGER,    DIMENSION(NM),                             INTENT(IN)    :: MODES_NUM
  COMPLEX*16, DIMENSION(DRESSINGFLOQUETDIMENSION,DRESSINGFLOQUETDIMENSION),       INTENT(OUT)   :: U_FD
  DOUBLE PRECISION, DIMENSION(DRESSINGFLOQUETDIMENSION), INTENT(OUT)   :: E_DRESSED
  INTEGER,                                               INTENT(INOUT) :: INFO

  INTEGER r,m,FIELD_INDEX
  INTEGER ND_,NM_
  INTEGER TOTAL_FREQUENCIES_
  TYPE(MODE), DIMENSION(:),ALLOCATABLE  :: FIELDS_
  INTEGER,    DIMENSION(DRESSINGFIELDS) :: MODES_NUM_

    !PARAMETERS NEEDED TO DEFINE THE SPARSE MATRIX
  INTEGER,    DIMENSION(:), ALLOCATABLE :: ROW_INDEX,COLUMN
  COMPLEX*16, DIMENSION(:), ALLOCATABLE :: VALUES
  !PARAMETERS TO DIAGONALIZE THE SPARSE MATRIX WITH MKL
  DOUBLE PRECISION :: E_L,E_R

!  write(*,*)"SP A:", DRESSINGFIELDS_INDICES
  DO r=1,DRESSINGFIELDS
     MODES_NUM_(r)=modes_num(DRESSINGFIELDS_INDICES(r))
  END DO

  TOTAL_FREQUENCIES_ = SUM(MODES_NUM_,1)
  ALLOCATE(FIELDS_(TOTAL_FREQUENCIES_))
  DO m=1,TOTAL_FREQUENCIES_
     ALLOCATE(FIELDS_(m)%V(ID%D_BARE,ID%D_BARE))
  END DO
 ! write(*,*) "SP:",dressingfields,modes_num_,total_frequencies_

  FIELD_INDEX = 1
  DO r=1,DRESSINGFIELDS
     DO m=1,MODES_NUM_(r)
        FIELDS_(FIELD_INDEX) = FIELDS(DRESSINGFIELDS_INDICES(r)+m-1)
        FIELD_INDEX = FIELD_INDEX+1
     END DO
  END DO

  CALL SETHAMILTONIANCOMPONENTS(ID,size(modes_num_,1),total_frequencies_,MODES_NUM_,FIELDS_,INFO)

  !--- FIND THE MULTIMODE FLOQUET SPECTRUM
  CALL MULTIMODEFLOQUETMATRIX_SP(ID,SIZE(MODES_NUM_,1),total_frequencies_,MODES_NUM_,FIELDS_,VALUES,ROW_INDEX,COLUMN,INFO)
!  write(*,*) real(Values)
!  write(*,*) size(values,1), size(row_index,1), size(column,1)
  E_L =  -4.0*MAXVAL(ABS(VALUES))
  E_R =   4.0*MAXVAL(ABS(VALUES))
  E_DRESSED = 0.0
  U_FD = 0.0
!  write(*,*) "Bounds:",E_L,E_R,DRESSINGFLOQUETDIMENSION,SIZE(VALUES,1),VALUES
  CALL MKLSPARSE_FULLEIGENVALUES(DRESSINGFLOQUETDIMENSION,SIZE(VALUES,1),VALUES,ROW_INDEX,COLUMN,E_L,E_R,E_DRESSED,U_FD,INFO)

  DEALLOCATE(FIELDS_)

END SUBROUTINE DRESSEDBASIS_SUBSET_SP


program ofmd_fft
use, intrinsic :: iso_fortran_env, only: output_unit
use types, only: dp
use constants, only: i_, K2au, density2gcm3, u2au, s2au, Ha2eV
use md, only: velocity_verlet, positions_random, &
                calc_min_distance, positions_fcc, positions_bcc
use ewald_sums, only: ewald_box
use random, only: randn
use utils, only: init_random, stop_error, assert, linspace, clock, loadtxt
use ofdft, only: read_pseudo
use ofdft_fft, only: free_energy_min, radial_potential_fourier, &
    reciprocal_space_vectors, real2fourier, fourier2real, logging_info
use interp3d, only: trilinear
use poisson3d_assembly, only: func2quad
implicit none

! All variables are in Hartree atomic units

! XLBOMD parameters:
integer, parameter :: K = 5
real(dp), parameter :: kappa = 1.82_dp, alpha = 0.018_dp
integer, parameter :: c0 = -6, c1 = 14, c2 = -8, c3 = -3, c4 = 4, c5 = -1

integer :: N
integer :: i, j, steps, u
real(dp) :: dt, L, t, rho, scf_eps
real(dp), allocatable :: V(:, :), X(:, :), f(:, :), m(:)
real(dp), allocatable :: R(:), Ven_rad(:), &
    G(:, :, :, :), G2(:, :, :)
real(dp), allocatable :: Ven0G(:, :, :)
complex(dp), allocatable :: VenG(:, :, :), neG(:, :, :)
real(dp), allocatable :: ne(:, :, :), R2(:)
real(dp) :: Temp, Ekin, Temp_current, t3, t4
real(dp) :: Ediff, Z, Am
real(dp) :: Een_correction
real(dp) :: Eee, Een, Ts, Exc, Etot, Enn
real(dp), allocatable :: fnn(:, :), q(:), fen(:, :)
integer :: dynamics, functional, Ng, Nspecies, start, Nmesh
integer :: cg_iter
real(dp) :: sigma
real(dp), dimension(:,:,:,:), allocatable :: ne_aux
character(len=10) :: atom_name
real(dp) :: mdt1, mdt2

logging_info = .false. ! Turn of the INFO warnings

call read_input("OFMD.input", Temp, rho, Nspecies, N, Am, atom_name, start, &
    dynamics, functional, Ng, scf_eps, steps, dt)
call read_pseudo("fem/" // trim(atom_name) // ".pseudo", R, Ven_rad, Z, Ediff)
allocate(V(3, N), f(3, N), m(N))
allocate(Ven0G(Ng, Ng, Ng), VenG(Ng, Ng, Ng), ne(Ng, Ng, Ng), neG(Ng, Ng, Ng))
allocate(G(Ng, Ng, Ng, 3), G2(Ng, Ng, Ng))
allocate(fnn(3, N), q(N), fen(3, N))
allocate(ne_aux(Ng, Ng, Ng, -K:1))

q = Z
m = Am * u2au ! Using Hydrogen mass in atomic mass units [u]
L = (sum(m) / rho)**(1._dp/3)
print *, "----------------------------------------------------------------"
print *, "Input Summary:"
print *, "N =", N
print *, "Ng =", Ng
print *, "MD steps =", steps
print *, "rho = ",  rho, "a.u. =", rho * density2gcm3, "g/cm^3"
print *, "T =", Temp, "a.u. =", Temp / K2au, "K ="
print *, "  =", Temp * Ha2eV, "eV"
print "(' dt =', f8.2, ' a.u. = ', es10.2, ' s = ', es10.2, ' ps')", dt, &
    dt/s2au, dt/s2au * 1e12_dp
print "(' SCF_eps =', es10.2, ' a.u. = ', es10.2, ' eV')", scf_eps, &
    scf_eps * Ha2eV
print *, "Initial position:", start
print *, "Atomic mass:", Am, "u"
print *, "Atomic name: ", trim(atom_name)
print *, "----------------------------------------------------------------"
print *
print *, "Calculated quantities:"
print *, "L =", L, "a.u."
print *

print *, "Converting aperiodic radial Ven to periodic cartesian Ven"
call radial_potential_fourier(R, Ven_rad, L, Z, Ven0G, Een_correction)
print *, "  Done."
print *, "Atomic charge Z =", Z
Nmesh = 10000
allocate(R2(Nmesh))
R2 = linspace(0._dp, L/2, Nmesh)

call reciprocal_space_vectors([L, L, L], G, G2)

select case(start)
    case (0)
        print *, "Initial position: pos.txt"
        call loadtxt("pos.txt", X)
        call assert(size(X, 1) == 3)
        call assert(size(X, 2) == N)
    case (3)
        print *, "Initial position: FCC"
        allocate(X(3, N))
        call positions_fcc(X, L)
    case (4)
        print *, "Initial position: BCC"
        allocate(X(3, N))
        call positions_bcc(X, L)
    case (5)
        print *, "Initial position: random"
        ! Make it deterministic for now
        !call init_random()
        sigma = 1
        allocate(X(3, N))
        call positions_random(X, L, 2**(1._dp/6)*sigma, 10)
    case default
        call stop_error("Invalid initial condition.")
end select

print *, "Positions:"
do i = 1, N
    print *, i, X(:, i)
end do
print *, "Distances:"
do i = 2, N
    print *, i, calc_min_distance(X(:, :i-1), L, X(:, i))
end do
print *

! Initialize velocities based on Maxwell-Boltzmann distribution
call randn(V)
V = V * sqrt(Temp / spread(m, 1, 3))

Ekin = calc_Ekin(V, m)
Temp_current = 2*Ekin/(3*N)

! The average temperature (i.e. if we average Temp_current for many runs) will
! be Temp. But we want to set it exactly to Temp, so we rescale the velocities.
V = V * sqrt(Temp / Temp_current)

print *, "MD start:"

t = 0
ne = N * Z / L**3

open(newunit=u, file="ofmd_results.txt", status="replace")
call write_results_header(u)
close(u)

t = 0
call cpu_time(t3)
do i = 1, steps
    mdt1 = clock()
    print *, "Starting MD iteration:", i

    if (i > 10) then
        ! ne_aux(:, :, :,  1) ... n_{i+1}
        ! ne_aux(:, :, :,  0) ... n_{i}
        ! ne_aux(:, :, :, -1) ... n_{i-1}
        ! ne_aux(:, :, :, -2) ... n_{i-2}
        ! ne_aux(:, :, :, -3) ... n_{i-3}
        ! ne_aux(:, :, :, -4) ... n_{i-4}
        ! ne_aux(:, :, :, -5) ... n_{i-5}
        if (i < 100) then
            ne_aux(:, :, :, 1) = 2*ne - ne_aux(:, :, :, -1) &
                + alpha*(c0*ne &
                + c1*ne_aux(:, :, :, -1) + c2*ne_aux(:, :, :, -2) &
                + c3*ne_aux(:, :, :, -3) + c4*ne_aux(:, :, :, -4) &
                + c5*ne_aux(:, :, :, -5))
        else
            ne_aux(:, :, :, 1) = 2*ne_aux(:, :, :, 0) - ne_aux(:, :, :, -1) &
                + kappa*(ne-ne_aux(:, :, :, 0)) + alpha*(c0*ne_aux(:, :, :, 0) &
                + c1*ne_aux(:, :, :, -1) + c2*ne_aux(:, :, :, -2) &
                + c3*ne_aux(:, :, :, -3) + c4*ne_aux(:, :, :, -4) &
                + c5*ne_aux(:, :, :, -5))
        end if
    else
        ne_aux(:, :, :, 1) = ne
    end if

    do j = 0, -K, -1
        ne_aux(:, :, :, j) = ne_aux(:, :, :, j+1)
    end do

    ne = abs(ne_aux(:, :, :, 1))

    if (i == 1) then
        call forces(X, f)
    else
        call velocity_verlet(dt, m, L, forces, f, V, X)
    end if
    Ekin = calc_Ekin(V, m)
    Temp_current = 2*Ekin/(3*N)

    print *, "Nuclear forces:"
    print *, fnn(:, 1)
    print *, fnn(:, 2)
    print *, fnn(:, 3)
    print *, fnn(:, 4)

    print *, "Electronic forces:"
    print *, fen(:, 1)
    print *, fen(:, 2)
    print *, fen(:, 3)
    print *, fen(:, 4)

    print *, "total forces:"
    print *, f(:, 1)
    print *, f(:, 2)
    print *, f(:, 3)
    print *, f(:, 4)

    mdt2 = clock()
    open(newunit=u, file="ofmd_results.txt", position="append", status="old")
    call write_results_line(u)
    close(u)

    call write_results_header(output_unit)
    call write_results_line(output_unit)

    t = t + dt
end do
call cpu_time(t4)
close(u)
print "('TIMINGS')"
print "('MD run:      ', f10.4, 's')", t4-t3
print "('MD step:     ', f10.4, 's')", (t4-t3)/steps

contains

    subroutine forces(X, f)
    real(dp), intent(in) :: X(:, :) ! positions
    real(dp), intent(out) :: f(:, :) ! forces
    real(dp) :: stress(6)
    real(dp) :: fac(Ng, Ng, Ng)
    integer :: i
    print *, "-------------------------------------"
    print *, "Calculating forces"
    ! Calculate nuclear forces
    print *, "Calculating nuclear forces"
    call ewald_box(L, X, q, Enn, fnn, stress)

    ! Calculate the electronic forces
    print *, "Calculating VenG"
    VenG = 0
    do i = 1, N
        ! Note: we use minus sign in the forward real -> fourier transform.
        ! Then the following holds:
        !
        !         F[f(x+b)] = F[f(x)]*e^{+i*G*b},
        !
        ! with plus sign in the exponential. Finally, we are expressing
        ! Ven0(x-X) using the above formula (with b=-X) in terms of Ven0(x),
        ! i.e.:
        !
        !         F[Ven0(x-X)] = F[Ven0(x)]*e^{-i*G*X},
        !
        ! with minus sign in the exponential.
        VenG = VenG - Ven0G * exp(i_ * &
            (G(:,:,:,1)*X(1,i) + G(:,:,:,2)*X(2,i) + G(:,:,:,3)*X(3,i)))
    end do
    call assert(abs(VenG(1, 1, 1)) < epsilon(1._dp)) ! The G=0 component

    ! Energy calculation
    print *, "Minimizing free energy"
    call free_energy_min(N*Z, N, L, G2, Temp, VenG, ne, scf_eps, &
            Eee, Een, Ts, Exc, Etot, cg_iter)

    ! Forces calculation
    print *, "ne -> neG"
    call real2fourier(ne, neG)

    Een = Een + Een_correction * real(neG(1, 1, 1), dp) * N
    Etot = Eee + Een + Ts + Exc

    fen = 0
    print *, "Calculating fen"
    do i = 1, N
        ! We have minus sign in the exponential, per the definition of VenG
        ! above (see the comment there).
        fac = L**3*Ven0G*aimag(neG*exp(-i_ * &
            (G(:,:,:,1)*X(1,i) + G(:,:,:,2)*X(2,i) + G(:,:,:,3)*X(3,i))))
        fen(1, i) = sum(G(:,:,:,1)*fac)
        fen(2, i) = sum(G(:,:,:,2)*fac)
        fen(3, i) = sum(G(:,:,:,3)*fac)
    end do

    f = fnn + fen
    print *, "Done calculating forces."
    print *, "-------------------------------------"
    end subroutine

    real(dp) pure function calc_Ekin(V, m) result(Ekin)
    real(dp), intent(in) :: V(:, :) ! velocities
    real(dp), intent(in) :: m(:) ! masses
    Ekin = sum(m*sum(V**2, dim=1))/2
    end function

    subroutine read_input(filename, T, density, Nspecies, N, Am, Aname, &
        start, dynamics, functional, Ng, scf_eps, steps, dt)
    ! Reads the input file, returns values in a.u.
    character(len=*), intent(in) :: filename
    real(dp), intent(out) :: T, density, dt, scf_eps, Am
    character(len=*), intent(out) :: Aname
    integer, intent(out) :: Nspecies, N, start, dynamics, functional, Ng, steps
    integer :: AN
    real(dp) :: AZ
    integer :: u
    real(dp) :: skip
    open(newunit=u, file=filename, status="old")
    read(u, *) T, density, Nspecies, N
    read(u, *) start
    read(u, *) dynamics, functional, Ng, skip, skip, scf_eps
    read(u, *) steps, dt
    read(u, *) Aname, AZ, Am, AN
    close(u)
    ! Convert to atomic units:
    T = T / Ha2eV
    density = density / density2gcm3
    scf_eps = scf_eps / Ha2eV

    if (AN /= N) call stop_error("Inconsistent number of atoms")
    end subroutine

    subroutine write_results_header(u)
    integer, intent(in) :: u
    write(u, '(12a17)') "Time [a.u.]", "Fe [eV]", "Unn [eV]", "K [eV]", &
        "F [eV]", "T [eV]", "Ts [eV]", "Een [eV]", "Eee [eV]", "Exc [eV]", &
        "CG iter", "MD step time [s]"
    end subroutine

    subroutine write_results_line(u)
    integer, intent(in) :: u
    write(u, '(10f17.6, i17, f17.6)') &
        t, Etot*Ha2eV/N, Enn*Ha2eV/N, Ekin*Ha2eV/N, &
        (Etot + Enn + Ekin) * Ha2eV / N, Temp_current * Ha2eV, Ts * Ha2eV / N, &
        Een * Ha2eV / N, Eee * Ha2eV / N, Exc * Ha2eV / N, cg_iter, &
        mdt2 - mdt1
    end subroutine

end program


SUBROUTINE sweep(g,e)

!-------------------------------------------------------------
!
!  Sweeps across the 3-D matrix
!   Starts at top, far, right corner (mu, eta, xi < 0), then sweeps
!   down all planes and rows, accounting for reflection if necessary. Then
!   sweeps up all planes and rows for xi>0
!
!-------------------------------------------------------------

USE invar
USE solvar
IMPLICIT NONE
INTEGER, INTENT(IN) :: g
INTEGER :: xs, xe, incx, i, m, n, xdir, l, ieq
REAL*8 :: sig, mu, x, a, b, fx, plm1, pl, ea
REAL*8, DIMENSION(0:anord) :: tsxs
REAL*8, DIMENSION(nx*sord), INTENT(IN) :: e
REAL*8, DIMENSION(nx*sord) :: et

! Initialize the flux solution to zero
f(:,g) = 0.0

! Start with loop over all angles
DO n = 1, apo
   ! Set up the angles
   mu  = ang(n)

   ! Set the incoming x-flux
   fx = psii(n,1)

   ! Perform two loops, one in positive x-direction, then negative
   DO xdir = 1, 2
      IF (xdir == 1) THEN
         xs = 1
         xe = nx
         incx = 1
      ELSE IF (xdir == 2) THEN
         xs = nx
         xe = 1
         incx = -1
         ! Reset the incoming x-flux for the x-hi bc
         fx = psii(n,2)
      END IF

      ! Start the loop in the positive x-direction
      DO i = xs, xe, incx
         x = dx(i)
         m = mat(i)
         sig = sigt(m,g)
         tsxs = sigs(m,:,g,g)

         ! Get spatial parameters
         ex = mu/x

         ! Initialize 'a' coefficient
         a = 0.0

         ! Contributions from DD outgoing substitutions
         a = a + 2.0*ex
         ! Contribution from total interaction
         a = a + sig

         ! Initialize rhs coefficient
         b = 0.0

         ! Contribution from incoming fluxes
         b = b + 2.0*ex*fx
         ! Contribution from scattering plus fixed sources
         ! Initialize Legendre polynomials
         plm1 = 0.0
         pl   = 1.0
         ea   = 0.0
         DO l = 0, anord
            ieq = (i-1)*sord + 1 + l
            et(ieq) = tsxs(l)*e(ieq) + sm(ieq,g)
            ea = ea + (2.0*l+1.0)*pl*et(ieq)
            CALL legpoly(l,incx,mu,plm1,pl)
         END DO
         b = b + ea

         ! Solve for the flux
         b = b/a

         ! Update the scalar flux
         plm1 = 0.0
         pl   = 1.0
         DO l = 0, anord
            ieq = (i - 1)*sord + 1 + l
            f(ieq,g) = f(ieq,g) + w(n)*pl*b
            CALL legpoly(l,incx,mu,plm1,pl)
         END DO

         ! Compute the outgoing fluxes with the DD equations
         fx = 2.0*b - fx

         ! Store the new angular fluxes at xe
         IF (i == xe) THEN
            psio(n,xdir,g) = fx
         END IF

      ! End loop over x cells
      END DO
   ! End loop over negative and positive x-directions
   END DO

! End loop over angles
END DO

RETURN
END SUBROUTINE sweep


c***********************************************************************
c> \file orrspace.f
c> \brief Solves the Orr-Sommerfeld equation for incompressible boundary
c>        layers solving first the Blasius equation for spatially
C>        growing waves.
c> \author S. Scott Collis
c***********************************************************************
      program OrrSom
c***********************************************************************
c
c     Purpose:  Solve the Orr-Sommerfeld equation using 4th order Runge-
c               Kutta explicit integration with orthogonalization using
c               the method of Conte.
c
c               This routine finds a single spatial eigensolution to
c               the Orr-Sommerfeld equation for the boundary layer.
c
c               This routine works rather well but to get good
c               eigenvalues you must really resolve the boundary-layer
c               profile well.  The OS shooting solution appears to be
c               quite sensitive to the profile?
c
c     Author:   Scott Collis
c
c     Date:     7-17-92
c
c     Revised:  2-8-93     changed to spatial solver
c               2-9-93     cleaned up alot of old diagnostics and
c                          unused variables.
c               2-11-93    Improved the output capabilities and made
c                          and input/output nondimensional using the
c                          displacement thickness.
c               1-11-2020  Update for gfortan and github
c
c***********************************************************************
c     Common variables
c***********************************************************************
      implicit    none
      integer     idim
      parameter   (idim=20000)
      integer     n
      complex     gamma
      real        U(0:idim), d2U(0:idim), ymin, ymax, h, beta
      real        Uspl(0:idim), d2Uspl(0:idim), ydat(0:idim), f2p

      common      /data/  n, u, d2u, ymin, ymax, h, f2p,
     &                    gamma, beta, uspl, d2uspl, ydat

      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
c***********************************************************************
      integer       neq, inunit
      parameter     (neq=4, inunit=20)
      complex       bc1(neq), bc2(neq), c
      real          testalpha, omegar, omegai, alphar, alphai
      real          Domega, fact, facti
      integer       i, nstep, niter
      integer       eigfun
      character*1   input
      character*20  infile
      character*80  string
c
c     Set eigfun=1 if you want eigenfunction and velocity
c     profiles generated.
c
c     fort.10     Mean velocity profile
c     fort.11     phi(y)
c     fort.12     phi'(y)
c     fort.13     phi''(y)
c     fort.14     phi'''(y)
c
c     fort.15     u disturbance
c     fort.16     v disturbance
c     fort.17     eigenvalues during sweep
c     fort.18     u, v eigenfunctions
c
      eigfun = 1
c
c     Get user input
c
      write (*,10)
  10  format (/,10x,'Solve Orr-Sommerfeld (Shooting)',/)
      write (*,20)
  20  format (5x,'This version solves the spatial problem',/)
      write (*,60)
  60  format (/,1x,'Enter filename ==> ',$)
      read (*,'(a)') infile
      open (unit=inunit, file=infile, form='formatted')
      string(1:1) = '#'
      do while (string(1:1) .eq. '#')
        read(inunit,'(a)',err=99) string
      end do
      call readi (string,n)
      read(inunit,'(a)') string
      call readi (string,nstep)
      read(inunit,'(a)') string
      call readr (string,testalpha)
      read(inunit,'(a)') string
      call readr (string,Re)
      read(inunit,'(a)') string
      call readr (string,beta)
      read(inunit,'(a)') string
      call readr (string,f2p)
      read(inunit,'(a)') string
      call readr (string,alphar)
      read(inunit,'(a)') string
      call readr (string,alphai)
      read(inunit,'(a)') string
      call readr (string,omegar)
      read(inunit,'(a)') string
      call readr (string,omegai)
      read(inunit,'(a)') string
      call readr (string,Ymin)
      read(inunit,'(a)') string
      call readr (string,Ymax)
      read(inunit,'(a)') string
      call readi (string,niter)
      read(inunit,'(a)') string
      call readr (string,domega)
      read(inunit,'(a)') string
      call readi (string,eigfun)
      close(inunit)
c
c     Echo input
c
      write (*,100) n
 100  format (/,1x,'n = ',i5)
      write (*,102) nstep
 102  format (1x,'nstep = ',i5)
      write (*,105) testalpha
 105  format (1x,'Test Alpha = ',e20.10)
      write (*,110) Re
 110  format (1x,'Re = ',e20.10)
      write (*,115) beta
 115  format (1x,'Beta = ',e20.10)
      write (*,117) f2p
 117  format (1x,'f2p = ',e20.10)
      write (*,140) ymin, ymax
 140  format (1x,'Ymin = ',e20.10,'  Ymax = ',e20.10)
      write (*,120) alphar, alphai
 120  format (1x,'alpha = (',e20.10,', ',e20.10,')')
      write (*,130) omegar, omegai
 130  format (1x,'omega = (',e20.10,', ',e20.10,')',/)
c
c     Set constants
c
      alpha = cmplx(alphar,alphai)
      omega = cmplx(omegar,omegai)
c
c     Fix to make my nondimensionalization match Mack's
c     using displacement thickness to nondimensionalize
c
#define USE_DISPLACEMENT_THICKNESS
#ifdef USE_DISPLACEMENT_THICKNESS
      write(*,150)
 150  format(/,1x,'Note:  changing nondimsensionalization to ',
     &       'displacement thickness to match Mack...',/)
      fact = SQRT(2.0)/1.7207876
      facti = 1.0/fact
      Re = Re*fact
      alpha = alpha*fact
      omega = omega*fact
      Domega = Domega*fact
      ymin = ymin*fact
      ymax = ymax*fact
      h = (ymax-ymin)/float(n)
#endif

      if (n .gt. idim) then
        write (*,300)
 300    format (/,/,1x,'N > Idim...Stopping',/,/)
        goto 210
      end if

      call SOLVE_BL
c
c     set the boundary conditions
c
      do i = 1, neq
        bc1(i) = 0.0
        bc2(i) = 0.0
      end do

      do i = 1, niter
        call CONTE(nstep,testalpha,neq,2,bc1,bc2,ymin,ymax,eigfun)
        write (17,30) REAL(omega*facti),
     &                REAL(alpha*facti),
     &                AIMAG(alpha*facti)
  30    format (1x,3(e17.8,2x))
        omegar = REAL(omega)+Domega
        omegai = AIMAG(omega)
        omega  = CMPLX(omegar,omegai)
      end do

      goto 210
c
c     Read error
c
  99  write (*,200)
 200  format(/,/1x,'Error in input file...',/,/)
 210  continue

      stop
      end

C***********************************************************************
      subroutine CONTE(nstep, testalpha, n, r, yo, yf, to, tf, eigfun)
C***********************************************************************
C
C     First order linear boundary value problem solver using Conte's
C     method.  Fourth order Runge-Kutta is used for time advancement
C
c***********************************************************************
      implicit    none
      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
C***********************************************************************
c
c     Argument declarations
c
      integer     nstep, n, r
      real        testalpha, to, tf
      complex     yo(n), yf(n)
      integer     eigfun
c
c     Other variables
c
      integer     i, j, k, m, q, s, mi, mj, qend, icount
      real        t, tq(0:nstep), h
      complex     B(n-r,0:nstep), Utemp(n)
      complex     U(n,n-r,0:nstep), P(n-r,n-r,0:nstep), z(n,n-r)
      complex     y(n,0:nstep), w(n-r), eta(n)
      complex     olderr, INPROD, ut(n,n-r), max, gamma
      complex     uu(0:nstep), vv(0:nstep)
c
c     Variables for ODEINT
c
      integer     nbad, nok
c
c     Used for 2nd order extrapolation
c
      complex     ctemp, cm1, cm2, err, errm1, errm2
      complex     At, Bt, Ct, qt, fd
      real        fdxr, fdxi, fdyr, fdyi
c
c     Temporary vars for automatic normalization
c
      real        aa, bb, cc
      real        test, pi
      logical     norm
c
c     externally used functions
c
      external    INPROD, FHOMO, FPART, RKQC
c
c     Variables for printing out the velocity
c
      integer     ntime
      real        x, time, dtime
c
c     set the normalization constraint
c
      pi = 3.14159265358979323846
c
c     compute the step size
c
      h = (tf-to)/nstep
c
c     Begin the eigenvalue iteration loop
c
      icount = 1
      err = 1.
      do while ((abs(err) .ge. 1.0e-8) .and. (icount .le. 50) .and.
     &          (abs(alpha-cm1) .ge. 1.0e-12) )
        q = 0
        tq(0) = tf
c
c       Set the initial conditions
c
        k = 0
        U(1,1,0) = CEXP(-alpha*tf)
        U(2,1,0) = (-alpha)*CEXP(-alpha*tf)
        U(3,1,0) = (alpha**2)*CEXP(-alpha*tf)
        U(4,1,0) = (-alpha**3)*CEXP(-alpha*tf)

        gamma = SQRT(alpha**2+(0.,1.)*alpha*Re*(1.0-omega/alpha))

        U(1,2,0) = CEXP(-gamma*tf)
        U(2,2,0) = (-gamma)*CEXP(-gamma*tf)
        U(3,2,0) = (gamma**2)*CEXP(-gamma*tf)
        U(4,2,0) = (-gamma**3)*CEXP(-gamma*tf)
c
c       Gram-Schmidt
c
        w(1) = SQRT(inprod(n, U(1,1,k), U(1,1,k)))
        do i = 1, n
          z(i,1) = U(i,1,k)/w(1)
        end do
        do mi = 2, (n-r)
          do i = 1, n
            eta(i) = U(i,mi,k)
          end do
          do mj = mi-1, 1, -1
            do i = 1, n
              eta(i) = eta(i) - inprod(n, U(1,mi,k), z(1,mj))*z(i,mj)
            end do
          end do
          w(mi) = SQRT(inprod(n, eta, eta))
          do i = 1, n
            z(i,mi) = eta(i)/w(mi)
          end do
        end do
c
c       Now update the U matrix with the orthonormal values
c
        do i = 1, n
          do m = 1, n-r
            U(i,m,k) = z(i,m)
          end do
        end do
c
c       Integrate the homo. and particular equations
c
        do k = 1, nstep
          t = tf - h*k
c
c         Loop thru all homogeneous solutions
c
          do m = 1, n-r
#ifdef USE_NR_ODEINT
            do i = 1, n
              Utemp(i) = U(i,m,k-1)
            end do
            call ODEINT(Utemp,n,t+h,t,1.E-6,-h,1.e-20,nok,nbad,
     &                  FHOMO,RKQC)
            do i = 1, n
              U(i,m,k) = Utemp(i)
            end do
#else
            call CRK4(n, U(1,m,k-1), U(1,m,k), t+h, -h, FHOMO)
#endif
          end do
c
c         Test to see if normalization is required
c
          norm = .false.
          do mi = 1, n-r
            do mj = 1, n-r
              if (mi .ne. mj) then
                aa = ABS(inprod(n, U(1,mi,k), U(1,mi,k)))
                bb = ABS(inprod(n, U(1,mj,k), U(1,mj,k)))
                cc = ABS(inprod(n, U(1,mi,k), U(1,mj,k)))
                test = cc/SQRT(aa*bb)
                if (test .gt. testalpha) norm = .true.
              end if
            end do
          end do
c
c         Perform normalization
c
          if ( norm .or. (k .eq. nstep) ) then
            q = q + 1
            tq(q) = t
            if (k .eq. nstep) then
              qend = q
            end if
c
c           Gram-Schmidt
c
            w(1) = SQRT(inprod(n, U(1,1,k), U(1,1,k)))
            do i = 1, n
              z(i,1) = U(i,1,k)/w(1)
            end do
            do mi = 2, (n-r)
              do i = 1, n
                eta(i) = U(i,mi,k)
              end do
              do mj = mi-1, 1, -1
                do i = 1, n
                  eta(i) = eta(i)-inprod(n,U(1,mi,k),z(1,mj))*z(i,mj)
                end do
              end do
              w(mi) = SQRT(inprod(n, eta, eta))
              do i = 1, n
                z(i,mi) = eta(i)/w(mi)
              end do
            end do
c
c           Now I have the orthonormal basis in z and
c           the norms in w so I can compute the P orthonormalization
c           matrix
c
            do j = 1, n-r
              do i = 1, j
                if (j .eq. i) then
                  P(j,i,q) = 1.0/w(j)
                else
                  P(j,i,q) = 0.0
                  do s = i, j-1
                    P(j,i,q) = P(j,i,q)-inprod(n,U(1,j,k),z(1,s))/w(j)*
     &              P(s,i,q)
                  end do
                end if
              end do
            end do
c
c           Check the P matrix
c
            if (.false.) then
              do i = 1, n
                do m = 1, n-r
                ut(i,m) = 0.0
                  do j = 1, n-r
                    ut(i,m) = ut(i,m) + U(i,j,k)*P(m,j,q)
                  end do
                end do
              end do

              do i = 1,n
                write (*,*) i,(ut(i,m), m = 1, n-r)
              end do
              write (*,*)
              do i = 1,n
                write (*,*) i,( z(i,m), m = 1, n-r)
              end do
              write (*,*)
              write (*,*)
            end if
c
c           Now update the U matrix with the orthonormal values
c
            do i = 1, n
              do m = 1, n-r
                U(i,m,k) = z(i,m)
              end do
            end do

          end if
        end do
c
c       strictly enforce the zero BC
c
        B(1,qend) = -U(1,2,nstep)/U(1,1,nstep)
        B(2,qend) = 1.0
c
c       perform extrapolation.  On first try, just try another
c       value, on the 2nd try do linear extrapolation, and
c       on the third try to quadratic extrapolation.
c
        ctemp = alpha
        if (icount .eq. 1) then
          cm2 = alpha
          err = U(2,1,nstep)*B(1,qend) + U(2,2,nstep)*B(2,qend)
          errm2 = err
          alpha = CMPLX( REAL(alpha)*1.01, AIMAG(alpha)*1.01)
        else if (icount .eq. 2) then
          cm1 = alpha
          err = U(2,1,nstep)*B(1,qend) + U(2,2,nstep)*B(2,qend)
          errm1 = err
          fdxr =  REAL(err-errm2) / REAL(alpha-cm2)
          fdxi = AIMAG(err-errm2) / REAL(alpha-cm2)
          fd = CMPLX( fdxr, fdxi )
          alpha = alpha - err/fd
        else
          err = U(2,1,nstep)*B(1,qend) + U(2,2,nstep)*B(2,qend)
          qt = (alpha-cm1)/(cm1-cm2)
          At = qt*err-qt*(1.+qt)*errm1+qt**2*errm2
          Bt = (2.*qt+1.)*err-(1.+qt)**2*errm1+qt**2*errm2
          Ct = (1.+qt)*err
          if ( ABS(Bt+SQRT(Bt**2-4.*At*Ct)) .gt.
     &         ABS(Bt-SQRT(Bt**2-4.*At*Ct)) )  then
            alpha = ctemp-(ctemp-cm1)*2.*Ct/(Bt+SQRT(Bt**2-4.*At*Ct))
          else
            alpha = ctemp-(ctemp-cm1)*2.*Ct/(Bt-SQRT(Bt**2-4.*At*Ct))
          end if
          cm2 = cm1
          cm1 = ctemp
          errm2 = errm1
          errm1 = err
        end if
        write (*,30) icount,real(ctemp)*1.7207877/SQRT(2.),
     &               aimag(ctemp)*1.7207877/SQRT(2.),
     &               real(err), aimag(err), abs(err)
  30    format (1x,i2,4x,e15.8,1x,e15.8,4x,e15.8,1x,e15.8,4x,e15.8)
        icount = icount + 1
      end do
c
c     Write out the converged eigenvalue
c
      alpha = ctemp
      write (*,40) real(omega)*1.7207877/SQRT(2.),
     &             aimag(omega)*1.7207877/SQRT(2.),
     &             real(alpha)*1.7207877/SQRT(2.),
     &             aimag(alpha)*1.7207877/SQRT(2.)
  40  format (/,'Omega = (',e15.8,1x,e15.8,')',4x,
     &          'Alpha = (',e15.8,1x,e15.8,')',/)
c
c     If you would like to see the eigenfunction
c
      if (eigfun.eq.1) then
c
c       Second Pass
c
        max = 0.0
        k = nstep
        do i = 1, n
          y(i,k) = 0.0
          do m = 1, n-r
            y(i,k) = y(i,k) + U(i,m,k)*B(m,q)
          end do
        end do
        do m = 1, n-r
          B(m,q-1) = 0.0
          do j = 1, n-r
            B(m,q-1) = B(m,q-1) + P(j,m,q)*B(j,q)
          end do
        end do
        do k = nstep-1, 0, -1
          t = tf - h*k
          if ( t .gt. tq(q-1) ) then
            q = q - 1
            do m = 1, n-r
              B(m,q-1) = 0.0
              do j = 1, n-r
                B(m,q-1) = B(m,q-1) + P(j,m,q)*B(j,q)
              end do
            end do
          end if
          do i = 1, n
            y(i,k) = 0.0
            do m = 1, n-r
              y(i,k) = y(i,k) + U(i,m,k)*B(m,q-1)
            end do
          end do
          if ( ABS(y(i,k)) .gt. ABS(max) ) then
            max = y(i,k)
          end if
        end do
c
c       Write out the eigenfunction, its higher derivatives, and
c       u and v disturbance velocities
c
        x = 0.0
        ntime = 8
        do k = 0, nstep
          t = (tf - h*k)*SQRT(2.)/1.7207876
          write (11,20) t, REAL(y(1,k)/max),AIMAG(y(1,k)/max)
          write (12,20) t, REAL(y(2,k)/max),AIMAG(y(2,k)/max)
          write (13,20) t, REAL(y(3,k)/max),AIMAG(y(3,k)/max)
          write (14,20) t, REAL(y(4,k)/max),AIMAG(y(4,k)/max)
          dtime = pi/REAL(omega)/ntime
          write (15,21) t, (REAL(y(2,k)/max*CEXP((0.,1.)*
     &                  (alpha*x-omega*i*dtime))), i = 0, ntime)
          write (16,21) t, (REAL((0.,-1.)*ALPHA*y(1,k)/max*CEXP((0.,1.)*
     &                  (alpha*x-omega*i*dtime))), i = 0, ntime)
  20      format ( 3(es16.8e3,1x) )
  21      format ( es16.8e3, 1x, 16(es16.8e3,1x) )
        end do
c
c.... form u and v
c
        max = 0.0
        do k = 0, nstep
          uu(k) = y(2,k)
          vv(k) = (0.,-1.)*ALPHA*y(1,k)
          if (abs(uu(k)) .gt. abs(max)) max = uu(k)
        end do
        do k = 0, nstep
          t = (tf - h*k)*SQRT(2.)/1.7207876
          uu(k) = uu(k) / max
          vv(k) = vv(k) / max
          write (18,21) t, real(uu(k)), aimag(uu(k)),
     &                     real(vv(k)), aimag(vv(k))
        end do
      end if
c
      return
      end

C***********************************************************************
      function INPROD(n, v1, v2)
C***********************************************************************
C
C     Perform and inner product on two complex vectors, v1 and v2
C
c***********************************************************************
      complex     v1(n), v2(n)
      complex     INPROD
      integer     n, i
c
c     The analytic inner product should yeild faster convergence
c
      INPROD = 0.0
      do i = 1, n
c       INPROD = INPROD + v1(i)*conjg(v2(i))
        INPROD = INPROD + v1(i)*v2(i)
      end do

      return
      end

C***********************************************************************
      subroutine FHOMO(neq, yo,t,yf)
C***********************************************************************
C
C     Function evaluation for the Orr-Sommerfeld equation
C
c***********************************************************************
c     Common variables
c***********************************************************************
      parameter   (idim=20000)
      integer     n
      complex     gamma
      real        U(0:idim), d2U(0:idim), ymin, ymax, h, beta
      real        Uspl(0:idim), d2Uspl(0:idim), ydat(0:idim), f2p

      common      /data/  n, u, d2u, ymin, ymax, h, f2p,
     &                    gamma, beta, uspl, d2uspl, ydat

      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
c***********************************************************************
      integer     neq
      complex     yo(neq), yf(neq)
      real        t
      real        UU, d2UU
      real        Pi
      pi = 3.14159265358979323846
c
c     Get the velocity field
c
      call SPEVAL(n+1,ydat,U,Uspl,t,UU)
      call SPEVAL(n+1,ydat,d2U,d2Uspl,t,d2UU)

      do j = 1 , neq-1
        yf(j) = yo(j+1)
      end do
      yf(neq) = (2.*alpha**2*yo(3)-alpha**4*yo(1)) +
     &          (0.,1.)*alpha*Re*((UU-omega/alpha)*
     &          (yo(3)-alpha**2*yo(1))-d2UU*yo(1))

      return
      end

C***********************************************************************
      subroutine FPART(neq, yo, t, yf)
C***********************************************************************
C
C     Function evaluation for the Orr-Sommerfeld equation
C
c***********************************************************************
c     Common variables
c***********************************************************************
      parameter   (idim=20000)
      integer     n
      complex     gamma
      real        U(0:idim), d2U(0:idim), ymin, ymax, h, beta
      real        Uspl(0:idim), d2Uspl(0:idim), ydat(0:idim), f2p

      common      /data/  n, u, d2u, ymin, ymax, h, f2p,
     &                    gamma, beta, uspl, d2uspl, ydat

      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
c***********************************************************************
      integer     neq
      complex     yo(neq), yf(neq)
      real        t
      real        UU, d2UU
      real        Pi
      pi = 3.14159265358979323846
c
c     Get the velocity field
c
      call SPEVAL(n+1,ydat,U,Uspl,t,UU)
      call SPEVAL(n+1,ydat,d2U,d2Uspl,t,d2UU)

      do j = 1 , neq-1
      yf(j) = yo(j+1)
      end do
      yf(neq) = 2.*alpha**2*yo(3)-alpha**4*yo(1) +
     &          (0.,1.)*alpha*Re*((UU-omega/alpha)*
     &          (yo(3)-alpha**2*yo(1))-d2UU*yo(1))

      return
      end

C***********************************************************************
      subroutine SOLVE_BL
C***********************************************************************
C
C     Integrate the boundary layer similarity equation to get velocity
C     profile.
C
c***********************************************************************
c     Common variables
c***********************************************************************
      parameter   (idim=20000)
      integer     n
      complex     gamma
      real        U(0:idim), d2U(0:idim), ymin, ymax, h, beta
      real        Uspl(0:idim), d2Uspl(0:idim), ydat(0:idim), f2p

      common      /data/  n, u, d2u, ymin, ymax, h, f2p,
     &                    gamma, beta, uspl, d2uspl, ydat

      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
c***********************************************************************
      integer     i, j, k, p
      real        xi(3,0:idim), f(3), eta(3), y
      real        k1(3), k2(3), k3(3), k4(3), err, x2old, f1old
      external    BLASIUS, RKQCR

      do j = 1,3
        do i = 0,n
          xi(j,i) = 0.0
        end do
        f(j) = 0.0
        eta(j) = 0.0
      end do
c
c     Set the boundary conditions including guess for f"(0)
c
      xi(1,0) = 0
      xi(2,0) = 0
      xi(3,0) = f2p
      err = 1.0
      p = 1
      do while ( abs(err) .gt. 1.e-10)

        do i = 1, n
          y = ymin + float(i)*h
#ifdef USE_NR_ODEINT
          do j = 1, 3
            eta(j) = xi(j,i-1)
          end do
          call ODEINTR(eta,3,y-h,y,1.E-7,h/2.,1.e-20,nok,nbad,
     &                 BLASIUS,RKQCR)
          do j = 1, 3
            xi(j,i) = eta(j)
          end do
#else
          call SRK4(3, xi(1,i-1), xi(1,i), y-h, h, BLASIUS)
#endif
        end do
c
c       Check f'(ymax)
c
        if (p .eq. 1) then
          xi3old = xi(3,0)
          xi(3,0) = xi(3,0)*.99
        else
          xi3temp = xi(3,0)
          xi(3,0) = xi(3,0)+((xi3old-xi(3,0))/(xi2old-xi(2,n)))*
     &              (1.0 - xi(2,n))
          xi3old = xi3temp
        end if
        p = p + 1
        xi2old = xi(2,n)
        err = 1.0 - xi2old
c       write (*,*) p, err
      end do
c
c     Now assemble the velocity field
c
      do i = 0, n
        u(i) = xi(2,i)
      end do
c
c     Compute 2nd order finite difference approximation to
c     second derivative
c
      do i = 1, n-1
        d2u(i) = (u(i+1)-2*u(i)+u(i-1))/(h)**2
      end do
      d2u(0) = (-u(4)+4*u(3)-5*u(2)+2*u(1))/(h)**2
      d2u(n) = (2*u(n)-5*u(n-1)+4*u(n-2)-u(n-3))/(h)**2
c
c     Need to interpolate the velocity profile to evaluate it at
c     arbitrary y
c
      do i = 0, n
        ydat(i) = ymin + i*h
      end do
      call SPLINE(n+1,ydat,u,uspl)
c
c     Use the spline result for the second derivative
c
c      do i = 0, n
c        d2u(i) = uspl(i)
c      end do
      call SPLINE(n+1,ydat,d2u,d2uspl)

      do i = 0, n
        y = (ymin + i*h)
        call SPEVAL(n+1,ydat,u,uspl,y,us)
        call SPEVAL(n+1,ydat,d2u,d2uspl,y,d2us)
        write (10,10) y/SQRT(2.)*1.7207876, us, d2us, u(i), d2u(i)
  10    format (1x,5(1PE20.13E2,1x))
      end do

      write (*,20)
  20  format (1x,'Velocity Profile completed...',/)

      return
      end
C***********************************************************************
      subroutine BLASIUS(neq,xi,y,f)
C***********************************************************************
C
C     Function evaluation for the boundary-layer similarity equation
C
c***********************************************************************
c     Common variables
c***********************************************************************
      parameter   (idim=20000)
      integer     n
      complex     gamma
      real        U(0:idim), d2U(0:idim), ymin, ymax, h, beta
      real        Uspl(0:idim), d2Uspl(0:idim), ydat(0:idim), f2p

      common      /data/  n, u, d2u, ymin, ymax, h, f2p,
     &                    gamma, beta, uspl, d2uspl, ydat

      complex     omega, alpha
      real        Re

      common      /eig/   omega, alpha, Re
c***********************************************************************
      integer     neq
      real        xi(neq), f(neq), y

      do j = 1 , 2
        f(j) = xi(j+1)
      end do
      f(3) = -xi(1)*xi(3)-beta*(1.-xi(2)**2)

      return
      end
C***********************************************************************
      SUBROUTINE SPLINE (N,X,Y,FDP)
C***********************************************************************
C
C     Cubic spline is used so that I can interpolate the velocity
C     profile.
C
C***********************************************************************
C-----THIS SUBROUTINE COMPUTES THE SECOND DERIVATIVES NEEDED
C-----IN CUBIC SPLINE INTERPOLATION.  THE INPUT DATA ARE:
C-----N = NUMBER OF DATA POINTS
C-----X = ARRAY CONTAINING THE VALUES OF THE INDEPENDENT VARIABLE
C-----    (ASSUMED TO BE IN ASCENDING ORDER)
C-----Y = ARRAY CONTAINING THE VALUES OF THE FUNCTION AT THE
C-----    DATA POINTS GIVEN IN THE X ARRAY
C-----THE OUTPUT IS THE ARRAY FDP WHICH CONTAINS THE SECOND
C-----DERIVATIVES OF THE INTERPOLATING CUBIC SPLINE.
      DIMENSION X(N),Y(N),A(N),B(N),C(N),R(N),FDP(N)
C-----COMPUTE THE COEFFICIENTS AND THE RHS OF THE EQUATIONS.
C-----THIS ROUTINE USES THE CANTILEVER CONDITION.  THE PARAMETER
C-----ALAMDA (LAMBDA) IS SET TO 1. BUT THIS CAN BE USER-MODIFIED.
C-----A,B,C ARE THE THREE DIAGONALS OF THE TRIDIAGONAL SYSTEM;
C-----R IS THE RIGHT HAND SIDE.  THESE ARE NOW ASSEMBLED.
      ALAMDA = 1.
      NM2 = N - 2
      NM1 = N - 1
      C(1) = X(2) - X(1)
      DO 1 I=2,NM1
      C(I) = X(I+1) - X(I)
      A(I) = C(I-1)
      B(I) = 2.*(A(I) + C(I))
      R(I) = 6.*((Y(I+1) - Y(I))/C(I) - (Y(I) - Y(I-1))/C(I-1))
    1 CONTINUE
      B(2) = B(2) + ALAMDA * C(1)
      B(NM1) = B(NM1) + ALAMDA * C(NM1)
C-----AT THIS POINT WE COULD CALL A TRIDIAGONAL SOLVER SUBROUTINE
C-----BUT THE NOTATION IS CLUMSY SO WE WILL SOLVE DIRECTLY.  THE
C-----NEXT SECTION SOLVES THE SYSTEM WE HAVE JUST SET UP.
      DO 2 I=3,NM1
      T = A(I)/B(I-1)
      B(I) = B(I) - T * C(I-1)
      R(I) = R(I) - T * R(I-1)
    2 CONTINUE
      FDP(NM1) = R(NM1)/B(NM1)
      DO 3 I=2,NM2
      NMI = N - I
      FDP(NMI) = (R(NMI) - C(NMI)*FDP(NMI+1))/B(NMI)
    3 CONTINUE
      FDP(1) = ALAMDA * FDP(2)
      FDP(N) = ALAMDA * FDP(NM1)
C-----WE NOW HAVE THE DESIRED DERIVATIVES SO WE RETURN TO THE
C-----MAIN PROGRAM.
      RETURN
      END
C***********************************************************************
      SUBROUTINE SPEVAL (N,X,Y,FDP,XX,F)
C***********************************************************************
C-----THIS SUBROUTINE EVALUATES THE CUBIC SPLINE GIVEN
C-----THE DERIVATIVE COMPUTED BY SUBROUTINE SPLINE.
C-----THE INPUT PARAMETERS N,X,Y,FDP HAVE THE SAME
C-----MEANING AS IN SPLINE.
C-----XX = VALUE OF INDEPENDENT VARIABLE FOR WHICH
C-----     AN INTERPOLATED VALUE IS REQUESTED
C-----F =  THE INTERPOLATED RESULT
      DIMENSION X(N),Y(N),FDP(N)
C-----THE FIRST JOB IS TO FIND THE PROPER INTERVAL.
      NM1 = N - 1
      DO 1 I=1,NM1
      IF (XX.LE.X(I+1)) GO TO 10
    1 CONTINUE
C-----NOW EVALUATE THE CUBIC
   10 DXM = XX - X(I)
      DXP = X(I+1) - XX
      DEL = X(I+1) - X(I)
      F = FDP(I)*DXP*(DXP*DXP/DEL - DEL)/6.
     1   +FDP(I+1)*DXM*(DXM*DXM/DEL - DEL)/6.
     2   +Y(I)*DXP/DEL + Y(I+1)*DXM/DEL
      RETURN
      END
C***********************************************************************
      subroutine READI(string, I)
C***********************************************************************
C
C     Read an integer from an input file.  This allows for comment
C     statements in the input file
C
c***********************************************************************
      integer       I, iloc, floc
      character*80  string

      iloc = index (string,'=')
      if (iloc .ne. 0) then
        iloc = iloc + 1
        floc = index (string,'#')
        if (floc .eq. 0) then
          floc = 80
        else
          floc = floc - 1
        end if
        read (string(iloc:floc),'(I10)',err=99) I
      else
        write (*,10)
  10    format (/,/,1x,'ERROR in input file...',/,/)
        stop
      end if
      goto 100
c
c     Error
c
  99  continue
      write (*,10)
      stop

 100  return
      end
C***********************************************************************
      subroutine READR(string, R)
C***********************************************************************
C
C     Read an integer from an input file.  This allows for comment
C     statements in the input file.
C
c***********************************************************************
      integer       iloc, floc
      real          R
      character*80  string

      iloc = index (string,'=')
      if (iloc .ne. 0) then
        iloc = iloc + 1
        floc = index (string,'#')
        if (floc .eq. 0) then
          floc = 80
        else
          floc = floc - 1
        end if
        read (string(iloc:floc),'(g20.10)',err=99) R
      else
        write (*,10)
 10     format (/,/,1x,'ERROR in input file...',/,/)
        stop
      end if
      goto 100
c
c     Error
c
  99  continue
      write (*,10)
      stop

 100  return
      end
C***********************************************************************
      subroutine CRK4(neq, yo, yf, to, h, FUNC)
C***********************************************************************
C
C     Advance one time step using fourth order (complex) Runge-Kutta
C
C     This is my own time advance routine that has been superseeded
C     by the adaptive routines, ODEINT and ODEINTR (requires NR license)
C
c***********************************************************************
      external    FUNC
      integer     neq
      real        to, h
      complex     yo(neq), yf(neq)
      complex     f(neq), k1(neq), k2(neq), k3(neq), k4(neq), q(neq)

      call FUNC(neq, yo, to, f)
      do j = 1 , neq
        k1(j) = h*f(j)
        q(j) = yo(j) + 0.5*k1(j)
      end do
      call FUNC(neq, q, to+0.5*h, f)
      do j = 1 , neq
        k2(j) = h*f(j)
        q(j) = yo(j) + 0.5*k2(j)
      end do
      call FUNC(neq, q, to+0.5*h, f)
      do j = 1 , neq
        k3(j) = h*f(j)
        q(j) = yo(j) + k3(j)
      end do
      call FUNC(neq, q, to+h, f)
      do j = 1 , neq
        k4(j) = h*f(j)
        yf(j) = yo(j)+k1(j)/6.+(k2(j)+k3(j))/3.+k4(j)/6.
      end do

      return
      end
C***********************************************************************
      subroutine SRK4(neq, yo, yf, to, h, FUNC)
C***********************************************************************
C
C     Advance one time step using fourth order (complex) Runge-Kutta
C
C     This is my own time advance routine that has been superseeded
C     by the adaptive routines, ODEINT and ODEINTR (requires NR license)
C
c***********************************************************************
      external    FUNC
      integer     neq
      real        to, h
      real        yo(neq), yf(neq)
      real        f(neq), k1(neq), k2(neq), k3(neq), k4(neq), q(neq)

      call FUNC(neq, yo, to, f)
      do j = 1 , neq
        k1(j) = h*f(j)
        q(j) = yo(j) + 0.5*k1(j)
      end do
      call FUNC(neq, q, to+0.5*h, f)
      do j = 1 , neq
        k2(j) = h*f(j)
        q(j) = yo(j) + 0.5*k2(j)
      end do
      call FUNC(neq, q, to+0.5*h, f)
      do j = 1 , neq
        k3(j) = h*f(j)
        q(j) = yo(j) + k3(j)
      end do
      call FUNC(neq, q, to+h, f)
      do j = 1 , neq
        k4(j) = h*f(j)
        yf(j) = yo(j)+k1(j)/6.+(k2(j)+k3(j))/3.+k4(j)/6.
      end do

      return
      end


module make_plot_module

  use iso_c_binding
  use variables_module
  use chemistry_module, only : nspecies

  implicit none

  integer, parameter :: pv_rho = 1
  integer, parameter :: pv_vx  = 2
  integer, parameter :: pv_vy  = 3
  integer, parameter :: pv_vz  = 4
  integer, parameter :: pv_T   = 5
  integer, parameter :: pv_Y   = 6

  private

  public :: make_plot_3d

contains

  subroutine make_plot_3d(lo, hi, pv, plo, phi, u, ulo, uhi) bind(c,name='make_plot_3d')
    integer, intent(in) :: lo(3), hi(3), plo(3), phi(3), ulo(3), uhi(3)
    double precision, intent(in)    ::  u(ulo(1):uhi(1),ulo(2):uhi(2),ulo(3):uhi(3),ncons)
    double precision, intent(inout) :: pv(plo(1):phi(1),plo(2):phi(2),plo(3):phi(3),nspecies+5)

    integer :: i, j, k, n, iwrk, ierr
    double precision :: rho, rhoinv, rwrk, Y(nspecies), ei, Tt

    do k = lo(3), hi(3)
       do j = lo(2), hi(2)
          do i = lo(1), hi(1)

             rho = u(i,j,k,irho)
             rhoinv = 1.d0/rho
             pv(i,j,k,pv_rho) = rho
             pv(i,j,k,pv_vx ) = u(i,j,k,imx) * rhoinv
             pv(i,j,k,pv_vy ) = u(i,j,k,imy) * rhoinv
             pv(i,j,k,pv_vz ) = u(i,j,k,imz) * rhoinv

             do n = 1, nspecies
                Y(n) = u(i,j,k,iry1+n-1) * rhoinv
                pv(i,j,k,pv_Y+n-1) = Y(n)
             end do

             ei = rhoinv*u(i,j,k,iene) - 0.5d0*(pv(i,j,k,pv_vx)**2+pv(i,j,k,pv_vy)**2+pv(i,j,k,pv_vz)**2)
             Tt = 1.d3
             call get_T_given_ey(ei, Y, iwrk, rwrk, Tt, ierr)
             pv(i,j,k,pv_T) = Tt

          end do
       end do
    end do
  end subroutine make_plot_3d

end module make_plot_module


      subroutine trap (x, y, n, sum)
      implicit double precision (a-h, o-z)

c     Trapeziodal integration of y(x), result in sum

      dimension x(n), y(n)

      sum = y(1) * (x(2) - x(1))
      do 10  i = 2, n-1
         sum = sum + y(i) * (x(i+1) - x(i-1))
   10 continue
      sum = sum + y(n) * (x(n) - x(n-1))
      sum = sum/2

      return
      end


!	Look up sorted integer array a.
integer function lookup(x,a)
implicit none
integer, intent(in) :: x,a(:)
!	pivot is pivot point.
integer :: lower,pivot,upper

!	The pivot cannot be less than 1 or greater than size(a).
lower= 1
upper=size(a)
!	The pivot cannot exceed size(a).
pivot=(lower+upper)/2
do

	if(a(pivot)==x) then
		lookup=pivot
		return
    end if
	if(a(pivot)<x) then
        lower=pivot
		pivot=max(lower+1,(lower+upper)/2)
	else
	    upper=pivot
	    pivot=min((lower+upper)/2,upper-1)
    end if

end do
end function lookup


!
!     (c) 2019-2020 Guide Star Engineering, LLC
!     This Software was developed for the US Nuclear Regulatory Commission (US NRC) under contract
!     "Multi-Dimensional Physics Implementation into Fuel Analysis under Steady-state and Transients (FAST)",
!     contract # NRC-HQ-60-17-C-0007
!
program test_units
  !! author:  Damian Rouson
  !!
  !! Verify the units-handling capabilities of the units class

  use units_interface, only : units, num_fundamental, K, m, kg, sec, SI
  use assertions_interface, only : assert
  implicit none

  call dimensionless_set_units
  call mass_units_setter_and_getter
  call velocity_energy_operations
  call length_operations
  call time_units_checks
  call temperature_units_checks
  call density_units_checks
  call stress_units_checks
  call power_units_checks

  sync all
  if (this_image()==1) print *,"Test passed."

contains

  pure subroutine dimensionless_set_units
    !! Verify default initialization and user-defined arithmetic for dimensionless quantities

    type(units) :: reynolds, mach

    call assert( reynolds%is_dimensionless(), "test-units: default initialization is dimensionless" )
      !! Verify dimensionless default initialization

    mach = reynolds**2.5
    call assert( mach%is_dimensionless(), "test-units: real power of dimensionless quantity" )
      !! Verify real power of dimensionless quantity

  end subroutine

  pure subroutine mass_units_setter_and_getter
    !! Verify setting and getting of mass units
    type(units) mass

    integer mass_units(num_fundamental)
    mass_units = 0      !! initially dimensionless
    mass_units(kg) = 1  !! set dimensions of mass

    call mass%set_units(exponents=mass_units,system=SI)
    call assert( mass%has_mass_units() .and. mass%get_system()==SI, "test-units: mass%has_mass_units() in SI" )

  end subroutine

  pure subroutine velocity_energy_operations
    !! Verify velocity, energy, and specific energy units, defined assignment, and  defined product and exponentiation operators

    type(units) mass, velocity, specific_energy, energy
    integer, dimension(num_fundamental) :: velocity_units,  mass_units
    velocity_units = 0
    velocity_units([m,sec])=[1,-1]
    mass_units = 0      !! initially dimensionless
    mass_units(kg) = 1  !! set dimensions of mass

    call velocity%set_units(exponents=velocity_units,system=SI)

    call assert( velocity%user_defined(), "test-units: velocity%user_defined()")
    call assert( velocity%has_velocity_units() .and. velocity%get_system()==SI,"test-units: velocity%has_velocity_units()")
      !! Verify setting and getting of velocity units

    specific_energy = velocity**2
    call assert(specific_energy%has_specific_energy_units() .and. specific_energy%get_system()==SI, &
      "test-units: specific_energy%has_specific_energy_units()")
      !! Verify integer exponentiation of dimensional quantity

    call mass%set_units(exponents=mass_units,system=SI)
    energy = mass*specific_energy
    call assert( energy%has_energy_units() .and. energy%get_system()==SI,"test-units: energy%has_energy_units()")
      !! Verify multiplication of dimensional quantities

  end subroutine

  pure subroutine length_operations
    !! Verify setting and getting of length dimensions and addition, subtraction and negation operators
    type(units) height, width, depth, difference, negative, ratio
    integer length_units(num_fundamental)
    length_units=0
    length_units(m)=1

    call height%set_units(exponents=length_units,system=SI)
    call assert( height%has_length_units() .and. height%get_system()==SI,"test-units: height%has_length_units()")
      !! Verify setting and getting of length units

    width = height
    call assert( width%has_length_units() .and. width%get_system()==SI,"test-units: width%has_length_units()")

    depth = width + height
    call assert( depth%has_length_units() .and. depth%get_system()==SI,"test-units: depth%has_length_units()")
      !! Verify units of additive length

    difference = width - depth
    call assert( difference%has_length_units() .and. difference%get_system()==SI,"test-units: difference%has_length_units()")
      !! Verify units of differential length

    negative = -depth
    call assert( negative%has_length_units() .and. difference%get_system()==SI,"test-units: difference%has_length_units()")
      !! Verify units of negated length

    ratio = width/depth
    call assert( ratio%is_dimensionless(),"test-units: ratio%is_dimensionless()")
      !! Verify dimensionless ratio of like quantities

  end subroutine

  pure subroutine time_units_checks
    type(units) waiting
    integer time_units(num_fundamental)
    time_units=0
    time_units(sec)=1

    call waiting%set_units(exponents=time_units,system=SI)
    call assert( waiting%has_time_units() .and. waiting%get_system()==SI,"test-units: waiting%has_time_units()")
      !! Verify units of time

  end subroutine

  pure subroutine temperature_units_checks
    type(units) preheat
    integer temperature_units(num_fundamental)
    temperature_units=0
    temperature_units(K)=1

    call preheat%set_units(exponents=temperature_units,system=SI)
    call assert( preheat%has_temperature_units() .and. preheat%get_system()==SI,"test-units: preheat%has_temperature_units()")
      !! Verify units of temperature

  end subroutine

  pure subroutine density_units_checks
    type(units) mud
    integer density_units(num_fundamental)
    density_units=0
    density_units([kg,m])=[1,-3]

    call mud%set_units(exponents=density_units,system=SI)
    call assert( mud%has_density_units() .and. mud%get_system()==SI,"test-units: mud%has_density_units()")
      !! Verify units of density

  end subroutine

  pure subroutine stress_units_checks
    type(units) :: anxiety
    integer stress_units(num_fundamental)
    stress_units=0
    stress_units([kg,m,sec])=[1,-1,-2]

    call anxiety%set_units(exponents=stress_units,system=SI)
    call assert( anxiety%has_stress_units() .and. anxiety%get_system()==SI,"test-units: mud%has_stress_units()")
      !! Verify units of stress

  end subroutine

  pure subroutine power_units_checks
    type(units) :: solar
    integer power_units(num_fundamental)
    power_units=0
    power_units([kg,m,sec])=[1,2,-3]

    call solar%set_units(exponents=power_units,system=SI)
    call assert( solar%has_power_units() .and. solar%get_system()==SI,"test-units: mud%has_power_units()")
      !! Verify units of power

  end subroutine

end program test_units


!
! Unit test for bug 3385969.  Boz literals are handled by OFP correctly
! but not by ROSE.
!
   integer, parameter :: INT8  = SELECTED_INT_KIND(16)
   integer(INT8), parameter :: ieee64_two = Z'4000000000000000'
   integer, parameter :: binary = B'1101'
   integer, parameter :: octal  = O'15'
   integer, parameter :: hex  = Z'D'

   print *, ieee64_two
   print *, binary
   print *, octal
   print *, hex

   if (binary .ne. 13 .and. octal .ne. 13 .and. hex .ne. 13) stop 1

end


! RUN: bbc -emit-fir %s -o - | FileCheck %s

! CHECK-LABEL: func @_QPrandom_test
subroutine random_test
    ! CHECK-DAG: [[ss:%[0-9]+]] = fir.alloca {{.*}}random_testEss
    ! CHECK-DAG: [[vv:%[0-9]+]] = fir.alloca {{.*}}random_testEvv
    integer ss, vv(40)
    ! CHECK-DAG: [[rr:%[0-9]+]] = fir.alloca {{.*}}random_testErr
    ! CHECK-DAG: [[aa:%[0-9]+]] = fir.alloca {{.*}}random_testEaa
    real rr, aa(5)
    ! CHECK: fir.call @_FortranARandomInit(%true{{.*}}, %false{{.*}}) : (i1, i1) -> none
    call random_init(.true., .false.)
    ! CHECK: [[box:%[0-9]+]] = fir.embox [[ss]]
    ! CHECK: [[argbox:%[0-9]+]] = fir.convert [[box]]
    ! CHECK: fir.call @_FortranARandomSeedSize([[argbox]]
    call random_seed(size=ss)
    print*, 'size: ', ss
    ! CHECK: fir.call @_FortranARandomSeedDefaultPut() : () -> none
    call random_seed()
    ! CHECK: [[box:%[0-9]+]] = fir.embox [[rr]]
    ! CHECK: [[argbox:%[0-9]+]] = fir.convert [[box]]
    ! CHECK: fir.call @_FortranARandomNumber([[argbox]]
    call random_number(rr)
    print*, rr
    ! CHECK: [[box:%[0-9]+]] = fir.embox [[vv]]
    ! CHECK: [[argbox:%[0-9]+]] = fir.convert [[box]]
    ! CHECK: fir.call @_FortranARandomSeedGet([[argbox]]
    call random_seed(get=vv)
  ! print*, 'get:  ', vv(1:ss)
    ! CHECK: [[box:%[0-9]+]] = fir.embox [[vv]]
    ! CHECK: [[argbox:%[0-9]+]] = fir.convert [[box]]
    ! CHECK: fir.call @_FortranARandomSeedPut([[argbox]]
    call random_seed(put=vv)
    print*, 'put:  ', vv(1:ss)
    ! CHECK: [[box:%[0-9]+]] = fir.embox [[aa]]
    ! CHECK: [[argbox:%[0-9]+]] = fir.convert [[box]]
    ! CHECK: fir.call @_FortranARandomNumber([[argbox]]
    call random_number(aa)
    print*, aa
  end


      subroutine eos(rho,
!-----------------------------------------------------------------------
! Reads and interpolates the equation of state tables to give
! temperatures, opacities etc
! Updated to fortran 90 PJC 22/05/2008
! Extended to opacities DHF 01/09/2009
!-----------------------------------------------------------------------

      use sphgravdata
      use eosdata

      implicit none

      integer :: i,j,p
      real :: mT,mT1,mT2,cT,cT1,cT2,T1,T2
      real :: mmu,mmu1,mmu2,cmu,cmu1,cmu2,mu1,mu2
      real :: mkap,mkap1,mkap2,ckap,ckap1,ckap2,kap1,kap2
      real :: mkbar,mkbar1,mkbar2,ckbar,ckbar1,ckbar2,kbar1,kbar2
      real :: cv

! Allocate output array
      allocate(gammamuT(5,npart))

! gammamuT columns:
! 1) gamma
! 2) mu
! 3) T
! 4) Opacity
! 5) Mass Weighted Opacity (Stamatellos et al 2007)

! Loop over all particles and evaluate gamma, mean molecular weight
! and temperature for each particle
      do p = 1,npart

! Find the relevant records in the table...
! ... for rho
         if (rho(p) < 1.0e-24) rho(p) = 1.0e-24
         i = 1
         do
            if ((eostable(i,1,1) >= rho(p)).or.(i == nrhopoints)) exit
            i = i + 1
         enddo

! ... and for internal energy
         if (vxyzu(4,p) < 0.5302e8) vxyzu(4,p) = 0.5302e8
         j = 1
         do
            if ((eostable(i-1,j,3) >= vxyzu(4,p)).or.(j==nUpoints)) exit
            j = j + 1
         enddo

! Interpolate over the j value at i-1
         mT1 = (eostable(i-1,j-1,2) - eostable(i-1,j,2))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
         cT1 = eostable(i-1,j,2) - mT1*eostable(i-1,j,3)
         T1 = mT1*vxyzu(4,p) + cT1

         mmu1 = (eostable(i-1,j-1,4) - eostable(i-1,j,4))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
         cmu1 = eostable(i-1,j,4) - mmu1*eostable(i-1,j,3)
         mu1 = mmu1*vxyzu(4,p) + cmu1

	 mkap1 = (eostable(i-1,j-1,6) - eostable(i-1,j,6))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
	 ckap1 = eostable(i-1,j,6) - mkap1*eostable(i-1,j,3)
	 kap1 = mkap1*vxyzu(4,p) + ckap1

	 mkbar1 = (eostable(i-1,j-1,5) - eostable(i-1,j,5))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
	ckbar1 = eostable(i-1,j,5) - mkbar1*eostable(i-1,j,3)
	kbar1 = mkbar1*vxyzu(4,p) + ckbar1

! Then interpolate over the j value at i
! Update j value as necessary
         j = 1
         do
            if ((eostable(i,j,3) >= vxyzu(4,p)).or.(j==nUpoints)) exit
            j = j + 1
         enddo

         mT2 = (eostable(i,j-1,2) - eostable(i,j,2))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         cT2 = eostable(i,j,2) - mT2*eostable(i,j,3)
         T2 = mT2*vxyzu(4,p) + cT2

         mmu2 = (eostable(i,j-1,4) - eostable(i,j,4))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         cmu2 = eostable(i,j,4) - mmu2*eostable(i,j,3)
         mu2 = mmu2*vxyzu(4,p) + cmu2

         mkap2 = (eostable(i,j-1,6) - eostable(i,j,6))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         ckap2 = eostable(i,j,6) - mkap2*eostable(i,j,3)
         kap2 = mkap2*vxyzu(4,p) + ckap2

	 mkbar2 = (eostable(i,j-1,5) - eostable(i,j,5))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         ckbar2 = eostable(i,j,5) - mkbar2*eostable(i,j,3)
         kbar2 = mkbar2*vxyzu(4,p) + ckbar2

! Finally interpolate over i at the fractional j value
         mT = (T2 - T1) / (eostable(i,1,1)-eostable(i-1,1,1))
         cT = T2 - mT*eostable(i,1,1)
         gammamuT(3,p) = mT*rho(p) + cT

         mmu = (mu2 - mu1) / (eostable(i,1,1)-eostable(i-1,1,1))
         cmu = mu2 - mmu*eostable(i,1,1)
         gammamuT(2,p) = mmu*rho(p) + cmu

	mkap = (kap2 - kap1) / (eostable(i,1,1)-eostable(i-1,1,1))
         ckap = kap2 - mkap*eostable(i,1,1)
         gammamuT(4,p) = mkap*rho(p) + ckap

	mkbar = (kbar2 - kbar1) / (eostable(i,1,1)-eostable(i-1,1,1))
         ckbar = kbar2 - mkbar*eostable(i,1,1)
         gammamuT(5,p) = mkbar*rho(p) + ckbar

! Evaluate the gamma value from gamma = 1 + k/(mH*mu*cv), with cv = U/T
         cv = vxyzu(4,p)/gammamuT(3,p)
         gammamuT(1,p) = 1.0d0 + (Boltzmannk / &
              (mH*gammamuT(2,p)*cv))

      enddo

      end subroutine eos


      subroutine rndrot
     x  (lrotv,idnode,mxnode,natms,xxx,yyy,zzz,vxx,vyy,vzz)

c*********************************************************************
c
c     dl_poly routine to perform a random rotation on a 3D rigid body
c
c     author - w.smith july 2000
c     copyright daresbury laboratory 2000
c
c*********************************************************************

#include 'dl_params.inc'

      logical lrotv

      dimension rot(9),qtn(4)
      dimension xxx(*),yyy(*),zzz(*)
      dimension vxx(*),vyy(*),vzz(*)
c
c     generate random orientation

      qtn(1)=2.d0*duni()-1.d0
      qtn(2)=2.d0*duni()-1.d0
      qtn(3)=2.d0*duni()-1.d0
      qtn(4)=2.d0*duni()-1.d0
      qqq=sqrt(qtn(1)**2+qtn(2)**2+qtn(3)**2+qtn(4)**2)
      qtn(1)=qtn(1)/qqq
      qtn(2)=qtn(2)/qqq
      qtn(3)=qtn(3)/qqq
      qtn(4)=qtn(4)/qqq

      rot(1) = qtn(1)**2+qtn(2)**2-qtn(3)**2-qtn(4)**2
      rot(2) = 2.d0*(qtn(2)*qtn(3) - qtn(1)*qtn(4))
      rot(3) = 2.d0*(qtn(2)*qtn(4) + qtn(1)*qtn(3))
      rot(4) = 2.d0*(qtn(2)*qtn(3) + qtn(1)*qtn(4))
      rot(5) = qtn(1)**2-qtn(2)**2+qtn(3)**2-qtn(4)**2
      rot(6) = 2.d0*(qtn(3)*qtn(4) - qtn(1)*qtn(2))
      rot(7) = 2.d0*(qtn(2)*qtn(4) - qtn(1)*qtn(3))
      rot(8) = 2.d0*(qtn(3)*qtn(4) + qtn(1)*qtn(2))
      rot(9) = qtn(1)**2-qtn(2)**2-qtn(3)**2+qtn(4)**2
c
c     perform coordinate rotation

      do i=1,natms

        txx=xxx(i)
        tyy=yyy(i)
        tzz=zzz(i)
        xxx(i)=rot(1)*txx+rot(4)*tyy+rot(7)*tzz
        yyy(i)=rot(2)*txx+rot(5)*tyy+rot(8)*tzz
        zzz(i)=rot(3)*txx+rot(6)*tyy+rot(9)*tzz

      enddo

c
c     perform velocity rotation

      if(lrotv)then

        do i=1,natms

          txx=vxx(i)
          tyy=vyy(i)
          tzz=vzz(i)
          vxx(i)=rot(1)*txx+rot(4)*tyy+rot(7)*tzz
          vyy(i)=rot(2)*txx+rot(5)*tyy+rot(8)*tzz
          vzz(i)=rot(3)*txx+rot(6)*tyy+rot(9)*tzz

        enddo

      endif

      return
      end


SUBROUTINE pd_forceZ (mjd, rsat, vsat, Fvec, PDr, PDv)

! ----------------------------------------------------------------------
! SUBROUTINE: pd_forceZ.f03
! ----------------------------------------------------------------------
! Purpose:
!  Satellite acceleration components based on the considered force model
! ----------------------------------------------------------------------
! Input arguments:
! - mjd:			Modified Julian Day number (including the fraction of the day) in Terrestrial Time (TT)
! - rsat:			Satellite Position vector (m)   in inertial frame (ICRF)
! - vsat:			Satellite Velocity vector (m/s) in inertial frame (ICRF)
!
! Output arguments:
! - Fvec:			Acceleration vector cartesian components in inertial frame (ICRF)
! - PDr: 			Partial derivatives matrix of the acceleration w.r.t. the position vector in ICRF
! - PDv: 			Partial derivatives matrix of the acceleration w.r.t. the velocity vector in ICRF
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou
! 			Cooperative Research Centre for Spatial Information, Australia
! Created:	February 2018
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      USE mdl_param
      USE mdl_eop
      USE mdl_planets
      USE mdl_tides
      USE m_force_gfm
      USE m_force_tides
      USE m_tides_ocean
      USE m_pd_geopotential
      USE m_matrixRxR
      USE mdl_config
      use pod_yaml
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      REAL (KIND = prec_d), INTENT(IN) :: mjd
      REAL (KIND = prec_d), INTENT(IN), DIMENSION(3) :: rsat, vsat
! ----------------------------------------------------------------------
! OUT
      REAL (KIND = prec_d), INTENT(OUT) :: Fvec(3)
      REAL (KIND = prec_d), INTENT(OUT) :: PDr(3,3), PDv(3,3)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      REAL (KIND = prec_d), DIMENSION(3) :: rsat_icrf, vsat_icrf, rsat_itrf, vsat_itrf, v_TRS_1, v_TRS_2
	  DOUBLE PRECISION EOP_cr(7)
      DOUBLE PRECISION CRS2TRS(3,3), TRS2CRS(3,3), d_CRS2TRS(3,3), d_TRS2CRS(3,3)
! ----------------------------------------------------------------------
      REAL (KIND = prec_d), DIMENSION(3) :: SF
      REAL (KIND = prec_q), DIMENSION(3) :: Fgrav_itrf , Fgrav_icrf, Fplanets_icrf, Ftides_icrf, Frelativity_icrf
      REAL (KIND = prec_d), DIMENSION(3) :: Fsrp_icrf
      REAL (KIND = prec_d) :: fx, fy, fz
! ----------------------------------------------------------------------
      REAL (KIND = prec_q) :: GMearth, aEarth
      INTEGER (KIND = prec_int8) :: n_max, m_max
      INTEGER(KIND = 4)          :: satsvn
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(3) :: aPlanets_icrf, a_perturb, a_iJ2, a_iJ2_icrf
      DOUBLE PRECISION  JD, Zbody(6)
      INTEGER  NTARG, NCTR, NTARG_body
      REAL (KIND = prec_q), DIMENSION(3) :: rbody
      REAL (KIND = prec_q) :: GMbody
      REAL (KIND = prec_q), DIMENSION(3) :: rSun, rMoon, rMoon_ITRS, rSun_ITRS
      REAL (KIND = prec_q) :: GM_moon, GM_sun
      REAL (KIND = prec_q) :: C20, Re
! ----------------------------------------------------------------------
      REAL (KIND = prec_q) :: ut1_utc
      REAL (KIND = prec_d) :: xp, yp
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(5,5) :: dCnm_solid1, dSnm_solid1
      REAL (KIND = prec_q), DIMENSION(3,3) :: dCnm_solid2, dSnm_solid2
      REAL (KIND = prec_q) :: dC20_perm
      REAL (KIND = prec_q) :: dC21_pse, dS21_pse, dC21_poc, dS21_poc
      INTEGER (KIND = prec_int8) :: sz_tides
      INTEGER (KIND = prec_int8) :: i , j
      REAL (KIND = prec_q) :: ax, ay, az
      REAL (KIND = prec_q), DIMENSION(3) :: a_tides, a_solidtides, a_ocean
      REAL (KIND = prec_q), DIMENSION(3) :: a_solid1, a_solid2, a_pse, a_poc
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: dCnm_tides, dSnm_tides
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: dCnm_ocean, dSnm_ocean
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(6) :: Zsat_GCRS, Zearth_GCRS
      REAL (KIND = prec_q), DIMENSION(3) :: vSun
      REAL (KIND = prec_q), DIMENSION(3) :: a_Schwarzschild, a_LenseThirring, a_deSitter
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int2) :: AllocateStatus, DeAllocateStatus
      CHARACTER (LEN=30) :: fmt_line
      INTEGER (KIND = prec_int2) :: ios
      CHARACTER (LEN=1) :: GNSSid
      INTEGER (KIND = prec_int4) :: PRN_no
      INTEGER :: eclpf, srpid
! ----------------------------------------------------------------------
      REAL (KIND = prec_q) :: beta_ppn, gama_ppn
      REAL (KIND = prec_q) :: c_light
! ----------------------------------------------------------------------
      !REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: U1, U2, U3
      REAL (KIND = prec_d), DIMENSION(3,3) :: Ugrav_icrf, Ugrav_itrf, Ugrav

! ----------------------------------------------------------------------
! Global variables used
! ----------------------------------------------------------------------
! Module mdl_param.f03:

! FMOD_GRAV, FMOD_NONGRAV
! GFM_Cnm, GFM_Snm
! GFM_Nmax, GFM_Mmax
! OCEAN_Nmax, OCEAN_Mmax
GMearth = GFM_GM
aEarth = GFM_ae
! ----------------------------------------------------------------------
! Module mdl_num.f90:

! Relativistic parameters
beta_ppn = ppn_beta
gama_ppn = ppn_gama
c_light = cslight
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
If (FMOD_GRAVFIELD == 0) THEN

! Module mdl_num
GMearth = GM_global
aEarth = Earth_radius

END IF
! ----------------------------------------------------------------------

! State Vector in ICRF
rsat_icrf = rsat
vsat_icrf = vsat

! ----------------------------------------------------------------------
! EOP data and Tranformation matrices between ITRF and ICRF
! ----------------------------------------------------------------------
If (FMOD_GRAVFIELD > 0 .OR. yml_planetary_perturbations_enabled .OR. yml_tidal_effects_enabled .OR. yml_rel_effects_enabled ) Then

CALL EOP (mjd, EOP_cr, CRS2TRS, TRS2CRS, d_CRS2TRS, d_TRS2CRS)

! EOP data corrected at computation epoch
! Polar motion coordinates
xp = EOP_cr(2)
yp = EOP_cr(3)
! UT1-UTC
ut1_utc = EOP_cr(4)

! State Vector in ITRF
! r in ITRF
CALL matrix_Rr (CRS2TRS, rsat_icrf , rsat_itrf)
! v in ITRF
! v_TRS = TRS2CRS * v_CRS + d_CRS2TRS * r_CRS
CALL matrix_Rr (CRS2TRS, vsat_icrf , v_TRS_1)
CALL matrix_Rr (d_CRS2TRS, rsat_icrf , v_TRS_2)
vsat_itrf = v_TRS_1 + v_TRS_2

End IF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Gravitational Effects
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Gravity Field
If (.not. yml_gravity_enabled) Then

! Setting to Zero
fx = 0.0D0
fy = 0.0D0
fz = 0.0D0
Fgrav_icrf = (/ fx, fy, fz /)

Ugrav_icrf(1,:) = (/0.0D0, 0.0D0, 0.0D0 /)
Ugrav_icrf(2,:) = (/0.0D0, 0.0D0, 0.0D0 /)
Ugrav_icrf(3,:) = (/0.0D0, 0.0D0, 0.0D0 /)

Else

If (FMOD_GRAVFIELD == 0) Then

! Central term of the Earth Gravity Field
Call force_gm (GMearth, rsat_icrf, fx,fy,fz )
Fgrav_icrf = (/ fx, fy, fz /)
! Partial derivatives
Call pd_gm (GMearth, rsat_icrf, Ugrav_icrf)

Else

! Earth Gravity Field model acceleration components (spherical harmonics expansion)
n_max = GFM_Nmax
m_max = GFM_Mmax
CALL force_gfm (GMearth, aEarth, rsat_itrf, n_max, m_max, GFM_Cnm, GFM_Snm , fx,fy,fz)
Fgrav_itrf = (/ fx, fy, fz /)

! Acceleration vector transformation from terrestrial to inertial frame
! Fgrav_icrf = TRS2CRS * Fgrav_itrf
CALL matrix_Rr (TRS2CRS, Fgrav_itrf , Fgrav_icrf)

! Partial derivatives
!Call pd_geopotential(GMearth, aEarth, rsat_itrf, n_max, m_max, GFM_Cnm, GFM_Snm, dCnm, dSnm , fx,fy,fz, Ugrav_itrf)
Call pd_geopotential(GMearth, aEarth, rsat_itrf, n_max, m_max, GFM_Cnm, GFM_Snm, Fgrav_itrf, Ugrav_itrf)

! Acceleration vector transformation from terrestrial to inertial frame
CALL matrix_Rr (TRS2CRS, Fgrav_itrf , Fgrav_icrf)

! PD matrix transformation ITRF to ICRF
! (df/dr)_Inertial = EOP(t) * (df/dr)_Terrestrial * inv( EOP(t) )

!! Allocatable arrays
!ALLOCATE (U1(3,3), STAT = AllocateStatus)
!ALLOCATE (U2(3,3), STAT = AllocateStatus)
!ALLOCATE (U3(3,3), STAT = AllocateStatus)

!!Call matrixRxR(TRS2CRS, Ugrav_itrf, Ugrav)
!U1 = TRS2CRS
!U2 = Ugrav_itrf
!Call matrixRxR (U1, U2, U3)
!Ugrav = U3

!!Call matrixRxR(Ugrav, TRANSPOSE(TRS2CRS) , Ugrav_icrf)
!!Call matrixRxR(Ugrav, CRS2TRS ,Ugrav_icrf)
!U1 = Ugrav
!U2 = TRANSPOSE(TRS2CRS)
!Call matrixRxR (U1, U2, U3)
!Ugrav_icrf = U3

Ugrav = MATMUL(TRS2CRS, Ugrav_itrf)
Ugrav_icrf = MATMUL(Ugrav, TRANSPOSE(TRS2CRS))

End IF

End If
! ----------------------------------------------------------------------

! variable initialisation (vSun)
vSun = 0.d0

! ----------------------------------------------------------------------
! Planetary/Lunar orbital perturbations
! ----------------------------------------------------------------------
If (yml_planetary_perturbations_enabled) Then

! Julian Day Number of the input epoch
JD = mjd + 2400000.5D0

! Center celestial body: Earth
NCTR = 3

! Planets perturbation loop
aPlanets_icrf = (/ 0.D0, 0.0D0, 0.0D0 /)
DO NTARG_body = 1 , 11
   IF (NTARG_body /= 3) THEN

! Celestial body's (NTARG) Cartesian coordinates w.r.t. Center body (NCTR)
      NTARG = NTARG_body
      CALL  PLEPH ( JD, NTARG, NCTR, Zbody )

! Cartesian coordinates of the celestial body in meters: KM to M
	  rbody(1) = Zbody(1) * 1000.D0
	  rbody(2) = Zbody(2) * 1000.D0
	  rbody(3) = Zbody(3) * 1000.D0

! GM gravity constants of the solar system bodies
      GMbody = GMconst(NTARG)

! ----------------------------------------------------------------------
! Point-mass perturbations vector due to the selected celestial body
      CALL force_gm3rd (rbody, rsat_icrf, GMbody , a_perturb)
! Overall (sum) planetary pertrubation acceleration vector
	  aPlanets_icrf = aPlanets_icrf + a_perturb
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Sun
      if (NTARG == 11) then
	  rSun = rbody
	  vSun = (/ Zbody(4), Zbody(5), Zbody(6) /) * 1000.D0 ! KM/sec to m/sec
      GM_sun = GMbody
! ----------------------------------------------------------------------
! Moon
      else if (NTARG == 10) then
	  rMoon = rbody
      GM_moon = GMbody
      end if
! ----------------------------------------------------------------------

   END IF
END DO

! ----------------------------------------------------------------------
! Indirect J2 effect
! ----------------------------------------------------------------------
! Sun and Moon position vectors in terrestrial reference frame
! Transformation GCRS to ITRS
CALL matrix_Rr (CRS2TRS,rMoon , rMoon_ITRS)
CALL matrix_Rr (CRS2TRS,rSun  , rSun_ITRS)

If (FMOD_GRAVFIELD > 0) Then
! C20 spherical harmonic coefficient of geopotential
C20 = GFM_Cnm(2+1,0+1)
!Re = GFM_ae

Else If (FMOD_GRAVFIELD == 0) then
C20 = -4.841694552725D-04
!Re = Earth_radius

End IF

! Earth Radius
Re = aEarth

! Indirect J2 effect of Sun and Moon
CALL indirectJ2(C20,Re,GM_moon,rMoon_ITRS,GM_sun,rSun_ITRS , a_iJ2)

! Acceleration vector transformation from terrestrial to inertial frame
CALL matrix_Rr (TRS2CRS, a_iJ2 , a_iJ2_icrf)
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Planetary/Lunar perturbation accleration
Fplanets_icrf = aPlanets_icrf + a_iJ2_icrf
! ----------------------------------------------------------------------

ELSE If (.not. yml_planetary_perturbations_enabled) Then

Fplanets_icrf = (/ 0.D0, 0.0D0, 0.0D0 /)

End IF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Tidal effects
! ----------------------------------------------------------------------
If (yml_tidal_effects_enabled) Then

! ----------------------------------------------------------------------
! Solid Earth Tides
! ----------------------------------------------------------------------
! Frequency-independent : Step 1 (IERS Conventions 2010)
If (BTEST(yml_tidal_effects, solid_nonfreq - one)) Then

      CALL tides_solid1(rMoon_ITRS, rSun_ITRS, GMearth, aEarth, GM_moon, GM_sun, dCnm_solid1, dSnm_solid1)

Else
      dCnm_solid1 = 0.0D0
      dSnm_solid1 = 0.0D0
End if

! Frequency-dependent : Step 2 (IERS Conventions 2010)
If (BTEST(yml_tidal_effects, solid_freq - one)) Then

      CALL tides_solid2(mjd, ut1_utc , dCnm_solid2, dSnm_solid2)

Else
      dCnm_solid2 = 0.0D0
      dSnm_solid2 = 0.0D0
End if

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

! ----------------------------------------------------------------------
! Zero Tide term
CALL tide_perm (dC20_perm)

! Tide system of the input gravity field model is set in the GFM_tide global variable in the module mdl_param.f03
if (GFM_tide == 'zero_tide') then
   dCnm_solid1(2+1,0+1) = dCnm_solid1(2+1,0+1) - dC20_perm
   !dCnm_solid2(2+1,0+1) = dCnm_solid2(2+1,0+1) - dC20_perm
end if
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Pole Tide
! ----------------------------------------------------------------------
! Solid Earth pole tide
dC21_pse = 0.0D0
dS21_pse = 0.0D0
If (BTEST(yml_tidal_effects, solid_pole - one)) Then
      CALL tide_pole_se (mjd,xp,yp , dC21_pse, dS21_pse)
End if

! Ocean pole tide
dC21_poc = 0.0D0
dS21_poc = 0.0D0
If (BTEST(yml_tidal_effects, ocean_pole - one)) Then
      CALL tide_pole_oc (mjd,xp,yp , dC21_poc, dS21_poc)
end if
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! dCnm, dSnm arrays sum

sz_tides = SIZE (dCnm_solid1,DIM=1)
ALLOCATE (dCnm_tides(sz_tides,sz_tides), STAT = AllocateStatus)
      IF (AllocateStatus /= 0) THEN
         PRINT *, "Error: Not enough memory"
         PRINT *, "Error: SUBROUTINE force_sum.f03"
         PRINT *, "Error: Allocatable Array: dCnm_tides | Nmax:", sz_tides
!         STOP "*** Not enough memory ***"
      END IF

ALLOCATE (dSnm_tides(sz_tides,sz_tides), STAT = AllocateStatus)
      IF (AllocateStatus /= 0) THEN
         PRINT *, "Error: Not enough memory"
         PRINT *, "Error: SUBROUTINE force_sum.f03"
         PRINT *, "Error: Allocatable Array: dSnm_tides | Nmax:", sz_tides
!         STOP "*** Not enough memory ***"
      END IF

dCnm_tides = dCnm_solid1
dSnm_tides = dSnm_solid1

Do i = 1 , 3
Do j = 1 , 3
   dCnm_tides(i,j) = dCnm_tides(i,j) + dCnm_solid2(i,j)
   dSnm_tides(i,j) = dSnm_tides(i,j) + dSnm_solid2(i,j)
End Do
End Do

dCnm_tides(2+1,1+1) = dCnm_tides(2+1,1+1) + dC21_pse + dC21_poc
dSnm_tides(2+1,1+1) = dSnm_tides(2+1,1+1) + dS21_pse + dS21_poc

! Acceleration cartesian components
CALL force_tides(rsat_itrf, GMearth, aEarth, sz_tides-1, sz_tides-1, dCnm_tides, dSnm_tides, ax,ay,az)
a_solidtides(1) = ax
a_solidtides(2) = ay
a_solidtides(3) = az

!PRINT *,"dCnm_tides",dCnm_tides
!PRINT *,"dSnm_tides",dSnm_tides

DEALLOCATE (dCnm_tides,   STAT = DeAllocateStatus)
DEALLOCATE (dSnm_tides,   STAT = DeAllocateStatus)
sz_tides = 0
! ----------------------------------------------------------------------
!PRINT *,"dCnm_solid1",dCnm_solid1
!PRINT *,"dSnm_solid1",dSnm_solid1
!PRINT *,"dCnm_solid2",dCnm_solid2
!PRINT *,"dSnm_solid2",dSnm_solid2
!PRINT *,"dC21_pse, dS21_pse", dC21_pse, dS21_pse
!PRINT *,"dC21_poc, dS21_poc", dC21_poc, dS21_poc
!PRINT *, "dC20_perm"  , dC20_perm

! ----------------------------------------------------------------------
! Ocean Tides
a_ocean = (/ 0.D0, 0.0D0, 0.0D0 /)
If (BTEST(yml_tidal_effects, ocean - one)) Then

      CALL tides_ocean(OCEAN_Nmax, OCEAN_Mmax, mjd, ut1_utc, dCnm_ocean, dSnm_ocean)

      ! Acceleration cartesian components
	  !CALL force_tides(rsat_itrf, GMearth, aEarth, sz_tides-1, sz_tides-1, dCnm_ocean, dSnm_ocean, ax,ay,az)
	  CALL force_tides(rsat_itrf, GMearth, aEarth, OCEAN_Nmax, OCEAN_Mmax, dCnm_ocean, dSnm_ocean, ax,ay,az)
      a_ocean (1) = ax
      a_ocean (2) = ay
      a_ocean (3) = az

      DEALLOCATE (dCnm_ocean,   STAT = DeAllocateStatus)
      DEALLOCATE (dSnm_ocean,   STAT = DeAllocateStatus)
      sz_tides = 0
End if
! ----------------------------------------------------------------------
!PRINT *,"dCnm_ocean",dCnm_ocean
!PRINT *,"dSnm_ocean",dSnm_ocean
!PRINT *,"FMOD_TIDES",FMOD_TIDES

! ----------------------------------------------------------------------
! Tides acceleration cartesian components : Overall effects
! ----------------------------------------------------------------------
!a_tides = a_solid1 + a_solid2 + a_ocean + a_pse + a_poc
a_tides = a_solidtides + a_ocean

! Acceleration vector transformation from terrestrial to inertial frame
CALL matrix_Rr (TRS2CRS, a_tides , Ftides_icrf)
! ----------------------------------------------------------------------
!PRINT *,"a_tides", a_tides
!PRINT *,"a_solid", a_solidtides
!PRINT *,"a_ocean", a_ocean

Else If (.not. yml_tidal_effects_enabled) Then

Ftides_icrf = (/ 0.D0, 0.0D0, 0.0D0 /)

End IF
! End of Tidal effects
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Relativistic effects
! ----------------------------------------------------------------------
If (yml_rel_effects_enabled) Then

! Satellite State Vector in GCRS
Zsat_GCRS = (/rsat_icrf(1), rsat_icrf(2), rsat_icrf(3), vsat_icrf(1), vsat_icrf(2), vsat_icrf(3) /)

! Earth state vector w.r.t. Sun
Zearth_GCRS = -1.0D0 * (/ rSun(1),rSun(2),rSun(3) , vSun(1),vSun(2),vSun(3) /)
!PRINT *,"Zearth_GCRS",Zearth_GCRS

! Schwarzschild terms
CALL rel_schwarzschild (Zsat_GCRS, Zearth_GCRS, GMearth, beta_ppn, gama_ppn, c_light, a_Schwarzschild)

! Lense-Thirring effects
! FIXME: why call function if we are going to ignore the result?
CALL rel_LenseThirring (Zsat_GCRS, Zearth_GCRS,  GMearth, gama_ppn, c_light , a_LenseThirring)
a_LenseThirring = (/ 0.D0, 0.0D0, 0.0D0 /)

! de Sitter effect or geodesic precesssion
CALL rel_deSitter (Zsat_GCRS, Zearth_GCRS, GMearth, GM_sun, gama_ppn, c_light , a_deSitter)

Frelativity_icrf = a_Schwarzschild + a_LenseThirring + a_deSitter

Else If (.not. yml_rel_effects_enabled) Then

Frelativity_icrf = (/ 0.D0, 0.0D0, 0.0D0 /)

End IF
! ----------------------------------------------------------------------
!print *,"a_Schwarzschild ", a_Schwarzschild
!print *,"a_LenseThirring ", a_LenseThirring
!print *,"a_deSitter      ", a_deSitter
!print *,"Frelativity_icrf", Frelativity_icrf

! ----------------------------------------------------------------------
! Non-Gravitational Effects
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Solar Radiation
! ----------------------------------------------------------------------
if (BTEST(yml_non_grav_effects, SOLARNG - one)) Then

! PRN: GNSS constellation ID letter + Satellite number
fmt_line = '(A1,I2.2)'
READ (PRN, fmt_line , IOSTAT=ios) GNSSid, PRN_no

! Eclipse status		!! Temporary !!  Upgrade for calling the Yaw attitude library
eclpf = 0

! SRP model
srpid =  yml_apriori_srp

CALL force_srp (GMearth, PRN_no, satsvn, eclpf, srpid, rsat_icrf, vsat_icrf, rSun, fx,fy,fz )
Fsrp_icrf = (/ fx, fy, fz /)

Else

	Fsrp_icrf = (/ 0.D0, 0.0D0, 0.0D0 /)

End IF
! ----------------------------------------------------------------------
!print *,"SRP model values per epoch"
!print *,"FMOD_NONGRAV(1)", FMOD_NONGRAV(1)
!print *,"Fsrp_icrf      ", Fsrp_icrf
!print *,"rsat_icrf", rsat_icrf
!print *,"vsat_icrf", vsat_icrf
!print *,"rSun     ", rSun
!print *,"GFM_GM   ", GFM_GM

! ----------------------------------------------------------------------
! Acceleration sum of the force model
! ----------------------------------------------------------------------
SF = Fgrav_icrf + Fplanets_icrf + Ftides_icrf + Frelativity_icrf + Fsrp_icrf
!SFx = SF(1)
!SFy = SF(2)
!SFz = SF(3)
Fvec = SF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Partial derivatives overall matrix
! ----------------------------------------------------------------------
! PD w.r.t position vector
PDr = Ugrav_icrf
! PD  w.r.t velocity vector
!PDv - Initialise to zero for now
! ----------------------------------------------------------------------
PDv = 0.d0

!print *,"Fgrav_icrf      ", Fgrav_icrf
!print *,"Fplanets_icrf   ", Fplanets_icrf
!print *,"Ftides_icrf     ", Ftides_icrf
!print *,"Frelativity_icrf", Frelativity_icrf

END