!**********************************************************************************************************************************
! LICENSING
! Copyright (C) 2015-2016  National Renewable Energy Laboratory
! Copyright (C) 2016-2017  Envision Energy USA, LTD
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
!     http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!**********************************************************************************************************************************
MODULE BeamDyn_Subs

   USE BeamDyn_Types
   USE NWTC_LAPACK

   IMPLICIT NONE

   INTEGER, PARAMETER :: BDKi = R8Ki
   INTEGER, PARAMETER :: FLAG_R1R2   = 0 !<   BD_CrvCompose flag = 0: R(rr) = R    (pp) R    (qq)
   INTEGER, PARAMETER :: FLAG_R1TR2  = 1 !<   BD_CrvCompose flag = 1: R(rr) = R(T) (pp) R    (qq)
   INTEGER, PARAMETER :: FLAG_R1R2T  = 2 !<   BD_CrvCompose flag = 2: R(rr) = R    (pp) R(T) (qq)
   INTEGER, PARAMETER :: FLAG_R1TR2T = 3 !<   BD_CrvCompose flag = 3: R(rr) = R(T) (pp) R(T) (qq)

   INTEGER, PARAMETER :: GAUSS_QUADRATURE = 1 !<   p%quadrature method: gaussian quadrature
   INTEGER, PARAMETER :: TRAP_QUADRATURE  = 2 !<   p%quadrature method: trapeziodal quadrature

CONTAINS

!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine determines the (N+1) Gauss-Lobatto-Legendre points x and weights w
!!
!! For details, see
!! \@book{Deville-etal:2002,
!!  author =    {M. O. Deville and P. F. Fischer and E. H. Mund},
!!  title =     {High-Order Methods for Incompressible Fluid Flow},
!!  publisher = {Cambridge University Press},
!!  address = {Cambridge},
!!  year =      2002
!!}
SUBROUTINE BD_GenerateGLL(N1, GLL_nodes, ErrStat, ErrMsg)

   ! input variables

   INTEGER(IntKi),          INTENT(IN   ) :: N1             !< 1 + the order of spectral element, or equivalently p%nodes_per_elem
   REAL(BDKi), ALLOCATABLE, INTENT(  OUT) :: GLL_nodes(:)   !< location of GLL nodes
   INTEGER(IntKi),          INTENT(  OUT) :: ErrStat        !< Error status of the operation
   CHARACTER(*),            INTENT(  OUT) :: ErrMsg         !< Error message if ErrStat /= ErrID_None

   REAL(BDKi)      , PARAMETER            :: tol   = 10.0_BDKi*EPSILON(tol) / 2.0_BDKi   ! tolerance for newton-raphson solve (ignores 1 significant digit)
   INTEGER(IntKi)  , PARAMETER            :: maxit = 1000                                ! maximum allowable iterations in newton-raphson solve

   REAL(BDKi)                             :: x_it        ! current NR-iteration value
   REAL(BDKi)                             :: x_old       ! last NR-iteration value
   REAL(BDKi)                             :: dleg(N1)    ! legendre polynomial
   INTEGER(IntKi)                         :: N           ! Order of spectral element
   INTEGER(IntKi)                         :: i           ! do-loop counter
   INTEGER(IntKi)                         :: j           ! do-loop counter
   INTEGER(IntKi)                         :: k           ! do-loop counter

   INTEGER(IntKi)                         :: ErrStat2
   CHARACTER(ErrMsgLen)                   :: ErrMsg2
   character(*), parameter                :: RoutineName = 'BD_GenerateGLL'

   ErrStat = ErrID_None
   ErrMsg  = ""

   N = N1 - 1

   CALL AllocAry(GLL_nodes,N1,'GLL points array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
      if (ErrStat >= AbortErrLev) return

   !bjj: these are getting overwritten later....
   ! enter known endpoints  [-1.0, 1.0]
   GLL_nodes(1) = -1.0_BDKi
   GLL_nodes(N1) = 1.0_BDKi

   DO i = 1, N1

      x_it = -COS(pi_D * REAL(i-1,BDKi) / N) ! initial guess - chebyshev points
      DO j = 1, maxit
         x_old = x_it
         dleg(1) = 1.0_BDKi
         dleg(2) = x_it
         DO k = 2,N
            dleg(k+1) = (  REAL(2*k - 1,BDKi) * dleg(k) * x_it &
                         - REAL(  k - 1,BDKi) * dleg(k-1) ) / REAL(k,BDKi)
         ENDDO
         x_it = x_it - ( x_it * dleg(N1) - dleg(N) ) / &
                       (REAL(N1,BDKi) * dleg(N1) )
         IF (ABS(x_it - x_old) .lt. tol) EXIT
      ENDDO

      GLL_nodes(i) = x_it
     !GLL_weights(i) = 2.0_BDKi / (REAL(N * N1, BDKi) * dleg(N1)**2 )

   ENDDO

END SUBROUTINE BD_GenerateGLL
!-----------------------------------------------------------------------------------------------------------------------------------

!> This subroutine computes the rotation tensor (RT=\f$ \phi \f$ )
!! given Wiener-Milenkovic rotation parameters, \f$ \vec{c} \f$,
!! where \f$ \vec{c}=4\tan\left(\frac{\phi}{4}\right)\bar{n} \f$.
SUBROUTINE BD_CrvMatrixR(cc,Rr)
!FIXME: would this routine benefit from matrix mathematics?  The basic formulation is matrix based.

   REAL(BDKi),    INTENT(IN   ):: cc(3)         !< \f$ \vec{c}\f$
   REAL(BDKi),    INTENT(  OUT):: Rr(3,3)       !< returned rotation tensor (transpose of DCM matrix)

   REAL(BDKi)                  :: c0            !< \f$ c_0 = ( 1 - c_1 c_1 - c_2 c_2 - c_3 c_3 ) / 2 \f$ (different from AIAA paper)
   REAL(BDKi)                  :: c1
   REAL(BDKi)                  :: c2
   REAL(BDKi)                  :: c3
   REAL(BDKi)                  :: tr0           !< \f$ 2 / ( 1 - c_0 )^2 \f$ (which is different from AIAA paper)

   !> Compare this routine with equation (14) from AIAA paper, "Geometric Nonlinear Analysis of Composite Beams Using
   !! Wiener-Milenkovic Parameters", Wang, et. al.\n
   !! \f$
   !!  \underline{\underline{R}} (\vec{c}) =    t_{r0}
   !! \begin{bmatrix}
   !! c_0^2 + c_1^2 - c_2^2 - c_3^2    & 2(c_1 c_2 + c_0 c_3)           & 2(c_1 c_3 - c_0 c_2)           \\
   !! 2(c_1 c_2 - c_0 c_3)             & c_0^2 - c_1^2 + c_2^2 - c_3^2  & 2(c_2 c_3 + c_0 c_1)           \\
   !! 2(c_1 c_3 + c_0 c_2)             & 2(c_2 c_3 - c_0 c_1)           & c_0^2 - c_1^2 - c_2^2 + c_3^2  \\
   !! \end{bmatrix}
   !! \f$
   !!
   !! Where \f$ c_0 = 2 - c_1 c_1 / 8 - c_2 c_2 / 8 - c_3 c_3 /8 \f$ and \f$ t_{r0} = 1/(4-c_0)^2 \f$
   !!
   !! _Note:_  The above equation is the transpose of what we have here.  This subroutine returns the transpose of the DCM,
   !!          not the DCM.
   !!
   !! Also, note that the formulation in this routine looks different, but can be shown as algebraically identical.

   c1  = cc(1)/4.0_BDKi
   c2  = cc(2)/4.0_BDKi
   c3  = cc(3)/4.0_BDKi
      ! \f$ c_0 \f$ is equivalent to \f$ 0.5 - (||\vec{c}||)^2 / 32 \f$
   c0  = 0.5_BDKi * (1.0_BDKi-c1*c1-c2*c2-c3*c3)         ! 1/4 the value of the the AIAA paper (after plugging in c1, c2, c3 conversions)

   tr0 = 1.0_BDKi - c0                          ! This is 1/4 the value of the AIAA paper, after converting c0.
   tr0 = 2.0_BDKi/(tr0*tr0)                     ! this is 32x the equivalent term from the AIAA paper.   This is well behaved and won't go to zero.

      ! The following terms can be shown to match the transpose of the DCM given in the AIAA paper.
   Rr(1,1) = tr0*(c1*c1 + c0*c0) - 1.0_BDKi
   Rr(2,1) = tr0*(c1*c2 + c0*c3)
   Rr(3,1) = tr0*(c1*c3 - c0*c2)

   Rr(1,2) = tr0*(c1*c2 - c0*c3)
   Rr(2,2) = tr0*(c2*c2 + c0*c0) - 1.0_BDKi
   Rr(3,2) = tr0*(c2*c3 + c0*c1)

   Rr(1,3) = tr0*(c1*c3 + c0*c2)
   Rr(2,3) = tr0*(c2*c3 - c0*c1)
   Rr(3,3) = tr0*(c3*c3 + c0*c0) - 1.0_BDKi

END SUBROUTINE BD_CrvMatrixR
!-----------------------------------------------------------------------------------------------------------------------------------

!> This subroutine calculutates the tangent vector, \f$ \underline{\underline{H}} \f$ defined in equation (35) of Wang, et. al.,
!! "BeamDyn: A hig-fidelity wind turbine blade solver in the FAST modular framework", Wind Energy, 2016.
!!
!! Note that the formulation here is slightly different than in the WE paper, but is algebraically equivalent.
SUBROUTINE BD_CrvMatrixH(cc,Hh)
!FIXME: would this routine benefit from matrix mathematics?  The basic formulation is matrix based.

   REAL(BDKi),INTENT(IN) ::cc(3)
   REAL(BDKi),INTENT(OUT)::Hh(3,3)

   REAL(BDKi):: cf1,cf2,cf3,cq,ocq,aa,cb0,cb1,cb2,cb3

      ! Note that the factor of 4 here is a rescaling that simplifies the equation implementation slightly.
   cf1 = cc(1)/4.0_BDKi
   cf2 = cc(2)/4.0_BDKi
   cf3 = cc(3)/4.0_BDKi
   cq  = cf1 * cf1 + cf2 * cf2 + cf3 * cf3   ! equivalent to \f$ (\vec{c} \dotproduct \vec{c}) / 16 \f$
   ocq = 1.0_BDKi + cq
   aa  = 2.0_BDKi * ocq * ocq
   cb0 = 2.0_BDKi * (1.0_BDKi - cq) / aa
   cb1 = cc(1)/aa
   cb2 = cc(2)/aa
   cb3 = cc(3)/aa

      !> The equation given in the WE paper is given as \n
      !! \f$
      !!    \underline{\underline{H}(\vec{c})} = 2/(4-c_0)^2 \left[ c_0 + \tilde{c} + \frac{1}{4} \underline{\vec{c}\vec{c}^T} \right]
      !! \f$
      !! where \f$ c_0 = 2 - \frac{1}{8} \underline{\vec{c}}^T \underline{\vec{c}} \f$
      !!
      !! This gives the matrix
      !!
      !! \f$
      !!  \underline{\underline{R} (\vec{c}) } =    \frac{2}{(4-c_0)^2}
      !!    \begin{bmatrix}
      !!       c_1 c_1 + c_0        &  c_1 c_2 - c_3     &  c_1 c_3 + c_2  \\
      !!       c_2 c_1 + c_3        &  c_2 c_2 + c_0     &  c_2 c_3 - c_1  \\
      !!       c_3 c_1 - c_2        &  c_3 c_2 + c_1     &  c_3 c_3 + c_0  \\
      !!    \end{bmatrix}
      !!    = 2/(4-c_0)^2 \left[ c_0 + \tilde{c} + \vec{c} \vec{c}^T / 4 \right]
      !! \f$\n
      !! where \f$ c_0 = 2 - \frac{1}{8} \vec{c}^T \vec{c} \f$

   Hh(1,1) = cb1 * cf1 + cb0
   Hh(2,1) = cb2 * cf1 + cb3
   Hh(3,1) = cb3 * cf1 - cb2

   Hh(1,2) = cb1 * cf2 - cb3
   Hh(2,2) = cb2 * cf2 + cb0
   Hh(3,2) = cb3 * cf2 + cb1

   Hh(1,3) = cb1 * cf3 + cb2
   Hh(2,3) = cb2 * cf3 - cb1
   Hh(3,3) = cb3 * cf3 + cb0

END SUBROUTINE BD_CrvMatrixH
!-----------------------------------------------------------------------------------------------------------------------------------
!>   This subroutine composes two Wiener-Milenkovic parameters pp and qq to find the resulting parameter rr
!!   This method is detailed in the paper: Bauchau, O.A., 2008, "Interpolation of finite rotations in flexible
!!   multi-body dynamics simulations", IMechE, Equation (9).
!!   see http://www.dymoresolutions.com/publications/BauchauEppleHeo08.pdf \n
!!   FLAG_R1R2:   flag = 0: R(rr) = R    (pp) R    (qq) \n
!!   FLAG_R1TR2:  flag = 1: R(rr) = R(T) (pp) R    (qq) \n
!!   FLAG_R1R2T:  flag = 2: R(rr) = R    (pp) R(T) (qq) \n
!!   FLAG_R1TR2T: flag = 3: R(rr) = R(T) (pp) R(T) (qq)
!!
SUBROUTINE BD_CrvCompose( rr, pp, qq, flag)
!FIXME: would this routine benefit from matrix mathematics?  The basic formulation is matrix based.

   REAL(BDKi),    INTENT(  OUT):: rr(3)     !< Composed rotation
   REAL(BDKi),    INTENT(IN   ):: pp(3)     !< Input rotation 1
   REAL(BDKi),    INTENT(IN   ):: qq(3)     !< Input rotation 2
   INTEGER       ,INTENT(IN   ):: flag      !< Option flag

   REAL(BDKi)                  :: pp0
   REAL(BDKi)                  :: p(3)
   REAL(BDKi)                  :: qq0
   REAL(BDKi)                  :: q(3)
   REAL(BDKi)                  :: tr1
   REAL(BDKi)                  :: Delta1
   REAL(BDKi)                  :: Delta2
   REAL(BDKi)                  :: dd1
   REAL(BDKi)                  :: dd2

      ! Set the local values pp and qq allowing for the transpose

   IF(flag==FLAG_R1TR2 .OR. flag==FLAG_R1TR2T) THEN ! "transpose" (negative) of first rotation parameter
       p = -pp
   ELSE
       p = pp
   ENDIF

   IF(flag==FLAG_R1R2T .OR. flag==FLAG_R1TR2T) THEN ! "transpose" (negative) of second rotation parameter
       q = -qq
   ELSE
       q = qq
   ENDIF

      !> ## Composing the resulting Wiener-Milenkovic parameter
      !!
      !! From the paper by Bauchau, the resulting parameter \f$ \underline{r} \f$ (denoted as rr in the code)
      !! can be composed from \f$ \underline{p} \f$ and \f$ \underline{q} \f$ (pp and qq, respectively, in the code) as:
      !!
      !!    \f$ \underline{r} = 4 \frac{ q_0 \underline{p} + p_0 \underline{q} + \tilde{p} \underline{q} }
      !!                               { \Delta_1 + \Delta_2 }          \f$
      !!
      !! where
      !!
      !!    \f$ p_0        =  2 - \frac{1}{8} \underline{p}^T \underline{p}
      !!                   = 2 - \frac{1}{8} \left( p_1 p_1 + p_2 p_2 + p_3 p_3 \right)   \f$ \n
      !!    \f$ q_0        =  2 - \frac{1}{8} \underline{q}^T \underline{q}
      !!                   = 2 - \frac{1}{8} \left( q_1 q_1 + q_2 q_2 + q_3 q_3 \right)   \f$ \n
      !!    \f$ \Delta_1   =  \left( 4 - p_0 \right) \left( 4 - q_0 \right)               \f$ \n
      !!    \f$ \Delta_2   =  p_0 q_0  - \underline{p}^T \underline{q}
      !!                   =  p_0 q_0 - \left( p_1 q_1 + p_2 q_2 + p_3 q_3 \right)        \f$ \n
      !!
      !!

      ! Calculate pp0 and qq0. See Bauchau for the mathematics here (equations 8 to 9 and interviening text)

   pp0 = 2.0_BDKi - dot_product(p,p) / 8.0_BDKi   ! p_0
   qq0 = 2.0_BDKi - dot_product(q,q) / 8.0_BDKi   ! q_0

   Delta1 = (4.0_BDKi - pp0) * (4.0_BDKi - qq0)   ! Delta_1 in Bauchau
   Delta2 = pp0 * qq0 - dot_product(p,q)          ! Delta_2 in Bauchau
   dd1 = Delta1 + Delta2                          ! Denomimator term for \Delta_2 >= 0
   dd2 = Delta1 - Delta2                          ! Denomimator term for \Delta_2 <  0

      ! Rescaling to remove singularities at +/- 2 \pi
      ! Note: changed this to test on \Delta_2 (instead of dd1 > dd2) for better consistency with documentation.
   IF ( Delta2 >= 0.0_BDKi ) THEN
       tr1 = 4.0_BDKi / dd1
   ELSE
       tr1 = -4.0_BDKi / dd2
   ENDIF

   rr = tr1 * (qq0*p + pp0*q + cross_product(p,q))

END SUBROUTINE BD_CrvCompose

!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes the CRV parameters given the rotation matrix
!> The algorithm for this subroutine can be found in Markley, 'Unit Quaternion from Rotation Matrix'
!> https://doi.org/10.2514/1.31730
SUBROUTINE BD_CrvExtractCrv(R, cc, ErrStat, ErrMsg)

   REAL(BDKi),       INTENT(IN   )  :: R(3,3)        !< Rotation Matrix
   REAL(BDKi),       INTENT(  OUT)  :: cc(3)         !< Crv paramters
   INTEGER(IntKi),   INTENT(  OUT)  :: ErrStat       !< Error status of the operation
   CHARACTER(*),     INTENT(  OUT)  :: ErrMsg        !< Error message if ErrStat /= ErrID_None

   REAL(BDKi)                  :: pivot(4) ! Trace of the rotation matrix and diagonal elements
   REAL(BDKi)                  :: sm(0:3)
   REAL(BDKi)                  :: em
   REAL(BDKi)                  :: Rr(3,3)
   INTEGER                     :: i        ! case indicator

   INTEGER(IntKi)              :: ErrStat2 ! Temporary Error status
   CHARACTER(ErrMsgLen)        :: ErrMsg2  ! Temporary Error message
   character(*), parameter     :: RoutineName = 'BD_CrvExtractCrv'

   ! Initialize ErrStat
   ErrStat = ErrID_None
   ErrMsg  = ""

   ! use the local rotation matrix variable to avoid side effects
   Rr = R

   !> Starting with equation (14) from AIAA paper, "Geometric Nonlinear Analysis of Composite Beams Using
   !! Wiener-Milenkovic Parameters", Wang, et. al. \n
   !! \f$
   !!  \underline{\underline{R}} (\vec{c}) =    t_{r0}
   !! \begin{bmatrix}
   !! c_0^2 + c_1^2 - c_2^2 - c_3^2    & 2(c_1 c_2 + c_0 c_3)           & 2(c_1 c_3 - c_0 c_2)           \\
   !! 2(c_1 c_2 - c_0 c_3)             & c_0^2 - c_1^2 + c_2^2 - c_3^2  & 2(c_2 c_3 + c_0 c_1)           \\
   !! 2(c_1 c_3 + c_0 c_2)             & 2(c_2 c_3 - c_0 c_1)           & c_0^2 - c_1^2 - c_2^2 + c_3^2  \\
   !! \end{bmatrix}
   !! \f$
   !!
   !! where \f$ c_0 = 2 - c_1 c_1 / 8 - c_2 c_2 / 8 - c_3 c_3 /8 \f$ and \f$ t_{r0} = 1/(4-c_0)^2 \f$
   !!
   !! _Note:_ The above equation does not match what is in the March 2016 BD manual (it is the transpose).
   !! It does, however, match equation 5.17 in the March 2016 "BeamDyn User's Guide and Theory Manual"

   ! mjs--determine whether R is a valid rotation matrix and correct it if not
   call BD_CheckRotMat(Rr, ErrStat2, ErrMsg2)
   CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
   if (ErrStat >= AbortErrLev) return

   ! mjs--find max value of T := Tr(Rr) and diagonal elements of Rr
   ! This tells us which denominator is largest (and less likely to produce numerical noise)
   pivot = (/ Rr(1,1) + Rr(2,2) + Rr(3,3), Rr(1,1), Rr(2,2), Rr(3,3)  /)
   i = maxloc(pivot, 1) - 1 ! our sm array starts at 0, so we need to subtract 1 here to get the correct index

      !> - Condition 1: \f$ \underline{\underline{R(3,3)}} > T \f$
      !! This implies that \f$ c_3^2 > c_0^2 \f$
   select case (i)
   case (3)
      !! We need the condition that c_3 is not zero for this set of equations.
      sm(0)  = Rr(2,1) - Rr(1,2)                           !  4 c_0 c_3 t_{r0}
      sm(1)  = Rr(1,3) + Rr(3,1)                           !  4 c_1 c_3 t_{r0}
      sm(2)  = Rr(2,3) + Rr(3,2)                           !  4 c_2 c_3 t_{r0}
      sm(3)  = 1.0_BDKi - Rr(1,1) - Rr(2,2) + Rr(3,3)      !  4 c_3 c_3 t_{r0}

      !> - Condition 2: \f$ \underline{\underline{R(2,2)}} > T \f$
      !! This implies that \f$ c_2^2 > c_0^2 \f$
   case (2)
      !! We need the condition that c_2 is not zero for this set of equations.
      sm(0)  = Rr(1,3) - Rr(3,1)                           !  4 c_0 c_2 t_{r0}
      sm(1)  = Rr(1,2) + Rr(2,1)                           !  4 c_1 c_2 t_{r0}
      sm(2)  = 1.0_BDKi - Rr(1,1) + Rr(2,2) - Rr(3,3)      !  4 c_2 c_2 t_{r0}
      sm(3)  = Rr(2,3) + Rr(3,2)                           !  4 c_3 c_2 t_{r0}

      !> - Condition 3: \f$ \underline{\underline{R(1,1)}} > T \f$
      !! This implies that \f$ c_1^2 > c_0^2 \f$
   case (1)
      !! We need the condition that c_1 is not zero for this set of equations.
      sm(0)  = Rr(3,2) - Rr(2,3)                           !  4 c_0 c_1 t_{r0}
      sm(1)  = 1.0_BDKi + Rr(1,1) - Rr(2,2) - Rr(3,3)      !  4 c_1 c_1 t_{r0}
      sm(2)  = Rr(1,2) + Rr(2,1)                           !  4 c_2 c_1 t_{r0}
      sm(3)  = Rr(1,3) + Rr(3,1)                           !  4 c_3 c_1 t_{r0}

      !> - Condition 4: all diagonal terms are less than the trace
   case (0)
      !! We need the condition that c_0 is not zero for this set of equations.
      sm(0)  = 1.0_BDKi + Rr(1,1) + Rr(2,2) + Rr(3,3)      !  4 c_0 c_0 t_{r0}
      sm(1)  = Rr(3,2) - Rr(2,3)                           !  4 c_1 c_0 t_{r0}
      sm(2)  = Rr(1,3) - Rr(3,1)                           !  4 c_2 c_0 t_{r0}
      sm(3)  = Rr(2,1) - Rr(1,2)                           !  4 c_3 c_0 t_{r0}
   end select

   em = sm(0) + SIGN( 2.0_BDKi*SQRT(sm(i)), sm(0) )
   em = 4.0_BDKi/em                                        ! 1 / ( 4 t_{r0} c_{i} ), assuming 0 <= c_0 < 4 and c_{i} > 0
   cc = em*sm(1:3)

END SUBROUTINE BD_CrvExtractCrv

SUBROUTINE BD_CheckRotMat(R, ErrStat, ErrMsg)
   !> This subroutine checks for rotation matrix validity.
   !> Returns:
   !>   ErrStat = 0 if valid
   !>   ErrStat = 4 (fatal error) if invalid

   REAL(BDKi),       INTENT(IN   )  :: R(3,3)       !< Rotation Matrix
   INTEGER(IntKi),   INTENT(  OUT)  :: ErrStat      !< Error status of the operation
   CHARACTER(*),     INTENT(  OUT)  :: ErrMsg       !< Error message if ErrStat /= ErrID_None
   REAL(BDKi)                       :: Rr(3,3)      !< Local Rotation Matrix variable
   INTEGER(IntKi)                   :: lwork = 27   !mjs--from LAPACK: dgesvd doc page, lwork >= MAX(1,3*MIN(M,N) + MAX(M,N),5*MIN(M,N))
   REAL(BDKi), ALLOCATABLE          :: work(:)      ! where M x N is dimension of R, and lwork is the dimension of work
   REAL(BDKi)                       :: S(3), U(3,3), VT(3,3) !mjs--these three are the SVD matrices (S is actually a vector)
   INTEGER(IntKi)                   :: ErrStat2     ! Temporary Error status
   CHARACTER(ErrMsgLen)             :: ErrMsg2      ! Temporary Error message
   LOGICAL                          :: ortho        !mjs--logical value indicating whether R is orthogonal
   INTEGER                          :: i
   character(*), parameter          :: RoutineName = 'BD_CheckRotMat'

   ! Initialize ErrStat
   ErrStat = ErrID_None
   ErrMsg  = ""

   ! use the local rotation matrix variable to avoid side effects
   Rr = R

   ! mjs--Start by determining if R is a valid rotation matrix using the properties:
   ! 1) the eigenvalues of an orthogonal matrix have complex modulus == 1, where
   !    the leading eigenvalue is +1 and the other two are a complex conjugate pair
   ! 2) a valid rotation matrix must have determinant == +1 i.e., the singular values == 1

   allocate(work(lwork))
   call LAPACK_gesvd('A', 'A', 3, 3, Rr, S, U, VT, work, lwork, ErrStat2, ErrMsg2)
   CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
   if (ErrStat >= AbortErrLev) return
   deallocate(work)

   ! mjs--If \f$ \underline{\underline{R}} \f$ is not a valid roatation tensor,
   !    and the correction is desired,
   !    compute \f$ \underline{\underline{R_{out}} \f$, the nearest orthogonal tensor
   !    to \f$ \underline{\underline{R}} \f$.
   !    This is done via computing SVD for \f$ \underline{\underline{R}} = USV^T \f$
   !    and setting \f$ \underline{\underline{R_{out}} = UV^T \f$
   !    otherwise, assign \f$ \underline{\underline{R_{out}}}  = \underline{\underline{R}} \f$

   do i = 1, 3
      ortho = equalrealnos(S(i), 1.0_BDKi)
      if (.not. ortho) then
         CALL SetErrStat(ErrID_Fatal, "Passed invalid rotation matrix", ErrStat, ErrMsg, RoutineName)
         if (ErrStat >= AbortErrLev) return
      end if
   end do

   ! mjs--after consulting with Mike Sprague, it was decided that instead of fixing the rotation matrix and
   ! notifying the user, the simulation should be stopped if an invalid rotation matrix is passed
   ! To change this and implement the fix, use the following lines
   ! ErrStat2 = ErrID_Info
   ! ErrMsg2 = 'Passed invalid rotation matrix--fixing via SVD'
   ! CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
   ! if (ErrStat >= AbortErrLev) return
   ! R = matmul(U, VT)

END SUBROUTINE BD_CheckRotMat

!------------------------------------------------------------------------------
!> This subroutine generates n-point gauss-legendre quadrature points and weights
!!
!! Subroutine is based on well-known formulas for generating Legendre polynomials, the
!! roots of which are the Gauss-Legendre quadrature points.  Also used are well-known
!! expressions for the quadrature weights associated with the GL quadrature points.
!! The basic idea of the logic is to use the roots of the Chebyshev polynomial as
!! an initial guess for the roots of the Legendre polynomial, and to then use Newton
!! iteration to find the "exact" roots.
SUBROUTINE BD_GaussPointWeight(n, x, w, ErrStat, ErrMsg)

   INTEGER(IntKi),INTENT(IN   ):: n       !< Number of Gauss point (p%nqp)
   REAL(BDKi),    INTENT(  OUT):: x(n)    !< Gauss point location (p%QPtN)
   REAL(BDKi),    INTENT(  OUT):: w(n)    !< Gauss point weight (p%QPtWeight)
   INTEGER(IntKi),INTENT(  OUT):: ErrStat !< Error status of the operation
   CHARACTER(*),  INTENT(  OUT):: ErrMsg  !< Error message if ErrStat /=

   ! local variables

   REAL(BDKi),  PARAMETER:: eps = 1000.0_BDKi*EPSILON(eps) / 2.0_BDKi   ! tolerance for newton-raphson solve (ignores 3 significant digits)

   INTEGER(IntKi) :: n_half

   REAL(BDKi)     :: n_real  ! real version of loop index
   INTEGER(IntKi) :: i       ! loop index
   INTEGER(IntKi) :: j       ! loop index
   REAL(BDKi)     :: j_real  ! real version of loop index

   INTEGER(IntKi) :: newton     ! newton-iteration index
   INTEGER(IntKi), parameter :: newton_max = 10 ! maximum number of newton iterations; should not need many!

   REAL(BDKi)     :: p1  ! legendre polynomial j-2
   REAL(BDKi)     :: p2  ! legendre polynomial j-1
   REAL(BDKi)     :: p3  ! legendre polynomial j

   REAL(BDKi)     :: dp3  ! derivative of legendre polynomial j

   if (n .lt. 2) then
      ErrStat = ErrID_Fatal
      ErrMsg  = 'BD_GaussPointWeight: invalid value of n.'
   else
      ErrStat = ErrID_None
      ErrMsg  = ''
   endif

   n_real = REAL(n,BDKi)

   n_half = (n+1) / 2  ! integer for "half" the points: e.g., n=5 => n_half=3, n=4 => n_half = 2

   ! loop through each of the "half" n points
   do i = 1, n_half

      ! Intial guess for ith root; based on ith root of Chebyshev polynomial
      x(i) = - cos( REAL( 2*i - 1, BDKi)  * pi_D / (2.0_BDKi * n_real) )

      ! initialize newton iteration counter
      newton = 0

      p3 = 1. ! some intial big number;  logic will always take at least one newton iteration
      do while (abs(p3) .gt. eps .and. newton .le. newton_max)

         newton = newton + 1  ! keep track of number of newton iterations

         ! build the legendre polynomial and its derivative evaluated at current root guess
         p1 = 1.0_BDKi  ! zeroth order legendre polynomial
         p2 = x(i)      ! first-order legendre polynomial

         do j = 2, n

            j_real = REAL(j, BDKi)

            ! recurrence relation for legendre polynomial
            p3 = ( REAL(2*j - 1, BDKi) * x(i) * p2 - REAL(j - 1, BDKi) * p1 ) / j_real

            ! derivative of legendre polynomial; needed for newton iteration
            dp3 = j_real * ( - x(i) * p3 + p2 ) / ( 1.0_BDKi - x(i)*x(i) )

            ! save values for next iteration
            p1 = p2
            p2 = p3

         enddo

         ! newton iteration: xnew = xold - P_n(xold) / P_n'(xold)
         x(i) = x(i) - p3 / dp3

      enddo

      if (abs(x(i)) .lt. eps) x(i) = 0.0_BDKi

      ! fill in the other "side" of the array
      x(n-i+1) = - x(i)

      ! calculate weight for this Gauss point location
      w(i) = 2.0_BDKi / ( ( 1.0_BDKi - x(i)*x(i) ) * dp3 * dp3 )
      w(n-i+1) = w(i)

   enddo

END SUBROUTINE BD_GaussPointWeight
!-----------------------------------------------------------------------------------------------------------------------------------
! This subroutine computes trapezoidal quadrature points and weights, p%QPtN and p%QPtWeight
SUBROUTINE BD_TrapezoidalPointWeight(p, station_eta, station_total)

   TYPE(BD_ParameterType),INTENT(INOUT):: p              !< BeamDyn parameters
   Integer(IntKi),INTENT(IN   )        :: station_total
   REAL(BDKi),INTENT(IN   )            :: station_eta(:)

   ! local variables
   REAL(BDKi)                 :: denom ! denominator for quadrature weight computations

   INTEGER(IntKi)             :: indx, temp_id0, temp_id1  ! temporary index into QPtN array
   INTEGER(IntKi), parameter  :: id0 = 1
   INTEGER(IntKi)             :: id1, j

!bjj: this assumes there is only one member

      ! compute the trapezoidal quadrature points, p%QPtN, and scale to range [-1,1]:
      !  If there is refinement, this will add new points between the specified ones. If p%refine == 1, can skip this.
   p%QPtN(1) = station_eta(1)
   DO j = 2,p%nqp
      indx =  1+(j-2_IntKi)/p%refine       ! note use of integer math here --> (J-2)/p%refine may not be integer.
      p%QPtN(j) =  station_eta(indx) + &
               ((station_eta(indx+1) - station_eta(indx))/p%refine) * (MOD(j-2,p%refine) + 1)
   ENDDO
   p%QPtN = 2.0_BDKi*p%QPtN - 1.0_BDKi     ! rescale range from [0, 1] to [-1,1]

      ! compute the trapezoidal quadrature weights, p%QPtWeight:
   id1 = station_total
   temp_id0 = (id0 - 1)*p%refine + 1            ! Starting index in QPtN --> always going to be 1
   temp_id1 = (id1 - 1)*p%refine + 1            ! ending index in QPtN --> will be  size(p%QPtN)
   denom = p%QPtN(temp_id1) - p%QPtN(temp_id0)  ! This is the range of QPtN --> for single member, is always == 2

   p%QPtWeight(1)     =  (p%QPtN(temp_id0+1) - p%QPtN(temp_id0    ))/denom
   DO j=2,p%nqp-1
      p%QPtWeight(j)  =  (p%QPtN(temp_id0+j) - p%QPtN(temp_id0+j-2))/denom
   ENDDO
   p%QPtWeight(p%nqp) =  (p%QPtN(temp_id1  ) - p%QPtN(temp_id1-1  ))/denom

END SUBROUTINE BD_TrapezoidalPointWeight

!-----------------------------------------------------------------------------------------------------------------------------------
!> This routine calculates y%BldMotion%TranslationDisp, y%BldMotion%Orientation, y%BldMotion%TranslationVel, and
!! y%BldMotion%RotationVel, which depend only on states (and indirectly, u%RootMotion), and parameters.
SUBROUTINE Set_BldMotion_NoAcc(p, x, m, y)

   TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
   TYPE(BD_ContinuousStateType), INTENT(IN   )  :: x           !< Continuous states at t
   TYPE(BD_MiscVarType),         INTENT(IN   )  :: m           !< misc/optimization variables
   TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< Outputs computed at t (Input only so that mesh con-
                                                               !!   nectivity information does not have to be recalculated)
   INTEGER(IntKi)                               :: i
   INTEGER(IntKi)                               :: j
   INTEGER(IntKi)                               :: temp_id
   INTEGER(IntKi)                               :: temp_id2
   REAL(BDKi)                                   :: cc(3)
   REAL(BDKi)                                   :: cc0(3)
   REAL(BDKi)                                   :: temp_R(3,3)
   CHARACTER(*), PARAMETER                      :: RoutineName = 'Set_BldMotion_NoAcc'

   ! The first node on the mesh is just the root location, but since the orientation of the root node may not
   ! RootMotion orientation (due to blade twist), we will calculated it as though it was a regular node

   ! now fill in the other nodes
   SELECT CASE (p%BldMotionNodeLoc)

   CASE (BD_MESH_FE)

      DO i=1,p%elem_total
         DO j=1,p%nodes_per_elem
            temp_id = (i-1)*(p%nodes_per_elem-1)+j      ! The last node of the first element is used as the first node in the second element.
            temp_id2= (i-1)*p%nodes_per_elem+j          ! Index to a node within element i

               ! Calculate the translational displacement of each GLL node in the FAST coordinate system,
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%TranslationDisp(1:3,temp_id2) = MATMUL(p%GlbRot,x%q(1:3,temp_id))

!bjj: note differences here compared to BDrot_to_FASTdcm
!adp: in BDrot_to_FASTdcm we are assuming that x%q(4:6,:) is zero because there is no rotatinoal displacement yet
               ! Find the rotation parameter in global coordinates (initial orientation + rotation parameters)
               ! referenced against the DCM of the blade root at T=0.
            CALL BD_CrvCompose( cc, x%q(4:6,temp_id), p%uuN0(4:6,j,i), FLAG_R1R2 )
            CALL BD_CrvCompose( cc0, p%Glb_crv, cc, FLAG_R1R2 )

               ! Create the DCM from the rotation parameters
            CALL BD_CrvMatrixR(cc0,temp_R)  ! returns temp_R (the transpose of the DCM orientation matrix)

               ! Store the DCM for the j'th node of the i'th element (in FAST coordinate system)
            y%BldMotion%Orientation(1:3,1:3,temp_id2) = TRANSPOSE(temp_R)

               ! Calculate the translation velocity and store in FAST coordinate system
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%TranslationVel(1:3,temp_id2) = MATMUL(p%GlbRot,x%dqdt(1:3,temp_id))

               ! Calculate the rotational velocity and store in FAST coordinate system
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%RotationVel(1:3,temp_id2) = MATMUL(p%GlbRot,x%dqdt(4:6,temp_id))

         ENDDO
      ENDDO

   CASE (BD_MESH_QP)

      DO i=1,p%elem_total
         DO j=1,p%nqp
            temp_id2 = (i-1)*p%nqp + j + p%qp_indx_offset            ! Index to a node within element i

               ! Calculate the translational displacement of each quadrature node in the FAST coordinate system,
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%TranslationDisp(1:3,temp_id2) = MATMUL(p%GlbRot,m%qp%uuu(1:3,j,i) )

!bjj: note differences here compared to BDrot_to_FASTdcm
!adp: in BDrot_to_FASTdcm we are assuming that x%q(4:6,:) is zero because there is no rotatinoal displacement yet
               ! Find the rotation parameter in global coordinates (initial orientation + rotation parameters)
               ! referenced against the DCM of the blade root at T=0.
            CALL BD_CrvCompose( cc, m%qp%uuu(4:6,j,i), p%uu0(4:6,j,i), FLAG_R1R2 )
            CALL BD_CrvCompose( cc0, p%Glb_crv, cc, FLAG_R1R2 )

            CALL BD_CrvMatrixR(cc0,temp_R)  ! returns temp_R (the transpose of the DCM orientation matrix)
               ! Store the DCM for the j'th node of the i'th element (in FAST coordinate system)
            y%BldMotion%Orientation(1:3,1:3,temp_id2) = TRANSPOSE(temp_R)

               ! Calculate the translation velocity and store in FAST coordinate system
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%TranslationVel(1:3,temp_id2) = MATMUL(p%GlbRot,m%qp%vvv(1:3,j,i))

               ! Calculate the rotational velocity and store in FAST coordinate system
               ! referenced against the DCM of the blade root at T=0.
            y%BldMotion%RotationVel(1:3,temp_id2) = MATMUL(p%GlbRot,m%qp%vvv(4:6,j,i))

         ENDDO
      ENDDO

   CASE (BD_MESH_STATIONS)
   END SELECT

END SUBROUTINE Set_BldMotion_NoAcc
!-----------------------------------------------------------------------------------------------------------------------------------
!> This routine calculates values for the y%BldMotion mesh.
SUBROUTINE Set_BldMotion_Mesh(p, u, x, m, y)

   TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
   TYPE(BD_InputType),           INTENT(IN   )  :: u           !< Inputs at t - in the FAST coordinate system (NOT BD)
   TYPE(BD_ContinuousStateType), INTENT(IN   )  :: x           !< Continuous states at t
   TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< misc/optimization variables ! intent(out) so that we can update the accelerations here...
   TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< Outputs computed at t (Input only so that mesh con-
                                                               !!   nectivity information does not have to be recalculated)
   INTEGER(IntKi)                               :: i
   INTEGER(IntKi)                               :: j
   INTEGER(IntKi)                               :: j_start !starting node on this element
   INTEGER(IntKi)                               :: idx_node
   INTEGER(IntKi)                               :: temp_id
   INTEGER(IntKi)                               :: temp_id2
   CHARACTER(*), PARAMETER                      :: RoutineName = 'Set_BldMotion_Mesh'

      ! set positions and velocities (not accelerations)
   call Set_BldMotion_NoAcc(p, x, m, y)

   ! Only need this bit for dynamic cases
   IF ( p%analysis_type /= BD_STATIC_ANALYSIS ) THEN

       ! now set the accelerations:

       ! The first node on the mesh is just the root location:
       y%BldMotion%TranslationAcc(:,1)    = u%RootMotion%TranslationAcc(:,1)
       y%BldMotion%RotationAcc(:,1)       = u%RootMotion%RotationAcc(:,1)

       SELECT CASE (p%BldMotionNodeLoc)

       CASE (BD_MESH_FE) ! This is how the NREL-version of BeamDyn works (output nodes at the finite element nodes)

          j_start = 2 ! we'll skip the first node on the first element; otherwise we will start at the first node on the other elements
          DO i=1,p%elem_total
             DO j=j_start,p%nodes_per_elem
                temp_id = (i-1)*(p%nodes_per_elem-1)+j
                temp_id2= (i-1)*p%nodes_per_elem+j

                y%BldMotion%TranslationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, m%RHS(1:3,temp_id) )

                y%BldMotion%RotationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, m%RHS(4:6,temp_id) )
             ENDDO
             j_start = 1
          ENDDO

       CASE (BD_MESH_QP)

          m%qp%aaa(1:3,1,1) = u%RootMotion%TranslationAcc(:,1)
          m%qp%aaa(4:6,1,1) = u%RootMotion%RotationAcc(:,1)

            ! Calculate the and acceleration term at t+dt (OtherState%acc is at t+dt)
            j_start = 2 ! we'll skip the first node on the first element; otherwise we will start at the first node on the other elements
            DO i=1,p%elem_total
                DO j=j_start,p%nqp

                ! Initialize to zero for summation, then recalculate accelerations at quadrature nodes based on accelerations at FE nodes
                m%qp%aaa(:,j,i) = 0.0_BDKi
                DO idx_node=j_start,p%nodes_per_elem
                    m%qp%aaa(:,j,i) = m%qp%aaa(:,j,i) + p%Shp(idx_node,j) * m%RHS(:,p%node_elem_idx(i,1)-1+idx_node)
                ENDDO

                temp_id2 = (i-1)*p%nqp + j + p%qp_indx_offset            ! Index to a node within element i

                    ! Calculate the translational acceleration of each quadrature node in the FAST coordinate system,
                    ! referenced against the DCM of the blade root at T=0.
                y%BldMotion%TranslationAcc(1:3,temp_id2) = MATMUL(p%GlbRot,m%qp%aaa(1:3,j,i) )

                y%BldMotion%RotationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, m%qp%aaa(4:6,j,i) )
                ENDDO
                j_start = 1
            ENDDO

       CASE (BD_MESH_STATIONS)
       END SELECT

   END IF

END SUBROUTINE Set_BldMotion_Mesh
!> This routine calculates values for the y%BldMotion mesh.
!-----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE Set_BldMotion_InitAcc(p, u, OtherState, m, y)

   TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
   TYPE(BD_InputType),           INTENT(IN   )  :: u           !< Inputs at t - in the FAST coordinate system (NOT BD)
   TYPE(BD_OtherStateType),      INTENT(IN   )  :: OtherState  !< Other states at t
   TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< misc/optimization variables ! intent(out) so that we can update the accelerations here...
   TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< Outputs computed at t (Input only so that mesh con-
                                                               !!   nectivity information does not have to be recalculated)
   INTEGER(IntKi)                               :: i
   INTEGER(IntKi)                               :: j
   INTEGER(IntKi)                               :: j_start !starting node on this element
   INTEGER(IntKi)                               :: temp_id
   INTEGER(IntKi)                               :: temp_id2
   CHARACTER(*), PARAMETER                      :: RoutineName = 'Set_BldMotion_InitAcc'

      ! The first node on the mesh is just the root location:
      y%BldMotion%TranslationAcc(:,1)    = u%RootMotion%TranslationAcc(:,1)
      y%BldMotion%RotationAcc(:,1)       = u%RootMotion%RotationAcc(:,1)

      SELECT CASE (p%BldMotionNodeLoc)

      CASE (BD_MESH_FE) ! This is how the NREL-version of BeamDyn works (output nodes at the finite element nodes)

         j_start = 2 ! we'll skip the first node on the first element; otherwise we will start at the first node on the other elements
         DO i=1,p%elem_total
            DO j=j_start,p%nodes_per_elem
               temp_id = (i-1)*(p%nodes_per_elem-1)+j
               temp_id2= (i-1)*p%nodes_per_elem+j

               y%BldMotion%TranslationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, OtherState%Acc(1:3,temp_id) )

               y%BldMotion%RotationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, OtherState%Acc(4:6,temp_id) )
            ENDDO
            j_start = 1
         ENDDO

      CASE (BD_MESH_QP)

         ! Calculate the and acceleration term at t+dt (OtherState%acc is at t+dt)
         j_start = 2 ! we'll skip the first node on the first element; otherwise we will start at the first node on the other elements
         DO i=1,p%elem_total
            DO j=j_start,p%nqp

               temp_id2 = (i-1)*p%nqp + j + p%qp_indx_offset            ! Index to a node within element i

                  ! Calculate the translational acceleration of each quadrature node in the FAST coordinate system,
                  ! referenced against the DCM of the blade root at T=0.
               y%BldMotion%TranslationAcc(1:3,temp_id2) = MATMUL(p%GlbRot,m%qp%aaa(1:3,j,i) )

               y%BldMotion%RotationAcc(1:3,temp_id2) = MATMUL(p%GlbRot, m%qp%aaa(4:6,j,i) )
            ENDDO
            j_start = 1
         ENDDO

      CASE (BD_MESH_STATIONS)
      END SELECT

END SUBROUTINE Set_BldMotion_InitAcc
!-----------------------------------------------------------------------------------------------------------------------------------
!> calculate Lagrangian interpolant tensor at ns points where basis
!! functions are assumed to be associated with (np+1) GLL points on [-1,1]
SUBROUTINE BD_diffmtc( nodes_per_elem,GLL_nodes,QPtN,nqp,Shp,ShpDer )

   ! See Bauchau equations 17.1 - 17.5

   INTEGER(IntKi),         INTENT(IN   )  :: nodes_per_elem !< Nodes per elemenent
   REAL(BDKi),             INTENT(IN   )  :: GLL_nodes(:)   !< GLL_nodes(p%nodes_per_elem): location of the (p%nodes_per_elem) p%GLL points
   REAL(BDKi),             INTENT(IN   )  :: QPtN(:)        !< Locations of quadrature points ([-1 1])
   INTEGER(IntKi),         INTENT(IN   )  :: nqp            !< number of quadrature points to consider. Should be size of 2nd index of Shp & ShpDer
   REAL(BDKi),             INTENT(INOUT)  :: Shp(:,:)       !< p%Shp    (or another Shp array for when we add outputs at arbitrary locations)
   REAL(BDKi),             INTENT(INOUT)  :: ShpDer(:,:)    !< p%ShpDer (or another Shp array for when we add outputs at arbitrary locations)

   REAL(BDKi)                  :: dnum
   REAL(BDKi)                  :: den
   REAL(BDKi),        PARAMETER:: eps = SQRT(EPSILON(eps)) !1.0D-08
   INTEGER(IntKi)              :: l
   INTEGER(IntKi)              :: j
   INTEGER(IntKi)              :: i
   INTEGER(IntKi)              :: k

   ! See Bauchau equations 17.1 - 17.5

   Shp(:,:)     = 0.0_BDKi
   ShpDer(:,:)  = 0.0_BDKi

   do j = 1,nqp
      do l = 1,nodes_per_elem

       if ((abs(QPtN(j)-1.).LE.eps).AND.(l.EQ.nodes_per_elem)) then           !adp: FIXME: do we want to compare to eps, or EqualRealNos???
         ShpDer(l,j) = REAL((nodes_per_elem)*(nodes_per_elem-1), BDKi)/4.0_BDKi
       elseif ((abs(QPtN(j)+1.).LE.eps).AND.(l.EQ.1)) then
         ShpDer(l,j) = -REAL((nodes_per_elem)*(nodes_per_elem-1), BDKi)/4.0_BDKi
       elseif (abs(QPtN(j)-GLL_nodes(l)).LE.eps) then
         ShpDer(l,j) = 0.0_BDKi
       else
         ShpDer(l,j) = 0.0_BDKi
         den = 1.0_BDKi
         do i = 1,nodes_per_elem
           if (i.NE.l) then
             den = den*(GLL_nodes(l)-GLL_nodes(i))
           endif
           dnum = 1.0_BDKi
           do k = 1,nodes_per_elem
             if ((k.NE.l).AND.(k.NE.i).AND.(i.NE.l)) then
               dnum = dnum*(QPtN(j)-GLL_nodes(k))
             elseif (i.EQ.l) then
               dnum = 0.0_BDKi
             endif
           enddo
           ShpDer(l,j) = ShpDer(l,j) + dnum
         enddo
         ShpDer(l,j) = ShpDer(l,j)/den
       endif
     enddo
   enddo

   do j = 1,nqp
      do l = 1,nodes_per_elem

       if(abs(QPtN(j)-GLL_nodes(l)).LE.eps) then
         Shp(l,j) = 1.0_BDKi
       else
         dnum = 1.0_BDKi
         den  = 1.0_BDKi
         do k = 1,nodes_per_elem
           if (k.NE.l) then
             den  = den *(GLL_nodes(l) - GLL_nodes(k))
             dnum = dnum*(QPtN(j) - GLL_nodes(k))
           endif
         enddo
         Shp(l,j) = dnum/den
       endif
     enddo
   enddo

 END SUBROUTINE BD_diffmtc
!-----------------------------------------------------------------------------------------------------------------------------------
!> this routine interpolates teh POS and CRV based on the nodal values
SUBROUTINE BD_Interp_Pos_CRV(p, eta, POS, CRV, ErrStat, ErrMsg)

   type(BD_ParameterType),       intent(in   )  :: p       ! BD Parameters
   real(BDki),                   intent(in   )  :: eta     ! location to interpolate to;  0 <= eta <= 1
   real(BDki),                   intent(  out)  :: POS(3)  ! output XYZ
   real(BDki),                   intent(  out)  :: CRV(3)  ! output rotation parameters

   INTEGER(IntKi), INTENT(  OUT)  :: ErrStat       !< Error status of the operation
   CHARACTER(*),   INTENT(  OUT)  :: ErrMsg        !< Error message if ErrStat /= ErrID_None

   ! local variables

   integer(IntKi) :: i            ! do loop
   integer(IntKi) :: j            ! do loop
   integer(IntKi) :: found        ! marker for finding element that eta lies in
   integer(IntKi) :: element      ! element where eta lies
   real(BDki)     :: eta_left     ! left eta value for an element
   real(BDki)     :: eta_right    ! right eta_value for an element
   real(BDki)     :: eta_local(1) ! eta_local in [-1,1] for finite element space

   real(BDki),allocatable     :: gll(:)      ! local gll points; ez enough to generate here
   real(BDki),allocatable     :: shp(:,:)    ! local shape function
   real(BDki),allocatable     :: shpder(:,:) ! local shape function deriv

   INTEGER(IntKi)                 :: ErrStat2      ! Temporary Error status
   CHARACTER(ErrMsgLen)           :: ErrMsg2       ! Temporary Error message
   character(*), parameter        :: RoutineName = 'BD_Interp_Pos_CRV'

   ErrStat = ErrID_None
   ErrMsg = ""

   ! find element in which eta resides
   eta_right = 0._BDKi
   found = 0
   eta_left = 0._BDki
   do i = 1, p%elem_total
      eta_right = eta_right + p%member_eta(i)
      if (eta .le. eta_right .and. found .eq. 0) then
         element = i
         found = 1
      endif
      if (found .eq. 0) then
         eta_left = eta_right
      endif
   enddo

   ! need to evaluate shp and shpder at eta_local in [-1,1] for the found element
   eta_local(1) = 2._BDKi * (eta - eta_left)/p%member_eta(element) - 1._BDKi

   call AllocAry(gll, p%nodes_per_elem, "local GLL nodes",ErrStat2, ErrMsg2)
      call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
   call AllocAry(shp, p%nodes_per_elem, 1,"local shape function",ErrStat2, ErrMsg2)
      call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
   call AllocAry(shpder, p%nodes_per_elem, 1,"local shape deriv function",ErrStat2, ErrMsg2)
      call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)

   if (ErrStat < AbortErrLev) then
      call BD_GenerateGLL(p%nodes_per_elem,gll,ErrStat2,ErrMsg2)
         call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)

      call bd_diffmtc(p%nodes_per_elem, gll, eta_local, 1, shp, shpder) ! evaluate shp and shpder at single point

      pos = 0._BDki
      crv = 0._BDki
      do i = 1, p%nodes_per_elem
         do j = 1, 3
            pos(j) = pos(j) + p%uuN0(j,  i,element)  *shp(i,1)
            CRV(j) = CRV(j) + p%uuN0(j+3,i,element)*shp(i,1)
         enddo
      enddo

   end if

   if (allocated(gll)) deallocate(gll)
   if (allocated(shp)) deallocate(shp)
   if (allocated(shpder)) deallocate(shpder)

END SUBROUTINE BD_Interp_Pos_CRV
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes initial CRV parameters
!! given geometry information
SUBROUTINE BD_ComputeIniNodalCrv(e1, phi, cc, ErrStat, ErrMsg)

   REAL(BDKi),     INTENT(IN   )  :: e1(:)         !< Tangent unit vector
   REAL(BDKi),     INTENT(IN   )  :: phi           !< Initial twist angle, in degrees
   REAL(BDKi),     INTENT(  OUT)  :: cc(:)         !< Initial Crv Parameter
   INTEGER(IntKi), INTENT(  OUT)  :: ErrStat       !< Error status of the operation
   CHARACTER(*),   INTENT(  OUT)  :: ErrMsg        !< Error message if ErrStat /= ErrID_None

   REAL(BDKi)                     :: e2(3)         !< Unit normal vector
   REAL(BDKi)                     :: Rr(3,3)       !< Initial rotation matrix
   REAL(BDKi)                     :: PhiRad        !< Phi in radians
   REAL(BDKi)                     :: Delta

   INTEGER(IntKi)                 :: ErrStat2      ! Temporary Error status
   CHARACTER(ErrMsgLen)           :: ErrMsg2       ! Temporary Error message
   CHARACTER(*), PARAMETER        :: RoutineName = 'BD_ComputeIniNodalCrv'

   ! Initialize ErrStat
   ErrStat = ErrID_None
   ErrMsg  = ""

   PhiRad = phi*D2R_D  ! convert to radians

      ! Note that e1 corresponds with the Z-axis direction in our formulation.  For Beam theory, this would be the x-axis.
   Rr(:,3)  =  e1(:)

   e2(3)    = -(e1(1)*COS(PhiRad) + e1(2)*SIN(PhiRad))/e1(3)
   Delta    = SQRT(1.0_BDKi + e2(3)*e2(3))
   e2(1)    = COS(PhiRad)
   e2(2)    = SIN(PhiRad)
   e2       = e2 / Delta
   Rr(:,1)  = e2
   Rr(:,2)  = Cross_Product(e1,e2)

   CALL BD_CrvExtractCrv(Rr, cc, ErrStat2, ErrMsg2)
      CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
      if (ErrStat >= AbortErrLev) return

END SUBROUTINE BD_ComputeIniNodalCrv
!-----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE ExtractRelativeRotation(R, p, rr, ErrStat, ErrMsg)
   real(R8Ki),             INTENT(in   )     :: R(3,3)       !< input rotation matrix (transpose of DCM; in BD coords)
   type(BD_ParameterType), INTENT(in   )     :: p            !< Parameters
   real(BDKi),             INTENT(  OUT)     :: rr(3)        !< W-M parameters of relative rotation
   INTEGER(IntKi),         INTENT(  OUT)     :: ErrStat      !< Error status of the operation
   CHARACTER(*),           INTENT(  OUT)     :: ErrMsg       !< Error message if ErrStat /= ErrID_None

   real(BDKi)                                :: R_WM(3)      ! W-M parameters of R
   real(BDKi)                                :: R_BD(3,3)    ! input rotation matrix in BDKi precision

   INTEGER(IntKi)                            :: ErrStat2     ! Temporary Error status
   CHARACTER(ErrMsgLen)                      :: ErrMsg2      ! Temporary Error message
   CHARACTER(*), PARAMETER                   :: RoutineName = 'ExtractRelativeRotation'

   ! Initialize ErrStat
   ErrStat = ErrID_None
   ErrMsg  = ""

! Calculate p(rr) = p(Glb_crv)^- (+) p(R)       where (+) is the curve compose
!  which is the same as operation as
!     R(rr) = R(Glb_crv)^T R

   ! note that the u%RootMotion mesh does not contain the initial twist, but p%Glb_crv does not have this twist, either.
   ! The relative rotation will be the same in this case.

   R_BD = R ! possible type conversion (only if BDKi /= R8Ki)

   CALL BD_CrvExtractCrv(R_BD,R_WM, ErrStat2, ErrMsg2)
      CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
      if (ErrStat >= AbortErrLev) return
   CALL BD_CrvCompose(rr,p%Glb_crv,R_WM,FLAG_R1TR2)         ! rr = p%Glb_crv^- composed with R_WM

   ! NOTE: the above calculation is not the inverse of what is in Set_BldMotion_NoAcc.  The reason is that this
   !       routine is only looking at RootMotion.  The Set_BldMotion_NoAcc routine is looking at the blade motion
   !       which at the root differs by the WM values in p%uuN0(4:6,1,1) from the RootMotion mesh.

END SUBROUTINE ExtractRelativeRotation
!-----------------------------------------------------------------------------------------------------------------------------------
FUNCTION BDrot_to_FASTdcm(rr,p) RESULT(dcm)
   real(BDKi),             intent(in) :: rr(3)        !< W-M parameters of relative rotation
   type(BD_ParameterType), intent(in) :: p            !< Parameters
   real(BDKi)                         :: dcm(3,3)     !< input rotation matrix (transpose of DCM; in BD coords)

   REAL(BDKi)                         :: temp_CRV2(3)   ! temp curvature parameters
   real(BDKi)                         :: R(3,3)         ! rotation matrix

! note differences in setting up meshes with Set_BldMotion_NoAcc
!adp: in the case of the meshes in Set_BldMotion_NoAcc, x%q(4:6,:) and m%qp%uuu(4:6,:,:) are not zero.  When this routine is called, they
!     are zero, and the expression in Set_BldMotion_NoAcc simplifies to this expression.

      ! rotate relative W-M rotations to global system?
   CALL BD_CrvCompose(temp_CRV2,p%Glb_crv,rr,FLAG_R1R2) !temp_CRV2 = p%Glb_crv composed with rr

      ! create rotation matrix from W-M parameters:
   CALL BD_CrvMatrixR(temp_CRV2,R) ! returns R (rotation matrix, the transpose of the DCM orientation matrix)

      ! get DCM from rotation matrix:
   dcm = TRANSPOSE(R)

END FUNCTION BDrot_to_FASTdcm
!-----------------------------------------------------------------------------------------------------------------------------------
END MODULE BeamDyn_Subs


program main
  use constants
  use structure, only: getmvir,getrvir
  implicit none

  real(kind=8) :: virial_mass,virial_velocity,virial_radius
  real(kind=8) :: nfw_concentration,virial_overdensity
  integer(kind=4) :: halo_type
  real(kind=8) :: disc_mass_fraction,disc_scale_length_fraction
  real(kind=8) :: disc_mass,disc_radius
  real(kind=8) :: bulge_mass_fraction,bulge_scale_length_fraction
  real(kind=8) :: bulge_mass,bulge_radius
  real(kind=8) :: black_hole_mass_fraction
  real(kind=8) :: black_hole_mass
  real(kind=8) :: velocity_anisotropy_amplitude,velocity_anisotropy_radius
  real(kind=8) :: fmax=1000.,fcut=0.5

  logical :: isbh                         ! Is a black hole present?

  integer(kind=4) :: nr                   ! Dummy index for looping over r,z arrays
  real(kind=8) :: rmax,rcut
  real(kind=8) :: lr,lrmin,lrmax,dlr   ! Log radius

  integer(kind=4) :: i,j,k,l,m,n,nn
  real(kind=8) :: sum

  character(kind=1,len=132) :: outfile,paramfile
  character(kind=1,len=10) :: istring
  logical:: fexist

  integer(kind=4) :: nr_tab,nr_max
  real(kind=8), allocatable :: r_tab(:)
  real(kind=8), allocatable :: sigr_tab(:)
  real(kind=8), allocatable :: sigp_tab(:)
  real(kind=8), allocatable :: sigm_tab(:)

  isbh=.false.

  if(command_argument_count().eq.0) stop 'Usage: compute_sigma.exe <parameter_file>'

  call get_command_argument(1,paramfile)

  inquire(file=paramfile,exist=fexist)

  if(fexist.eqv..false.) stop 'Error: parameter file does not exist'
  call set_parameters(paramfile,virial_mass,virial_velocity,nfw_concentration,&
     & virial_overdensity,halo_type,disc_mass_fraction,disc_scale_length_fraction,&
     & bulge_mass_fraction,bulge_scale_length_fraction,black_hole_mass_fraction,&
     & velocity_anisotropy_amplitude,velocity_anisotropy_radius)
  if(black_hole_mass_fraction.gt.0.0) isbh=.true.

  ! Define dimension and bounds of the mesh

  outfile='projected_props.'
  if(halo_type.eq.0) then
     outfile=trim(outfile)//'hern_halo.'
  else
     outfile=trim(outfile)//'nfw_halo.'
  end if

  if(bulge_mass_fraction.gt.0.0) outfile=trim(outfile)//'hern_bulge.'

  if(velocity_anisotropy_radius.le.0.0) then
     if(velocity_anisotropy_amplitude.lt.0) then
        write(istring,'(f5.2)') velocity_anisotropy_amplitude
     else
        write(istring,'(f4.2)') velocity_anisotropy_amplitude
     endif
  else
     istring='var'
  end if

  outfile=trim(outfile)//trim(istring)//'.txt'

  if(virial_overdensity.eq.0.0) virial_overdensity=deltavir

  if(virial_velocity.gt.0.0) then
     if(virial_mass.ne.getmvir(virial_velocity,virial_overdensity,rhocrit0))&
          & virial_mass=getmvir(virial_velocity,virial_overdensity,rhocrit0)
  end if

  bulge_mass = bulge_mass_fraction
  bulge_radius = bulge_scale_length_fraction
  disc_mass = disc_mass_fraction
  disc_radius = disc_scale_length_fraction
  black_hole_mass = black_hole_mass_fraction

  virial_radius=getrvir(virial_mass,virial_overdensity,rhocrit0)
  nr_tab=500
  lrmin=dlog10(virial_radius)-2.7
  lrmax=dlog10(virial_radius)+1.1
  dlr=(lrmax-lrmin)/real(nr_tab-1)

  allocate(r_tab(nr_tab))       ! Tabulated radius
  allocate(sigr_tab(nr_tab))    ! Tabulated mass
  allocate(sigp_tab(nr_tab))    ! Tabulated mass
  allocate(sigm_tab(nr_tab))    ! Tabulated mass

  lr=lrmin

  do nr=1,nr_tab
     r_tab(nr)=10**lr
     lr=lr+dlr
  end do

  write(*,*) 'Radial limits [kpc] :',r_tab(1),r_tab(nr_tab)
  write(*,*)

  call compute_sigma(nr_tab,r_tab,&
       & virial_mass,virial_radius,nfw_concentration,&
       & virial_overdensity,halo_type,&
       & bulge_mass,bulge_radius,&
       & disc_mass,disc_radius,&
       & black_hole_mass,fmax,fcut,&
       & velocity_anisotropy_amplitude,velocity_anisotropy_radius,&
       & sigr_tab,sigp_tab,sigm_tab)

  open(32,file=outfile,status='unknown')
  write(32,*) virial_mass,virial_radius,virial_velocity
  write(32,*) bulge_mass,bulge_radius
  write(32,*) black_hole_mass
  write(32,*) velocity_anisotropy_amplitude,velocity_anisotropy_radius
  do i=1,nr_tab
     write(32,*) r_tab(i),sigr_tab(i),sigp_tab(i),sigm_tab(i)
  end do
  close(32)

end program main


!* Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
!* See https://llvm.org/LICENSE.txt for license information.
!* SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception

!   KIND with various basic datatypes

program p
 real(4) a
 real(8) b
 complex(4) c
 complex(8) d
 integer(1) e
 integer(2) f
 integer(4) g
 integer(8) h
 integer result(8)
 integer expect(8)
 data expect/4,8,4,8,1,2,4,8/
 result(1) = kind(a)
 result(2) = kind(b)
 result(3) = kind(c)
 result(4) = kind(d)
 result(5) = kind(e)
 result(6) = kind(f)
 result(7) = kind(g)
 result(8) = kind(h)
! print *,result
 call check(result,expect,8)
end program


submodule (stdlib_stats) stdlib_stats_mean

  use, intrinsic:: ieee_arithmetic, only: ieee_value, ieee_quiet_nan
  use stdlib_error, only: error_stop
  use stdlib_optval, only: optval
  implicit none

contains

      module function mean_all_1_rdp_rdp (x, mask) result(res)
        real(dp), intent(in) :: x(:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_1_rdp_rdp
      module function mean_all_2_rdp_rdp (x, mask) result(res)
        real(dp), intent(in) :: x(:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_2_rdp_rdp
      module function mean_all_3_rdp_rdp (x, mask) result(res)
        real(dp), intent(in) :: x(:,:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_3_rdp_rdp
      module function mean_all_4_rdp_rdp (x, mask) result(res)
        real(dp), intent(in) :: x(:,:,:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_4_rdp_rdp
      module function mean_all_1_cdp_cdp (x, mask) result(res)
        complex(dp), intent(in) :: x(:)
        logical, intent(in), optional :: mask
        complex(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_1_cdp_cdp
      module function mean_all_2_cdp_cdp (x, mask) result(res)
        complex(dp), intent(in) :: x(:,:)
        logical, intent(in), optional :: mask
        complex(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_2_cdp_cdp
      module function mean_all_3_cdp_cdp (x, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:)
        logical, intent(in), optional :: mask
        complex(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_3_cdp_cdp
      module function mean_all_4_cdp_cdp (x, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:,:)
        logical, intent(in), optional :: mask
        complex(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(x) / real(size(x, kind = int64), dp)

      end function mean_all_4_cdp_cdp

      module function mean_all_1_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(real(x, dp)) / real(size(x, kind = int64), dp)

      end function mean_all_1_iint32_dp
      module function mean_all_2_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(real(x, dp)) / real(size(x, kind = int64), dp)

      end function mean_all_2_iint32_dp
      module function mean_all_3_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(real(x, dp)) / real(size(x, kind = int64), dp)

      end function mean_all_3_iint32_dp
      module function mean_all_4_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:,:)
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        res = sum(real(x, dp)) / real(size(x, kind = int64), dp)

      end function mean_all_4_iint32_dp

      module function mean_1_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 1) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_1_rdp_rdp
      module function mean_2_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 2) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_2_rdp_rdp
      module function mean_3_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 3) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_3_rdp_rdp
      module function mean_4_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 4) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_4_rdp_rdp
      module function mean_1_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        complex(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 1) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_1_cdp_cdp
      module function mean_2_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 2) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_2_cdp_cdp
      module function mean_3_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 3) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_3_cdp_cdp
      module function mean_4_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 4) then
          res = sum(x, dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_4_cdp_cdp

      module function mean_1_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 1) then
          res = sum(real(x, dp), dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_1_iint32_dp
      module function mean_2_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 2) then
          res = sum(real(x, dp), dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_2_iint32_dp
      module function mean_3_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 3) then
          res = sum(real(x, dp), dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_3_iint32_dp
      module function mean_4_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in), optional :: mask
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (.not.optval(mask, .true.)) then
          res = ieee_value(1._dp, ieee_quiet_nan)
          return
        end if

        if (dim >= 1 .and. dim <= 4) then
          res = sum(real(x, dp), dim) / real(size(x, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_4_iint32_dp

      module function mean_mask_all_1_rdp_rdp(x, mask) result(res)
        real(dp), intent(in) :: x(:)
        logical, intent(in) :: mask(:)
        real(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_1_rdp_rdp
      module function mean_mask_all_2_rdp_rdp(x, mask) result(res)
        real(dp), intent(in) :: x(:,:)
        logical, intent(in) :: mask(:,:)
        real(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_2_rdp_rdp
      module function mean_mask_all_3_rdp_rdp(x, mask) result(res)
        real(dp), intent(in) :: x(:,:,:)
        logical, intent(in) :: mask(:,:,:)
        real(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_3_rdp_rdp
      module function mean_mask_all_4_rdp_rdp(x, mask) result(res)
        real(dp), intent(in) :: x(:,:,:,:)
        logical, intent(in) :: mask(:,:,:,:)
        real(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_4_rdp_rdp
      module function mean_mask_all_1_cdp_cdp(x, mask) result(res)
        complex(dp), intent(in) :: x(:)
        logical, intent(in) :: mask(:)
        complex(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_1_cdp_cdp
      module function mean_mask_all_2_cdp_cdp(x, mask) result(res)
        complex(dp), intent(in) :: x(:,:)
        logical, intent(in) :: mask(:,:)
        complex(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_2_cdp_cdp
      module function mean_mask_all_3_cdp_cdp(x, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:)
        logical, intent(in) :: mask(:,:,:)
        complex(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_3_cdp_cdp
      module function mean_mask_all_4_cdp_cdp(x, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:,:)
        logical, intent(in) :: mask(:,:,:,:)
        complex(dp) :: res

        res = sum(x, mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_4_cdp_cdp

      module function mean_mask_all_1_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:)
        logical, intent(in) :: mask(:)
        real(dp) :: res

        res = sum(real(x, dp), mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_1_iint32_dp
      module function mean_mask_all_2_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:)
        logical, intent(in) :: mask(:,:)
        real(dp) :: res

        res = sum(real(x, dp), mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_2_iint32_dp
      module function mean_mask_all_3_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:)
        logical, intent(in) :: mask(:,:,:)
        real(dp) :: res

        res = sum(real(x, dp), mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_3_iint32_dp
      module function mean_mask_all_4_iint32_dp(x, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:,:)
        logical, intent(in) :: mask(:,:,:,:)
        real(dp) :: res

        res = sum(real(x, dp), mask) / real(count(mask, kind = int64), dp)

      end function mean_mask_all_4_iint32_dp

      module function  mean_mask_1_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:)
        real(dp) :: res

        if (dim >= 1 .and. dim <= 1) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_1_rdp_rdp
      module function  mean_mask_2_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (dim >= 1 .and. dim <= 2) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_2_rdp_rdp
      module function  mean_mask_3_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (dim >= 1 .and. dim <= 3) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_3_rdp_rdp
      module function  mean_mask_4_rdp_rdp(x, dim, mask) result(res)
        real(dp), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (dim >= 1 .and. dim <= 4) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_4_rdp_rdp
      module function  mean_mask_1_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:)
        complex(dp) :: res

        if (dim >= 1 .and. dim <= 1) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_1_cdp_cdp
      module function  mean_mask_2_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:)
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (dim >= 1 .and. dim <= 2) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_2_cdp_cdp
      module function  mean_mask_3_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:)
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (dim >= 1 .and. dim <= 3) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_3_cdp_cdp
      module function  mean_mask_4_cdp_cdp(x, dim, mask) result(res)
        complex(dp), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:,:)
        complex(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (dim >= 1 .and. dim <= 4) then
          res = sum(x, dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_4_cdp_cdp

      module function mean_mask_1_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:)
        real(dp) :: res

        if (dim >= 1 .and. dim <= 1) then
          res = sum(real(x, dp), dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_1_iint32_dp
      module function mean_mask_2_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim))

        if (dim >= 1 .and. dim <= 2) then
          res = sum(real(x, dp), dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_2_iint32_dp
      module function mean_mask_3_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim))

        if (dim >= 1 .and. dim <= 3) then
          res = sum(real(x, dp), dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_3_iint32_dp
      module function mean_mask_4_iint32_dp(x, dim, mask) result(res)
        integer(int32), intent(in) :: x(:,:,:,:)
        integer, intent(in) :: dim
        logical, intent(in) :: mask(:,:,:,:)
        real(dp) :: res(merge(size(x, 1), size(x, 2), mask=1<dim), merge(size(x, 2), size(x, 3), mask=2<dim), merge(size(x, 3),&
            & size(x, 4), mask=3<dim))

        if (dim >= 1 .and. dim <= 4) then
          res = sum(real(x, dp), dim, mask) / real(count(mask, dim), dp)
        else
          call error_stop("ERROR (mean): wrong dimension")
        end if

      end function mean_mask_4_iint32_dp

end submodule


! cable_constants.f90
!
! Source file containing constants for CABLE
!
! Development by Ying-Ping Wang, Eva Kowalczyk, Ray Leuning
! Gab Abramowitz, Martin Dix, Harvey Davies, Mike Raupach,
!
! bugs to bernard.pak@csiro.au
!
! This file contains modules:
!  math_constants
!  physical_constants
!  other_constants
!  photosynthetic_constants
!  spatial_heterogeneity

!=========================================================================
MODULE cable_physical_constants
  USE cable_dimensions, ONLY : i_d, r_1
  IMPLICIT NONE
  PRIVATE i_d, r_1
  REAL(r_1), PARAMETER :: capp   = 1004.64  ! air spec. heat capacity (J/kg/K)
  REAL(r_1), PARAMETER :: dheat  = 21.5E-6  ! molecular diffusivity for heat
  REAL(r_1), PARAMETER :: grav   = 9.80     ! gravity acceleration (m/s2)
  REAL(r_1), PARAMETER :: rgas   = 8.3143   ! universal gas const  (J/mol/K)
  REAL(r_1), PARAMETER :: rmair  = 0.02897  ! molecular wt: dry air (kg/mol)
  REAL(r_1), PARAMETER :: rmh2o  = 0.018016 ! molecular wt: water	(kg/mol)
  REAL(r_1), PARAMETER :: sboltz = 5.67e-8  ! Stefan-Boltz. constant (W/m2/K4)
  REAL(r_1), PARAMETER :: tfrz   = 273.16   ! Temp (K) corresp. to 0 C
  ! Teten coefficients
  REAL(r_1), PARAMETER :: tetena = 6.106  ! ??? refs?
  REAL(r_1), PARAMETER :: tetenb = 17.27
  REAL(r_1), PARAMETER :: tetenc = 237.3
  ! Aerodynamic parameters, diffusivities, water density:
  REAL(r_1), PARAMETER :: vonk   = 0.40 ! von Karman constant
  REAL(r_1), PARAMETER :: a33    = 1.25 ! inertial sublayer sw/us
  REAL(r_1), PARAMETER :: csw    = 0.50 ! canopy sw decay (Weil theory)
  REAL(r_1), PARAMETER :: ctl    = 0.40 ! Wagga wheat (RDD 1992, Challenges)
  REAL(r_1), PARAMETER :: apol   = 0.70 ! Polhausen coeff: single-sided plate
  REAL(r_1), PARAMETER :: prandt = 0.71 ! Prandtl number: visc/diffh
  REAL(r_1), PARAMETER :: schmid = 0.60 ! Schmidt number: visc/diffw
  REAL(r_1), PARAMETER :: diffwc = 1.60 ! diffw/diffc = H2O/CO2 diffusivity
  REAL(r_1), PARAMETER :: rhow   = 1000.0 ! liquid water density   [kg/m3]
  REAL(r_1), PARAMETER :: emleaf = 1.0  ! leaf emissivity
  REAL(r_1), PARAMETER :: emsoil = 1.0  ! soil emissivity
  REAL(r_1), PARAMETER :: cr = 0.3	! element drag coefficient
  REAL(r_1), PARAMETER :: cs = 0.003    ! substrate drag coefficient
  REAL(r_1), PARAMETER :: beta  = cr/cs ! ratio cr/cs
  REAL(r_1), PARAMETER :: ccd   = 15.0  ! constant in d/h equation
  REAL(r_1), PARAMETER :: ccw   = 2.0   ! ccw=(zw-d)/(h-d)
  REAL(r_1), PARAMETER :: usuhm = 0.3   ! (max of us/uh)
  ! Turbulence parameters:
  INTEGER(i_d), PARAMETER :: niter = 4  ! number of iterations for za/L
  REAL(r_1), PARAMETER :: zetmul = 0.4  ! if niter=2, final zeta=zetmul*zetar(2)
  REAL(r_1), PARAMETER :: zeta0  = 0.0  ! initial value of za/L
  REAL(r_1), PARAMETER :: zetneg = -10.0 ! negative limit on za/L when niter>=3
  REAL(r_1), PARAMETER :: zetpos = 0.5  ! positive limit on za/L when niter>=3
  REAL(r_1), PARAMETER :: zdlin  = 1.0  ! height frac of d below which TL linear
  REAL(r_1), PARAMETER :: umin   = 1.0
END MODULE cable_physical_constants


!=============================================================================80
!                         Histogram of Gaussian Grid
!=============================================================================80
!       Discussion:
!Generate a histogram for a distribution function: P(r) (gaussain example)
!Requires a set of points to
!==============================================================================!
!       Modified:                                                              !
!   14 July 2021                                                               !
!       Author:                                                                !
!   Shane Flynn                                                                !
!==============================================================================!
module hist_mod
implicit none
!==============================================================================!
!                            Global Variables                                  !
!==============================================================================!
!NG              ==>Number of points to bin
!d               ==>Coordinate dimensionality (x^i=x_1,x_2,...x_d)
!rrange          ==>(d) domain range
!==============================================================================!
integer,  parameter :: d = 2         !should work for any d, tested for d=2 only
integer :: NG
double precision :: rrange(d)
!==============================================================================!
contains
!==============================================================================!
function P(r)
!==============================================================================!
!Target Distribution Function
!==============================================================================!
!P              ==>evaluate P(r)
!r              ==>(d) ith points coordinate r^i_1,..,r^i_d
!==============================================================================!
implicit none
double precision :: r(d),P
double precision, parameter :: pie = acos(-1d0)
!==============================================================================!
P = (2.*pie)**(-d/2.)*exp(-0.5*sum(r(:)**2))
end function P
!==============================================================================!
subroutine find_range(r)
!==============================================================================!
!Domain Range
!==============================================================================!
!rmin           ==>(d) minimum of the range domain
!rmax           ==>(d) maximum of the range domain
!r              ==>(d,NG) ith points coordinate r^i_1,..,r^i_d
!==============================================================================!
implicit none
integer :: i, k
double precision :: rmin(d), rmax(d), r(d,NG)
!==============================================================================!
rmin(:) = r(:,1)
rmax(:) = r(:,1)
do i=2,NG
   do k=1,d
      if(rmin(k) > r(k,i)) rmin(k) = r(k,i)
      if(rmax(k) < r(k,i)) rmax(k) = r(k,i)
   enddo
enddo
do k=1,d
   write(*,*) k,'  range=',rrange(k)
   rrange(k) = 1/(rmax(k)-rmin(k))
enddo
end subroutine find_range
!==============================================================================!
subroutine histogram(r)
!==============================================================================!
!Generate a histogram for a given set of points
!==============================================================================!
implicit none
integer :: i, j, k
double precision, parameter :: rmin=0d0, rmar=2d0
double precision, parameter :: dr=(rmar-rmin)/50
double precision :: hist(1:100), r(d,NG), dist_aver, dist, dist1, Pr
do i=1,NG
   Pr = P(r(:,i))
   dist1 = 1d10
   do j=1,NG
      if(i.ne.j) then
         dist = sqrt(sum((rrange(:)*(r(:,i)-r(:,j)))**2))
         if(dist < dist1) dist1=dist
      endif
   enddo
   dist_aver = dist_aver+dist1*Pr**(1./d)
enddo
dist_aver = dist_aver/NG
write(*,*) 'dist_aver=', dist_aver
hist=0d0
do i=1,NG
   Pr = P(r(:,i))
   do j=1,NG
      if(i.ne.j) then
         dist=sqrt(sum((rrange(:)*(r(:,i)-r(:,j)))**2))*Pr**(1./d)/dist_aver
         k = (dist-rmin)/dr+1
         if(k>100) k=100
         hist(k) = hist(k)+1
      endif
   enddo
enddo
open (8, file='hist.dat')
do i=1,100
   write(8,*) rmin+dr*(i-0.5),hist(i)
enddo
write(8,*)
end subroutine histogram
!==============================================================================!
end module hist_mod
!==============================================================================!
program hist
use  hist_mod
!==============================================================================!
implicit none
integer :: i
double precision,allocatable :: r(:,:)
!==============================================================================!
!                              Read Input File                                 !
!==============================================================================!
read(*,*) NG
 !==============================================================================!
allocate(r(d,NG))
!==============================================================================!
!                               read in grid
!==============================================================================!
open(21,file='grid.dat')
do i=1,NG
   read(21,*) r(:,i)
enddo
close(21)
!==============================================================================!
!                         Generate QRG (greedy search)
!==============================================================================!
call find_range(r)
write(*,*) 'Compute the pair correlation function'
open (8, file='hist.dat')
call histogram(r)
close(8)
end program hist


module sphericaldfMod

 private
 public :: initdf,maxE,halodf,gasRfromq,rhofromfi,fifromr,totalmass,rmax

 integer,parameter :: ngrid=3000
 real, parameter :: pi=3.1415926535897932385
 real :: r(0:ngrid),dens(0:ngrid),ddensdr(0:ngrid)
 real :: cmass(0:ngrid),fi(0:ngrid),df(0:ngrid)
 real :: ddrhodfi2(0:ngrid)
 real :: extdens(0:ngrid),extmass(0:ngrid),tmass(0:ngrid)

 real, parameter :: F_ZERO_R_LIMIT=0.

 integer, parameter :: ntheta=50

! density parameters
 real :: rs, rvir,rhos
 real :: rcut,cnorm,rmax,rmin

 contains

 real function rho(r)
  real :: r
  if(r.LE.rvir) then
   rho=rhos*rs**3/(rs+r)/(rs**2+r**2)
  else
   rho=rhos*cnorm*rs/(rs+r)*exp(-r**2/rcut**2)
  endif
 end function

 real function drhodr(r)
  real :: r
  if(r.LE.rvir) then
   drhodr=-rho(r)*(1/(rs+r)+2*r/(rs**2+r**2))
  else
   drhodr=-rho(r)*(1/(rs+r)+2*r/rcut**2)
  endif
 end function

 real function ddrhodr2(r)
  real :: r
  if(r.LE.rvir) then
   ddrhodr2=-drhodr(r)*(1/(rs+r)+2*r/(rs**2+r**2))+rho(r)*(1/(rs+r)**2-2/(rs**2+r**2)+4*r**2/(rs**2+r**2)**2)
  else
   ddrhodr2=rho(r)*((1/(rs+r)+2*r/rcut**2)**2+1/(rs+r)**2-2/rcut**2)
  endif
 end function

 function maxE()
  real maxE
  maxE=fi(0)
 end function

 function totalmass()
  real totalmass
  totalmass=cmass(ngrid)
 end function

 function halodf(e) result(f)
  real :: f,e
  f=invertcumul(ngrid+1,df,fi,e)
 end function

 function gasRfromq(q) result(gasr)
  real :: gasr,q,cm
  cm=q*cmass(ngrid)
  gasr=invertcumul(ngrid+1,r,cmass,cm)
 end function

 function fifromr(rad) result (lfi)
  real :: rad,lfi
  lfi=invertcumul(ngrid+1,fi,r,rad)
 end function

 function rhofromfi(rad) result (lrho)
  real :: rad,lrho
  lrho=invertcumul(ngrid+1,dens,fi,rad)
 end function

 function int_theta(densfunc,r) result(s)
 real,external :: densfunc
 real :: dctheta,s,r
 integer :: is
  s=0
  dctheta=1.0/ntheta
  s=s+densfunc(0.,r)+densfunc(r,0.)
  do is=1,ntheta-1,2
    ctheta=is*dctheta
    s=s+4*densfunc(r*sqrt(1-ctheta**2),r*ctheta)
  enddo
  do is=2,ntheta-2,2
    ctheta=is*dctheta
    s=s+2*densfunc(r*sqrt(1-ctheta**2),r*ctheta)
  enddo
  s=s*dctheta/3.
  s=s
 end function

 subroutine initdf(densfunc,rscale,virR,rho0)
  integer :: i,j
  real :: fac,int
  real, optional :: rscale,virR,rho0
  real, external :: densfunc

  if(present(rscale)) then
   rs=rscale
  else
   print*,'Burkert scale length?'
   read*,rs
  endif

  if(present(virR)) then
   rvir=virR
  else
   print*,'Burkert virial radius?'
   read*,rvir
  endif

  if(present(rho0)) then
   rhos=rho0
  else
   print*,'Burkert density constant?'
   read*,rhos
  endif

  rcut=Sqrt(rvir**2+rs**2)
  cnorm=exp(rvir**2/rcut**2)/(1+rvir**2/rs**2)

  rmin=1.e-4*rcut
  rmax=6*rcut

  print*,'rcut,rmax:',rcut,rmax

  fac=exp(log(rmax/rmin)/(1.*(ngrid-1)))
!  fac=(rmax-rmin)/(ngrid-1.)

  r(1)=rmin
  dens(1)=rho(rmin)
  ddensdr(1)=drhodr(rmin)
  do i=2,ngrid-1
   r(i)=r(i-1)*fac
!   r(i)=r(i-1)+fac
   dens(i)=rho(r(i))
   ddensdr(i)=drhodr(r(i))
  enddo
  r(ngrid)=rmax
  dens(ngrid)=rho(rmax)
  ddensdr(ngrid)=drhodr(rmax)
  r(0)=0.
  dens(0)=dens(1) ! assumption !
  ddensdr(0)=0.
!  dens(0)=(4*dens(1)-dens(2))/3.
!  ddensdr(0)=(dens(1)-dens(0))/r(1)

! external density  averaged over theta
  extdens(0)=densfunc(0.,0.)

  do i=1,ngrid
   extdens(i)=int_theta(densfunc,r(i))
  enddo

! note: instead of the theta-averaged external density, use the external
! averaged fi??

  cmass(0.)=0.
  cmass(1.)=4/3.*Pi*r(1)**3*dens(0)
  do i=2,ngrid
   cmass(i)=cmass(i-1)+4*Pi*(r(i)-r(i-1))*(r(i)**2*dens(i)+r(i-1)**2*dens(i-1))/2.
  enddo

  extmass(0.)=0.
  extmass(1.)=4/3.*Pi*r(1)**3*extdens(0)
  do i=2,ngrid
   extmass(i)=extmass(i-1)+4*Pi*(r(i)-r(i-1))*(r(i)**2*extdens(i)+r(i-1)**2*extdens(i-1))/2.
  enddo
  print*,'extmass:',extmass(ngrid)
  print*,'cmass:',cmass(ngrid)

  tmass=cmass+extmass

  fi(ngrid)=0
  do i=ngrid-1,1,-1
   fi(i)=fi(i+1)+(r(i+1)-r(i))*(tmass(i)/r(i)**2+tmass(i+1)/r(i+1)**2)/2.
  enddo
  fi(0)=fi(1)+r(1)*(F_ZERO_R_LIMIT+tmass(1)/r(1)**2)/2.

  do i=1,ngrid
   ddrhodfi2(i)=(r(i)**2/tmass(i))**2* &
    ((2/r(i)-4*Pi*r(i)**2*(dens(i)+extdens(i))/tmass(i))*ddensdr(i)+ddrhodr2(r(i)))
  enddo
  ddrhodfi2(0)=ddrhodfi2(1)

  df(ngrid)=0.
  do i=ngrid-1,1,-1
   int=0
   do j=ngrid-1,i+1,-1
   int=int+(fi(j)-fi(j+1))* &
       (ddrhodfi2(j)/sqrt(fi(i)-fi(j))+ddrhodfi2(j+1)/sqrt(fi(i)-fi(j+1)))/2.
   enddo
   int=int+2./3.*Sqrt((fi(i)-fi(i+1)))*(2*ddrhodfi2(i)+ddrhodfi2(i+1))
   df(i)=1/sqrt(8.)/pi**2*(int-1/sqrt(fi(i))*ddensdr(ngrid)*r(ngrid)**2/cmass(ngrid))
  enddo
  df(0)=df(1)

! print*,'rmax,fi0,cmass:', rmax,fi(0),cmass(ngrid)

 end subroutine

  function invertcumul(n,xlist,ylist,y) result(x)
  integer :: n,bin,up,low
  real :: xlist(n),ylist(n),x,y,u,s

  if(ylist(1).LT.ylist(n)) then
   s=1
  else
   s=-1
  endif

  if(s*y.LE.s*ylist(1)) then
   x=xlist(1)
   return
  endif
  if(s*y.GE.s*ylist(n)) then
   x=xlist(n)
   return
  endif
  up=n
  low=1
  do while((up-low).GT.1)
   bin=(low+up)/2
   if(s*y.LT.s*ylist(bin)) then
    up=bin
   else
    low=bin
   endif
  enddo
  bin=up
  u=(y-ylist(bin-1))/(ylist(bin)-ylist(bin-1))
  x=(1-u)*xlist(bin-1)+u*xlist(bin)
  return
  end function

end module sphericaldfMod

 real function zerofunc(r,z)
  zerofunc=0.
 end function

subroutine initsdf(rscl,rv,rhs,densfunc)
 use sphericaldfMod
 real rscl,rv,rhs
 real, external :: zerofunc
 real, optional,external :: densfunc

 if(present(densfunc)) then
  call initdf(densfunc,rscl,rv,rhs)
 else
  call initdf(zerofunc,rscl,rv,rhs)
 endif

end subroutine

function sdfpsi00()
 use sphericaldfMod
 real sdfpsi00
 sdfpsi00=-maxE()
end function

function sdffi(r)
 use sphericaldfMod
 real sdffi,r
! r>rmax??
 sdffi=-fifromr(r)
end function

function Fhalo3(E)
 use sphericaldfMod
 real Fhalo3,E
 Fhalo3=0
 if(E.LT.0) return
 Fhalo3=halodf(E)
end function

function sdfdenspsi(psi,psi0)
 use sphericaldfMod
  real sdfdenspsi,psi,rpsi
 sdfdenspsi=0
 rpsi=sdfpsi00()+psi-psi0
 if(rpsi.ge.0) return
 sdfdenspsi=rhofromfi(-rpsi)
! print*,rpsi,psi,maxE()
! stop
end function

 function sdens(r,z)
  real r,z,sdens
  real rmdisk,zdisk,outdisk,drtrunc,erfarg,truncfac,con,diskconst,rdisk
  zdisk=0.5
  rdisk=3
  rmdisk=50.
  diskconst=rmdisk/(4.0*3.1415*rdisk**2*zdisk)
  outdisk=12
  drtrunc=.5
  sdens = 0
  if( abs(z/zdisk) .gt. 30.) return
   erfarg=(r-outdisk)/1.4142136/drtrunc
  if (erfarg.gt.4.) return
  if (erfarg.lt.-4) then
   trunfac=1
  else
   trunfac=0.5*erfc(erfarg)
  endif
  if (z.eq.0.) then
   con=1
  else
   con = exp(-abs(z/zdisk))
   con = (2.0*con/(1.0 + con*con))**2
  endif
  sdens = diskconst*exp(-r/rdisk)*con*trunfac
  end function

! program test
!  use sphericaldfMod
!  implicit none
!    integer i,j,np,count
!    real, external ::sdens,zerofunc
!
!  call initdf(sdens,6.,72.,0.04)
!
!  do i=0,ngrid-1
!   print*,r(i),dens(i),ddensdr(i)
!   print*,r(i),extdens(i),extmass(i)
!   print*,r(i),fi(i),df(i)
!  enddo
! end program


      SUBROUTINE MFFTB6(C,FAC)
*
*   PURPOSE:
*       ELEMENTARY GENTLEMAN-SANDE RADIX 3 STEP APPLIED TO A VECTOR-OF
*       2-VECTORS-OF-COMPLEX C[IMS,NM [IVS,NV [IES,NE]]].
*       SEE REF.[1] FOR NOTATIONS.
*       THIS ROUTINE CAN BE USED ONLY BY ROUTINE MFFTDM, WHICH CONTROLS
*       ITS OPERATION THROUGH THE MFFTPA COMMON
*
*   DUMMY ARGUMENTS :
*
*   C   ARRAY BEING FOURIER  TRANSFORMED
*   FAC PHASE FACTORS, PREPARED BY MFFTP; NOT MODIFIED IN OUTPUT
*
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      COMMON /MFFTPA/  IMS,IVS,IES,NM,NV,NE,MX,LX,MLIM,MSTEP,LLIM,LSTEP,
     $ NUSTEP,IVLIM,ILIM,MD2LIM,LD2LIM
      INTEGER NUSTEP
      DOUBLE COMPLEX C(0:NUSTEP-1,0:2),FAC(0:*)
      DOUBLE COMPLEX T0,T1,T2,F1,F2
      REAL *8 SIN60
      PARAMETER ( SIN60 =  8.6602540378443864E-1)

*..  MU > 0
            MUF=LX
            DO 200 MU=MSTEP,MLIM,MSTEP
              F1=CONJG(FAC(MUF))
              F2=CONJG(FAC(MUF*2))
              DO 190 LAM=MU,MU+LLIM,LSTEP
                DO 180 IV=LAM,LAM+IVLIM,IVS
                DO 180 I=IV,IV+ILIM,IES
                  T0=C(I,1)+C(I,2)
                  T2=(C(I,1)-C(I,2))*SIN60
                  T1=C(I,0)-0.5*T0
                  C(I,0)=C(I,0)+T0
                  C(I,1)=(T1-DCMPLX(-DIMAG(T2),DREAL(T2)))*F1
                  C(I,2)=(T1+DCMPLX(-DIMAG(T2),DREAL(T2)))*F2
  180           CONTINUE
  190         CONTINUE
              MUF=MUF+LX
200         CONTINUE
            DO 193 LAM=0,LLIM,LSTEP
              DO 183 IV=LAM,LAM+IVLIM,IVS
              DO 183 I=IV,IV+ILIM,IES
                T0=C(I,1)+C(I,2)
                T2=(C(I,1)-C(I,2))*SIN60
                T1=C(I,0)-0.5*T0
                C(I,0)=C(I,0)+T0
                C(I,1)=(T1-DCMPLX(-DIMAG(T2),DREAL(T2)))
                C(I,2)=(T1+DCMPLX(-DIMAG(T2),DREAL(T2)))
183           CONTINUE
193         CONTINUE
      END