MODULE NWTC_Num

   ! This module contains numeric-type routines with non-system-specific logic and references.

   ! It contains the following routines:

   !     SUBROUTINE AddOrSub2Pi   ( OldAngle, NewAngle )
   !     SUBROUTINE BSortReal     ( RealAry, NumPts )
   !     FUNCTION   CROSS_PRODUCT ( Vector1, Vector2 )
   !     FUNCTION   EqualRealNos  ( ReNum1, ReNum2 )
   !     SUBROUTINE GL_Pts        ( IPt, NPts, Loc, Wt [, ErrStat] )
   !     FUNCTION   IndexCharAry  ( CVal, CAry )
   !     FUNCTION   InterpBin     ( XVal, XAry, YAry, ILo, AryLen )             ! Generic interface for InterpBinComp and InterpBinReal.
   !     FUNCTION   InterpBinComp ( XVal, XAry, YAry, ILo, AryLen )
   !     FUNCTION   InterpBinReal ( XVal, XAry, YAry, ILo, AryLen )
   !     FUNCTION   InterpStp     ( XVal, XAry, YAry, ILo, AryLen )             ! Generic interface for InterpStpComp and InterpStpReal.
   !     FUNCTION   InterpStpComp ( XVal, XAry, YAry, Ind, AryLen )
   !     FUNCTION   InterpStpReal ( XVal, XAry, YAry, Ind, AryLen )
   !     SUBROUTINE LocateStp     ( XVal, XAry, Ind, AryLen )
   !     FUNCTION   Mean          ( Ary, AryLen )                               ! Function to calculate the mean value of a vector array.
   !     SUBROUTINE MPi2Pi        ( Angle )
   !     SUBROUTINE RombergInt    ( f, a, b, R, err, eps, ErrStat )
   !     SUBROUTINE SetConstants
   !     SUBROUTINE SmllRotTrans  ( RotationType, Theta1, Theta2, Theta3, TransMat, ErrTxt )
   !     SUBROUTINE SortUnion     ( Ary1, N1, Ary2, N2, Ary, N )
   !     FUNCTION   StdDevFn      ( Ary, AryLen, Mean )                         ! Function to calculate the standard deviation of a vector array.

   USE                             NWTC_IO

   IMPLICIT NONE

!=======================================================================

      ! Global numeric-related variables.

   REAL(DbKi)                   :: D2R_D                                        ! Factor to convert degrees to radians in double precision.
   REAL(DbKi)                   :: Inf_D                                        ! IEEE value for NaN (not-a-number) in double precision
   REAL(DbKi)                   :: NaN_D                                        ! IEEE value for Inf (infinity) in double precision
   REAL(DbKi)                   :: Pi_D                                         ! Ratio of a circle's circumference to its diameter in double precision.
   REAL(DbKi)                   :: PiBy2_D                                      ! Pi/2 in double precision.
   REAL(DbKi)                   :: R2D_D                                        ! Factor to convert radians to degrees in double precision.
   REAL(DbKi)                   :: RPM2RPS_D                                    ! Factor to convert revolutions per minute to radians per second in double precision.
   REAL(DbKi)                   :: RPS2RPM_D                                    ! Factor to convert radians per second to revolutions per minute in double precision.
   REAL(DbKi)                   :: TwoByPi_D                                    ! 2/Pi in double precision.
   REAL(DbKi)                   :: TwoPi_D                                      ! 2*Pi in double precision.

   REAL(ReKi)                   :: D2R                                          ! Factor to convert degrees to radians.
   REAL(ReKi)                   :: Inf                                          ! IEEE value for NaN (not-a-number)
   REAL(ReKi)                   :: NaN                                          ! IEEE value for Inf (infinity)
   REAL(ReKi)                   :: Pi                                           ! Ratio of a circle's circumference to its diameter.
   REAL(ReKi)                   :: PiBy2                                        ! Pi/2.
   REAL(ReKi)                   :: R2D                                          ! Factor to convert radians to degrees.
   REAL(ReKi)                   :: RPM2RPS                                      ! Factor to convert revolutions per minute to radians per second.
   REAL(ReKi)                   :: RPS2RPM                                      ! Factor to convert radians per second to revolutions per minute.
   REAL(ReKi)                   :: TwoByPi                                      ! 2/Pi.
   REAL(ReKi)                   :: TwoPi                                        ! 2*Pi.

   INTEGER, ALLOCATABLE         :: IntIndx  (:,:)                               ! The array of indices holding that last index used for interpolation in IntBlade().

!=======================================================================

      ! Create interface for a generic InterpBin that actually uses specific routines.

   INTERFACE InterpBin
      MODULE PROCEDURE InterpBinComp
      MODULE PROCEDURE InterpBinReal
   END INTERFACE

      ! Create interface for a generic InterpStp that actually uses specific routines.

   INTERFACE InterpStp
      MODULE PROCEDURE InterpStpComp
      MODULE PROCEDURE InterpStpReal
   END INTERFACE

CONTAINS

!=======================================================================
   SUBROUTINE AddOrSub2Pi ( OldAngle, NewAngle )

      ! This routine is used to convert NewAngle to an angle within 2*Pi of
      !   OldAngle by adding or subtracting 2*Pi accordingly; it then sets
      !   OldAngle equal to NewAngle.  This routine is useful for converting
      !   angles returned from a call to the ATAN2() FUNCTION into angles that may
      !   exceed the -Pi to Pi limit of ATAN2().  For example, if the nacelle yaw
      !   angle was 179deg in the previous time step and the yaw angle increased
      !   by 2deg in the new time step, we want the new yaw angle returned from a
      !   call to the ATAN2() FUNCTION to be 181deg instead of -179deg.  This
      !   routine assumes that the angle change between calls is not more than
      !   2*Pi in absolute value.  OldAngle should be SAVEd in the calling
      !   routine.

      ! Argument declarations:

   REAL(ReKi), INTENT(INOUT)    :: OldAngle                                     ! Angle from which NewAngle will be converted to within 2*Pi of, rad.
   REAL(ReKi), INTENT(INOUT)    :: NewAngle                                     ! Angle to be converted to within 2*Pi of OldAngle, rad.

      ! Local declarations:

   REAL(ReKi)                   :: DelAngle                                     ! The difference between OldAngle and NewAngle, rad.

      ! Add or subtract 2*Pi in order to convert NewAngle two within 2*Pi of
      !   OldAngle:

   DelAngle = OldAngle - NewAngle

   DO WHILE ( ABS( DelAngle ) >= TwoPi )

      NewAngle = NewAngle + SIGN( TwoPi, DelAngle )
      DelAngle = OldAngle - NewAngle

   END DO

      ! Set OldAngle to equal NewAngle:

   OldAngle = NewAngle

   RETURN
   END SUBROUTINE AddOrSub2Pi
!=======================================================================
   SUBROUTINE BSortReal ( RealAry, NumPts )

      ! This routine sorts a list of real numbers.  It uses the buble sort algorithm,
      ! which is only suitable for short lists.

      ! Argument declarations:

   INTEGER, INTENT(IN)          :: NumPts                                       ! The length of the list to be sorted.

   REAL(ReKi), INTENT(INOUT)    :: RealAry(NumPts)                              ! The list of real numbers to be sorted.

      ! Local declarations:

   REAL(ReKi)                   :: Temp                                         ! Temporary variable to hold the current element.

   INTEGER                      :: I                                            ! Index into the array.

   LOGICAL                      :: Change                                       ! Flag to indicate if a change of order was made.

      ! Sort the list

   Change = .TRUE.

   DO WHILE ( Change )

      Change = .FALSE.

      DO I=2,NumPts
         IF ( RealAry(I) < RealAry(I-1) )  THEN
            Temp           = RealAry(I)
            RealAry(I)   = RealAry(I-1)
            RealAry(I-1) = Temp
            Change         = .TRUE.
         END IF
      END DO ! I

   END DO ! WHILE

   RETURN
   END SUBROUTINE BSortReal ! ( RealAry, NumPts )
!=======================================================================
   FUNCTION Cross_Product(Vector1, Vector2)

      ! This function computes the cross product of two 3-element arrays:
      ! Cross_Product = Vector1 X Vector2 (resulting in a vector)

      ! Argument declarations.

   REAL(ReKi), INTENT(IN )         :: Vector1       (3)
   REAL(ReKi), INTENT(IN )         :: Vector2       (3)

      ! Function definition
   REAL(ReKi)                      :: Cross_Product (3)        ! = Vector1 X Vector2 (resulting in a vector)

   Cross_Product(1) = Vector1(2)*Vector2(3) - Vector1(3)*Vector2(2)
   Cross_Product(2) = Vector1(3)*Vector2(1) - Vector1(1)*Vector2(3)
   Cross_Product(3) = Vector1(1)*Vector2(2) - Vector1(2)*Vector2(1)

   RETURN
   END FUNCTION Cross_Product
!=======================================================================
!   SUBROUTINE GetPermMat ( InpMat, PMat, ErrStat )
!
!      ! This subroutine computes a permutation matrix, PMat, for a given
!      ! input matrix, InpMat. It assumes that InpMat is of full rank
!      ! and for now, the matrices are 3 x 3.
!
!      ! passed variables
!
!   REAL(ReKi), INTENT(IN )         :: InpMat       (3,3)
!   REAL(ReKi), INTENT(OUT )        :: PMat         (3,3) !this could be integer, but we'll leave it real now
!   INTEGER,    INTENT(OUT )        :: ErrStat            ! a non-zero value indicates an error in the permutation matrix algorithm
!
!      ! local variables
!   INTEGER                         :: iCol               ! loop counter
!   INTEGER                         :: iRow               ! loop counter
!   INTEGER                         :: MaxCol             ! holds index of maximum value in a column
!
!   LOGICAL                         :: ChkCols     (3)    ! a check to make sure we have only one non-zero element per column
!
!      ! initialize some variables
!   PMat    = 0.0
!   ChkCols = .FALSE.
!   ErrStat = 0
!
!      ! find the pivots
!   DO iRow = 1,3
!
!      MaxCol = 1        ! initialize max index
!      DO iCol = 2,3
!         IF ( ABS(InpMat(iRow,iCol)) > ABS(InpMat(iRow,MaxCol)) ) &
!            MaxCol = iCol
!      END DO ! iCol
!
!      IF ( ChkCols(MaxCol) ) THEN   ! we can have only 1 non-zero entry per row and column, but we've just violated that!
!         CALL ProgAbort( ' Error in GetPermMat(): InpMat is not full rank.', TrapErrors = .TRUE. )
!         ErrStat = 1
!      END IF
!
!      PMat(MaxCol, iRow) = SIGN( 1.0_ReKi, InpMat(iRow,MaxCol) )  ! technically a permutation matrix would only have +1.0 (not -1.0)
!      ChkCols(MaxCol)    = .TRUE.
!
!   END DO ! iRow
!
!   RETURN
!   END SUBROUTINE GetPermMat ! ( InpMat, PMat, ErrStat )

!=======================================================================
   FUNCTION EqualRealNos ( ReNum1, ReNum2 )

      ! This function compares 2 real numbers and determines if they
      ! are "almost" equal, i.e. within some relative tolerance

      ! passed variables

   REAL(ReKi), INTENT(IN )         :: ReNum1                      ! the first  real number to compare
   REAL(ReKi), INTENT(IN )         :: ReNum2                      ! the second real number to compare

   LOGICAL                         :: EqualRealNos                ! the function definition -- returns .true. if the numbers are almost equal

      ! local variables
   REAL(ReKi), PARAMETER           :: Eps = EPSILON(ReNum1)       ! machine precision
   REAL(ReKi), PARAMETER           :: Tol = 100.*Eps / 2.         ! absolute tolerance (ignore the last 2 significant digits)

   REAL(ReKi)                      :: Fraction

      ! make sure we're never trying to get more precision than Tol

   Fraction = MAX( ABS(ReNum1+ReNum2), 1.0_ReKi )

      ! determine if ReNum1 and ReNum2 are approximately equal

   IF ( ABS(ReNum1 - ReNum2) <= Fraction*Tol ) THEN  ! the relative error, (comparison suggestion from Lahey.com)
      EqualRealNos = .TRUE.
   ELSE
      EqualRealNos = .FALSE.
   ENDIF

   END FUNCTION EqualRealNos
!=======================================================================
   FUNCTION GetSmllRotAngs ( DCMat, ErrStat )

      ! This subroutine computes the angles that make up the input
      ! direction cosine matrix, DCMat

      ! passed variables

   REAL(ReKi), INTENT(IN )         :: DCMat          (3,3)
   INTEGER,    INTENT(OUT )        :: ErrStat               ! a non-zero value indicates an error in the permutation matrix algorithm

   REAL(ReKi)                      :: GetSmllRotAngs ( 3 )

      ! local variables
   REAL(ReKi)                      :: denom                 ! the denominator of the resulting matrix
   REAL(ReKi), PARAMETER           :: LrgAngle  = 0.4       ! Threshold for when a small angle becomes large (about 23deg).  This comes from: COS(SmllAngle) ~ 1/SQRT( 1 + SmllAngle^2 ) and SIN(SmllAngle) ~ SmllAngle/SQRT( 1 + SmllAngle^2 ) results in ~5% error when SmllAngle = 0.4rad.
   REAL(ReKi), PARAMETER           :: TOL = EPSILON(TOL)    ! tolerance for division by zero

      ! initialize output angles (just in case there is an error that prevents them from getting set)

   GetSmllRotAngs = 0.0
   ErrStat        = 0

      ! calculate the small angles
   GetSmllRotAngs(1) = DCMat(2,3) - DCMat(3,2)
   GetSmllRotAngs(2) = DCMat(3,1) - DCMat(1,3)
   GetSmllRotAngs(3) = DCMat(1,2) - DCMat(2,1)

   denom             = DCMat(1,1) + DCMat(2,2) + DCMat(3,3) - 1

   IF ( ABS(denom) > TOL ) THEN
      GetSmllRotAngs = GetSmllRotAngs / denom

               ! check that the angles are, in fact, small
      IF ( ANY( ABS(GetSmllRotAngs) > LrgAngle ) ) THEN
         CALL ProgWarn( ' Angles in GetSmllRotAngs() are larger than '//TRIM(Num2LStr(LrgAngle))//' radians.' )
         ErrStat = 1
      END IF

   ELSE
            ! check that the angles are, in fact, small (denom should be close to 2 if angles are small)
      CALL ProgAbort( ' Denominator is zero in GetSmllRotAngs().', TrapErrors = .TRUE. )
      ErrStat = -1

   END IF

   END FUNCTION GetSmllRotAngs ! ( DCMat, PMat, ErrStat )
!=======================================================================
   SUBROUTINE GL_Pts ( IPt, NPts, Loc, Wt, ErrStat )

      ! This funtion returns the non-dimensional (-1:+1) location of the given Gauss-Legendre Quadrature point and its weight.
      ! The values came from Carnahan, Brice; Luther, H.A.; Wilkes, James O.  (1969)  "Applied Numerical Methods."

      ! Argument declarations.

   REAL(ReKi)                     :: Loc                                         ! The location of the specified point.
   REAL(ReKi)                     :: Wt                                          ! The weight for the specified point.

   INTEGER, INTENT(OUT), OPTIONAL :: ErrStat                                     ! Error status; if present, program does not abort on error
   INTEGER, INTENT(INOUT)         :: IPt                                         ! The quadrature point in question.
   INTEGER, INTENT(INOUT)         :: NPts                                        ! The number of points used in the quadrature.

   IF ( PRESENT(ErrStat) ) ErrStat = 0

      ! Check to see if the number of points and the specific point are valid values.

   IF ( ( NPts < 1 ) .OR. ( NPts > 6 ) )  THEN
      CALL ProgAbort ( ' In function GL_Loc, the number of points used for Gauss-Legendre Quadrature must be between 1 and 6' &
                    //' (inclusive).  Instead, it is "'//TRIM( Int2LStr( NPts ) )//'".', PRESENT(ErrStat) )
      IF ( PRESENT(ErrStat) ) THEN ! this should always be true here
         ErrStat = 1
         RETURN
      END IF
   END IF

   IF ( ( Ipt < 1 ) .OR. ( Ipt > NPts ) )  THEN
      CALL ProgAbort ( ' In function GL_Loc, the point being used for Gauss-Legendre Quadrature must be between 1 and ' &
                   //TRIM( Int2LStr( NPts ) )//' (inclusive).  Instead, it is "'//TRIM( Int2LStr( Ipt ) )//'".', PRESENT(ErrStat) )
      IF ( PRESENT(ErrStat) ) THEN
         ErrStat = 1
         RETURN
      END IF
   END IF

      ! Set the location and weight of the point.

   SELECT CASE ( NPts )
      CASE ( 1 )                         ! Case 1 is really just rectangular integration.
         Loc = 0.0
         Wt  = 2.0
      CASE ( 2 )
         SELECT CASE ( Ipt )
            CASE ( 1 )
               Loc = -0.5773503
               Wt  =  1.0
            CASE ( 2 )
               Loc = 0.5773503
               Wt  = 1.0
          END SELECT ! Ipt
      CASE ( 3 )
         SELECT CASE ( Ipt )
            CASE ( 1 )
               Loc = -0.7745967
               Wt  =  0.5555556
            CASE ( 2 )
               Loc =  0.0
               Wt  =  0.8888889
            CASE ( 3 )
               Loc =  0.7745967
               Wt  =  0.5555556
         END SELECT ! Ipt
      CASE ( 4 )
         SELECT CASE ( Ipt )
            CASE ( 1 )
               Loc = -0.8611363
               Wt  =  0.3478548
            CASE ( 2 )
               Loc = -0.3399810
               Wt  =  0.6521452
            CASE ( 3 )
               Loc =  0.3399810
               Wt  =  0.6521452
            CASE ( 4 )
               Loc =  0.8611363
               Wt  =  0.3478548
         END SELECT ! Ipt
      CASE ( 5 )
         SELECT CASE ( Ipt )
            CASE ( 1 )
               Loc = -0.9061798
               Wt  =  0.2369269
            CASE ( 2 )
               Loc = -0.5384693
               Wt  =  0.4786287
            CASE ( 3 )
               Loc =  0.0
               Wt  =  0.5688889
            CASE ( 4 )
               Loc =  0.5384693
               Wt  =  0.4786287
            CASE ( 5 )
               Loc =  0.9061798
               Wt  =  0.2369269
         END SELECT ! Ipt
      CASE ( 6 )
         SELECT CASE ( Ipt )
            CASE ( 1 )
               Loc = -0.9324695
               Wt  =  0.1713245
            CASE ( 2 )
               Loc = -0.6612094
               Wt  =  0.3607616
            CASE ( 3 )
               Loc = -0.2386192
               Wt  =  0.4679139
            CASE ( 4 )
               Loc =  0.2386192
               Wt  =  0.4679139
            CASE ( 5 )
               Loc =  0.6612094
               Wt  =  0.3607616
            CASE ( 6 )
               Loc =  0.9324695
               Wt  =  0.1713245
         END SELECT ! Ipt
   END SELECT ! Npts

   RETURN
   END SUBROUTINE GL_Pts ! ( IPt, NPts, Loc, Wt [, ErrStat] )
!=======================================================================
   FUNCTION IndexCharAry( CVal, CAry )

      ! This funtion returns an integer index such that CAry(IndexCharAry) = CVal. If
      ! no element in the array matches CVal, the value -1 is returned.  The routine
      ! performs a binary search on the input array to determine if CVal is an
      ! element of the array; thus, CAry must be sorted and stored in increasing
      ! alphebetical (ASCII) order. The routine does not check that the array is
      ! sorted.  The routine assumes that CVal is type CHARACTER and CAry
      ! is an array of CHARACTERS.

      ! Function declaration.

   INTEGER                      :: IndexCharAry                                   ! This function

      ! Argument declarations.

   CHARACTER(*), INTENT(IN)     :: CVal                                           ! String to find.
   CHARACTER(*), INTENT(IN)     :: CAry(:)                                        ! Array of strings to search.

      ! Local declarations.

   INTEGER                      :: IHi                                             ! The high index into the arrays.
   INTEGER                      :: IMid                                            ! The mid-point index between IHi and ILo.
   INTEGER                      :: ILo

      ! Initialize some variables

   ILo = 1
   IHi = SIZE(CAry)

   IF (     CVal == CAry(ILo) ) THEN
      IndexCharAry = ILo
   ELSEIF ( CVal == CAry(IHi) ) THEN
      IndexCharAry = IHi
   ELSE
      IndexCharAry = -1

         ! Let's search!

      DO WHILE ( IHi-ILo > 1 )

         IMid = ( IHi + ILo )/2

         IF( CVal > CAry(IMid) ) THEN
            ILo = IMid
         ELSEIF (CVal < CAry(IMid) ) THEN
            IHi = IMid
         ELSE !Found it
            IndexCharAry = IMid
            EXIT
         END IF

      END DO

   END IF

   RETURN

   END FUNCTION IndexCharAry
!=======================================================================
   FUNCTION InterpBinComp( XVal, XAry, YAry, ILo, AryLen )

      ! This funtion returns a y-value that corresponds to an input x-value by interpolating into the arrays.
      ! It uses a binary interpolation scheme that takes about log(AryLen)/log(2) steps to converge.
      ! It returns the first or last YAry() value if XVal is outside the limits of XAry().
      ! This routine assumes YAry is COMPLEX.

      ! Function declaration.

   COMPLEX(ReKi)                :: InterpBinComp                                   ! This function.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the arrays.
   INTEGER, INTENT(INOUT)       :: ILo                                             ! The low index into the arrays.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.

   COMPLEX(ReKi), INTENT(IN)    :: YAry    (AryLen)                                ! Array of Y values to be interpolated.

      ! Local declarations.

   INTEGER                      :: IHi                                             ! The high index into the arrays.
   INTEGER                      :: IMid                                            ! The mid-point index between IHi and ILo.

      ! Let's check the limits first.

   IF ( XVal <= XAry(1) )  THEN
      InterpBinComp = YAry(1)
      ILo           = 1
      RETURN
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      InterpBinComp = YAry(AryLen)
      ILo           = AryLen - 1
      RETURN
   END IF

      ! Let's interpolate!

   ILo  = 1
   IHi  = AryLen

   DO WHILE ( IHi-ILo > 1 )

      IMid = ( IHi + ILo )/2

      IF ( XVal >= XAry(IMid) ) THEN
         ILo = IMid
      ELSE
         IHi = IMid
      END IF

   END DO

   InterpBinComp = YAry(ILo) + ( YAry(IHi) - YAry(ILo) )*( XVal - XAry(ILo) )/( XAry(IHi) - XAry(ILo) )

   RETURN
   END FUNCTION InterpBinComp ! ( XVal, XAry, YAry, ILo, AryLen )
!=======================================================================
   FUNCTION InterpBinReal( XVal, XAry, YAry, ILo, AryLen )

      ! This funtion returns a y-value that corresponds to an input x-value by interpolating into the arrays.
      ! It uses a binary interpolation scheme that takes about log(AryLen)/log(2) steps to converge.
      ! It returns the first or last YAry() value if XVal is outside the limits of XAry().
      ! This routine assumes YAry is REAL.

      ! Function declaration.

   REAL(ReKi)                   :: InterpBinReal                                   ! This function.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the arrays.
   INTEGER, INTENT(INOUT)       :: ILo                                             ! The low index into the arrays.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.
   REAL(ReKi), INTENT(IN)       :: YAry    (AryLen)                                ! Array of Y values to be interpolated.

      ! Local declarations.

   INTEGER                      :: IHi                                             ! The high index into the arrays.
   INTEGER                      :: IMid                                            ! The mid-point index between IHi and ILo.

      ! Let's check the limits first.

   IF ( XVal <= XAry(1) )  THEN
      InterpBinReal = YAry(1)
      ILo           = 1
      RETURN
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      InterpBinReal = YAry(AryLen)
      ILo           = AryLen - 1
      RETURN
   END IF

      ! Let's interpolate!

   ILo  = 1
   IHi  = AryLen

   DO WHILE ( IHi-ILo > 1 )

      IMid = ( IHi + ILo )/2

      IF ( XVal >= XAry(IMid) ) THEN
         ILo = IMid
      ELSE
         IHi = IMid
      END IF

   END DO

   InterpBinReal = YAry(ILo) + ( YAry(IHi) - YAry(ILo) )*( XVal - XAry(ILo) )/( XAry(IHi) - XAry(ILo) )

   RETURN
   END FUNCTION InterpBinReal ! ( XVal, XAry, YAry, ILo, AryLen )
!=======================================================================
   FUNCTION InterpStpComp( XVal, XAry, YAry, Ind, AryLen )

      ! This funtion returns a y-value that corresponds to an input x-value by interpolating into the arrays.
      ! It uses the passed index as the starting point and does a stepwise interpolation from there.  This is
      ! especially useful when the calling routines save the value from the last time this routine was called
      ! for a given case where XVal does not change much from call to call.  When there is no correlation
      ! from one interpolation to another, InterpBin() may be a better choice.
      ! It returns the first or last YAry() value if XVal is outside the limits of XAry().
      ! This routine assumes YAry is COMPLEX.

      ! Function declaration.

   COMPLEX(ReKi)                :: InterpStpComp                                   ! This function.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the arrays.
   INTEGER, INTENT(INOUT)       :: Ind                                             ! Initial and final index into the arrays.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.

   COMPLEX(ReKi), INTENT(IN)    :: YAry    (AryLen)                                ! Array of Y values to be interpolated.

      ! Let's check the limits first.

   IF ( XVal <= XAry(1) )  THEN
      InterpStpComp = YAry(1)
      Ind           = 1
      RETURN
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      InterpStpComp = YAry(AryLen)
      Ind           = AryLen - 1
      RETURN
   END IF

     ! Let's interpolate!

   Ind = MAX( MIN( Ind, AryLen-1 ), 1 )

   DO

      IF ( XVal < XAry(Ind) )  THEN

         Ind = Ind - 1

      ELSE IF ( XVal >= XAry(Ind+1) )  THEN

         Ind = Ind + 1

      ELSE

         InterpStpComp = ( YAry(Ind+1) - YAry(Ind) )*( XVal - XAry(Ind) )/( XAry(Ind+1) - XAry(Ind) ) + YAry(Ind)
         RETURN

      END IF

   END DO

   RETURN
   END FUNCTION InterpStpComp ! ( XVal, XAry, YAry, Ind, AryLen )
!=======================================================================
   FUNCTION InterpStpReal( XVal, XAry, YAry, Ind, AryLen )

      ! This funtion returns a y-value that corresponds to an input x-value by interpolating into the arrays.
      ! It uses the passed index as the starting point and does a stepwise interpolation from there.  This is
      ! especially useful when the calling routines save the value from the last time this routine was called
      ! for a given case where XVal does not change much from call to call.  When there is no correlation
      ! from one interpolation to another, InterpBin() may be a better choice.
      ! It returns the first or last YAry() value if XVal is outside the limits of XAry().
      ! This routine assumes YAry is REAL.

      ! Function declaration.

   REAL(ReKi)                   :: InterpStpReal                                   ! This function.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the arrays.
   INTEGER, INTENT(INOUT)       :: Ind                                             ! Initial and final index into the arrays.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.
   REAL(ReKi), INTENT(IN)       :: YAry    (AryLen)                                ! Array of Y values to be interpolated.

      ! Let's check the limits first.

   IF ( XVal <= XAry(1) )  THEN
      InterpStpReal = YAry(1)
      Ind           = 1
      RETURN
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      InterpStpReal = YAry(AryLen)
      Ind           = AryLen - 1
      RETURN
   END IF

     ! Let's interpolate!

   Ind = MAX( MIN( Ind, AryLen-1 ), 1 )

   DO

      IF ( XVal < XAry(Ind) )  THEN

         Ind = Ind - 1

      ELSE IF ( XVal >= XAry(Ind+1) )  THEN

         Ind = Ind + 1

      ELSE

         InterpStpReal = ( YAry(Ind+1) - YAry(Ind) )*( XVal - XAry(Ind) )/( XAry(Ind+1) - XAry(Ind) ) + YAry(Ind)
         RETURN

      END IF

   END DO

   RETURN
   END FUNCTION InterpStpReal ! ( XVal, XAry, YAry, Ind, AryLen )
!=======================================================================
   SUBROUTINE LocateBin( XVal, XAry, Ind, AryLen )

      ! This subroutine finds the lower-bound index of an input x-value located in an array.
      ! On return, Ind has a value such that
      !           XAry(Ind) <= XVal < XAry(Ind+1), with the exceptions that
      !             Ind = 0 when XVal < XAry(1), and
      !          Ind = AryLen when XAry(AryLen) <= XVal.
      !
      ! It uses a binary interpolation scheme that takes about log(AryLen)/log(2) steps to converge.
      ! If the index doesn't change much between calls, LocateStp() may be a better option.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the array.
   INTEGER, INTENT(OUT)         :: Ind                                             ! Final (low) index into the array.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.

      ! Local declarations.

   INTEGER                      :: IHi                                             ! The high index into the arrays.
   INTEGER                      :: IMid                                            ! The mid-point index between IHi and Ind.

      ! Let's check the limits first.

   IF ( XVal < XAry(1) )  THEN
      Ind = 0
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      Ind = AryLen
   ELSE
         ! Let's interpolate!

      Ind  = 1
      IHi  = AryLen

      DO WHILE ( IHi-Ind > 1 )

         IMid = ( IHi + Ind )/2

         IF ( XVal >= XAry(IMid) ) THEN
            Ind = IMid
         ELSE
            IHi = IMid
         END IF

      END DO

   END IF

   RETURN
   END SUBROUTINE LocateBin
!=======================================================================
   SUBROUTINE LocateStp( XVal, XAry, Ind, AryLen )

      ! This subroutine finds the lower-bound index of an input x-value located in an array.
      ! On return, Ind has a value such that
      !           XAry(Ind) <= XVal < XAry(Ind+1), with the exceptions that
      !             Ind = 0 when XVal < XAry(1), and
      !          Ind = AryLen when XAry(AryLen) <= XVal.
      !
      ! It uses the passed index as the starting point and does a stepwise search from there.  This is
      ! especially useful when the calling routines save the value from the last time this routine was called
      ! for a given case where XVal does not change much from call to call.  When there is no correlation
      ! from one interpolation to another, a binary search may be a better choice.

      ! Argument declarations.

   INTEGER, INTENT(IN)          :: AryLen                                          ! Length of the array.
   INTEGER, INTENT(INOUT)       :: Ind                                             ! Initial and final index into the array.

   REAL(ReKi), INTENT(IN)       :: XAry    (AryLen)                                ! Array of X values to be interpolated.
   REAL(ReKi), INTENT(IN)       :: XVal                                            ! X value to be interpolated.

      ! Let's check the limits first.

   IF ( XVal < XAry(1) )  THEN
      Ind = 0
   ELSE IF ( XVal >= XAry(AryLen) )  THEN
      Ind = AryLen
   ELSE

      Ind = MAX( MIN( Ind, AryLen-1 ), 1 )

      DO

         IF ( XVal < XAry(Ind) )  THEN

            Ind = Ind - 1

         ELSE IF ( XVal >= XAry(Ind+1) )  THEN

            Ind = Ind + 1

         ELSE

            RETURN

         END IF

      END DO

   END IF

   RETURN

   END SUBROUTINE LocateStp
!=======================================================================
   FUNCTION Mean ( Ary, AryLen )

      ! This routine calculates the mean value of an array.

      ! Function declaration.

   REAL(ReKi)                   :: Mean                                         ! This function.

      ! Argument declarations:

   INTEGER, INTENT(IN)          :: AryLen                                       ! Length of the array.

   REAL(ReKi), INTENT(IN)       :: Ary  (AryLen)                                ! Input array.

      ! Local declarations.

   INTEGER                      :: I                                            ! The index into the array.

   Mean = 0.0

   DO I=1,AryLen
      Mean = Mean + Ary(I)
   END DO ! I

   Mean = Mean/AryLen

   RETURN
   END FUNCTION Mean ! ( Ary, AryLen )
!=======================================================================
   SUBROUTINE MPi2Pi ( Angle )

      ! This routine ensures that Angle lies between -pi and pi.

      ! Argument declarations:

   REAL(ReKi), INTENT(INOUT)    :: Angle

      ! Get the angle between 0 and 2Pi.

   Angle = MODULO( Angle, TwoPi )

      ! Get the angle between -Pi and Pi.

   IF ( Angle > Pi )  THEN
      Angle = Angle - TwoPi
   END IF

   RETURN
   END SUBROUTINE MPi2Pi
!=======================================================================
   SUBROUTINE RombergInt(f, a, b, R, err, eps, ErrStat)

      ! This routine is used to integrate funciton f over the interval [a, b]. This routine
      ! is useful for sufficiently smooth (e.g., analytic) integrands, integrated over
      ! intervals which contain no singularities, and where the endpoints are also nonsingular.
      !
      ! f is an external function. For example f(x) = 1 + x.
      !
      !   FUNCTION f(x)
      !      USE PRECISION
      !      IMPLICIT NONE
      !
      !      REAL(ReKi) f
      !      REAL(ReKi) x
      !
      !      f = 1 + x
      !
      !      RETURN
      !   END FUNCTION f

   IMPLICIT NONE

      ! Argument declarations:

   REAL(ReKi), EXTERNAL              :: f               ! Integrand function name
   REAL(ReKi), INTENT(IN)            :: a               ! Lower integration limit
   REAL(ReKi), INTENT(IN)            :: b               ! Upper integration limit
   REAL(ReKi), INTENT(IN)            :: eps             ! Absolute error bound
   REAL(ReKi), INTENT(OUT)           :: R               ! The result of integration
   REAL(ReKi), INTENT(OUT)           :: err             ! Actual absolute error
   INTEGER, INTENT(OUT), OPTIONAL    :: ErrStat         ! Error status; if present, program does not abort on error

      ! Local declarations:

   INTEGER                           :: m, i, j, k
   INTEGER, PARAMETER                :: mmax = 50       ! Maximum iteration number for m
   INTEGER, PARAMETER                :: imax = 50       ! Maximum iteration number for i

   REAL(ReKi), ALLOCATABLE           :: T(:,:)
   REAL(ReKi)                        :: h               ! Step length
   REAL(ReKi)                        :: sumf

      ! Initialize T
   ALLOCATE( T( mmax, imax ) )
   T = 0

   T(1, 1) = 0.5*(b - a)*( f(a) + f(b) )

   k = 2
   DO m = 1, mmax-2
      h = (b-a)*(0.5)**m

      sumf = 0
      DO i = 1, 2**(m-1)
         sumf = sumf + f(a + (2*i-1)*h)
         k = k + 1
      END DO

      T( m+1, 1) = 0.5*T( m, 1 )+ h * sumf

      DO j = 1, m
         T(m-j+1, j+1) = ( 4.0**j * T(m-j+2, j) - T(m-j+1, j) )/(4.0**j - 1.0)

            ! absolute error
         err = ABS( T(m-j+1, j+1) - T( m-j+2, j ) )

            ! set k >=9 to prevent early terminations
         IF( (err .LT. eps) .and. (k >= 9) ) THEN

               ! return the intergration result if the conditions are met
            R = T(m-j+1, j+1)

            IF( ALLOCATED(T) ) DEALLOCATE(T)

            RETURN
         END IF

      END DO

   END DO

   err = ABS( T(m-j+1, j+1) - T( m-j+2, j ) )
   R = T(m-j+1, j+1)

   IF( ALLOCATED(T) ) DEALLOCATE(T)

      ! Return error message if the maximum iteration number is reached.
   CALL ProgAbort ( ' In subroutine RombergInt, the iteration reaches the maximum number. The integration did NOT converge! ', &
                    PRESENT(ErrStat) )
   IF ( PRESENT(ErrStat) ) THEN
      ErrStat = 1
      RETURN
   END IF

   RETURN
END SUBROUTINE RombergInt
!=======================================================================
   SUBROUTINE SetConstants( )

      ! This routine computes numeric constants stored in the NWTC Library

!   USE, INTRINSIC :: ieee_arithmetic  !use this for compilers that have implemented

      ! local variables for getting values of NaN and Inf (not necessary when using ieee_arithmetic)
   REAL(DbKi)                          :: Neg_D          ! a negative real(DbKi) number
   REAL(ReKi)                          :: Neg            ! a negative real(ReKi) number

      ! Constants based upon Pi:

   Pi_D      = ACOS( -1.0_DbKi )
   D2R_D     = Pi_D/180.0_DbKi
   R2D_D     = 180.0_DbKi/Pi_D
   PiBy2_D   = Pi_D/2.0_DbKi
   RPM2RPS_D = Pi_D/30.0_DbKi
   RPS2RPM_D = 30.0_DbKi/Pi_D
   TwoByPi_D = 2.0_DbKi/Pi_D
   TwoPi_D   = 2.0_DbKi*Pi_D

   Pi      = ACOS( -1.0_ReKi )
   D2R     = Pi/180.0_ReKi
   R2D     = 180.0_ReKi/Pi
   PiBy2   = Pi/2.0_ReKi
   RPM2RPS = Pi/30.0_ReKi
   RPS2RPM = 30.0_ReKi/Pi
   TwoByPi =  2.0_ReKi/Pi
   TwoPi   =  2.0_ReKi*Pi

      ! IEEE constants:

!   NaN_D = ieee_value(0.0_DbKi, ieee_quiet_nan)
!   Inf_D = ieee_value(0.0_DbKi, ieee_positive_inf)
!
!   NaN   = ieee_value(0.0_ReKi, ieee_quiet_nan)
!   Inf   = ieee_value(0.0_DbKi, ieee_positive_inf)

      ! set variables to negative numbers to calculate NaNs (compilers may complain when taking sqrt of negative constants)
   Neg   = -1.0_ReKi
   Neg_D = -1.0_DbKi

   NaN_D = SQRT ( Neg_D )
   Inf_D = Pi_D / 0.0_DbKi

   NaN   = SQRT ( Neg )
   Inf   = Pi / 0.0_ReKi

   RETURN
   END SUBROUTINE SetConstants
!=======================================================================
   SUBROUTINE SmllRotTrans( RotationType, Theta1, Theta2, Theta3, TransMat, ErrTxt )

      ! This routine computes the 3x3 transformation matrix, TransMat,
      !   to a coordinate system x (with orthogonal axes x1, x2, x3)
      !   resulting from three rotations (Theta1, Theta2, Theta3) about the
      !   orthogonal axes (X1, X2, X3) of coordinate system X.  All angles
      !   are assummed to be small, as such, the order of rotations does
      !   not matter and Euler angles do not need to be used.  This routine
      !   is used to compute the transformation matrix (TransMat) between
      !   undeflected (X) and deflected (x) coordinate systems.  In matrix
      !   form:
      !      {x1}   [TransMat(Theta1, ] {X1}
      !      {x2} = [         Theta2, ]*{X2}
      !      {x3}   [         Theta3 )] {X3}

      ! The transformation matrix, TransMat, is the closest orthonormal
      !   matrix to the nonorthonormal, but skew-symmetric, Bernoulli-Euler
      !   matrix:
      !          [   1.0    Theta3 -Theta2 ]
      !      A = [ -Theta3   1.0    Theta1 ]
      !          [  Theta2 -Theta1   1.0   ]
      !
      !   In the Frobenius Norm sense, the closest orthornormal matrix is:
      !      TransMat = U*V^T,
      !
      !   where the columns of U contain the eigenvectors of A*A^T and the
      !   columns of V contain the eigenvectors of A^T*A (^T = transpose).
      !   This result comes directly from the Singular Value Decomposition
      !   (SVD) of A = U*S*V^T where S is a diagonal matrix containing the
      !   singular values of A, which are SQRT( eigenvalues of A*A^T ) =
      !   SQRT( eigenvalues of A^T*A ).

      ! The algebraic form of the transformation matrix, as implemented
      !   below, was derived symbolically by J. Jonkman by computing U*V^T
      !   by hand with verification in Mathematica.

      ! Passed Variables:

   REAL(ReKi), INTENT(IN )             :: Theta1                                          ! The small rotation about X1, (rad).
   REAL(ReKi), INTENT(IN )             :: Theta2                                          ! The small rotation about X2, (rad).
   REAL(ReKi), INTENT(IN )             :: Theta3                                          ! The small rotation about X3, (rad).
   REAL(ReKi), INTENT(OUT)             :: TransMat (3,3)                                  ! The resulting transformation matrix from X to x, (-).

   CHARACTER(*), INTENT(IN)            :: RotationType                                    ! The type of rotation; used to inform the user where a large rotation is occuring upon such an event.
   CHARACTER(*), INTENT(IN ), OPTIONAL :: ErrTxt                                          ! an additional message to be displayed as a warning (typically the simulation time)

      ! Local Variables:

   REAL(ReKi)                          :: ComDenom                                        ! = ( Theta1^2 + Theta2^2 + Theta3^2 )*SQRT( 1.0 + Theta1^2 + Theta2^2 + Theta3^2 )
   REAL(ReKi), PARAMETER               :: LrgAngle  = 0.4                                 ! Threshold for when a small angle becomes large (about 23deg).  This comes from: COS(SmllAngle) ~ 1/SQRT( 1 + SmllAngle^2 ) and SIN(SmllAngle) ~ SmllAngle/SQRT( 1 + SmllAngle^2 ) results in ~5% error when SmllAngle = 0.4rad.
   REAL(ReKi)                          :: Theta11                                         ! = Theta1^2
   REAL(ReKi)                          :: Theta12S                                        ! = Theta1*Theta2*[ SQRT( 1.0 + Theta1^2 + Theta2^2 + Theta3^2 ) - 1.0 ]
   REAL(ReKi)                          :: Theta13S                                        ! = Theta1*Theta3*[ SQRT( 1.0 + Theta1^2 + Theta2^2 + Theta3^2 ) - 1.0 ]
   REAL(ReKi)                          :: Theta22                                         ! = Theta2^2
   REAL(ReKi)                          :: Theta23S                                        ! = Theta2*Theta3*[ SQRT( 1.0 + Theta1^2 + Theta2^2 + Theta3^2 ) - 1.0 ]
   REAL(ReKi)                          :: Theta33                                         ! = Theta3^2
   REAL(ReKi)                          :: SqrdSum                                         ! = Theta1^2 + Theta2^2 + Theta3^2
   REAL(ReKi)                          :: SQRT1SqrdSum                                    ! = SQRT( 1.0 + Theta1^2 + Theta2^2 + Theta3^2 )

   LOGICAL,    SAVE                    :: FrstWarn  = .TRUE.                              ! When .TRUE., indicates that we're on the first warning.

      ! Display a warning message if at least one angle gets too large in
      !   magnitude:

   IF ( ( ( ABS(Theta1) > LrgAngle ) .OR. ( ABS(Theta2) > LrgAngle ) .OR. ( ABS(Theta3) > LrgAngle ) ) .AND. FrstWarn )  THEN

      CALL ProgWarn(' Small angle assumption violated in SUBROUTINE SmllRotTrans() due to'// &
                     ' a large '//TRIM(RotationType)//'. The solution may be inaccurate.'// &
                     ' Simulation continuing, but future warnings will be suppressed.')
      IF ( PRESENT(ErrTxt) ) THEN
         CALL WrScr(' Additional debugging message from SUBROUTINE SmllRotTrans(): '//TRIM(ErrTxt) )
      END IF

      FrstWarn = .FALSE.   ! Don't enter here again!

   ENDIF

      ! Compute some intermediate results:

   Theta11      = Theta1*Theta1
   Theta22      = Theta2*Theta2
   Theta33      = Theta3*Theta3

   SqrdSum      = Theta11 + Theta22 + Theta33
   SQRT1SqrdSum = SQRT( 1.0 + SqrdSum )
   ComDenom     = SqrdSum*SQRT1SqrdSum

   Theta12S     = Theta1*Theta2*( SQRT1SqrdSum - 1.0 )
   Theta13S     = Theta1*Theta3*( SQRT1SqrdSum - 1.0 )
   Theta23S     = Theta2*Theta3*( SQRT1SqrdSum - 1.0 )

      ! Define the transformation matrix:

   IF ( ComDenom == 0.0 )  THEN  ! All angles are zero and matrix is ill-conditioned (the matrix is derived assuming that the angles are not zero); return identity

      TransMat(1,:) = (/ 1.0, 0.0, 0.0 /)
      TransMat(2,:) = (/ 0.0, 1.0, 0.0 /)
      TransMat(3,:) = (/ 0.0, 0.0, 1.0 /)

   ELSE                          ! At least one angle is nonzero

      TransMat(1,1) = ( Theta11*SQRT1SqrdSum + Theta22              + Theta33              )/ComDenom
      TransMat(2,2) = ( Theta11              + Theta22*SQRT1SqrdSum + Theta33              )/ComDenom
      TransMat(3,3) = ( Theta11              + Theta22              + Theta33*SQRT1SqrdSum )/ComDenom
      TransMat(1,2) = (  Theta3*SqrdSum + Theta12S )/ComDenom
      TransMat(2,1) = ( -Theta3*SqrdSum + Theta12S )/ComDenom
      TransMat(1,3) = ( -Theta2*SqrdSum + Theta13S )/ComDenom
      TransMat(3,1) = (  Theta2*SqrdSum + Theta13S )/ComDenom
      TransMat(2,3) = (  Theta1*SqrdSum + Theta23S )/ComDenom
      TransMat(3,2) = ( -Theta1*SqrdSum + Theta23S )/ComDenom

   ENDIF

   RETURN
   END SUBROUTINE SmllRotTrans
!=======================================================================
   SUBROUTINE SortUnion ( Ary1, N1, Ary2, N2, Ary, N )

      ! This routine takes two sorted arrays and finds the sorted union of the two.

      ! Note: If the same value is found in both arrays, only one is kept.  However, if either
      !       array as multiple occurances of the same value, the largest multiple will be
      !       kept.  Duplicates should be eliminated externally if this is not desirable.

      ! Argument declarations:

   INTEGER, INTENT(OUT)         :: N                                            ! The length of the output array.
   INTEGER, INTENT(IN)          :: N1                                           ! The length of the first input array.
   INTEGER, INTENT(IN)          :: N2                                           ! The length of the second input array.

   REAL(ReKi), INTENT(OUT)      :: Ary(N1+N2)                                   ! The sorted union.
   REAL(ReKi), INTENT(IN)       :: Ary1(N1)                                     ! The first list of sorted real numbers.
   REAL(ReKi), INTENT(IN)       :: Ary2(N2)                                     ! The second list of sorted real numbers.

      ! Local declarations:

   INTEGER                      :: I1                                           ! Index into the first array.
   INTEGER                      :: I2                                           ! Index into the second array.

   I1 = 1
   I2 = 1
   N  = 1

   DO WHILE ( ( I1 <= N1 ) .AND. ( I2 <= N2 ) )

      IF ( Ary1(I1) < Ary2(I2) )  THEN
         Ary(N) = Ary1(I1)
         I1 = I1 + 1
      ELSE IF ( Ary1(I1) > Ary2(I2) )  THEN
         Ary(N) = Ary2(I2)
         I2 = I2 + 1
      ELSE
         Ary(N) = Ary1(I1)
         I1 = I1 + 1
         I2 = I2 + 1
      END IF

      N  = N  + 1

   END DO ! WHILE

     ! We've reached the end of one array, but we need to add the end
     ! of the other array if we haven't reached the end of it yet.

   IF ( I1 <= N1 ) THEN
      Ary(N:N+N1-I1) = Ary1(I1:)
      N = N+N1-I1
   ELSEIF ( I2 <= N2 ) THEN
      Ary(N:N+N2-I2) = Ary2(I2:)
      N = N+N2-I2
   ELSE
      N = N - 1
   ENDIF

   RETURN
   END SUBROUTINE SortUnion ! ( Ary1, N1, Ary2, N2, Ary, N )
!=======================================================================
   FUNCTION StdDevFn ( Ary, AryLen, Mean )

      ! This routine calculates the standard deviation of a population contained in Ary.

      ! Function declaration.

   REAL(ReKi)                   :: StdDevFn                                     ! This function.

      ! Argument declarations:

   INTEGER, INTENT(IN)          :: AryLen                                       ! Length of the array.

   REAL(ReKi), INTENT(IN)       :: Ary  (AryLen)                                ! Input array.
   REAL(ReKi), INTENT(IN)       :: Mean                                         ! The previously calculated mean of the array.

      ! Local declarations.

   REAL(DbKi)                   :: Sum                                          ! A temporary sum.

   INTEGER                      :: I                                            ! The index into the array.

   Sum = 0.0_DbKi

   DO I=1,AryLen
      Sum = Sum + ( Ary(I) - Mean )**2
   END DO ! I

   StdDevFn = SQRT( Sum/( AryLen - 1 ) )

   RETURN
   END FUNCTION StdDevFn ! ( Ary, AryLen, Mean )
!=======================================================================

END MODULE NWTC_Num


program Numbers
    complex::a,b,c,t=(0,1)
    integer :: i=10
    real :: x=5.17
    logical :: lg
    character :: ch

    print *,cmplx(i,x)

    !complex arithmetic
    a=(1,2)
    b=(3,4)
    print *,a*b*t

    !check kind
    print *,kind(a),kind(i),kind(x),kind(lg),kind(ch)
end program


!******************************************************************************
!*
!*                          LINEAR SYSTEM SOLUTION
!*
!******************************************************************************

  !==========================================================================
  ! 1) AUTHOR: F. Galliano
  !
  ! 2) HISTORY:
  !    - Created 08/2007
  !    - LINSYST_GAUSS & the LU routines updated on 11/2012
  !
  ! 3) DESCRIPTION: Solve a linear system of equations.
  !==========================================================================

MODULE linear_system

  USE utilities, ONLY:
  IMPLICIT NONE
  PRIVATE

  PUBLIC :: linsyst_gauss, tridag, LU_decomp, linsyst_LU, cholesky_decomp, &
            linsyst_cholesky

  INTERFACE linsyst_gauss
    MODULE PROCEDURE linsyst_gauss_1D, linsyst_gauss_2D
  END INTERFACE linsyst_gauss

CONTAINS

  !==========================================================================
  ! X[N,M] = LINSYST_GAUSS_2D(A[N,N],B[N,M][,INVERSE[N,N],ONLY_INVERSE])
  !
  !   Solves the linear systems A[N,N].X[N,M] = B[N,M], using the Gauss-Jordan
  ! elimination with full pivot's algorithm (Num. Rec. 2007, p. 44).
  !   In other words, it simultaneously solves the M linear systems defined by
  ! A(:,:).X(:,1) = B(:,1), ..., A(:,:).X(:,M) = B(:,M). A natural product of
  ! the operation is the inverse matrix of A (INVERSE). If ONLY_INVERSE is
  ! true, then only the inverse matrix is solved, the solution is not.
  !==========================================================================

  FUNCTION linsyst_gauss_2D (A,B,inverse,only_inverse)

    USE utilities, ONLY: DP, warning, swap, outerprod, outerand
    IMPLICIT NONE

    REAL(DP), INTENT(IN) :: A(:,:), B(:,:)
    REAL(DP), INTENT(OUT), OPTIONAL, DIMENSION(SIZE(A,1),SIZE(A,2)) :: inverse
    LOGICAL, INTENT(IN), OPTIONAL :: only_inverse
    REAL(DP), DIMENSION(SIZE(A,1),SIZE(A,2)) :: invA
    REAL(DP), DIMENSION(SIZE(B,1),SIZE(B,2)) :: linsyst_gauss_2D

    LOGICAL :: calcsol
    INTEGER, DIMENSION(SIZE(A,1)) :: indxc, indxr, ipiv
    REAL(DP), DIMENSION(SIZE(A,1)) :: dumc
    REAL(DP) :: pivinv
    INTEGER, TARGET, DIMENSION(2) :: irc
    INTEGER, POINTER :: irow, icol
    INTEGER :: N, M, i, l
    LOGICAL, DIMENSION(SIZE(A,1)) :: lpiv

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

    ! Initializing
    N = SIZE(B,1)
    M = SIZE(B,2)
    irow => irc(1)
    icol => irc(2)
    ipiv(:) = 0
    invA = A
    IF (PRESENT(only_inverse)) THEN
      calcsol = only_inverse
    ELSE
      calcsol = .True.
    END IF
    linsyst_gauss_2D = B

    ! Gauss-Jordan elimination with pivoting algorithm
    gauss_elim: DO i=1,N
      lpiv = (ipiv == 0)
      irc = MAXLOC(ABS(invA),OUTERAND(lpiv,lpiv))
      ipiv(icol) = ipiv(icol) + 1
      IF (ipiv(icol) > 1) CALL WARNING("LINSYST_GAUSS","Singular matrix")
      pivot: IF (irow /= icol) THEN
        CALL SWAP(invA(irow,:),invA(icol,:))
        IF (calcsol) &
          CALL SWAP(linsyst_gauss_2D(irow,:),linsyst_gauss_2D(icol,:))
      END IF pivot
      indxr(i) = irow
      indxc(i) = icol
      IF (invA(icol,icol) == 0._DP) &
        CALL WARNING("LINSYST_GAUSS","Singular matrix")
      pivinv = 1._DP / invA(icol,icol)
      invA(icol,icol) = 1._DP
      invA(icol,:) = invA(icol,:) * pivinv
      IF (calcsol) &
        linsyst_gauss_2D(icol,:) = linsyst_gauss_2D(icol,:) * pivinv
      dumc = invA(:,icol)
      invA(:,icol) = 0._DP
      invA(icol,icol) = pivinv
      invA(1:icol-1,:) = invA(1:icol-1,:) &
                         - OUTERPROD(dumc(1:icol-1),invA(icol,:))
      invA(icol+1:,:) = invA(icol+1:,:) &
                         - OUTERPROD(dumc(icol+1:),invA(icol,:))
      IF (calcsol) THEN
        linsyst_gauss_2D(1:icol-1,:) = linsyst_gauss_2D(1:icol-1,:) &
                            - OUTERPROD(dumc(1:icol-1),linsyst_gauss_2D(icol,:))
        linsyst_gauss_2D(icol+1:,:) = linsyst_gauss_2D(icol+1:,:) &
                            - OUTERPROD(dumc(icol+1:),linsyst_gauss_2D(icol,:))
      END IF
    END DO gauss_elim

    ! Unscramble the inverse matrix
    unscramble: IF (PRESENT(inverse)) THEN
      DO l=N,1,-1
        IF (indxr(l) /= indxc(l)) CALL SWAP(invA(:,indxr(l)),invA(:,indxc(l)))
      END DO
      inverse = invA
    END IF unscramble

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

  END FUNCTION linsyst_gauss_2D

  !==========================================================================
  ! Overload of LINSYST_GAUSS_2D in the case where X and B are of rank 1.
  !==========================================================================

  FUNCTION linsyst_gauss_1D (A,B,inverse)

    USE utilities, ONLY: DP
    IMPLICIT NONE

    REAL(DP), INTENT(IN) :: A(:,:), B(:)
    REAL(DP), INTENT(OUT), OPTIONAL, DIMENSION(SIZE(A,1),SIZE(A,2)) :: inverse
    REAL(DP), DIMENSION(SIZE(B)) :: linsyst_gauss_1D

    INTEGER :: N

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

    N = SIZE(B)

    IF (PRESENT(inverse)) THEN
      linsyst_gauss_1D = RESHAPE(LINSYST_GAUSS_2D(A,RESHAPE(B,[N,1]),inverse), &
                                 [N])
    ELSE
      linsyst_gauss_1D = RESHAPE(LINSYST_GAUSS_2D(A,RESHAPE(B,[N,1])),[N])
    END IF

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

  END FUNCTION linsyst_gauss_1D

  !==========================================================================
  ! X[N] = TRIDAG(A[N-1],B[N],C[N-1],R[N])
  !
  !   Solves a tridiagonal matrix system of diagonal B[N], off-diaognal
  ! elements A[N-1] and C[N-1], and right-hand side R[N].
  !==========================================================================

  FUNCTION tridag (a,b,c,r)

    USE utilities, ONLY: DP, strike
    IMPLICIT NONE

    REAL(DP), DIMENSION(:), INTENT(IN) :: a, b, c, r
    REAL(DP), DIMENSION(SIZE(b)) :: tridag

    REAL(DP), DIMENSION(SIZE(b)) :: gam
    INTEGER :: N, j
    REAL(DP) :: bet

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

    ! Check input
    N = SIZE(b)
    IF (SIZE(r) /= N .OR. SIZE(a) /= N-1 .OR. SIZE(c) /= N-1) &
      CALL STRIKE("TRIDAG","wrong input.")
    bet = b(1)
    IF (bet == 0.0) CALL STRIKE("TRIDAG","system of order N-1.")

    ! Decomposition & substitution
    tridag(1) = r(1)/bet
    decomp: DO j=2,N
      gam(j) = c(j-1)/bet
      bet = b(j) - a(j-1)*gam(j)
      IF (bet == 0.0) CALL STRIKE("TRIDAG","system not solvable.")
      tridag(j) = (r(j)-a(j-1)*tridag(j-1))/bet
    END DO decomp
    backsubst: DO j=n-1,1,-1
      tridag(j) = tridag(j) - gam(j+1)*tridag(j+1)
    END DO backsubst

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

  END FUNCTION tridag

  !==========================================================================
  ! LU[N,N] = LU_DECOMP(A[N,N],indx[N],d)
  !
  !   L.U. decomposition of square matrix A, following Num. Rec. 2007,
  ! Chap. 2.3.
  !==========================================================================

  FUNCTION LU_decomp (A,indx,d)

    USE utilities, ONLY: DP, warning, tinyDP, swap, outerprod
    IMPLICIT NONE

    REAL(DP), INTENT(IN), DIMENSION(:,:) :: A
    INTEGER, INTENT(OUT), DIMENSION(SIZE(A,1)) :: indx
    INTEGER, INTENT(OUT) :: d
    REAL(DP), DIMENSION(SIZE(A,1),SIZE(A,2)) :: LU_decomp

    REAL(DP), DIMENSION(SIZE(A,1)) :: vv
    INTEGER :: j, imax, N

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

    d = 1
    N = SIZE(A,1)
    LU_decomp(:,:) = A(:,:)
    vv = MAXVAL(ABS(LU_decomp),DIM=2)
    IF (ANY(vv == 0._DP)) CALL WARNING("LU_DECOMP","Singular matrix")
    vv = 1._DP / vv
    DO j=1,N
      imax = (j-1) + MAXVAL(MAXLOC(vv(j:N)*ABS(LU_decomp(j:N,j))))
      IF (j /= imax) THEN
        CALL SWAP (LU_decomp(imax,:),LU_decomp(j,:))
        d = -d
        vv(imax) = vv(j)
      END IF
      indx(j) = imax
      IF (LU_decomp(j,j) == 0._DP) LU_decomp(j,j) = tinyDP
      LU_decomp(j+1:N,j) = LU_decomp(j+1:N,j) / LU_decomp(j,j)
      LU_decomp(j+1:N,j+1:N) = LU_decomp(j+1:N,j+1:N) &
                              - OUTERPROD(LU_decomp(j+1:N,j),LU_decomp(j,j+1:N))
    END DO

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

  END FUNCTION LU_decomp

  !==========================================================================
  ! X[N] = LU_SOLVE(LU[N,N],B[N],indx[N])
  !
  !   L.U. decomposition for solving of linear system x=LU.b, following
  ! Num. Rec. 2007, Chap. 2.3.
  !==========================================================================

  FUNCTION linsyst_LU (LU,b,indx)

    USE utilities, ONLY: DP
    IMPLICIT NONE

    REAL(DP), INTENT(IN), DIMENSION(:,:) :: LU
    REAL(DP), INTENT(IN), DIMENSION(SIZE(LU,1)) :: b
    INTEGER, INTENT(IN), DIMENSION(SIZE(LU,1)) :: indx
    REAL(DP), DIMENSION(SIZE(b)) :: linsyst_LU

    REAL(DP) :: summ
    INTEGER :: i, ii, ll, N

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

    N = SIZE(b)
    linsyst_LU(:) = b(:)
    ii = 0

    DO i=1,N
      ll = indx(i)
      summ = linsyst_LU(ll)
      linsyst_LU(ll) = linsyst_LU(i)
      IF (ii /= 0) THEN
        summ = summ - DOT_PRODUCT(LU(i,ii:i-1),linsyst_LU(ii:i-1))
      ELSE IF (summ /= 0._DP) THEN
        ii = i
      END IF
      linsyst_LU(i) = summ
    END DO
    DO i=N,1,-1
      linsyst_LU(i) = linsyst_LU(i) - DOT_PRODUCT(LU(i,i+1:N),linsyst_LU(i+1:N))
      linsyst_LU(i) = linsyst_LU(i) / LU(i,i)
    END DO

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

  END FUNCTION linsyst_LU

  !==========================================================================
  ! L[N,N] = CHOLESKY_DECOMP(A[N,N],NOPOSDEF)
  !
  !   Compute the Cholesky decomposition of the symmetric positive definite
  ! matrix A[N,N]. L[N,N] is the lower triangle of the decomposition. In the
  ! end, A = L.L^T.
  !==========================================================================

  FUNCTION cholesky_decomp (A,noposdef)

    USE utilities, ONLY: DP, strike
    IMPLICIT NONE

    REAL(DP), DIMENSION(:,:), INTENT(IN) :: A
    LOGICAL, INTENT(OUT), OPTIONAL :: noposdef
    REAL(DP), DIMENSION(SIZE(A,1),SIZE(A,2)) :: cholesky_decomp

    REAL(DP), DIMENSION(SIZE(A,1)) :: diag
    REAL(DP) :: summ
    INTEGER :: i, N

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

    N = SIZE(A,1)
    cholesky_decomp = A
    IF (PRESENT(noposdef)) noposdef = .False.
    row: DO i=1,N
      summ = cholesky_decomp(i,i) &
             - DOT_PRODUCT(cholesky_decomp(i,1:i-1),cholesky_decomp(i,1:i-1))
      IF (summ <= 0._DP) THEN
        IF (PRESENT(noposdef)) THEN
          noposdef = .True.
          RETURN
        ELSE
          CALL STRIKE("CHOLESKY_DECOMP","Matrix is not positive definite")
        END IF
      END IF
      diag(i) = SQRT(summ)
      cholesky_decomp(i+1:N,i) = (cholesky_decomp(i,i+1:N) &
        - MATMUL(cholesky_decomp(i+1:N,1:i-1),cholesky_decomp(i,1:i-1)))/diag(i)
    END DO row

    ! Build the matrix
    lower_triangle: FORALL (i=1:N)
      cholesky_decomp(i,i+1:N) = 0._DP
      cholesky_decomp(i,i) = diag(i)
    END FORALL lower_triangle

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

  END FUNCTION cholesky_decomp

  !==========================================================================
  ! X[N] = LINSYST_CHOLESKY(L[N,N],b[N])
  !
  !   Solves the equation A[N,N].x[N]=b[N], given A and b, using the Cholesky de
  ! composition of A=L.L^T.
  !==========================================================================

  FUNCTION linsyst_cholesky (L,b)

    USE utilities, ONLY: DP
    IMPLICIT NONE

    REAL(DP), DIMENSION(:,:), INTENT(IN) :: L
    REAL(DP), DIMENSION(SIZE(L,1)), INTENT(IN) :: b
    REAL(DP), DIMENSION(SIZE(L,1)) :: linsyst_cholesky

    INTEGER :: i, N

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

    N = SIZE(L,1)
    DO i=1,N
      linsyst_cholesky(i) = (b(i) &
                       - DOT_PRODUCT(L(i,1:i-1),linsyst_cholesky(1:i-1)))/L(i,i)
    END DO
    DO i=N,1,-1
      linsyst_cholesky(i) = (linsyst_cholesky(i) &
                       - DOT_PRODUCT(L(i+1:N,i),linsyst_cholesky(i+1:N)))/L(i,i)
    END DO

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

  END FUNCTION linsyst_cholesky

END MODULE linear_system


      FUNCTION BESSJ0 (X)
      IMPLICIT NONE
      REAL *8 X,BESSJ0,AX,FR,FS,Z,FP,FQ,XX

!     This subroutine calculates the First Kind Bessel Function of
!     order 0, for any real number X. The polynomial approximation by
!     series of Chebyshev polynomials is used for 0<X<8 and 0<8/X<1.
!     REFERENCES:
!     M.ABRAMOWITZ,I.A.STEGUN, HANDBOOK OF MATHEMATICAL FUNCTIONS, 1965.
!     C.W.CLENSHAW, NATIONAL PHYSICAL LABORATORY MATHEMATICAL TABLES,
!     VOL.5, 1962.

      REAL *8 Y,P1,P2,P3,P4,P5,R1,R2,R3,R4,R5,R6  &
               ,Q1,Q2,Q3,Q4,Q5,S1,S2,S3,S4,S5,S6
      DATA P1,P2,P3,P4,P5 /1.D0,-.1098628627D-2,.2734510407D-4, &
      -.2073370639D-5,.2093887211D-6 /
      DATA Q1,Q2,Q3,Q4,Q5 /-.1562499995D-1,.1430488765D-3, &
      -.6911147651D-5,.7621095161D-6,-.9349451520D-7 /
      DATA R1,R2,R3,R4,R5,R6 /57568490574.D0,-13362590354.D0, &
      651619640.7D0,-11214424.18D0,77392.33017D0,-184.9052456D0 /
      DATA S1,S2,S3,S4,S5,S6 /57568490411.D0,1029532985.D0, &
      9494680.718D0,59272.64853D0,267.8532712D0,1.D0 /
      IF(X.EQ.0.D0) GO TO 1
      AX = ABS (X)
      IF (AX.LT.8.) THEN
      Y = X*X
      FR = R1+Y*(R2+Y*(R3+Y*(R4+Y*(R5+Y*R6))))
      FS = S1+Y*(S2+Y*(S3+Y*(S4+Y*(S5+Y*S6))))
      BESSJ0 = FR/FS
      ELSE
      Z = 8./AX
      Y = Z*Z
      XX = AX-.785398164
      FP = P1+Y*(P2+Y*(P3+Y*(P4+Y*P5)))
      FQ = Q1+Y*(Q2+Y*(Q3+Y*(Q4+Y*Q5)))
      BESSJ0 = SQRT(.636619772/AX)*(FP*COS(XX)-Z*FQ*SIN(XX))
      ENDIF
      RETURN
    1 BESSJ0 = 1.D0
      RETURN
      END


      SUBROUTINE ttest(data1,n1,data2,n2,t,prob)
      INTEGER n1,n2
      REAL prob,t,data1(n1),data2(n2)
CU    USES avevar,betai
      REAL ave1,ave2,df,var,var1,var2,betai
      call avevar(data1,n1,ave1,var1)
      call avevar(data2,n2,ave2,var2)
      df=n1+n2-2
      var=((n1-1)*var1+(n2-1)*var2)/df
      t=(ave1-ave2)/sqrt(var*(1./n1+1./n2))
      prob=betai(0.5*df,0.5,df/(df+t**2))
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/ttest.for


module schroed_util
use types, only: dp
use linalg, only: eigh, eigvals
use utils, only: stop_error
implicit none
private
public lho, radial

contains

subroutine lho(xq, wq, B, Bp, lam, condA, condB)
real(dp), intent(in) :: xq(:), wq(:),  B(:,:), Bp(:,:)
real(dp), allocatable, intent(out) :: lam(:)
real(dp), intent(out) :: condA, condB
real(dp), allocatable :: Am(:,:), Bm(:,:), c(:,:), hq(:)
real(dp) :: En
integer :: i, j, Nb
Nb = size(B, 2)
allocate(hq(size(xq)))
allocate(Am(Nb,Nb), Bm(Nb,Nb), c(Nb,Nb), lam(Nb))
print *, "Assembly"
! Construct matrices A and B
do i = 1, Nb
    do j = 1, Nb
        ! A
        ! Both of these work:
        hq = Bp(:,i)*Bp(:,j)/2 + B(:,i)*B(:,j)*(xq**2)/2
        Am(i,j) = sum(wq*hq)

        ! B
        hq = B(:,i)*B(:,j)
        Bm(i,j) = sum(wq*hq)
    end do
end do

print *, "Checking symmetry"
do j = 1, Nb
    do i = 1, j-1
        if (max(abs(Am(i,j)), abs(Am(j,i))) > tiny(1._dp)) then
            if (abs(Am(i,j)-Am(j,i)) / max(abs(Am(i,j)), abs(Am(j,i))) &
                    > 1e-8_dp) then
                print *, i, j, Am(i,j)-Am(j,i), Am(i,j), Am(j,i)
                call stop_error("Am not symmetric")
            end if
        end if
        if (abs(Bm(i,j)-Bm(j,i)) > 1e-12_dp) call stop_error("Bm not symmetric")
   end do
end do

print *, "Eigensolver"

lam = eigvals(Am)
condA = maxval(abs(lam))/minval(abs(lam))
print "('cond A: ', es10.2)", condA
lam = eigvals(Bm)
condB = maxval(abs(lam))/minval(abs(lam))
print "('cond B: ', es10.2)", condB

! Solve an eigenproblem
call eigh(Am, Bm, lam, c)

print *, "n, energy, exact energy, error"
do i = 1, min(Nb, 20)
    En = 0.5_dp + (i-1)
    print "(i4, f30.8, f18.8, es12.2)", i, lam(i), En, abs(lam(i)-En)
end do
end subroutine

subroutine radial(Nb, xq, wq, B, Bp, Z, l)
integer, intent(in) :: Nb, Z, l
real(dp), intent(in) :: xq(:), wq(:),  B(:,:), Bp(:,:)
real(dp), allocatable :: Am(:,:), Bm(:,:), c(:,:), lam(:), hq(:)
real(dp) :: En
integer :: i, j
allocate(hq(size(xq)))
allocate(Am(Nb,Nb), Bm(Nb,Nb), c(Nb,Nb), lam(Nb))
print *, "Assembly"
! Construct matrices A and B
do i = 1, Nb
    do j = 1, Nb
        ! A
        ! Both of these work:
        hq = Bp(:,i)*Bp(:,j)/2 + B(:,i)*B(:,j)*(-Z/xq+l*(l+1)/(2*xq**2))
        Am(i,j) = sum(wq*hq)

        ! B
        hq = B(:,i)*B(:,j)
        Bm(i,j) = sum(wq*hq)
    end do
end do

print *, "Checking symmetry"
do j = 1, Nb
    do i = 1, j-1
        if (max(abs(Am(i,j)), abs(Am(j,i))) > tiny(1._dp)) then
            if (abs(Am(i,j)-Am(j,i)) / max(abs(Am(i,j)), abs(Am(j,i))) &
                    > 1e-8_dp) then
                print *, i, j, Am(i,j)-Am(j,i), Am(i,j), Am(j,i)
                call stop_error("Am not symmetric")
            end if
        end if
        if (abs(Bm(i,j)-Bm(j,i)) > 1e-12_dp) call stop_error("Bm not symmetric")
   end do
end do

print *, "Eigensolver"
! Solve an eigenproblem
call eigh(Am, Bm, lam, c)

print *, "n, energy, exact energy, error"
do i = 1, min(Nb, 20)
    En = -Z**2/(2._dp*(i+l)**2)
    print "(i4, f30.8, f18.8, es12.2)", i, lam(i), En, abs(lam(i)-En)
end do
end subroutine

end module


!------------------------------------------------------------------------
!EV The following two routines concern natural cubic spline interpolation
!   of a function of two independent variables.
!------------------------------------------------------------------------

SUBROUTINE splie2(x2a, ya, m, n, y2a)
    !
    ! Given an m by n tabulated function ya(1:m,1:n), and tabulated independent
    ! variable x2a(1:n), this routine constructs 1-dimensional natural cubic
    ! splines of the 2nd dimension (1:n) of ya and returns the 2nd-derivatives in
    ! the array y2a(1:m,1:n).  From "Numerical Recipes" (FORTRAN, 1992 Ed.),
    ! Ch. 3.6, p. 121.  Call this routine only once, before a sequence of calls to
    ! SPLIN2.  The process is of order  m x n.
    IMPLICIT NONE
    ! Passed variables:
    INTEGER m, n
    REAL x2a(n), ya(m, n), y2a(m, n)
    ! Local variables:
    INTEGER i, j, NN
    REAL bound
    PARAMETER (NN = 100)     ! Maximum expected value of n
    REAL ytmp(NN), y2tmp(NN)
    ! Boundary condition threshold for natural cubic spline:
    SAVE bound
    DATA bound/1.e30/
    !
    do i = 1, m
        do j = 1, n
            ytmp(j) = ya(i, j)
        enddo
        ! Natural spline:
        call spline(x2a, ytmp, n, bound, bound, y2tmp)
        do j = 1, n
            y2a(i, j) = y2tmp(j)
        enddo
    enddo
    !
    return
END


PROGRAM radiation

! This program calculates the radiation pattern coefficients for a given
! set of fault parameters: strike, rake, and dip (in degrees).  The program
! then loops through all takeoff angles and azimuths and produces an output
! file with the format:
! Azimuth/Takeoff_Angle/Amplitude

IMPLICIT NONE

! Declare parameters

REAL, PARAMETER :: d2r = 0.01745329252          !Degrees => Radians

! Declare variables

REAL, DIMENSION (:,:), ALLOCATABLE :: amps_SH   !Array storing SH amplitude
REAL, DIMENSION (:,:), ALLOCATABLE :: amps_SV   !Array storing SV amplitudes
REAL, DIMENSION (:,:), ALLOCATABLE :: amps_P    !Array storing P amplitudes

REAL :: ft_strk					!Fault strike
REAL :: psi					!Angle between strike and azimuth
REAL :: rake					!Fault rake
REAL :: dip					!Fault dip
REAL :: takeoff, azi, lamda, delta, fi
REAL :: MAX_P,temp, tanvalue

CHARACTER (len=30) file1, file2, file3		!Name of files to dump amps into

INTEGER :: i_h					!Loop index: Take off angle
INTEGER :: az_s					!Loop index: Station azimuth

INTEGER :: r, c                                 !Write out loop indexes
INTEGER :: naz1,naz2                            !Begin and end azimuths
INTEGER :: nih1,nih2                            !Begin and end take off angles

! Number of values in each array
naz1 = 0
naz2 = 360
nih1 = 0
nih2 = 90

! Initialize arrays => for finer spacing, increase the number of values and don't
! forget to divide azimuths and take off angles by this increment
ALLOCATE( amps_SH(naz1:naz2,nih1:nih2) )
ALLOCATE( amps_SV(naz1:naz2,nih1:nih2) )
ALLOCATE(  amps_P(naz1:naz2,nih1:nih2) )

! Read in fault parameters
WRITE (*,*) "Enter the strike, dip, and rake (degrees): "
READ (*,*) ft_strk, dip, rake

! Make file
!WRITE (*,*) "Enter the filenames (SH,SV,P): "
!READ (*,*) file1, file2, file3
file1 = 'SH_radiation'
file2 = 'SV_radiation'
file3 = 'P_radiation'
OPEN (UNIT=1, FILE=file1, STATUS='REPLACE', ACTION='WRITE')
OPEN (UNIT=2, FILE=file2, STATUS='REPLACE', ACTION='WRITE')
OPEN (UNIT=3, FILE=file3, STATUS='REPLACE', ACTION='WRITE')
MAX_P=0.0

lamda = rake*d2r
delta = dip*d2r
fi =  ft_strk * d2r
! Find amplitudes and place in array
DO i_h = nih1, nih2

    takeoff = i_h*d2r

  DO az_s = naz1, naz2

!      psi = ( ft_strk - REAL(az_s) )*d2r
      azi = az_s * d2r
      psi =  azi - fi
      !SH Energy

       amps_SH(az_s,i_h) = cos(lamda)*cos(delta)*cos(takeoff)*sin(psi)&
                   + cos(lamda)*sin(delta)*sin(takeoff)*cos(2*psi)&
                   + sin(lamda)*cos(2*delta)*cos(takeoff)*cos(psi)&
                   - 0.5*sin(lamda)*sin(2*delta)*sin(takeoff)*sin(2*psi)

      !SV Energy
      amps_SV(az_s,i_h) = sin(lamda)*cos(2*delta)*cos(2*takeoff)*sin(psi)&
                   - cos(lamda)*cos(delta)*cos(2*takeoff)*cos(psi)&
                   + 0.5*cos(lamda)*sin(delta)*sin(2*takeoff)*sin(2*psi)&
                   - 0.5*sin(lamda)*sin(2*delta)*sin(2*takeoff)*(1+sin(psi)**2)

      !P Energy
      amps_P(az_s,i_h) = cos(lamda)*sin(delta)*(sin(takeoff)**2)*sin(2*psi)&
                   - cos(lamda)*cos(delta)*sin(2*takeoff)*cos(psi)&
                   + sin(lamda)*sin(2*delta)*(cos(takeoff)**2&
                   - sin(takeoff)**2*sin(psi)**2 )&
                   + sin(lamda)*cos(2*delta)*sin(2*takeoff)*sin(psi)

      temp = amps_SV(az_s,i_h)
      if (ABS(temp) > MAX_P ) MAX_P = ABS(temp)
  END DO
END DO

! Dump data into the files
Do c = nih1,nih2
   Do r = naz1, naz2

    ! Eastern hemisphere 0 => 180
!      WRITE (1,100) REAL(r), tanvalue, amps_SH(r,c)
!      WRITE (2,100) REAL(r), tanvalue, amps_SV(r,c)
!      WRITE (3,100) REAL(r), tanvalue, amps_P(r,c)

      ! Amps SH
      WRITE (1,'(2F18.8, F12.8)') REAL(r), REAL(c), amps_SH(r,c)
      WRITE (2,'(2F18.8, F12.8)') REAL(r), REAL(c), amps_SV(r,c)
      WRITE (3,'(2F18.8, F12.8)') REAL(r), REAL(c), amps_P(r,c)

  END DO
END DO

END PROGRAM