text: !**********************************************************************************************************************************
  ! LICENSING
  ! Copyright (C) 2015-2016  National Renewable Energy Laboratory
  !
  ! 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
  
     USE BeamDyn_IO
     USE BeamDyn_Subs
     USE NWTC_LAPACK
  
     IMPLICIT NONE
  
     PRIVATE
  
     ! ..... Public Subroutines....................................................................
  
     PUBLIC :: BD_Init                           ! Initialization routine
     PUBLIC :: BD_End                            ! Ending routine (includes clean up)
     PUBLIC :: BD_UpdateStates                   ! Loose coupling routine for solving for constraint states, integrating
     PUBLIC :: BD_CalcOutput                     ! Routine for computing outputs
     PUBLIC :: BD_CalcConstrStateResidual        ! Tight coupling routine for returning the constraint state residual
     PUBLIC :: BD_UpdateDiscState                ! Tight coupling routine for updating discrete states
     
  
  CONTAINS
  
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This routine is called at the start of the simulation to perform initialization steps.
  !! The parameters are set here and not changed during the simulation.
  !! The initial states and initial guess for the input are defined.
  SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, InitOut, ErrStat, ErrMsg )
  
     TYPE(BD_InitInputType),            INTENT(IN   )  :: InitInp     !< Input data for initialization routine
     TYPE(BD_InputType),                INTENT(  OUT)  :: u           !< An initial guess for the input; input mesh must be defined
     TYPE(BD_ParameterType),            INTENT(  OUT)  :: p           !< Parameters
     TYPE(BD_ContinuousStateType),      INTENT(  OUT)  :: x           !< Initial continuous states
     TYPE(BD_DiscreteStateType),        INTENT(  OUT)  :: xd          !< Initial discrete states
     TYPE(BD_ConstraintStateType),      INTENT(  OUT)  :: z           !< Initial guess of the constraint states
     TYPE(BD_OtherStateType),           INTENT(  OUT)  :: OtherState  !< Initial other states
     TYPE(BD_OutputType),               INTENT(  OUT)  :: y           !< Initial system outputs (outputs are not calculated;
                                                                      !!    only the output mesh is initialized)
     TYPE(BD_MiscVarType),              INTENT(  OUT)  :: MiscVar     !<  Misc variables for optimization (not copied in glue code)
     REAL(DbKi),                        INTENT(INOUT)  :: Interval    !< Coupling interval in seconds: the rate that
                                                                      !!   (1) Mod1_UpdateStates() is called in loose coupling &
                                                                      !!   (2) Mod1_UpdateDiscState() is called in tight coupling.  !   Input is the suggested time from the glue code;
                                                                      !!   Output is the actual coupling interval that will be used
                                                                      !!   by the glue code.
     TYPE(BD_InitOutputType),           INTENT(  OUT)  :: InitOut     !< Output for initialization routine
     INTEGER(IntKi),                    INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                      INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     ! local variables
     TYPE(BD_InputFile)      :: InputFileData    ! Data stored in the module's input file
     INTEGER(IntKi)          :: i                ! do-loop counter
     INTEGER(IntKi)          :: j                ! do-loop counter
     INTEGER(IntKi)          :: k                ! do-loop counter
     INTEGER(IntKi)          :: m                ! do-loop counter
     INTEGER(IntKi)          :: id0
     INTEGER(IntKi)          :: id1
     INTEGER(IntKi)          :: temp_int
     INTEGER(IntKi)          :: temp_id
     INTEGER(IntKi)          :: temp_id2
     REAL(BDKi)              :: temp_Coef(4,4)
     REAL(BDKi)              :: temp66(6,6)
     REAL(BDKi)              :: temp_twist
     REAL(BDKi)              :: eta
     REAL(BDKi)              :: temp_POS(3)
     REAL(BDKi)              :: temp_e1(3)
     REAL(BDKi)              :: temp_CRV(3)
     REAL(BDKi),PARAMETER    :: EPS = 1.0D-10
     REAL(BDKi),ALLOCATABLE  :: GLL(:)
     REAL(BDKi),ALLOCATABLE  :: temp_GL(:)
     REAL(BDKi),ALLOCATABLE  :: temp_w(:)
     REAL(BDKi),ALLOCATABLE  :: temp_ratio(:,:)
     REAL(BDKi),ALLOCATABLE  :: SP_Coef(:,:,:)
     REAL(BDKi)              :: TmpDCM(3,3)
     REAL(BDKi)              :: denom
  
     INTEGER(IntKi)          :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)    :: ErrMsg2                      ! Temporary Error message
     character(*), parameter :: RoutineName = 'BD_Init'
     
     
    ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     ! Initialize the NWTC Subroutine Library
  
     CALL NWTC_Init( )
  
     ! Display the module information
  
     CALL DispNVD( BeamDyn_Ver )
        
     CALL BD_ReadInput(InitInp%InputFile,InputFileData,InitInp%RootName,Interval,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
     CALL BD_ValidateInputData( InputFileData, ErrStat2, ErrMsg2 )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        ! this routine sets *some* of the parameters (basically the "easy" ones)
    call setParameters(InitInp, InputFileData, p, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        
     ! Compute coefficients for cubic spline fit, clamped at two ends
     CALL AllocAry(SP_Coef,InputFileData%kp_total-1,4,4,'Spline coefficient matrix',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if   
        
     SP_Coef(:,:,:) = 0.0_BDKi
     temp_id = 1
     temp_id2 = 0
     DO i=1,InputFileData%member_total
         temp_id2= temp_id + InputFileData%kp_member(i) - 1
         CALL BD_ComputeIniCoef(InputFileData%kp_member(i),InputFileData%kp_coordinate(temp_id:temp_id2,1:4),&
                                SP_Coef(temp_id:temp_id2-1,1:4,1:4), ErrStat2, ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         temp_id = temp_id2
     ENDDO
     ! Compute blade/member/segment lengths and the ratios between member/segment and blade lengths
     
     p%member_length(:,:) = 0.0_BDKi
     p%segment_length(:,:) = 0.0_BDKi
     CALL BD_ComputeMemberLength(InputFileData%member_total,InputFileData%kp_member,&
                                 InputFileData%kp_coordinate,SP_Coef,&
                                 p%segment_length,p%member_length,p%blade_length,&
                                 ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
     
     ! Temporary GLL point intrinsic coordinates array
     CALL AllocAry(GLL,p%node_elem,'GLL points array',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ! Temporary GLL weight function array
     CALL AllocAry(temp_w,p%node_elem,'GLL weight array',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
     p%uuN0(:,:) = 0.0_BDKi
     GLL(:) = 0.0_BDKi
     temp_w(:) = 0.0_BDKi
     CALL BD_GenerateGLL(p%node_elem-1,GLL,temp_w,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
     DEALLOCATE(temp_w)
     if (ErrStat >= AbortErrLev) then
        call cleanup()
        return
     end if
     p%GL(:) = 0.0_BDKi
     p%GLw(:) = 0.0_BDKi
     IF(p%quadrature .EQ. 1) THEN
         ! p%Gauss: the DistrLoad mesh node location
         ! p%Gauss: physical coordinates of Gauss points and two end points
         CALL AllocAry(p%Gauss,6,p%ngp*p%elem_total+2,'p%Gauss',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
               call cleanup()
               return
            end if
  
         p%Gauss(:,:) = 0.0_BDKi
         CALL BD_GaussPointWeight(p%ngp,p%GL,p%GLw,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
               call cleanup()
               return
            end if
     ELSEIF(p%quadrature .EQ. 2) THEN
         CALL AllocAry(p%Gauss,6,p%ngp,'p%Trapezoidal',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ! Temporary Gauss point intrinsic coordinates array
         p%Gauss(:,:) = 0.0_BDKi
  
         id0 = 1
         p%GL(1) = InputFileData%InpBl%station_eta(id0)
         DO j = 2,p%ngp
              p%GL(j) = InputFileData%InpBl%station_eta(id0+(j-2)/p%refine) + &
                      ((InputFileData%InpBl%station_eta(id0+(j-2)/p%refine + 1) - &
                        InputFileData%InpBl%station_eta(id0+(j-2)/p%refine))/p%refine) * &
                    (MOD(j-2,p%refine) + 1)
         ENDDO
     ENDIF
  
       ! initialize for first step (i=1)
     temp_id = 0
     id0 = 1
     DO i=1,p%elem_total
         id1 = id0 + InputFileData%kp_member(i) - 1
         DO j=1,p%node_elem
             eta = (GLL(j) + 1.0_BDKi)/2.0_BDKi
             DO k=1,InputFileData%kp_member(i)-1
                 temp_id2 = temp_id + k
                 IF(eta - p%segment_length(temp_id2,3) <= EPS) THEN !bjj: would it be better to use equalRealNos and compare with 0? 1D-10 stored in single precision scares me as a limit
                     DO m=1,4
                         temp_Coef(m,1:4) = SP_Coef(temp_id2,1:4,m)
                     ENDDO
                     eta = InputFileData%kp_coordinate(id0,1) + &
                           eta * (InputFileData%kp_coordinate(id1,1) - InputFileData%kp_coordinate(id0,1))
                     ! Compute GLL point physical coordinates in blade frame
                     CALL BD_ComputeIniNodalPosition(temp_Coef,eta,temp_POS,temp_e1,temp_twist,&
                                                     ErrStat2, ErrMsg2)
                        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                     ! Compute initial rotation parameters at GLL points in blade frame
                     CALL BD_ComputeIniNodalCrv(temp_e1,temp_twist,temp_CRV,ErrStat2,ErrMsg2)
                        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                     temp_id2 = (j-1)*p%dof_node
                     p%uuN0(temp_id2+1:temp_id2+3,i) = temp_POS(1:3)
                     p%uuN0(temp_id2+4:temp_id2+6,i) = temp_CRV(1:3)
                     EXIT
                 ENDIF
             ENDDO
         ENDDO
         IF(p%quadrature .EQ. 1) THEN
             DO j=1,p%ngp
                 eta = (p%GL(j) + 1.0_BDKi)/2.0_BDKi
                 DO k=1,InputFileData%kp_member(i)-1
                     temp_id2 = temp_id + k
                     IF(eta - p%segment_length(temp_id2,3) <= EPS) THEN
                         DO m=1,4
                             temp_Coef(m,1:4) = SP_Coef(temp_id2,1:4,m)
                         ENDDO
                         eta = InputFileData%kp_coordinate(id0,1) + &
                               eta * (InputFileData%kp_coordinate(id1,1) - InputFileData%kp_coordinate(id0,1))
                         CALL BD_ComputeIniNodalPosition(temp_Coef,eta,temp_POS,temp_e1,temp_twist,&
                                                         ErrStat2, ErrMsg2)
                            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                         CALL BD_ComputeIniNodalCrv(temp_e1,temp_twist,temp_CRV,ErrStat2,ErrMsg2)
                            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                         temp_id2 = (i-1)*p%ngp+j+1
                         p%Gauss(1:3,temp_id2) = temp_POS(1:3)
                         p%Gauss(4:6,temp_id2) = temp_CRV(1:3)
                         EXIT
                     ENDIF
                 ENDDO
             ENDDO
         ELSEIF(p%quadrature .EQ. 2) THEN
             DO j = 1,p%ngp
                 IF( j .EQ. 1) THEN
                     DO m=1,4
                         temp_Coef(m,1:4) = SP_Coef(1,1:4,m)
                     ENDDO
                     eta = InputFileData%kp_coordinate(id0,1)
                 ELSE
                     DO m=1,4
                         temp_Coef(m,1:4) = SP_Coef( (j-2)/p%refine + 1,1:4,m)
                     ENDDO
                     eta = InputFileData%kp_coordinate(id0+(j-2)/p%refine,1) + &
                           ((InputFileData%kp_coordinate(id0+(j-2)/p%refine + 1,1) - &
                             InputFileData%kp_coordinate(id0+(j-2)/p%refine,1))/p%refine) * &
                           (MOD(j-2,p%refine) + 1)
                 ENDIF
                 CALL BD_ComputeIniNodalPosition(temp_Coef,eta,temp_POS,temp_e1,temp_twist,&
                                                 ErrStat2, ErrMsg2)
                    CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                 CALL BD_ComputeIniNodalCrv(temp_e1,temp_twist,temp_CRV,ErrStat2,ErrMsg2)
                    CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                 p%Gauss(1:3,j) = temp_POS(1:3)  
                 p%Gauss(4:6,j) = temp_CRV(1:3)
             ENDDO
         ENDIF
  
           ! set for next time:
        temp_id = temp_id + InputFileData%kp_member(i) - 1
        id0 = id1           
         
     ENDDO
     IF(p%quadrature .EQ. 1) THEN
         p%Gauss(1:3,1) = p%uuN0(1:3,1)
         p%Gauss(4:6,1) = p%uuN0(4:6,1)
         
         temp_int = p%node_elem * p%dof_node
         p%Gauss(1:3,p%ngp*p%elem_total+2) = p%uuN0(temp_int-5:temp_int-3,p%elem_total)
         p%Gauss(4:6,p%ngp*p%elem_total+2) = p%uuN0(temp_int-2:temp_int,p%elem_total)
     ENDIF
     DEALLOCATE(SP_Coef)
  
     IF(p%quadrature .EQ. 1) THEN
         ! Compute sectional propertities ( 6 by 6 stiffness and mass matrices)
         ! at Gauss points
         CALL AllocAry(temp_ratio,p%ngp,p%elem_total,'temp_ratio',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL AllocAry(temp_GL,p%ngp,'temp_GL',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
               call cleanup()
               return
            end if
         DO i=1,p%ngp
             temp_GL(i) = (p%GL(i) + 1.0_BDKi)/2.0_BDKi
         ENDDO
  
        temp_ratio(:,:) = 0.0_BDKi
        DO j=1,p%ngp
           temp_ratio(j,1) = temp_GL(j)*p%member_length(1,2)
        ENDDO
        DO i=2,p%elem_total
           DO j=1,i-1
                 temp_ratio(:,i) = temp_ratio(:,i) + p%member_length(j,2)
           ENDDO
           DO j=1,p%ngp
                 temp_ratio(j,i) = temp_ratio(j,i) + temp_GL(j)*p%member_length(i,2)
           ENDDO
        ENDDO
  
         CALL AllocAry(p%Stif0_GL,6,6,p%ngp*p%elem_total,'Stif0_GL',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL AllocAry(p%Mass0_GL,6,6,p%ngp*p%elem_total,'Mass0_GL',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
               call cleanup()
               return
            end if
         p%Stif0_GL(:,:,:) = 0.0_BDKi
         p%Mass0_GL(:,:,:) = 0.0_BDKi
         DO i=1,p%elem_total
             DO j=1,p%ngp
                 temp_id = (i-1)*p%ngp+j
                 DO k=1,InputFileData%InpBl%station_total
                     IF(temp_ratio(j,i) - InputFileData%InpBl%station_eta(k) <= EPS) THEN
                         IF(ABS(temp_ratio(j,i) - InputFileData%InpBl%station_eta(k)) <= EPS) THEN
                             p%Stif0_GL(1:6,1:6,temp_id) = InputFileData%InpBl%stiff0(1:6,1:6,k)
                             p%Mass0_GL(1:6,1:6,temp_id) = InputFileData%InpBl%mass0(1:6,1:6,k)
                         ELSE
                             temp66(:,:) = 0.0_BDKi
                             temp66(1:6,1:6) = (InputFileData%InpBl%stiff0(1:6,1:6,k)-InputFileData%InpBl%stiff0(1:6,1:6,k-1)) / &
                                               (InputFileData%InpBl%station_eta(k) - InputFileData%InpBl%station_eta(k-1))
                             p%Stif0_GL(1:6,1:6,temp_id) = temp66(1:6,1:6) * temp_ratio(j,i) + &
                                                           InputFileData%InpBl%stiff0(1:6,1:6,k-1) - &
                                                           temp66(1:6,1:6) * InputFileData%InpBl%station_eta(k-1)
                             temp66(:,:) = 0.0_BDKi
                             temp66(1:6,1:6) = (InputFileData%InpBl%mass0(1:6,1:6,k)-InputFileData%InpBl%mass0(1:6,1:6,k-1)) / &
                                               (InputFileData%InpBl%station_eta(k) - InputFileData%InpBl%station_eta(k-1))
                             p%Mass0_GL(1:6,1:6,temp_id) = temp66(1:6,1:6) * temp_ratio(j,i) + &
                                                           InputFileData%InpBl%mass0(1:6,1:6,k-1) - &
                                                           temp66(1:6,1:6) * InputFileData%InpBl%station_eta(k-1)
                         ENDIF
                         EXIT
                     ENDIF
                 ENDDO
             ENDDO
         ENDDO
         DEALLOCATE(temp_GL)
         DEALLOCATE(temp_ratio)
     ELSEIF(p%quadrature .EQ. 2) THEN
         CALL AllocAry(p%Stif0_GL,6,6,p%ngp,'Stif0_TZ',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL AllocAry(p%Mass0_GL,6,6,p%ngp,'Mass0_TZ',ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
               call cleanup()
               return
            end if
         p%Stif0_GL(:,:,:) = 0.0_BDKi
         p%Mass0_GL(:,:,:) = 0.0_BDKi
  
         temp_id = 0
         id0 = 1
         id1 = InputFileData%kp_member(1)
             
         DO j = 1,p%ngp
             IF( j .EQ. 1) THEN
                 p%Stif0_GL(1:6,1:6,temp_id*p%refine + j) = InputFileData%InpBl%stiff0(1:6,1:6,temp_id*p%refine + j)
                 p%Mass0_GL(1:6,1:6,temp_id*p%refine + j) = InputFileData%InpBl%mass0(1:6,1:6,temp_id*p%refine + j)
             ELSE
                 p%Stif0_GL(1:6,1:6,temp_id*p%refine + j) = InputFileData%InpBl%stiff0(1:6,1:6,id0+(j-2)/p%refine) + &
                     ((InputFileData%InpBl%stiff0(1:6,1:6,id0+(j-2)/p%refine + 1) - &
                      InputFileData%InpBl%stiff0(1:6,1:6,id0+(j-2)/p%refine))/p%refine) * &
                     (MOD(j-2,p%refine) + 1)
                     
                 p%Mass0_GL(1:6,1:6,temp_id*p%refine + j) = InputFileData%InpBl%mass0(1:6,1:6,id0+(j-2)/p%refine) + &
                     ((InputFileData%InpBl%mass0(1:6,1:6,id0+(j-2)/p%refine + 1) - &
                      InputFileData%InpBl%mass0(1:6,1:6,id0+(j-2)/p%refine))/p%refine) * &
                     (MOD(j-2,p%refine) + 1)
             ENDIF
         ENDDO
     ENDIF
     
     if (ErrStat >= AbortErrLev) then
        call cleanup()
        return
     end if
     p%Shp(:,:) = 0.0_BDKi
     p%Der(:,:) = 0.0_BDKi
     p%Jacobian(:,:) = 0.0_BDKi
  
     CALL BD_InitShpDerJaco(p%quadrature,p%GL,GLL,p%uuN0,&
             p%node_elem,p%elem_total,p%dof_node,p%ngp,    &
             p%refine,InputFileData%kp_member,             &
             p%Shp,p%Der,p%GLw,p%Jacobian,                 &
             ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
  
        ! calculate rrN0 (Initial relative rotation array)
     DO i = 1,p%elem_total
         CALL BD_NodalRelRot(p%uuN0(:,i),p%node_elem,p%dof_node,p%rrN0(:,i),ErrStat2,ErrMsg2)  !computes p%rrN0(:,i)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ENDDO
            
     p%uu0(:,:)  = 0.0_BDKi
     p%E10(:,:)  = 0.0_BDKi
     DO i = 1,p%elem_total
         DO j = 1,p%ngp
             temp_id = (j-1)*p%dof_node
             temp_id2= (j-1)*(p%dof_node/2)
             CALL BD_GaussPointDataAt0(p%Shp(:,j),p%Der(:,j),p%Jacobian(j,i),&
                     p%uuN0(:,i),p%rrN0(:,i),p%node_elem,p%dof_node,&
                     p%uu0(temp_id +1:temp_id +6,i),&
                     p%E10(temp_id2+1:temp_id2+3,i),ErrStat2,ErrMsg2) !computes p%uu0(temp_id+1:temp_id+6,i) and p%E10(temp_id2+1:temp_id2+3,i)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDDO
     ENDDO
     !CALL WrScr( "Finished reading input" )
  
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
  
           ! compute blade mass, CG, and IN for summary file:
     CALL BD_ComputeBladeMassNew(p%Mass0_GL,p%Gauss,p%elem_total,p%node_elem,&
                                 p%dof_node,p%quadrature,p%ngp,p%GLw,p%Jacobian,&
                                 p%blade_mass,p%blade_CG,p%blade_IN,ErrStat2,ErrMsg2) !computes p%blade_mass,p%blade_CG,p%blade_IN
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        
  ! Actuator
     p%UsePitchAct = InputFileData%UsePitchAct
     
     if (p%UsePitchAct) then
     
        p%pitchK = InputFileData%pitchK 
        p%pitchC = InputFileData%pitchC 
        p%pitchJ = InputFileData%pitchJ 
        
           ! calculate (I-hA)^-1
        
        p%torqM(1,1) =  p%pitchJ + p%pitchC*p%dt
        p%torqM(2,1) = -p%pitchK * p%dt
        p%torqM(1,2) =  p%pitchJ * p%dt
        p%torqM(2,2) =  p%pitchJ
        denom        =  p%pitchJ + p%pitchC*p%dt + p%pitchK*p%dt**2
        if (EqualRealNos(denom,0.0_BDKi)) then
           call SetErrStat(ErrID_Fatal,"Cannot invert matrix for pitch actuator: J+c*dt+k*dt^2 is zero.",ErrStat,ErrMsg,RoutineName)
        else         
           p%torqM(:,:) =  p%torqM / denom
        end if
                 
        TmpDCM(:,:) = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
        temp_CRV(:) = EulerExtract(TmpDCM)
        xd%thetaP = -temp_CRV(3)    
        xd%thetaPD = 0.0_BDKi
     end if   
  ! END Actuator
                       
     
        ! Define and initialize system inputs (set up and initialize input meshes) here:
     call Init_u(InitInp, p, u, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
              
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        ! allocate and initialize continuous states (need to do this after initializing inputs):
     call Init_ContinuousStates(p, u, x, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        ! allocate and initialize other states:
     call Init_OtherStates(p, OtherState, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                             
        ! initialize outputs (need to do this after initializing inputs and continuous states)      
     call Init_y(p, x, u, y, ErrStat2, ErrMsg2)
        call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        
        ! allocate and initialize misc vars (do this after initializing input and output meshes):
     call Init_MiscVars(p, u, y, MiscVar, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
        
        ! set initializaiton outputs
     call SetInitOut(p, InitOut, errStat, errMsg)
        call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                    
     !...............................................
              
         ! Print the summary file if requested:
     if (InputFileData%SumPrint) then
        call BD_PrintSum( p, u, y, x, MiscVar, InitInp%RootName, ErrStat2, ErrMsg2 )
        call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )      
     end if
        
     !...............................................
     
     z%DummyConstrState = 0.0_BDKi
     
     call Cleanup()
        
     return
  contains
        subroutine Cleanup()
     
           if (allocated(GLL       )) deallocate(GLL       )
           if (allocated(temp_GL   )) deallocate(temp_GL   )
           if (allocated(temp_w    )) deallocate(temp_w    )
           if (allocated(temp_ratio)) deallocate(temp_ratio)
           if (allocated(SP_Coef   )) deallocate(SP_Coef   )
        
           call BD_DestroyInputFile( InputFileData, ErrStat2, ErrMsg2)
           
        end subroutine Cleanup            
  END SUBROUTINE BD_Init
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine initializes data in the InitOut type, which is returned to the glue code.
  subroutine SetInitOut(p, InitOut, ErrStat, ErrMsg)
  
     type(BD_InitOutputType),       intent(  out)  :: InitOut          !< output data
     type(BD_ParameterType),        intent(in   )  :: p                !< 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                 ! loop counter
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'SetInitOut'
     
     
     
        ! Initialize variables for this routine
  
     errStat = ErrID_None
     errMsg  = ""
     
     call AllocAry( InitOut%WriteOutputHdr, p%numOuts, 'WriteOutputHdr', errStat2, errMsg2 )
        call SetErrStat( errStat2, errMsg2, errStat, errMsg, RoutineName )
     
     call AllocAry( InitOut%WriteOutputUnt, p%numOuts, 'WriteOutputUnt', errStat2, errMsg2 )
        call SetErrStat( errStat2, errMsg2, errStat, errMsg, RoutineName )
        
     if (ErrStat >= AbortErrLev) return
     
     do i=1,p%NumOuts
        InitOut%WriteOutputHdr(i) = p%OutParam(i)%Name
        InitOut%WriteOutputUnt(i) = p%OutParam(i)%Units
     end do
           
     InitOut%Ver = BeamDyn_Ver
     
  end subroutine SetInitOut
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine allocates and initializes most (not all) of the parameters used in BeamDyn.
  subroutine SetParameters(InitInp, InputFileData, p, ErrStat, ErrMsg)
     type(BD_InitInputType),       intent(in   )  :: InitInp           !< Input data for initialization routine
     type(BD_InputFile),           intent(in   )  :: InputFileData     !< data from the input file
     type(BD_ParameterType),       intent(inout)  :: p                 !< Parameters  ! intent(out) only because it changes p%NdIndx
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
  
     
     !local variables
     REAL(BDKi)                                   :: TmpPos(3)
     REAL(BDKi)                                   :: temp_POS(3)  
           
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'SetParameters'
     
     
     
     ErrStat = ErrID_None
     ErrMsg  = ""   
     
              
     !Read inputs from Driver/Glue code
     !1 Global position vector
     !2 Global rotation tensor
     !3 Gravity vector
     p%GlbPos(1)     = InitInp%GlbPos(3)
     p%GlbPos(2)     = InitInp%GlbPos(1)
     p%GlbPos(3)     = InitInp%GlbPos(2)
  
     p%GlbRot = TRANSPOSE(InitInp%GlbRot)
     CALL BD_CrvExtractCrv(p%GlbRot,TmpPos,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     p%Glb_crv(1) = TmpPos(3)
     p%Glb_crv(2) = TmpPos(1)
     p%Glb_crv(3) = TmpPos(2)
     CALL BD_CrvMatrixR(p%Glb_crv,p%GlbRot,ErrStat2,ErrMsg2) !given p%Glb_crv, returns p%GlbRot
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     temp_POS(1) = InitInp%gravity(3)
     temp_POS(2) = InitInp%gravity(1)
     temp_POS(3) = InitInp%gravity(2)
     p%gravity = MATMUL(TRANSPOSE(p%GlbRot),temp_POS)
  
             
     !....................
     ! data copied/derived from input file
     !....................
     ! Analysis type: 1 Static 2 Dynamic
     p%analysis_type  = InputFileData%analysis_type
     ! Numerical damping coefficient: [0,1].
     ! No numerical damping if rhoinf = 1; maximum numerical damping if rhoinf = 0.
     p%rhoinf = InputFileData%rhoinf
     ! Time step size
     p%dt = InputFileData%DTBeam
     ! Compute generalized-alpha time integrator coefficients given rhoinf
     CALL BD_TiSchmComputeCoefficients(p%rhoinf,p%dt,p%coef, ErrStat2, ErrMsg2) !requires p%rhoinf,p%dt; sets p%coef
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ! Maximum number of iterations in Newton-Ralphson algorithm
     p%niter = InputFileData%NRMax
     ! Tolerance used in stopping criterion
     p%tol = InputFileData%stop_tol
     ! Total number of elements
     p%elem_total = InputFileData%member_total      
     ! Total number of key points
     !p%kp_total = InputFileData%kp_total  !bjj: not used
        
     ! Number of nodes per elelemt
     p%node_elem  = InputFileData%order_elem + 1   
     ! Factorization frequency
     p%n_fact = InputFileData%n_fact
     ! Quadrature method: 1 Gauss 2 Trapezoidal
     p%quadrature = InputFileData%quadrature
     
     IF(p%quadrature .EQ. 1) THEN
         ! Number of Gauss points
         p%ngp = p%node_elem !- 1
     ELSEIF(p%quadrature .EQ. 2) THEN 
         p%refine = InputFileData%refine
         p%ngp = (InputFileData%kp_member(1) - 1)*p%refine + 1
     ENDIF
  
     ! Degree-of-freedom (DoF) per node
     p%dof_node   = 6
     ! Total number of (finite element) nodes
     p%node_total  = p%elem_total*(p%node_elem-1) + 1         
     ! Total number of (finite element) dofs
     p%dof_total   = p%node_total*p%dof_node   
     
     p%dof_elem = p%dof_node   * p%node_elem
     p%rot_elem = p%dof_node/2 * p%node_elem
  
        
     !................................
     ! allocate some parameter arrays
     !................................
     CALL AllocAry(p%member_length, InputFileData%member_total,2,'member length array', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%segment_length,InputFileData%kp_total-1,  3,'segment length array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     CALL AllocAry(p%Shp,     p%node_elem,p%ngp,       'p%Shp',     ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%Der,     p%node_elem,p%ngp,       'p%Der',     ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%Jacobian,p%ngp,      p%elem_total,'p%Jacobian',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     CALL AllocAry(p%uuN0, p%dof_node*p%node_elem,   p%elem_total,'uuN0 (initial position) array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
     CALL AllocAry(p%rrN0,(p%dof_node*p%node_elem)/2,p%elem_total,'p%Nrr0',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%uu0,  p%dof_node*p%ngp,         p%elem_total,'p%uu0', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%E10,  3*p%ngp,                  p%elem_total,'p%E10', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     ! Quadrature point and weight arrays in natural frame
     !    If Gauss: Gauss points and weights
     !    If Trapezoidal: quadrature points and weights
     CALL AllocAry(p%GL, p%ngp,'p%GL',              ErrStat2,ErrMsg2) ; CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(p%GLw,p%ngp,'p%GLw weight array',ErrStat2,ErrMsg2) ; CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     !bjj: these values aren't used anywhere
     !! Key point coordinates
     !CALL AllocAry(p%kp_coordinate,p%kp_total,3,'Key point coordinates array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     !! Number of key points in each member
     !CALL AllocAry(p%kp_member,InputFileData%member_total,'Number of key points in each memeber',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     
     if (ErrStat >= AbortErrLev) return
           
     !p%kp_coordinate = InputFileData%kp_coordinate(:,1:3)
     !p%kp_member     = InputFileData%kp_member
     
     !...............................................      
     ! Physical damping flag and 6 damping coefficients
     !...............................................
     p%damp_flag  = InputFileData%InpBl%damp_flag
     p%beta       = InputFileData%InpBl%beta
        
     !...............................................
     ! set parameters for pitch actuator:
     !...............................................
        
        
        
     !...............................................
     ! set parameters for File I/O data:
     !...............................................
        ! allocate arry for mapping PointLoad to BldMotion (for writeOutput); array is initialized in Init_y
     CALL AllocAry(p%NdIndx,p%node_total,'p%NdIndx',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
              
     p%OutFmt    = InputFileData%OutFmt
     
     p%numOuts   = InputFileData%NumOuts  
     p%NNodeOuts = InputFileData%NNodeOuts      
     p%OutNd     = InputFileData%OutNd
  
     p%OutInputs = .false.  ! will get set to true in SetOutParam if we request the inputs as output values
  
     call SetOutParam(InputFileData%OutList, p, ErrStat2, ErrMsg2 ) ! requires: p%NumOuts, p%NNodeOuts, p%UsePitchAct; sets: p%OutParam.
        call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
        if (ErrStat >= AbortErrLev) return        
        
  end subroutine SetParameters
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> this routine initializes the outputs, y, that are used in the BeamDyn interface for coupling in the FAST framework.
  subroutine Init_y( p, x, u, y, ErrStat, ErrMsg)
  
     type(BD_ParameterType),       intent(inout)  :: p                 !< Parameters  -- intent(out) only because it changes p%NdIndx
     type(BD_ContinuousStateType), intent(in   )  :: x                 !< Continuous states
     type(BD_InputType),           intent(inout)  :: u                 !< Inputs
     type(BD_OutputType),          intent(inout)  :: y                 !< Outputs
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
  
        ! local variables
     real(R8Ki)                                   :: DCM(3,3)          ! must be same type as mesh orientation fields
     real(ReKi)                                   :: Pos(3)            ! must be same type as mesh position fields 
     
     real(BDKi)                                   :: TmpDCM(3,3)
     real(BDKi)                                   :: temp_POS(3)
     real(BDKi)                                   :: temp_CRV(3)
     
     integer(intKi)                               :: temp_id        
     integer(intKi)                               :: i,j,indx          ! loop counters
     integer(intKi)                               :: NNodes            ! number of nodes in mesh
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'Init_y'
     
     
     
     ErrStat = ErrID_None
     ErrMsg  = ""   
     
     !.................................
     ! y%BldForce (used only for WriteOutput)
     !.................................
     CALL MeshCopy ( SrcMesh  = u%PointLoad      &
                   , DestMesh = y%BldForce       &
                   , CtrlCode = MESH_SIBLING     &
                   , IOS      = COMPONENT_OUTPUT &
                   , Force    = .TRUE.           &
                   , Moment   = .TRUE.           &
                   , ErrStat  = ErrStat2         &
                   , ErrMess  = ErrMsg2          )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) RETURN
        ! initialization (not necessary)
     !y%BldForce%Force(:,:)  = 0.0_BDKi
     !y%BldForce%Moment(:,:) = 0.0_BDKi
        
     !.................................
     ! y%ReactionForce (for coupling with ElastoDyn)
     !.................................
        
     CALL MeshCopy( SrcMesh   = u%RootMotion     &
                   , DestMesh = y%ReactionForce  &
                   , CtrlCode = MESH_SIBLING     &
                   , IOS      = COMPONENT_OUTPUT &
                   , Force    = .TRUE.           &
                   , Moment   = .TRUE.           &
                   , ErrStat  = ErrStat2         &
                   , ErrMess  = ErrMsg2          )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) RETURN
           
        ! initialization (not necessary)
        
     !y%ReactionForce%Force(:,:)  = 0.0_BDKi
     !y%ReactionForce%Moment(:,:) = 0.0_BDKi
  
        
     !.................................
     ! y%BldMotion (for coupling with AeroDyn)
     !.................................
        
     NNodes = p%node_elem*p%elem_total
     CALL MeshCreate( BlankMesh        = y%BldMotion        &
                     ,IOS              = COMPONENT_OUTPUT   &
                     ,NNodes           = NNodes             &
                     ,TranslationDisp  = .TRUE.             &
                     ,Orientation      = .TRUE.             &
                     ,TranslationVel   = .TRUE.             &
                     ,RotationVel      = .TRUE.             &
                     ,TranslationAcc   = .TRUE.             &
                     ,RotationAcc      = .TRUE.             &
                     ,ErrStat          = ErrStat2           &
                     ,ErrMess          = ErrMsg2             )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) RETURN
     
     
        ! position nodes
     DO i=1,p%elem_total
        DO j=1,p%node_elem
           
             temp_id = (j-1)*p%dof_node
             
             temp_POS = p%GlbPos + MATMUL(p%GlbRot,p%uuN0(temp_id+1:temp_id+3,i))
             Pos(1) = temp_POS(2)
             Pos(2) = temp_POS(3)
             Pos(3) = temp_POS(1)
             
             temp_CRV = MATMUL(p%GlbRot,p%uuN0(temp_id+4:temp_id+6,i))
             CALL BD_CrvCompose(temp_POS,p%Glb_crv,temp_CRV,0,ErrStat2,ErrMsg2) !given p%Glb_crv and temp_CRV, returns temp_POS
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                
             temp_CRV(1) = temp_POS(2)
             temp_CRV(2) = temp_POS(3)
             temp_CRV(3) = temp_POS(1)
             
             CALL BD_CrvMatrixR(temp_CRV,TmpDCM,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )  
                
                ! possible type conversions here:
             DCM = TRANSPOSE(TmpDCM)
             
             temp_id = (i-1)*p%node_elem+j
             CALL MeshPositionNode ( Mesh    = y%BldMotion   &
                                    ,INode   = temp_id       &
                                    ,Pos     = Pos           &
                                    ,ErrStat = ErrStat2      &
                                    ,ErrMess = ErrMsg2       &
                                    ,Orient  = DCM           )
              CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
             
         ENDDO
     ENDDO
     
        ! create elements and create index array
     
     !NNodes = p%node_elem*p%elem_total
     p%NdIndx(1) = 1
     indx = 2
     DO i=1,NNodes-1
        
        if (.not. equalRealNos( REAL( TwoNorm( y%BldMotion%Position(:,i)-y%BldMotion%Position(:,i+1) ), SiKi ), 0.0_SiKi ) ) then
           p%NdIndx(indx) = i + 1
           indx = indx + 1;
           ! do not connect nodes that are collocated
            CALL MeshConstructElement( Mesh     = y%BldMotion      &
                                      ,Xelement = ELEMENT_LINE2    &
                                      ,P1       = i                &
                                      ,P2       = i+1              &
                                      ,ErrStat  = ErrStat2         &
                                      ,ErrMess  = ErrMsg2          )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        end if
        
     ENDDO
           
              
     CALL MeshCommit ( Mesh    = y%BldMotion     &
                      ,ErrStat = ErrStat2        &
                      ,ErrMess = ErrMsg2         )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
        if (ErrStat>=AbortErrLev) RETURN
     
     
        ! initialization (used for initial guess to AeroDyn)
  
     CALL Set_BldMotion_NoAcc(p, x, y, ErrStat2, ErrMsg2)        
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
     !y%BldMotion%TranslationDisp = 0.0_BDKi
     !y%BldMotion%Orientation     = y%BldMotion%RefOrientation
     !y%BldMotion%TranslationVel  = 0.0_BDKi
     !y%BldMotion%RotationVel     = 0.0_BDKi
     y%BldMotion%TranslationAcc  = 0.0_BDKi
     y%BldMotion%RotationAcc     = 0.0_BDKi
     
     
     !.................................
     ! y%WriteOutput (for writing columns to output file)
     !.................................
     
     call AllocAry( y%WriteOutput, p%numOuts, 'WriteOutput', errStat2, errMsg2 )
        call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     
  end subroutine Init_y
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> this routine initializes the inputs, u, that are used in the BeamDyn interface for coupling in the FAST framework.
  subroutine Init_u( InitInp, p, u, ErrStat, ErrMsg )
  
     type(BD_InitInputType),       intent(in   )  :: InitInp           !< Input data for initialization routine
     type(BD_ParameterType),       intent(in   )  :: p                 !< Parameters
     type(BD_InputType),           intent(inout)  :: u                 !< Inputs
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
     
     
     real(R8Ki)                                   :: DCM(3,3)          ! must be same type as mesh orientation fields
     real(ReKi)                                   :: Pos(3)            ! must be same type as mesh position fields
  
     real(BDKi)                                   :: TmpDCM(3,3)
     real(BDKi)                                   :: temp_POS(3)
     real(BDKi)                                   :: temp_CRV(3)
     
     integer(intKi)                               :: temp_id           
     integer(intKi)                               :: i,j               ! loop counters
     integer(intKi)                               :: NNodes            ! number of nodes in mesh
     
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'Init_u'
     
     ErrStat = ErrID_None
     ErrMsg  = ""
     
     !.................................
     ! u%HubMotion (from ElastoDyn for pitch actuator)
     !.................................
  
     CALL MeshCreate( BlankMesh        = u%HubMotion        &
                     ,IOS              = COMPONENT_INPUT    &
                     ,NNodes           = 1                  &
                     , TranslationDisp = .TRUE.             &
                     , Orientation     = .TRUE.             &
                     ,ErrStat          = ErrStat2           &
                     ,ErrMess          = ErrMsg2            )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) return
        
        ! possible type conversions here:
     DCM = InitInp%HubRot 
     Pos = InitInp%HubPos
     CALL MeshPositionNode ( Mesh    = u%HubMotion          &
                           , INode   = 1                    &
                           , Pos     = Pos                  &
                           , ErrStat = ErrStat2             &
                           , ErrMess = ErrMsg2              &
                           , Orient  = DCM                  )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     CALL MeshConstructElement ( Mesh = u%HubMotion         &
                               , Xelement = ELEMENT_POINT   &
                               , P1       = 1               &
                               , ErrStat  = ErrStat2        &
                               , ErrMess  = ErrMsg2         )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     CALL MeshCommit(u%HubMotion, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
     
        ! initial guesses
     u%HubMotion%TranslationDisp(1:3,1) = 0.0_ReKi
     u%HubMotion%Orientation(1:3,1:3,1) = InitInp%HubRot
        
     !.................................
     ! u%RootMotion (for coupling with ElastoDyn)
     !.................................
  
     CALL MeshCreate( BlankMesh        = u%RootMotion          &
                     ,IOS              = COMPONENT_INPUT       &
                     ,NNodes           = 1                     &
                     , TranslationDisp = .TRUE.                &
                     , TranslationVel  = .TRUE.                &
                     , TranslationAcc  = .TRUE.                &
                     , Orientation     = .TRUE.                &
                     , RotationVel     = .TRUE.                &
                     , RotationAcc     = .TRUE.                &
                     ,ErrStat         = ErrStat2               &
                     ,ErrMess         = ErrMsg2                )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) return
              
    !! ! place single node at origin; position affects mapping/coupling with other modules
    !! temp_POS(:) = p%GlbPos(1:3) + MATMUL(p%GlbRot,p%uuN0(1:3,1))
    !! Pos(1) = temp_POS(2)
    !! Pos(2) = temp_POS(3)
    !! Pos(3) = temp_POS(1)
    !!
    !! temp_CRV(1) = p%Glb_crv(2)
    !! temp_CRV(2) = p%Glb_crv(3)
    !! temp_CRV(3) = p%Glb_crv(1)
    !!CALL BD_CrvMatrixR(temp_CRV,TmpDCM,ErrStat2,ErrMsg2)
    !!    CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
    !! DCM = TRANSPOSE(TmpDCM)
        
     DCM = InitInp%GlbRot 
     Pos = InitInp%GlbPos      
     CALL MeshPositionNode ( Mesh    = u%RootMotion &
                           , INode   = 1            &
                           , Pos     = Pos          &
                           , ErrStat = ErrStat2     &
                           , ErrMess = ErrMsg2      &
                           , Orient  = DCM          ) 
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
           
     CALL MeshConstructElement ( Mesh     = u%RootMotion       &
                               , Xelement = ELEMENT_POINT      &
                               , P1       = 1                  &
                               , ErrStat  = ErrStat2           &
                               , ErrMess  = ErrMsg2            )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
     
     CALL MeshCommit ( Mesh    = u%RootMotion    &
                      ,ErrStat = ErrStat2        &
                      ,ErrMess = ErrMsg2         )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        ! initial guesses
     u%RootMotion%TranslationDisp(1:3,1) = InitInp%RootDisp(1:3)
     u%RootMotion%Orientation(1:3,1:3,1) = InitInp%RootOri(1:3,1:3)
     u%RootMotion%TranslationVel(1:3,1)  = InitInp%RootVel(1:3)
     u%RootMotion%RotationVel(1:3,1)     = InitInp%RootVel(4:6)
     u%RootMotion%TranslationAcc(:,:)    = 0.0_ReKi
     u%RootMotion%RotationAcc(:,:)       = 0.0_ReKi
              
     !.................................
     ! u%PointLoad (currently not used in FAST)
     !.................................
     
     CALL MeshCreate( BlankMesh       = u%PointLoad      &
                     ,IOS             = COMPONENT_INPUT  &
                     ,NNodes          = p%node_total     &
                     ,Force           = .TRUE.           &
                     ,Moment          = .TRUE.           &
                     ,ErrStat         = ErrStat2         &
                     ,ErrMess         = ErrMsg2          )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat>=AbortErrLev) return
  
     DO i=1,p%elem_total
         DO j=1,p%node_elem
             temp_id = (j-1) * p%dof_node
             temp_POS = p%GlbPos(1:3) + MATMUL(p%GlbRot,p%uuN0(temp_id+1:temp_id+3,i))
             Pos(1) = temp_POS(2)
             Pos(2) = temp_POS(3)
             Pos(3) = temp_POS(1)
             
             temp_CRV = MATMUL(p%GlbRot,p%uuN0(temp_id+4:temp_id+6,i))
             CALL BD_CrvCompose(temp_POS,p%Glb_crv,temp_CRV,0,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
             temp_CRV(1) = temp_POS(2)
             temp_CRV(2) = temp_POS(3)
             temp_CRV(3) = temp_POS(1)
             CALL BD_CrvMatrixR(temp_CRV,TmpDCM,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
             DCM = TRANSPOSE(TmpDCM)
             
             temp_id = (i-1)*(p%node_elem-1)+j           
             CALL MeshPositionNode ( Mesh    = u%PointLoad  &
                                    ,INode   = temp_id      &
                                    ,Pos     = Pos          &
                                    ,ErrStat = ErrStat2     &
                                    ,ErrMess = ErrMsg2      &
                                    , Orient = DCM ) 
                 CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDDO
     ENDDO      
        
     DO i=1,p%node_total
         CALL MeshConstructElement( Mesh     = u%PointLoad      &
                                   ,Xelement = ELEMENT_POINT    &
                                   ,P1       = i                &
                                   ,ErrStat  = ErrStat2         &
                                   ,ErrMess  = ErrMsg2          )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         
     ENDDO
        
     CALL MeshCommit ( Mesh    = u%PointLoad     &
                      ,ErrStat = ErrStat2        &
                      ,ErrMess = ErrMsg2         )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
        ! initial guesses
     u%PointLoad%Force  = 0.0_ReKi
     u%PointLoad%Moment = 0.0_ReKi      
        
     !.................................
     ! u%DistrLoad (for coupling with AeroDyn)
     !.................................
              
     NNodes = size(p%Gauss,2)   
     CALL MeshCreate( BlankMesh  = u%DistrLoad      &
                     ,IOS        = COMPONENT_INPUT  &
                     ,NNodes     = NNodes           &
                     ,Force      = .TRUE.           &
                     ,Moment     = .TRUE.           &
                     ,ErrStat    = ErrStat2         &
                     ,ErrMess    = ErrMsg2          )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
        if (ErrStat>=AbortErrLev) return
  
     DO i=1,NNodes
        temp_POS(1:3) = p%GlbPos(1:3) + MATMUL(p%GlbRot,p%Gauss(1:3,i))
        Pos(1) = temp_POS(2)
        Pos(2) = temp_POS(3)
        Pos(3) = temp_POS(1)
        
        temp_CRV(:) = MATMUL(p%GlbRot,p%Gauss(4:6,i))
        CALL BD_CrvCompose(temp_POS,p%Glb_crv,temp_CRV,0,ErrStat2,ErrMsg2)
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        temp_CRV(1) = temp_POS(2)
        temp_CRV(2) = temp_POS(3)
        temp_CRV(3) = temp_POS(1)
        CALL BD_CrvMatrixR(temp_CRV,TmpDCM,ErrStat2,ErrMsg2)
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        DCM = TRANSPOSE(TmpDCM)
        
        CALL MeshPositionNode ( Mesh    = u%DistrLoad  &
                               ,INode   = i            &
                               ,Pos     = Pos          &
                               ,ErrStat = ErrStat2     &
                               ,ErrMess = ErrMsg2      &
                               ,Orient  = DCM ) 
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ENDDO   
     
     DO i=1,NNodes-1
        CALL MeshConstructElement( Mesh      = u%DistrLoad      &
                                   ,Xelement = ELEMENT_LINE2    &
                                   ,P1       = i                &
                                   ,P2       = i+1              &
                                   ,ErrStat  = ErrStat2         &
                                   ,ErrMess  = ErrMsg2          )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ENDDO
  
     
     CALL MeshCommit ( Mesh    = u%DistrLoad     &
                      ,ErrStat = ErrStat2        &
                      ,ErrMess = ErrMsg2         )
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
        ! initial guesses
     u%DistrLoad%Force  = 0.0_ReKi
     u%DistrLoad%Moment = 0.0_ReKi      
     
        
  end subroutine Init_u
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> this subroutine initializes the misc variables.
  subroutine Init_MiscVars( p, u, y, m, ErrStat, ErrMsg )
     type(BD_ParameterType),       intent(in   )  :: p                 !< Parameters
     type(BD_InputType),           intent(inout)  :: u                 !< Inputs   ! intent(out) because I'm copying it
     type(BD_OutputType),          intent(inout)  :: y                 !< Outputs  ! intent(out) because I'm copying it
     type(BD_MiscVarType),         intent(inout)  :: m                 !< misc/optimization variables
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
     
     
     
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'Init_OtherStates'
     
     ErrStat = ErrID_None
     ErrMsg  = ""
     
     m%Un_Sum = -1
     !if (p%analysis_type == BD_DYNAMIC_ANALYSIS) then
           ! allocate arrays at initialization so we don't waste so much time on memory allocation/deallocation on each step
        
        CALL AllocAry(m%StifK_LU,p%dof_total-6,p%dof_total-6,'StifK_LU',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%RHS_LU,  p%dof_total-6,              'RHS_LU',  ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        CALL AllocAry(m%StifK,p%dof_total,p%dof_total,'StifK',      ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%MassM,p%dof_total,p%dof_total,'MassM',      ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%RHS,              p%dof_total,'RHS',        ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%F_PointLoad,      p%dof_total,'F_PointLoad',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%indx,             p%dof_total,'indx',       ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        CALL AllocAry(m%Nuuu, p%dof_elem,            'Nuuu', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%Nrrr, p%rot_elem,            'Nrrr', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%Nvvv, p%dof_elem,            'Nvvv', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%Naaa, p%dof_elem,            'Naaa', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%elf,  p%dof_elem,             'elf', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%elk,  p%dof_elem,p%dof_elem,  'elk', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%elg,  p%dof_elem,p%dof_elem,  'elg', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%elm,  p%dof_elem,p%dof_elem,  'elm', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%EStif0_GL, 6,6,p%ngp,   'EStif0_GL', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%EMass0_GL, 6,6,p%ngp,   'EMass0_GL', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL AllocAry(m%DistrLoad_GL,6,p%ngp,'DistrLoad_GL', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
              
     !end if
     
     ! create copy of u%DistrLoad at y%BldMotion locations (for WriteOutput only)
     if (p%OutInputs) then
        
           ! y%BldMotion and u%DistrLoad are not siblings, so we need to create  
           ! mapping to output u%DistrLoad values at y%BldMotion nodes 
           ! (not making these new meshes siblings of old ones because FAST needs to
           ! do this same trick and we can't have more than one sibling of a mesh)
                   
        CALL MeshCopy ( SrcMesh  = y%BldMotion                  &
                      , DestMesh = m%u_DistrLoad_at_y           &
                      , CtrlCode = MESH_COUSIN                  &  ! Like a sibling, except using new memory for position/refOrientation and elements
                      , IOS      = COMPONENT_INPUT              &
                      , Force    = .TRUE.                       &
                      , Moment   = .TRUE.                       &
                      , ErrStat  = ErrStat2                     &
                      , ErrMess  = ErrMsg2                      )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        CALL MeshCopy ( SrcMesh  = u%DistrLoad                  &
                      , DestMesh = m%y_BldMotion_at_u           &
                      , CtrlCode = MESH_COUSIN                  &
                      , IOS      = COMPONENT_OUTPUT             &
                      , Orientation     = .TRUE.                &
                      , TranslationDisp = .TRUE.                &
                      , ErrStat         = ErrStat2              &
                      , ErrMess         = ErrMsg2               )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
           
        CALL MeshMapCreate( u%DistrLoad, m%u_DistrLoad_at_y, m%Map_u_DistrLoad_to_y, ErrStat2, ErrMsg2 )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        CALL MeshMapCreate( y%BldMotion, m%y_BldMotion_at_u, m%Map_y_BldMotion_to_u, ErrStat2, ErrMsg2 )
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
                 
        m%u_DistrLoad_at_y%remapFlag = .false.
        m%y_BldMotion_at_u%remapFlag = .false.
     end if
        
        ! create copy of inputs, which will be converted to internal BeamDyn coordinate system 
        ! (put in miscVars so we don't have to create new meshes each call)
     CALL BD_CopyInput(u, m%u, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     CALL BD_CopyInput(u, m%u2, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        
  end subroutine Init_MiscVars   
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> this subroutine initializes the other states.
  subroutine Init_OtherStates( p, OtherState, ErrStat, ErrMsg )
     type(BD_ParameterType),       intent(in   )  :: p                 !< Parameters
     type(BD_OtherStateType),      intent(inout)  :: OtherState        !< Other states
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
     
     
     
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'Init_OtherStates'
     
     ErrStat = ErrID_None
     ErrMsg  = ""
  
          
     ! Allocate other states: Acceleration and algorithm accelerations for generalized-alpha time integator
     CALL AllocAry(OtherState%acc,p%dof_total,'OtherState%acc',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(OtherState%xcc,p%dof_total,'OtherState%xcc',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     if (ErrStat>=AbortErrLev) return   
              
     OtherState%InitAcc = .false. ! accelerations have not been initialized, yet
        
     OtherState%acc(:) = 0.0_BDKi
     OtherState%xcc(:) = 0.0_BDKi
              
  end subroutine Init_OtherStates   
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> this subroutine initializes the continuous states.
  subroutine Init_ContinuousStates( p, u, x, ErrStat, ErrMsg )
     type(BD_ParameterType),       intent(inout)  :: p                 !< Parameters !sets the copy-of-state values
     type(BD_InputType),           intent(inout)  :: u                 !< Inputs  !intent(out) because of mesh copy, otherwise not changed
     type(BD_ContinuousStateType), intent(inout)  :: x                 !< Continuous states
     integer(IntKi),               intent(  out)  :: ErrStat           !< Error status of the operation
     character(*),                 intent(  out)  :: ErrMsg            !< Error message if ErrStat /= ErrID_None
     
     
     TYPE(BD_InputType)                           :: u_tmp             ! A copy of the initial input (guess), converted to BeamDyn internal coordinates
     integer(intKi)                               :: ErrStat2          ! temporary Error status
     character(ErrMsgLen)                         :: ErrMsg2           ! temporary Error message
     character(*), parameter                      :: RoutineName = 'Init_ContinuousStates'
     
     
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     ! Allocate continuous states
     CALL AllocAry(x%q,      p%dof_total,'x%q',      ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(x%dqdt,   p%dof_total,'x%dqdt',   ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
     x%q(:)    = 0.0_BDKi
     x%dqdt(:) = 0.0_BDKi
           
     
        ! create copy of inputs, u, to convert to BeamDyn-internal system inputs, u_tmp, which is used to initialize states:
     CALL BD_CopyInput(u, u_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
        ! convert to BeamDyn-internal system inputs, u_tmp:      
     CALL BD_InputGlobalLocal(p,u_tmp,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     
        ! initialize states, given parameters and initial inputs (in BD coordinates)
     CALL BD_CalcIC(u_tmp,p,x,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     call cleanup()
     
  contains
     subroutine cleanup()
        call BD_DestroyInput( u_tmp, ErrStat2, ErrMsg2)
     end subroutine cleanup   
  end subroutine Init_ContinuousStates   
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This routine is called at the end of the simulation.
  SUBROUTINE BD_End( u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg )
  
     TYPE(BD_InputType),           INTENT(INOUT)  :: u           !< System inputs
     TYPE(BD_ParameterType),       INTENT(INOUT)  :: p           !< Parameters
     TYPE(BD_ContinuousStateType), INTENT(INOUT)  :: x           !< Continuous states
     TYPE(BD_DiscreteStateType),   INTENT(INOUT)  :: xd          !< Discrete states
     TYPE(BD_ConstraintStateType), INTENT(INOUT)  :: z           !< Constraint states
     TYPE(BD_OtherStateType),      INTENT(INOUT)  :: OtherState  !< Other states
     TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< System outputs
     TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),               INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     ! Place any last minute operations or calculations here:
  
     ! Close files here:
     if (m%Un_Sum > 0) CLOSE(m%Un_Sum)
  
     
     ! Destroy the input data:
  
     CALL BD_DestroyInput( u, ErrStat, ErrMsg )
  
     ! Destroy the parameter data:
  
     CALL BD_DestroyParam( p, ErrStat, ErrMsg )
  
     ! Destroy the state data:
  
     CALL BD_DestroyContState(   x,           ErrStat, ErrMsg )
     CALL BD_DestroyDiscState(   xd,          ErrStat, ErrMsg )
     CALL BD_DestroyConstrState( z,           ErrStat, ErrMsg )
     CALL BD_DestroyOtherState(  OtherState,  ErrStat, ErrMsg )
  
     ! Destroy misc data
     CALL BD_DestroyMisc(  m,  ErrStat, ErrMsg )
     
     ! Destroy the output data:
  
     CALL BD_DestroyOutput( y, ErrStat, ErrMsg )
  
  END SUBROUTINE BD_End
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This is a loose coupling routine for solving constraint states, integrating continuous states, and updating discrete and other 
  !! states. Continuous, constraint, discrete, and other states are updated to values at t + Interval.
  SUBROUTINE BD_UpdateStates( t, n, u, utimes, p, x, xd, z, OtherState, m, ErrStat, ErrMsg )
  
     REAL(DbKi),                      INTENT(IN   ) :: t          !< Current simulation time in seconds
     INTEGER(IntKi),                  INTENT(IN   ) :: n          !< Current simulation time step n = 0,1,...
     TYPE(BD_InputType),              INTENT(INOUT) :: u(:)       !< Inputs at utimes
     REAL(DbKi),                      INTENT(IN   ) :: utimes(:)  !< Times associated with u(:), in seconds
     TYPE(BD_ParameterType),          INTENT(IN   ) :: p          !< Parameters
     TYPE(BD_ContinuousStateType),    INTENT(INOUT) :: x          !< Input: Continuous states at t;
                                                                  !!   Output: Continuous states at t + dt
     TYPE(BD_DiscreteStateType),      INTENT(INOUT) :: xd         !< Input: Discrete states at t;
                                                                  !!   Output: Discrete states at t + dt
     TYPE(BD_ConstraintStateType),    INTENT(INOUT) :: z          !< Input: Initial guess of constraint states at t + dt;
                                                                  !!   Output: Constraint states at t + dt
     TYPE(BD_OtherStateType),         INTENT(INOUT) :: OtherState !< Other states: Other states at t;
                                                                  !!   Output: Other states at t + dt
     TYPE(BD_MiscVarType),            INTENT(INOUT) :: m          !< misc/optimization variables
     INTEGER(IntKi),                  INTENT(  OUT) :: ErrStat    !< Error status of the operation
     CHARACTER(*),                    INTENT(  OUT) :: ErrMsg     !< Error message if ErrStat /= ErrID_None
  
     
     
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     
           
     IF(p%analysis_type == BD_DYNAMIC_ANALYSIS) THEN
         CALL BD_GA2( t, n, u, utimes, p, x, xd, z, OtherState, m, ErrStat, ErrMsg )
     ELSEIF(p%analysis_type == BD_STATIC_ANALYSIS) THEN
         CALL BD_Static( t, n, u, utimes, p, x, xd, z, OtherState, m, ErrStat, ErrMsg )
     ENDIF
  
  END SUBROUTINE BD_UpdateStates
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> Routine for computing outputs, used in both loose and tight coupling.
  SUBROUTINE BD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg )
  
     REAL(DbKi),                   INTENT(IN   )  :: t           !< Current simulation time in seconds
     TYPE(BD_InputType),           INTENT(INOUT)  :: u           !< Inputs at t
     TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
     TYPE(BD_ContinuousStateType), INTENT(IN   )  :: x           !< Continuous states at t
     TYPE(BD_DiscreteStateType),   INTENT(IN   )  :: xd          !< Discrete states at t
     TYPE(BD_ConstraintStateType), INTENT(IN   )  :: z           !< Constraint states at t
     TYPE(BD_OtherStateType),      INTENT(IN   )  :: OtherState  !< Other states at t
     TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< Outputs computed at t (Input only so that mesh con-
                                                                 !!   nectivity information does not have to be recalculated)
     TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),               INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     TYPE(BD_OtherStateType)                      :: OS_tmp
     TYPE(BD_ContinuousStateType)                 :: x_tmp
     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_cc(3)
     REAL(BDKi)                                   :: temp_R(3,3)
     REAL(BDKi)                                   :: temp6(6)
     REAL(BDKi)                                   :: temp_Force(p%dof_total)
     REAL(ReKi)                                   :: AllOuts(0:MaxOutPts)
     !REAL(BDKi)                                   :: temp_ReactionForce(6)
     INTEGER(IntKi)                               :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                         :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                      :: RoutineName = 'BD_CalcOutput'
     
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     AllOuts = 0.0_ReKi
  
     CALL BD_CopyContState(x, x_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CopyOtherState(OtherState, OS_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
     CALL BD_CopyInput(u, m%u, MESH_UPDATECOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     ! Actuator
     IF( p%UsePitchAct ) THEN
         CALL PitchActuator_SetBC(p, m%u, xd, AllOuts)
     ENDIF
     ! END Actuator
     
     CALL BD_CopyInput(m%u, m%u2, MESH_UPDATECOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
  
        ! set y%BldMotion%TranslationDisp, y%BldMotion%Orientation, y%BldMotion%TranslationVel, and y%BldMotion%RotationVel:
     CALL Set_BldMotion_NoAcc(p, x, y, ErrStat2, ErrMsg2)      
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
        ! calculate y%BldMotion accelerations:
     CALL BD_InputGlobalLocal(p,m%u,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_BoundaryGA2(x_tmp,p,m%u,t,OS_tmp,m,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     IF(p%analysis_type .EQ. BD_DYNAMIC_ANALYSIS) THEN
         CALL BD_CalcForceAcc(m%u,p,m,x_tmp,OS_tmp,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         DO i=1,p%elem_total
             DO j=1,p%node_elem
                 temp_id = ((i-1)*(p%node_elem-1)+j-1)*p%dof_node
                 temp_id2= (i-1)*p%node_elem+j
                 IF(i .EQ. 1 .AND. j .EQ. 1) THEN
                     temp6(1:3) = m%u%RootMotion%TranslationAcc(1:3,1)
                     temp6(4:6) = m%u%RootMotion%RotationAcc(1:3,1)
                 ELSE
                     temp6(1:3) = OS_tmp%Acc(temp_id+1:temp_id+3)
                     temp6(4:6) = OS_tmp%Acc(temp_id+4:temp_id+6)
                 ENDIF
                     temp_cc(:) = MATMUL(p%GlbRot,temp6(1:3))
                     y%BldMotion%TranslationAcc(1,temp_id2) = temp_cc(2)
                     y%BldMotion%TranslationAcc(2,temp_id2) = temp_cc(3)
                     y%BldMotion%TranslationAcc(3,temp_id2) = temp_cc(1)
                     temp_cc(:) = MATMUL(p%GlbRot,temp6(4:6))
                     y%BldMotion%RotationAcc(1,temp_id2) = temp_cc(2)
                     y%BldMotion%RotationAcc(2,temp_id2) = temp_cc(3)
                     y%BldMotion%RotationAcc(3,temp_id2) = temp_cc(1)
             ENDDO
         ENDDO
         CALL BD_DynamicSolutionForce(x_tmp%q,x_tmp%dqdt,OS_tmp%Acc, m%u, p, m, temp_Force,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ELSEIF(p%analysis_type .EQ. BD_STATIC_ANALYSIS) THEN
         CALL BD_StaticSolutionForce( x%q,x%dqdt,p, m, m%u,temp_Force,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ENDIF
  
     IF(p%analysis_type .EQ. BD_DYNAMIC_ANALYSIS) THEN
         temp6(1:6) = -OS_tmp%Acc(1:6)
         temp_cc(:) = MATMUL(p%GlbRot,temp6(1:3))
         y%ReactionForce%Force(1,1) = temp_cc(2)
         y%ReactionForce%Force(2,1) = temp_cc(3)
         y%ReactionForce%Force(3,1) = temp_cc(1)
         temp_cc(:) = MATMUL(p%GlbRot,temp6(4:6))
         y%ReactionForce%Moment(1,1) = temp_cc(2)
         y%ReactionForce%Moment(2,1) = temp_cc(3)
         y%ReactionForce%Moment(3,1) = temp_cc(1)
     ENDIF
     DO i=1,p%node_total
         temp_id = (i-1)*p%dof_node
         temp6(:) = temp_Force(temp_id+1:temp_id+6)
         temp_cc(:) = MATMUL(p%GlbRot,temp6(1:3))
         y%BldForce%Force(1,i) = temp_cc(2)
         y%BldForce%Force(2,i) = temp_cc(3)
         y%BldForce%Force(3,i) = temp_cc(1)
         temp_cc(:) = MATMUL(p%GlbRot,temp6(4:6))
         y%BldForce%Moment(1,i) = temp_cc(2)
         y%BldForce%Moment(2,i) = temp_cc(3)
         y%BldForce%Moment(3,i) = temp_cc(1)
     ENDDO
  
     !-------------------------------------------------------   
     !  compute RootMxr and RootMyr for ServoDyn and
     !  get values to output to file:  
     !-------------------------------------------------------   
     ! Actuator
     call Calc_WriteOutput( p, AllOuts, y, m, ErrStat2, ErrMsg2 )  !uses m%u2  
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
      
     y%RootMxr = AllOuts( RootMxr )
     y%RootMyr = AllOuts( RootMyr )
     
     !...............................................................................................................................   
     ! Place the selected output channels into the WriteOutput(:) array with the proper sign:
     !...............................................................................................................................   
  
     do i = 1,p%NumOuts  ! Loop through all selected output channels
        y%WriteOutput(i) = p%OutParam(i)%SignM * AllOuts( p%OutParam(i)%Indx )
     end do             ! i - All selected output channels
        
     
     call cleanup()
     return
     
  contains
     subroutine cleanup()
     ! Actuator
        CALL BD_DestroyContState(x_tmp, ErrStat2, ErrMsg2 )
        CALL BD_DestroyOtherState(OS_tmp, ErrStat2, ErrMsg2 )
     end subroutine cleanup 
  END SUBROUTINE BD_CalcOutput
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> Routine for updating discrete states
  SUBROUTINE BD_UpdateDiscState( t, n, u, p, x, xd, z, OtherState, m, ErrStat, ErrMsg )
  
     REAL(DbKi),                        INTENT(IN   )  :: t           !< Current simulation time in seconds
     INTEGER(IntKi),                    INTENT(IN   )  :: n           !< Current step of the simulation: t = n*dt
     TYPE(BD_InputType),                INTENT(IN   )  :: u           !< Inputs at t
     TYPE(BD_ParameterType),            INTENT(IN   )  :: p           !< Parameters
     TYPE(BD_ContinuousStateType),      INTENT(IN   )  :: x           !< Continuous states at t
     TYPE(BD_DiscreteStateType),        INTENT(INOUT)  :: xd          !< Input: Discrete states at t;
                                                                      !!   Output: Discrete states at t + dt
     TYPE(BD_ConstraintStateType),      INTENT(IN   )  :: z           !< Constraint states at t
     TYPE(BD_OtherStateType),           INTENT(IN   )  :: OtherState  !< Other states at t
     TYPE(BD_MiscVarType),              INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),                    INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                      INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     ! local variables
     REAL(BDKi)                                        :: temp_R(3,3) 
     REAL(BDKi)                                        :: Hub_theta_Root(3) 
     REAL(BDKi)                                        :: u_theta_pitch 
     
        ! Initialize ErrStat
  
        ErrStat = ErrID_None
        ErrMsg  = ""
  
        ! Update discrete states here:
        
  ! Actuator
     IF( p%UsePitchAct ) THEN
        !bjj: note that we've cheated a bit here because we have inputs at t+dt
         temp_R = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
         Hub_theta_Root = EulerExtract(temp_R)
         u_theta_pitch = -Hub_theta_Root(3)
                
         xd%thetaP  = p%torqM(1,1)*xd%thetaP + p%torqM(1,2)*xd%thetaPD + &
                                             p%torqM(1,2)*(p%pitchK*p%dt/p%pitchJ)*(-Hub_theta_Root(3))
         xd%thetaPD = p%torqM(2,1)*xd%thetaP + p%torqM(2,2)*xd%thetaPD + &
                                               p%torqM(2,2)*(p%pitchK*p%dt/p%pitchJ)*(-Hub_theta_Root(3))
         
  
     ENDIF
  ! END Actuator      
        
  END SUBROUTINE BD_UpdateDiscState
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> Routine for solving for the residual of the constraint state equations
  SUBROUTINE BD_CalcConstrStateResidual( t, u, p, x, xd, z, OtherState, m, Z_residual, ErrStat, ErrMsg )
  
     REAL(DbKi),                        INTENT(IN   )  :: t           !< Current simulation time in seconds
     TYPE(BD_InputType),                INTENT(IN   )  :: u           !< Inputs at t
     TYPE(BD_ParameterType),            INTENT(IN   )  :: p           !< Parameters
     TYPE(BD_ContinuousStateType),      INTENT(IN   )  :: x           !< Continuous states at t
     TYPE(BD_DiscreteStateType),        INTENT(IN   )  :: xd          !< Discrete states at t
     TYPE(BD_ConstraintStateType),      INTENT(IN   )  :: z           !< Constraint states at t (possibly a guess)
     TYPE(BD_OtherStateType),           INTENT(IN   )  :: OtherState  !< Other states at t
     TYPE(BD_ConstraintStateType),      INTENT(  OUT)  :: Z_residual  !< Residual of the constraint state equations using
                                                                      !!     the input values described above
     TYPE(BD_MiscVarType),              INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),                    INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                      INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
  
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
  
     ! Solve for the constraint states here:
  
     Z_residual%DummyConstrState = 0
  
     END SUBROUTINE BD_CalcConstrStateResidual
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine output elemental nodal quantity vector given global quantity vector
  SUBROUTINE BD_ElemNodalDisp(uu,node_elem,dof_node,nelem,Nu,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: uu(:) 
     INTEGER(IntKi),INTENT(IN   ):: node_elem 
     INTEGER(IntKi),INTENT(IN   ):: dof_node 
     INTEGER(IntKi),INTENT(IN   ):: nelem 
     REAL(BDKi),    INTENT(  OUT):: Nu(:) 
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi):: i ! Index counter
     INTEGER(IntKi):: j ! Index counter
     INTEGER(IntKi):: temp_id1 ! Counter
     INTEGER(IntKi):: temp_id2 ! Counter
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=1,node_elem
         DO j=1,dof_node
             temp_id1 = (i-1)*dof_node+j
             temp_id2 = ((nelem - 1)*(node_elem-1)+i-1)*dof_node+j
             Nu(temp_id1) = uu(temp_id2)
         ENDDO
     ENDDO
  
  END SUBROUTINE BD_ElemNodalDisp
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the relative rotation at each node 
  !! in a finite element with respects to the first node.
  SUBROUTINE BD_NodalRelRot(Nu,node_elem,dof_node,Nr,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: Nu(:) 
     INTEGER(IntKi),INTENT(IN   ):: node_elem
     INTEGER(IntKi),INTENT(IN   ):: dof_node
     REAL(BDKi),    INTENT(  OUT):: Nr(:) 
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i 
     INTEGER(IntKi)              :: k
     INTEGER(IntKi)              :: temp_id 
     REAL(BDKi)                  :: Nu_temp1(3)
     REAL(BDKi)                  :: Nu_temp(3)
     REAL(BDKi)                  :: Nr_temp(3)
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_NodalRelRot'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     Nr = 0.0_BDKi
     Nu_temp1(:) = 0.0_BDKi
     Nu_temp1(:) = Nu(4:6)
     DO i=1,node_elem
         temp_id = (i - 1) * dof_node
         DO k=1,3
             Nu_temp(k) = Nu(temp_id+k+3)
         ENDDO
         CALL BD_CrvCompose(Nr_temp,Nu_temp1,Nu_temp,1,ErrStat2,ErrMsg2) !This doesn't currently throw an error, so I'm going to ignore it
         DO k=1,3
             temp_id = (i-1)*3+k
             Nr(temp_id) = Nr_temp(k)
         ENDDO
     ENDDO
  
  
  END SUBROUTINE BD_NodalRelRot
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_ElementMatrixGA2(Nuuu,Nrrr,Nvvv,Naaa,           &
                                 EStif0_GL,EMass0_GL,gravity,DistrLoad_GL,&
                                 damp_flag,beta,                          &
                                 ngp,gw,hhx,hpx,Jaco,uu0,E10,             &
                                 node_elem,dof_node,fact,elk,elf,elm,elg, &
                                 ErrStat,ErrMsg)
  
     REAL(BDKi),     INTENT(IN   ):: Nuuu(:)
     REAL(BDKi),     INTENT(IN   ):: Nrrr(:)
     REAL(BDKi),     INTENT(IN   ):: Nvvv(:)
     REAL(BDKi),     INTENT(IN   ):: Naaa(:)
     REAL(BDKi),     INTENT(IN   ):: EStif0_GL(:,:,:)
     REAL(BDKi),     INTENT(IN   ):: EMass0_GL(:,:,:)
     REAL(BDKi),     INTENT(IN   ):: gravity(:)
     REAL(BDKi),     INTENT(IN   ):: DistrLoad_GL(:,:)
     INTEGER(IntKi), INTENT(IN   ):: damp_flag
     REAL(BDKi),     INTENT(IN   ):: beta(:)
     INTEGER(IntKi), INTENT(IN   ):: ngp
     REAL(BDKi),     INTENT(IN   ):: gw(:)
     REAL(BDKi),     INTENT(IN   ):: hhx(:,:)
     REAL(BDKi),     INTENT(IN   ):: hpx(:,:)
     REAL(BDKi),     INTENT(IN   ):: Jaco(:)
     REAL(BDKi),     INTENT(IN   ):: uu0(:)
     REAL(BDKi),     INTENT(IN   ):: E10(:)
     INTEGER(IntKi), INTENT(IN   ):: node_elem
     INTEGER(IntKi), INTENT(IN   ):: dof_node
     LOGICAL,        INTENT(IN   ):: fact
     REAL(BDKi),     INTENT(  OUT):: elk(:,:)
     REAL(BDKi),     INTENT(  OUT):: elf(:)
     REAL(BDKi),     INTENT(  OUT):: elm(:,:)
     REAL(BDKi),     INTENT(  OUT):: elg(:,:)
     INTEGER(IntKi), INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),   INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                   :: RR0(3,3)
     REAL(BDKi)                   :: kapa(3)
     REAL(BDKi)                   :: E1(3)
     REAL(BDKi)                   :: Stif(6,6)
     REAL(BDKi)                   :: cet
     REAL(BDKi)                   :: uuu(6)
     REAL(BDKi)                   :: uup(3)
     REAL(BDKi)                   :: Fc(6)
     REAL(BDKi)                   :: Fd(6)
     REAL(BDKi)                   :: Fg(6)
     REAL(BDKi)                   :: Oe(6,6)
     REAL(BDKi)                   :: Pe(6,6)
     REAL(BDKi)                   :: Qe(6,6)
     REAL(BDKi)                   :: Sd(6,6)
     REAL(BDKi)                   :: Od(6,6)
     REAL(BDKi)                   :: Pd(6,6)
     REAL(BDKi)                   :: Qd(6,6)
     REAL(BDKi)                   :: betaC(6,6)
     REAL(BDKi)                   :: Gd(6,6)
     REAL(BDKi)                   :: Xd(6,6)
     REAL(BDKi)                   :: Yd(6,6)
     REAL(BDKi)                   :: vvv(6)
     REAL(BDKi)                   :: vvp(6)
     REAL(BDKi)                   :: aaa(6)
     REAL(BDKi)                   :: mmm
     REAL(BDKi)                   :: mEta(3)
     REAL(BDKi)                   :: rho(3,3)
     REAL(BDKi)                   :: Fi(6)
     REAL(BDKi)                   :: Mi(6,6)
     REAL(BDKi)                   :: Ki(6,6)
     REAL(BDKi)                   :: Gi(6,6)
     INTEGER(IntKi)               :: igp
     INTEGER(IntKi)               :: i
     INTEGER(IntKi)               :: j
     INTEGER(IntKi)               :: m
     INTEGER(IntKi)               :: n
     INTEGER(IntKi)               :: temp_id1
     INTEGER(IntKi)               :: temp_id2
     INTEGER(IntKi)               :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)         :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER      :: RoutineName = 'BD_ElementMatrixGA2'
  
     ErrStat  = ErrID_None
     ErrMsg   = ""
     elk(:,:) = 0.0_BDKi
     elf(:)   = 0.0_BDKi
     elg(:,:) = 0.0_BDKi
     elm(:,:) = 0.0_BDKi
  
     DO igp=1,ngp
  
         temp_id1 = (igp-1)*dof_node
         temp_id2 = (igp-1)*dof_node/2
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx(:,igp),hpx(:,igp),Jaco(igp),Nuuu,Nrrr,&
               uu0(temp_id1+1:temp_id1+6),E10(temp_id2+1:temp_id2+3),node_elem,dof_node,&
               uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         mmm          =  EMass0_GL(1,1,igp)
         mEta(1)      =  0.0_BDKi
         mEta(2)      = -EMass0_GL(1,6,igp)
         mEta(3)      =  EMass0_GL(1,5,igp)
         rho          =  EMass0_GL(4:6,4:6,igp)
         CALL BD_GaussPointDataMass(hhx(:,igp),hpx(:,igp),Jaco(igp),Nvvv,Naaa,RR0,node_elem,dof_node,&
                                    vvv,aaa,vvp,mEta,rho,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_InertialForce(mmm,mEta,rho,vvv,aaa,fact,Fi,Mi,Gi,Ki,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(damp_flag .NE. 0) THEN
             CALL BD_DissipativeForce(beta,Stif,vvv,vvp,E1,fact,Fc,Fd,Sd,Od,Pd,Qd,&
                                      betaC,Gd,Xd,Yd,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDIF
         CALL BD_GravityForce(mmm,mEta,gravity,Fg,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         Fd(:) = Fd(:) - Fg(:) - DistrLoad_GL(:,igp)
  
         IF(fact) THEN
             DO i=1,node_elem
                 DO j=1,node_elem
                    DO n=1,dof_node
                       temp_id2 = (j-1)*dof_node+n
                          DO m=1,dof_node
                             temp_id1 = (i-1)*dof_node+m
                             elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                hhx(i,igp)*Qe(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                             elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                hhx(i,igp)*Pe(m,n)*hpx(j,igp)*gw(igp)
                             elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                hpx(i,igp)*Oe(m,n)*hhx(j,igp)*gw(igp)
                             elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                hpx(i,igp)*Stif(m,n)*hpx(j,igp)*gw(igp)/Jaco(igp)
                             elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                hhx(i,igp)*Ki(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                             elm(temp_id1,temp_id2) = elm(temp_id1,temp_id2) + &
                                hhx(i,igp)*Mi(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                             elg(temp_id1,temp_id2) = elg(temp_id1,temp_id2) + &
                                hhx(i,igp)*Gi(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                         ENDDO
                     ENDDO
                 ENDDO
             ENDDO
  
             IF(damp_flag .NE. 0) THEN
                 DO i=1,node_elem
                     DO j=1,node_elem
                        DO n=1,dof_node
                           temp_id2 = (j-1)*dof_node+n
                              DO m=1,dof_node
                                 temp_id1 = (i-1)*dof_node+m
                                     elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                        hhx(i,igp)*Qd(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                                     elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                        hhx(i,igp)*Pd(m,n)*hpx(j,igp)*gw(igp)
                                     elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                        hpx(i,igp)*Od(m,n)*hhx(j,igp)*gw(igp)
                                     elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                                        hpx(i,igp)*Sd(m,n)*hpx(j,igp)*gw(igp)/Jaco(igp)
                                     elg(temp_id1,temp_id2) = elg(temp_id1,temp_id2) + &
                                        hhx(i,igp)*Xd(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                                     elg(temp_id1,temp_id2) = elg(temp_id1,temp_id2) + &
                                        hhx(i,igp)*Yd(m,n)*hpx(j,igp)*gw(igp)
                                     elg(temp_id1,temp_id2) = elg(temp_id1,temp_id2) + &
                                        hpx(i,igp)*Gd(m,n)*hhx(j,igp)*gw(igp)
                                     elg(temp_id1,temp_id2) = elg(temp_id1,temp_id2) + &
                                        hpx(i,igp)*betaC(m,n)*hpx(j,igp)*gw(igp)/Jaco(igp)
                             ENDDO
                         ENDDO
                     ENDDO
                 ENDDO
             ENDIF
   
         ENDIF
  
         DO i=1,node_elem
             DO j=1,dof_node
                 temp_id1 = (i-1) * dof_node+j
                 elf(temp_id1) = elf(temp_id1) - hhx(i,igp)*Fd(j)*Jaco(igp)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) - hpx(i,igp)*Fc(j)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) - hhx(i,igp)*Fi(j)*Jaco(igp)*gw(igp)
             ENDDO
         ENDDO
  
  
     ENDDO
  
     if (ErrStat >= AbortErrLev) then
         return
     end if
  
     RETURN
  
  END SUBROUTINE BD_ElementMatrixGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes initial Gauss point values: uu0, E10, and Stif
  SUBROUTINE BD_GaussPointDataAt0(hhx,hpx,Jaco,Nuu0,Nrr0,node_elem,dof_node,uu0,E10,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: hhx(:)         !< Shape function
     REAL(BDKi),    INTENT(IN   ):: hpx(:)         !< Derivative of shape function
     REAL(BDKi),    INTENT(IN   ):: Jaco           !< Jacobian value
     REAL(BDKi),    INTENT(IN   ):: Nuu0(:)        !< Element initial nodal position array
     REAL(BDKi),    INTENT(IN   ):: Nrr0(:)        !< Element initial nodal relative rotation array
     INTEGER(IntKi),INTENT(IN   ):: node_elem      !< Number of node in one element
     INTEGER(IntKi),INTENT(IN   ):: dof_node       !< DoF per node (=6)
     REAL(BDKi),    INTENT(  OUT):: uu0(:)         !< Initial position array at Gauss point
     REAL(BDKi),    INTENT(  OUT):: E10(:)         !< E10 = x_0^\prime at Gauss point
     INTEGER(IntKi),INTENT(  OUT):: ErrStat        !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg         !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: hhi
     REAL(BDKi)                  :: hpi
     REAL(BDKi)                  :: rot0_temp(3)
     REAL(BDKi)                  :: rotu_temp(3)
     REAL(BDKi)                  :: rot_temp(3)
     REAL(BDKi)                  :: R0_temp(3,3)
     INTEGER(IntKi)              :: inode
     INTEGER(IntKi)              :: temp_id
     INTEGER(IntKi)              :: temp_id2
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_GaussPointDataAt0'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     uu0(:) = 0.0_BDKi
     E10(:) = 0.0_BDKi
     DO inode=1,node_elem
         hhi = hhx(inode)
         hpi = hpx(inode)/Jaco
         temp_id = (inode-1)*dof_node
         temp_id2 = (inode-1)*dof_node/2
         DO i=1,3
             uu0(i) = uu0(i) + hhi*Nuu0(temp_id+i)
             uu0(i+3) = uu0(i+3) + hhi*Nrr0(temp_id2+i)
  !           E10(i) = E10(i) + hpi*Nuu0(temp_id+i)
         ENDDO
     ENDDO
  
     rot0_temp = 0.0_BDKi
     rotu_temp = 0.0_BDKi
     DO i=1,3
         rot0_temp(i) = Nuu0(i+3)
         rotu_temp(i) = uu0(i+3)
     ENDDO
     rot_temp = 0.0_BDKi
     CALL BD_CrvCompose(rot_temp,rot0_temp,rotu_temp,0,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     DO i=1,3
         uu0(i+3) = rot_temp(i)
     ENDDO
  
     R0_temp = 0.0_BDKi
     CALL BD_CrvMatrixR(uu0(4:6),R0_temp,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     E10(:) = R0_temp(:,1)
  
  END SUBROUTINE BD_GaussPointDataAt0
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes Gauss point values: 1) uuu, 2) uup, 3) E1
  !! 4) RR0, 5) kapa, 6) Stif, and 7) cet
  SUBROUTINE BD_GaussPointData(hhx,hpx,Jaco,Nuuu,Nrrr,uu0,E10,node_elem,dof_node,&
                               uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: hhx(:)      !< Shape function
     REAL(BDKi),    INTENT(IN   ):: hpx(:)      !< Derivative of shape function
     REAL(BDKi),    INTENT(IN   ):: Jaco        !< Jacobian
     REAL(BDKi),    INTENT(IN   ):: Nuuu(:)     !< Element nodal displacement array
     REAL(BDKi),    INTENT(IN   ):: Nrrr(:)     !< Element nodal relative rotation array
     REAL(BDKi),    INTENT(IN   ):: uu0(:)      !< Initial position array at Gauss point
     REAL(BDKi),    INTENT(IN   ):: E10(:)      !< E10 = x_0^\prime at Gauss point
     INTEGER(IntKi),INTENT(IN   ):: node_elem   !< Number of node in one element
     INTEGER(IntKi),INTENT(IN   ):: dof_node    !< Number of DoF per node (=6)
     REAL(BDKi),    INTENT(  OUT):: uuu(:)      !< Displacement(and rotation)  arrary at Gauss point
     REAL(BDKi),    INTENT(  OUT):: uup(:)      !< Derivative of displacement wrt axix at Gauss point
     REAL(BDKi),    INTENT(  OUT):: E1(:)       !< E1 = x_0^\prime + u^\prime at Gauss point
     REAL(BDKi),    INTENT(  OUT):: RR0(:,:)    !< Rotation tensor at Gauss point
     REAL(BDKi),    INTENT(  OUT):: kapa(:)     !< Curvature starin vector at Gauss point
     REAL(BDKi),    INTENT(  OUT):: cet         !< Extension-torsion coefficient at Gauss point
     REAL(BDKi),    INTENT(  OUT):: Stif(:,:)   !< C/S stiffness matrix resolved in inertial frame at Gauss point
     INTEGER(IntKi),INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: rrr(3)
     REAL(BDKi)                  :: rrp(3)
     REAL(BDKi)                  :: hhi
     REAL(BDKi)                  :: hpi
     REAL(BDKi)                  :: cc(3)
     REAL(BDKi)                  :: rotu_temp(3)
     REAL(BDKi)                  :: rot_temp(3)
     REAL(BDKi)                  :: rot0_temp(3)
     REAL(BDKi)                  :: Wrk(3,3)
     REAL(BDKi)                  :: tempR6(6,6)
     INTEGER(IntKi)              :: inode
     INTEGER(IntKi)              :: temp_id
     INTEGER(IntKi)              :: temp_id2
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_GaussPointData'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     uuu = 0.0_BDKi
     uup = 0.0_BDKi
     rrr = 0.0_BDKi
     rrp = 0.0_BDKi
     DO inode=1,node_elem
         hhi = hhx(inode)
         hpi = hpx(inode)/Jaco
         temp_id = (inode-1)*dof_node
         temp_id2 = (inode-1)*dof_node/2
         DO i=1,3
             uuu(i) = uuu(i) + hhi*Nuuu(temp_id+i)
             uup(i) = uup(i) + hpi*Nuuu(temp_id+i)
             rrr(i) = rrr(i) + hhi*Nrrr(temp_id2+i)
             rrp(i) = rrp(i) + hpi*Nrrr(temp_id2+i)
         ENDDO
     ENDDO
  
     E1 = 0.0_BDKi
     rotu_temp = 0.0_BDKi
     DO i=1,3
         E1(i) = E10(i) + uup(i)
         rotu_temp(i) = Nuuu(i+3)
     ENDDO
     rot_temp(:)  = 0.0_BDKi
     rot0_temp(:) = 0.0_BDKi
     CALL BD_CrvCompose(rot_temp,rotu_temp,rrr,0,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     DO i=1,3
         uuu(i+3) = rot_temp(i)
         rot0_temp(i) = uu0(i+3)
     ENDDO
  
     cc = 0.0_BDKi
     RR0 = 0.0_BDKi
     CALL BD_CrvCompose(cc,rot_temp,rot0_temp,0,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CrvMatrixR(cc,RR0,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     tempR6 = 0.0_BDKi
     DO i=1,3
         DO j=1,3
             tempR6(i,j) = RR0(i,j)
             tempR6(i+3,j+3) = RR0(i,j)
         ENDDO
     ENDDO
  
     cet = 0.0_BDKi
     cet = Stif(5,5) + Stif(6,6)
     Stif = MATMUL(tempR6,MATMUL(Stif,TRANSPOSE(tempR6)))
  
     Wrk = 0.0_BDKi
     kapa = 0.0_BDKi
     CALL BD_CrvMatrixH(rrr,Wrk)
     cc = MATMUL(Wrk,rrp)
     CALL BD_CrvMatrixR(rotu_temp,Wrk,ErrStat2,ErrMsg2)
     kapa = MATMUL(Wrk,cc)
  
  END SUBROUTINE BD_GaussPointData
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the elastic forces Fc and Fd
  !! It also calcuates the linearized matrices Oe, Pe, and Qe for N-R algorithm
  SUBROUTINE BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: E1(:)
     REAL(BDKi),    INTENT(IN   ):: RR0(:,:)
     REAL(BDKi),    INTENT(IN   ):: kapa(:)
     REAL(BDKi),    INTENT(IN   ):: Stif(:,:)
     REAL(BDKi),    INTENT(IN   ):: cet
     LOGICAL,       INTENT(IN   ):: fact
     REAL(BDKi),    INTENT(  OUT):: Fc(:)
     REAL(BDKi),    INTENT(  OUT):: Fd(:)
     REAL(BDKi),    INTENT(  OUT):: Oe(:,:)
     REAL(BDKi),    INTENT(  OUT):: Pe(:,:)
     REAL(BDKi),    INTENT(  OUT):: Qe(:,:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: eee(6)
     REAL(BDKi)                  :: fff(6)
     REAL(BDKi)                  :: tempS(3)
     REAL(BDKi)                  :: tempK(3)
     REAL(BDKi)                  :: Wrk(3)
     REAL(BDKi)                  :: e1s
     REAL(BDKi)                  :: k1s
     REAL(BDKi)                  :: Wrk33(3,3)
     REAL(BDKi)                  :: C11(3,3)
     REAL(BDKi)                  :: C12(3,3)
     REAL(BDKi)                  :: C21(3,3)
     REAL(BDKi)                  :: C22(3,3)
     REAL(BDKi)                  :: epsi(3,3)
     REAL(BDKi)                  :: mu(3,3)
     INTEGER(IntKi)              :: i
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     eee(:)   = 0.0_BDKi
     tempS(:) = 0.0_BDKi
     tempK(:) = 0.0_BDKi
     DO i=1,3
         eee(i) = E1(i) - RR0(i,1)
         eee(i+3) = kapa(i)
  
         tempS(i) = eee(i)
         tempK(i) = eee(i+3)
     ENDDO
     fff = 0.0_BDKi
     fff = MATMUL(Stif,eee)
  
     Wrk(:) = 0.0_BDKi
     Wrk(:) = MATMUL(TRANSPOSE(RR0),tempS)
     e1s = Wrk(1)      !epsilon_{11} in material basis
  
     Wrk(:) = 0.0_BDKi
     Wrk(:) = MATMUL(TRANSPOSE(RR0),tempK)
     k1s = Wrk(1)      !kapa_{1} in material basis
  
     ! Incorporate extension-twist coupling
     DO i=1,3
         fff(i) = fff(i) + 0.5_BDKi*cet*k1s*k1s*RR0(i,1)
         fff(i+3) = fff(i+3) + cet*e1s*k1s*RR0(i,1)
     ENDDO
  
     Fc(:)    = 0.0_BDKi
     Fc(:)    = fff(:)
     Wrk(:)   = 0.0_BDKi
     Wrk(1:3) = fff(1:3)
     Fd(:)    = 0.0_BDKi
     Fd(4:6)  = MATMUL(TRANSPOSE(BD_Tilde(E1)),Wrk)
  
     IF(fact) THEN
         C11(:,:) = 0.0_BDKi
         C12(:,:) = 0.0_BDKi
         C21(:,:) = 0.0_BDKi
         C22(:,:) = 0.0_BDKi
         C11(1:3,1:3) = Stif(1:3,1:3)
         C12(1:3,1:3) = Stif(1:3,4:6)
         C21(1:3,1:3) = Stif(4:6,1:3)
         C22(1:3,1:3) = Stif(4:6,4:6)
  
         Wrk = 0.0_BDKi
         DO i=1,3
             Wrk(i) = RR0(i,1)
         ENDDO
         Wrk33(:,:) = 0.0_BDKi
         Wrk33(:,:) = OuterProduct(Wrk,Wrk)
         C12(:,:) = C12(:,:) + cet*k1s*Wrk33(:,:) 
         C21(:,:) = C21(:,:) + cet*k1s*Wrk33(:,:)
         C22(:,:) = C22(:,:) + cet*e1s*Wrk33(:,:)
  
         epsi(:,:) = 0.0_BDKi
         mu(:,:)   = 0.0_BDKi
         epsi(:,:) = MATMUL(C11,BD_Tilde(E1))
         mu(:,:)   = MATMUL(C21,BD_Tilde(E1))
  
         Wrk(:) = 0.0_BDKi
         Oe(:,:)     = 0.0_BDKi
         Oe(1:3,4:6) = epsi(1:3,1:3)
         Oe(4:6,4:6) = mu(1:3,1:3)
  
         Wrk(1:3) = fff(1:3)
         Oe(1:3,4:6) = Oe(1:3,4:6) - BD_Tilde(Wrk)
         Wrk(:) = 0.0_BDKi
         Wrk(1:3) = fff(4:6)
         Oe(4:6,4:6) = Oe(4:6,4:6) - BD_Tilde(Wrk)
  
         Wrk(: ) = 0.0_BDKi
         Pe(:,:) = 0.0_BDKi
         Wrk(1:3) = fff(1:3)
         Pe(4:6,1:3) = BD_Tilde(Wrk) + TRANSPOSE(epsi)
         Pe(4:6,4:6) = TRANSPOSE(mu)
  
         Wrk33(:,:) = 0.0_BDKi
         Qe(:,:)    = 0.0_BDKi
         Wrk33(1:3,1:3) = Oe(1:3,4:6)
         Qe(4:6,4:6) = MATMUL(TRANSPOSE(BD_Tilde(E1)),Wrk33)
     ENDIF
  
  END SUBROUTINE BD_ElasticForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the mass quantities at the Gauss point
  !! 1) velocity; 2) acceleration; 3) derivative of velocity wrt axis
  !! 4) mass matrix components (mmm,mEta,rho)
  SUBROUTINE BD_GaussPointDataMass(hhx,hpx,Jaco,Nvvv,Naaa,RR0,node_elem,dof_node,&
                                   vvv,aaa,vvp,mEta,rho,ErrStat,ErrMsg)
     REAL(BDKi),     INTENT(IN   ):: hhx(:)
     REAL(BDKi),     INTENT(IN   ):: hpx(:)
     REAL(BDKi),     INTENT(IN   ):: Jaco
     REAL(BDKi),     INTENT(IN   ):: Nvvv(:)
     REAL(BDKi),     INTENT(IN   ):: Naaa(:)
     REAL(BDKi),     INTENT(IN   ):: RR0(:,:)
     INTEGER(IntKi), INTENT(IN   ):: node_elem
     INTEGER(IntKi), INTENT(IN   ):: dof_node
     REAL(BDKi),     INTENT(  OUT):: vvv(:)
     REAL(BDKi),     INTENT(  OUT):: vvp(:)
     REAL(BDKi),     INTENT(  OUT):: aaa(:)
     REAL(BDKi),     INTENT(INOUT):: mEta(:)
     REAL(BDKi),     INTENT(INOUT):: rho(:,:)
     INTEGER(IntKi), INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),   INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                   :: hhi
     REAL(BDKi)                   :: hpi
     INTEGER(IntKi)               :: inode
     INTEGER(IntKi)               :: temp_id
     INTEGER(IntKi)               :: i
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     vvv(:) = 0.0_BDKi
     vvp(:) = 0.0_BDKi
     aaa(:) = 0.0_BDKi
  
     DO inode=1,node_elem
         hhi = hhx(inode)
         hpi = hpx(inode)/Jaco
         temp_id = (inode-1)*dof_node
         DO i=1,dof_node
             vvv(i) = vvv(i) + hhi * Nvvv(temp_id+i)
             vvp(i) = vvp(i) + hpi * Nvvv(temp_id+i)
             aaa(i) = aaa(i) + hhi * Naaa(temp_id+i)
         ENDDO
     ENDDO
  
     mEta = MATMUL(RR0,mEta)
     rho = MATMUL(RR0,MATMUL(rho,TRANSPOSE(RR0)))
  
  END SUBROUTINE BD_GaussPointDataMass
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the inertial force Fi
  !! It also calcuates the linearized matrices Mi, Gi, and Ki for N-R algorithm
  SUBROUTINE BD_InertialForce(m00,mEta,rho,vvv,aaa,fact,Fi,Mi,Gi,Ki,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: m00
     REAL(BDKi),    INTENT(IN   ):: mEta(:)
     REAL(BDKi),    INTENT(IN   ):: rho(:,:)
     REAL(BDKi),    INTENT(IN   ):: vvv(:)
     REAL(BDKi),    INTENT(IN   ):: aaa(:)
     LOGICAL,       INTENT(IN   ):: fact
     REAL(BDKi),    INTENT(  OUT):: Fi(6)
     REAL(BDKi),    INTENT(  OUT):: Mi(6,6)
     REAL(BDKi),    INTENT(  OUT):: Gi(6,6)
     REAL(BDKi),    INTENT(  OUT):: Ki(6,6)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: beta(3)
     REAL(BDKi)                  :: gama(3)
     REAL(BDKi)                  :: nu(3)
     REAL(BDKi)                  :: epsi(3,3)
     REAL(BDKi)                  :: mu(3,3)
     REAL(BDKi)                  :: ome(3)
     REAL(BDKi)                  :: omd(3)
     REAL(BDKi)                  :: tempV(3)
     REAL(BDKi)                  :: tempA(3)
     INTEGER(IntKi)              :: i
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     Fi(:)   = 0.0_BDKi
     Mi(:,:) = 0.0_BDKi
     Gi(:,:) = 0.0_BDKi
     Ki(:,:) = 0.0_BDKi
  
     ome(:) = vvv(4:6)
     omd(:) = aaa(4:6)
     tempV(:) = vvv(1:3)
     tempA(:) = aaa(1:3)
  
     beta = MATMUL(BD_Tilde(ome),mEta)
     gama = MATMUL(rho,ome)
     nu = MATMUL(rho,omd)
  
     !Compute Fi
     Fi(1:3)= m00*tempA+MATMUL(BD_Tilde(omd),mEta)+MATMUL(BD_Tilde(ome),beta)
     Fi(4:6) = MATMUL(BD_Tilde(mEta),tempA) + nu + MATMUL(BD_Tilde(ome),gama)
  
     IF(fact) THEN
         !Mass Matrix
         Mi = 0.0_BDKi
         DO i=1,3
             Mi(i,i) = m00
         ENDDO
         Mi(1:3,4:6) = TRANSPOSE(BD_Tilde(mEta))
         Mi(4:6,1:3) = BD_Tilde(mEta)
         Mi(4:6,4:6) = rho
  
         !Gyroscopic Matrix
         Gi = 0.0_BDKi
         epsi = MATMUL(BD_Tilde(ome),rho)
         mu = MATMUL(BD_Tilde(ome),TRANSPOSE(BD_Tilde(mEta)))
         Gi(1:3,4:6) = TRANSPOSE(BD_Tilde(beta)) + mu
         Gi(4:6,4:6) = epsi - BD_Tilde(gama)
  
         !Stiffness Matrix
         Ki = 0.0_BDKi
         Ki(1:3,4:6) = MATMUL(BD_Tilde(omd),TRANSPOSE(BD_Tilde(mEta))) +&
                      &MATMUL(BD_Tilde(ome),mu)
         Ki(4:6,4:6) = MATMUL(BD_Tilde(tempA),BD_Tilde(mEta)) + &
                      &MATMUL(rho,BD_Tilde(omd)) - BD_Tilde(nu) +&
                      &MATMUL(epsi,BD_Tilde(ome)) - &
                      &MATMUL(BD_Tilde(ome),BD_Tilde(gama))
     ENDIF
  
  END SUBROUTINE BD_InertialForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the dissipative forces and added it to Fc and Fd
  !! It also calcuates the linearized matrices Sd, Od, Pd and Qd 
  !! betaC, Gd, Xd, Yd for N-R algorithm
  SUBROUTINE BD_DissipativeForce(beta,Stiff,vvv,vvp,E1,fact,Fc,Fd,&
                                    Sd,Od,Pd,Qd,betaC,Gd,Xd,Yd, &
                                    ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: beta(:)
     REAL(BDKi),    INTENT(IN   ):: Stiff(:,:)
     REAL(BDKi),    INTENT(IN   ):: vvv(:)
     REAL(BDKi),    INTENT(IN   ):: vvp(:)
     REAL(BDKi),    INTENT(IN   ):: E1(:)
     LOGICAL,       INTENT(IN   ):: fact
     REAL(BDKi),    INTENT(INOUT):: Fc(:)
     REAL(BDKi),    INTENT(INOUT):: Fd(:)
     REAL(BDKi),    INTENT(  OUT):: Sd(:,:)
     REAL(BDKi),    INTENT(  OUT):: Od(:,:)
     REAL(BDKi),    INTENT(  OUT):: Pd(:,:)
     REAL(BDKi),    INTENT(  OUT):: Qd(:,:)
     REAL(BDKi),    INTENT(  OUT):: betaC(:,:)
     REAL(BDKi),    INTENT(  OUT):: Gd(:,:)
     REAL(BDKi),    INTENT(  OUT):: Xd(:,:)
     REAL(BDKi),    INTENT(  OUT):: Yd(:,:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: ome(3)
     REAL(BDKi)                  :: eed(6)
     REAL(BDKi)                  :: ffd(6)
     REAL(BDKi)                  :: D11(3,3)
     REAL(BDKi)                  :: D12(3,3)
     REAL(BDKi)                  :: D21(3,3)
     REAL(BDKi)                  :: D22(3,3)
     REAL(BDKi)                  :: b11(3,3)
     REAL(BDKi)                  :: b12(3,3)
     REAL(BDKi)                  :: alpha(3,3)
     REAL(BDKi)                  :: temp_b(6,6)
     INTEGER(IntKi)              :: i
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     Sd(:,:)    = 0.0_BDKi
     Od(:,:)    = 0.0_BDKi
     Pd(:,:)    = 0.0_BDKi
     Qd(:,:)    = 0.0_BDKi
     betaC(:,:) = 0.0_BDKi
     Gd(:,:)    = 0.0_BDKi
     Xd(:,:)    = 0.0_BDKi
     Yd(:,:)    = 0.0_BDKi
  
     ome(1:3) = vvv(4:6)
     ! Compute strain rates
     eed(1:6) = vvp(1:6)
     eed(1:3) = eed(1:3) + MATMUL(BD_Tilde(E1),ome)
  
     ! Compute damping matrix
     temp_b(:,:) = 0.0_BDKi
     DO i=1,6
         temp_b(i,i) = beta(i)
     ENDDO
     betaC(1:6,1:6) = MATMUL(temp_b,Stiff(:,:))
     D11(1:3,1:3) = betaC(1:3,1:3)
     D12(1:3,1:3) = betaC(1:3,4:6)
     D21(1:3,1:3) = betaC(4:6,1:3)
     D22(1:3,1:3) = betaC(4:6,4:6)
  
     ! Compute dissipative force
     ffd(1:6) = MATMUL(betaC,eed)
     Fc(1:6) = Fc(1:6) + ffd(1:6)
     Fd(4:6) = Fd(4:6) + MATMUL(BD_Tilde(ffd(1:3)),E1)
  
     IF(fact) THEN
         ! Compute stiffness matrix Sd
         Sd(1:3,1:3) = MATMUL(D11,TRANSPOSE(BD_Tilde(ome)))
         Sd(1:3,4:6) = MATMUL(D12,TRANSPOSE(BD_Tilde(ome)))
         Sd(4:6,1:3) = MATMUL(D21,TRANSPOSE(BD_Tilde(ome)))
         Sd(4:6,4:6) = MATMUL(D22,TRANSPOSE(BD_Tilde(ome)))
  
         ! Compute stiffness matrix Pd
         Pd(:,:) = 0.0_BDKi
         b11(1:3,1:3) = MATMUL(TRANSPOSE(BD_Tilde(E1)),D11)
         b12(1:3,1:3) = MATMUL(TRANSPOSE(BD_Tilde(E1)),D12)
         Pd(4:6,1:3) = BD_Tilde(ffd(1:3)) + MATMUL(b11,TRANSPOSE(BD_Tilde(ome)))
         Pd(4:6,1:3) = MATMUL(b12,TRANSPOSE(BD_Tilde(ome)))
  
         ! Compute stiffness matrix Od
         alpha(1:3,1:3) = BD_Tilde(vvp(1:3)) - MATMUL(BD_Tilde(ome),BD_Tilde(E1))
         Od(1:3,4:6) = MATMUL(D11,alpha) - BD_Tilde(ffd(1:3))
         Od(4:6,4:6) = MATMUL(D21,alpha) - BD_Tilde(ffd(4:6))
  
         ! Compute stiffness matrix Qd
         Qd(4:6,4:6) = MATMUL(TRANSPOSE(BD_Tilde(E1)),Od(1:3,4:6))
  
         ! Compute gyroscopic matrix Gd
         Gd(1:3,4:6) = TRANSPOSE(b11)
         Gd(4:6,4:6) = TRANSPOSE(b12)
  
         ! Compute gyroscopic matrix Xd
         Xd(4:6,4:6) = MATMUL(TRANSPOSE(BD_Tilde(E1)),Gd(1:3,4:6))
  
         ! Compute gyroscopic matrix Yd
         Yd(4:6,1:3) = b11
         Yd(4:6,4:6) = b12
     ENDIF
  
  END SUBROUTINE BD_DissipativeForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the gravity forces Fg
  SUBROUTINE BD_GravityForce(m00,mEta,grav,Fg,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: m00
     REAL(BDKi),    INTENT(IN   ):: mEta(:)
     REAL(BDKi),    INTENT(IN   ):: grav(:)
     REAL(BDKi),    INTENT(  OUT):: Fg(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     Fg(:)   = 0.0_BDKi
  
     Fg(1:3) = m00 * grav(1:3)
     Fg(4:6) = MATMUL(BD_Tilde(mEta),grav)
  
  END SUBROUTINE BD_GravityForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine assembles total stiffness matrix.
  SUBROUTINE BD_AssembleStiffK(nelem,node_elem,dof_elem,dof_node,ElemK,GlobalK,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: ElemK(:,:)    !< Element  matrix
     INTEGER(IntKi),INTENT(IN   ):: nelem         !< Number of elements
     INTEGER(IntKi),INTENT(IN   ):: node_elem     !< Nodes per element
     INTEGER(IntKi),INTENT(IN   ):: dof_elem      !< Degrees of freedom per element
     INTEGER(IntKi),INTENT(IN   ):: dof_node      !< Degrees of freedom per node
     REAL(BDKi),    INTENT(INOUT):: GlobalK(:,:)  !< Global stiffness matrix
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: temp_id1
     INTEGER(IntKi)              :: temp_id2
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=1,dof_elem
         temp_id1 = (nelem-1)*(node_elem-1)*dof_node+i
         DO j=1,dof_elem
             temp_id2 = (nelem-1)*(node_elem-1)*dof_node+j
             GlobalK(temp_id1,temp_id2) = GlobalK(temp_id1,temp_id2) + ElemK(i,j)
         ENDDO
     ENDDO
  
  END SUBROUTINE BD_AssembleStiffK
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine assembles global force vector.
  SUBROUTINE BD_AssembleRHS(nelem,dof_elem,node_elem,dof_node,ElemRHS,GlobalRHS,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: ElemRHS(:)    !< Total element force (Fc, Fd, Fb)
     INTEGER(IntKi),INTENT(IN   ):: nelem         !< Number of elements
     INTEGER(IntKi),INTENT(IN   ):: dof_elem      !< Degrees of freedom per element
     INTEGER(IntKi),INTENT(IN   ):: node_elem     !< Nodes per element
     INTEGER(IntKi),INTENT(IN   ):: dof_node      !< Degrees of freedom per node
     REAL(BDKi),    INTENT(INOUT):: GlobalRHS(:)  !< Global force vector
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: temp_id
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=1,dof_elem
         temp_id = (nelem-1)*(node_elem-1)*dof_node+i
         GlobalRHS(temp_id) = GlobalRHS(temp_id)+ElemRHS(i)
     ENDDO
  
  END SUBROUTINE BD_AssembleRHS
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine updates the 1) displacements/rotations(uf)
  !! 2) linear/angular velocities(vf); 3) linear/angular accelerations(af); and 
  !! 4) algorithmic accelerations(xf) given the increments obtained through
  !! N-R algorithm
  SUBROUTINE BD_UpdateDynamicGA2(ainc,uf,vf,af,xf,coef,node_total,dof_node,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: ainc(:)
     REAL(DbKi),    INTENT(IN   ):: coef(:)
     INTEGER(IntKi),INTENT(IN   ):: node_total
     INTEGER(IntKi),INTENT(IN   ):: dof_node
     REAL(BDKi),    INTENT(INOUT):: uf(:)
     REAL(BDKi),    INTENT(INOUT):: vf(:)
     REAL(BDKi),    INTENT(INOUT):: af(:)
     REAL(BDKi),    INTENT(INOUT):: xf(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: rotf_temp(3)
     REAL(BDKi)                  :: roti_temp(3)
     REAL(BDKi)                  :: rot_temp(3)
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: temp_id
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_UpdateDynamicGA2'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=2, node_total
         temp_id = (i - 1) * dof_node
         rotf_temp = 0.0_BDKi
         roti_temp = 0.0_BDKi
         rot_temp = 0.0_BDKi
         DO j = 1, 3
             uf(temp_id+j) = uf(temp_id+j) + coef(8) * ainc(temp_id+j)
             rotf_temp(j) = uf(temp_id+3+j)
             roti_temp(j) = coef(8) * ainc(temp_id+3+j)
         ENDDO
         CALL BD_CrvCompose(rot_temp,roti_temp,rotf_temp,0,ErrStat2,ErrMsg2)
         DO j = 1, 3
             uf(temp_id+3+j) = rot_temp(j)
         ENDDO
  
         DO j=1, 6
             vf(temp_id+j) = vf(temp_id+j) + coef(7) * ainc(temp_id+j)
             af(temp_id+j) = af(temp_id+j) + ainc(temp_id+j)
             xf(temp_id+j) = xf(temp_id+j) + coef(9) * ainc(temp_id+j)
         ENDDO
     ENDDO
  
  END SUBROUTINE BD_UpdateDynamicGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes Global mass matrix and force vector for the beam.
  SUBROUTINE BD_GenerateDynamicElementAcc(uuN,vvN,u, p, m, ErrStat, ErrMsg)
  
     REAL(BDKi),            INTENT(IN   ):: uuN(:)       !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(IN   ):: vvN(:)       !< Velocity of Mass 1: m/s
     TYPE(BD_InputType),    INTENT(IN   ):: u            !< Inputs at t
     TYPE(BD_ParameterType),INTENT(IN   ):: p            !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m            !< Misc/optimization variables
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat      !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg       !< Error message if ErrStat /= ErrID_None
  
     
     INTEGER(IntKi)                  :: nelem ! number of elements
  !   INTEGER(IntKi)                  :: i ! Index counter
     INTEGER(IntKi)                  :: j ! Index counter
     INTEGER(IntKi)                  :: temp_id ! Index counter
     INTEGER(IntKi)                  :: temp_id2 ! Index counter
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER         :: RoutineName = 'BD_GenerateDynamicElementAcc'
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     m%RHS(:)     = 0.0_BDKi
     m%MassM(:,:) = 0.0_BDKi
  
  
     DO nelem=1,p%elem_total
        
         CALL BD_ElemNodalDisp(uuN,p%node_elem,p%dof_node,nelem,m%Nuuu,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_NodalRelRot(m%Nuuu,p%node_elem,p%dof_node,m%Nrrr,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElemNodalDisp(vvN,p%node_elem,p%dof_node,nelem,m%Nvvv,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         if (ErrStat >= AbortErrLev) return
  
         IF(p%quadrature .EQ. 1) THEN
             temp_id = (nelem-1)*p%ngp + 1
             temp_id2 = (nelem-1)*p%ngp
         ELSEIF(p%quadrature .EQ. 2) THEN
             temp_id = (nelem-1)*p%ngp
             temp_id2= temp_id
         ENDIF
  
         DO j=1,p%ngp
             m%EStif0_GL(:,:,j)    = p%Stif0_GL(1:6,1:6,temp_id2+j)
             m%EMass0_GL(:,:,j)    = p%Mass0_GL(1:6,1:6,temp_id2+j)
             m%DistrLoad_GL(1:3,j) = u%DistrLoad%Force(1:3,temp_id+j)
             m%DistrLoad_GL(4:6,j) = u%DistrLoad%Moment(1:3,temp_id+j)
         ENDDO
  
         CALL BD_ElementMatrixAcc(m%Nuuu,m%Nrrr,m%Nvvv,&
                                  m%EStif0_GL,m%EMass0_GL,p%gravity,m%DistrLoad_GL,&
                                  p%ngp,p%GLw,p%Shp,p%Der,p%Jacobian(:,nelem),p%uu0(:,nelem),p%E10(:,nelem),&
                                  p%node_elem,p%dof_node,p%damp_flag,p%beta,&
                                  m%elf,m%elm,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         CALL BD_AssembleStiffK(nelem,p%node_elem,p%dof_elem,p%dof_node,m%elm, m%MassM,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_AssembleRHS(nelem,p%dof_elem,p%node_elem,p%dof_node,m%elf, m%RHS,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         if (ErrStat >= AbortErrLev) return
  
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_GenerateDynamicElementAcc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine total element forces and mass matrices
  SUBROUTINE BD_ElementMatrixAcc(Nuuu,Nrrr,Nvvv,&
                                 EStif0_GL,EMass0_GL,gravity,DistrLoad_GL,&
                                 ngp,gw,hhx,hpx,Jaco,uu0,E10,   &
                                 node_elem,dof_node,damp_flag,beta,&
                                 elf,elm,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: Nuuu(:)             !< Nodal displacement of Mass 1 for each element
     REAL(BDKi),    INTENT(IN   ):: Nrrr(:)             !< Nodal rotation parameters for displacement of Mass 1
     REAL(BDKi),    INTENT(IN   ):: Nvvv(:)             !< Nodal velocity of Mass 1: m/s for each element
     REAL(BDKi),    INTENT(IN   ):: EStif0_GL(:,:,:)    !< Nodal material properties for each element
     REAL(BDKi),    INTENT(IN   ):: EMass0_GL(:,:,:)    !< Nodal material properties for each element
     REAL(BDKi),    INTENT(IN   ):: gravity(:)          !< gravitational acceleration
     REAL(BDKi),    INTENT(IN   ):: DistrLoad_GL(:,:)   !< Nodal material properties for each element
     INTEGER(IntKi),INTENT(IN   ):: ngp                 !< Number of Gauss points
     REAL(BDKi),    INTENT(IN   ):: gw(:)
     REAL(BDKi),    INTENT(IN   ):: hhx(:,:)
     REAL(BDKi),    INTENT(IN   ):: hpx(:,:)
     REAL(BDKi),    INTENT(IN   ):: Jaco(:)
     REAL(BDKi),    INTENT(IN   ):: uu0(:)
     REAL(BDKi),    INTENT(IN   ):: E10(:)
     INTEGER(IntKi),INTENT(IN   ):: node_elem           !< Node per element
     INTEGER(IntKi),INTENT(IN   ):: dof_node            !< Degrees of freedom per node
     INTEGER(IntKi),INTENT(IN   ):: damp_flag           !< Degrees of freedom per node
     REAL(BDKi),    INTENT(IN   ):: beta(:)
     REAL(BDKi),    INTENT(  OUT):: elf(:)              !< Total element force (Fd, Fc, Fb)
     REAL(BDKi),    INTENT(  OUT):: elm(:,:)            !< Total element mass matrix
     INTEGER(IntKi),INTENT(  OUT):: ErrStat             !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg              !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: temp_Naaa(dof_node*node_elem)
     REAL(BDKi)                  :: RR0(3,3)
     REAL(BDKi)                  :: kapa(3)
     REAL(BDKi)                  :: E1(3)
     REAL(BDKi)                  :: Stif(6,6)
     REAL(BDKi)                  :: cet
     REAL(BDKi)                  :: uuu(6)
     REAL(BDKi)                  :: uup(3)
     REAL(BDKi)                  :: Fc(6)
     REAL(BDKi)                  :: Fd(6)
     REAL(BDKi)                  :: Fg(6)
     REAL(BDKi)                  :: vvv(6)
     REAL(BDKi)                  :: vvp(6)
     REAL(BDKi)                  :: mmm
     REAL(BDKi)                  :: mEta(3)
     REAL(BDKi)                  :: rho(3,3)
     REAL(BDKi)                  :: Fb(6)
     REAL(BDKi)                  :: Mi(6,6)
     REAL(BDKi)                  :: Oe(6,6)
     REAL(BDKi)                  :: Pe(6,6)
     REAL(BDKi)                  :: Qe(6,6)
     REAL(BDKi)                  :: Sd(6,6)
     REAL(BDKi)                  :: Od(6,6)
     REAL(BDKi)                  :: Pd(6,6)
     REAL(BDKi)                  :: Qd(6,6)
     REAL(BDKi)                  :: betaC(6,6)
     REAL(BDKi)                  :: Gd(6,6)
     REAL(BDKi)                  :: Xd(6,6)
     REAL(BDKi)                  :: Yd(6,6)
     REAL(BDKi)                  :: temp_aaa(6)
     LOGICAL                     :: fact
     INTEGER(IntKi)              :: igp
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: m
     INTEGER(IntKi)              :: n
     INTEGER(IntKi)              :: temp_id1
     INTEGER(IntKi)              :: temp_id2
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_ElementMatrixAcc'
  
     ErrStat   = ErrID_None
     ErrMsg    = ""
     elf(:)    = 0.0_BDKi
     elm(:,:)  = 0.0_BDKi
     temp_Naaa = 0.0_BDKi
     
     fact = .FALSE.
  
     DO igp=1,ngp
  
         temp_id1 = (igp-1)*dof_node
         temp_id2 = (igp-1)*dof_node/2
         Stif(:,:) = 0.0_BDKi
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx(:,igp),hpx(:,igp),Jaco(igp),Nuuu,Nrrr,&
               uu0(temp_id1+1:temp_id1+6),E10(temp_id2+1:temp_id2+3),node_elem,dof_node,&
               uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         mmm          = 0.0_BDKi
         rho          = 0.0_BDKi
         mmm          = EMass0_GL(1,1,igp)
         mEta(1)      = 0.0_BDKi
         mEta(2)      = -EMass0_GL(1,6,igp)
         mEta(3)      =  EMass0_GL(1,5,igp)
         rho(1:3,1:3) = EMass0_GL(4:6,4:6,igp)
         CALL BD_GaussPointDataMass(hhx(:,igp),hpx(:,igp),Jaco(igp),Nvvv,temp_Naaa,RR0,&
               node_elem,dof_node, vvv,temp_aaa,vvp,mEta,rho,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_MassMatrix(mmm,mEta,rho,Mi,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_GyroForce(mEta,rho,vvv,Fb,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_GravityForce(mmm,mEta,gravity,Fg,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(damp_flag .NE. 0) THEN
             CALL BD_DissipativeForce(beta,Stif,vvv,vvp,E1,fact,Fc,Fd,Sd,Od,Pd,Qd,&
                                      betaC,Gd,Xd,Yd,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDIF
  
         Fd(:) = Fd(:) + Fb(:) - DistrLoad_GL(:,igp) - Fg(:)
  
         DO i=1,node_elem
             DO j=1,node_elem
                 DO m=1,dof_node
                     temp_id1 = (i-1)*dof_node+m
                     DO n=1,dof_node
                         temp_id2 = (j-1)*dof_node+n
                         elm(temp_id1,temp_id2) = elm(temp_id1,temp_id2) + hhx(i,igp)*Mi(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                     ENDDO
                 ENDDO
             ENDDO
         ENDDO
         DO i=1,node_elem
             DO j=1,dof_node
                 temp_id1 = (i-1) * dof_node+j
                 elf(temp_id1) = elf(temp_id1) - hpx(i,igp)*Fc(j)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) - hhx(i,igp)*Fd(j)*Jaco(igp)*gw(igp)
             ENDDO
         ENDDO
  
         if (ErrStat >= AbortErrLev) then
            return
         end if
  
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_ElementMatrixAcc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the mass matrix.
  SUBROUTINE BD_MassMatrix(m00,mEta,rho,Mi,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: m00        !< Mass density at Gauss point
     REAL(BDKi),    INTENT(IN   ):: mEta(:)    !< m\Eta resolved in inertia frame at Gauss point
     REAL(BDKi),    INTENT(IN   ):: rho(:,:)   !< Tensor of inertia resolved in inertia frame at Gauss point
     REAL(BDKi),    INTENT(  OUT):: Mi(:,:)    !< Mass matrix
     INTEGER(IntKi),INTENT(  OUT):: ErrStat    !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg     !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     Mi(:,:) = 0.0_BDKi
  
     DO i=1,3
         Mi(i,i) = m00
     ENDDO
     Mi(1:3,4:6) = TRANSPOSE(BD_Tilde(mEta))
     Mi(4:6,1:3) = BD_Tilde(mEta)
     Mi(4:6,4:6) = rho
  
  
  END SUBROUTINE BD_MassMatrix
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes gyroscopic forces
  SUBROUTINE BD_GyroForce(mEta,rho,vvv,Fb,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: mEta(:)       !< m\Eta resolved in inertia frame at Gauss point
     REAL(BDKi),    INTENT(IN   ):: rho(:,:)      !< Tensor of inertia resolved in inertia frame at Gauss point
     REAL(BDKi),    INTENT(IN   ):: vvv(:)        !< Velocities at Gauss point (including linear and angular velocities)
     REAL(BDKi),    INTENT(  OUT):: Fb(:)         !< Gyroscopic forces
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     REAL(BDKi)                  :: Bi(6,6)
     REAL(BDKi)                  :: ome(3)
     REAL(BDKi)                  :: temp33(3,3)
     REAL(BDKi)                  :: temp6(6)
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     Fb(:)   = 0.0_BDKi
  
     ome = 0.0_BDKi
     ome(:) = vvv(4:6)
     Bi= 0.0_BDKi
     temp33 = 0.0_BDKi
     temp33 = MATMUL(BD_Tilde(ome),TRANSPOSE(BD_Tilde(mEta)))
     DO i=1,3
         DO j=1,3
             Bi(i,j+3) = temp33(i,j)
         ENDDO
     ENDDO
     temp33 = 0.0_BDKi
     temp33 = MATMUL(BD_Tilde(ome),rho)
     DO i=1,3
         DO j=1,3
             Bi(i+3,j+3) = temp33(i,j)
         ENDDO
     ENDDO
  
     temp6 = 0.0_BDKi
     temp6(1:6) = vvv(1:6)
  
     Fb(:) = MATMUL(Bi,temp6)
  
  END SUBROUTINE BD_GyroForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates elemetal internal forces
  SUBROUTINE BD_ElementMatrixForce(Nuuu,Nrrr,Nvvv,&
                                   EStif0_GL,EMass0_GL,     &
                                   damp_flag,beta,          &
                                   ngp,gw,hhx,hpx,Jaco,uu0,E10,&
                                   node_elem,dof_node,elf,&
                                   ErrStat,ErrMsg)
  
     REAL(BDKi),     INTENT(IN   ):: Nuuu(:)          !< Nodal displacement of Mass 1 for each element
     REAL(BDKi),     INTENT(IN   ):: Nrrr(:)          !< Nodal rotation parameters for displacement of Mass 1
     REAL(BDKi),     INTENT(IN   ):: Nvvv(:)          !< Nodal velocity of Mass 1: m/s for each element
     REAL(BDKi),     INTENT(IN   ):: EStif0_GL(:,:,:) !< Nodal material properties for each element
     REAL(BDKi),     INTENT(IN   ):: EMass0_GL(:,:,:) !< Nodal material properties for each element
     INTEGER(IntKi), INTENT(IN   ):: damp_flag        !< Number of Gauss points
     REAL(BDKi),     INTENT(IN   ):: beta(:)
     INTEGER(IntKi), INTENT(IN   ):: ngp              !< Number of Gauss points
     REAL(BDKi),     INTENT(IN   ):: gw(:)
     REAL(BDKi),     INTENT(IN   ):: hhx(:,:)
     REAL(BDKi),     INTENT(IN   ):: hpx(:,:)
     REAL(BDKi),     INTENT(IN   ):: Jaco(:)
     REAL(BDKi),     INTENT(IN   ):: uu0(:)
     REAL(BDKi),     INTENT(IN   ):: E10(:)
     INTEGER(IntKi), INTENT(IN   ):: node_elem        !< Node per element
     INTEGER(IntKi), INTENT(IN   ):: dof_node         !< Degrees of freedom per node
     REAL(BDKi),     INTENT(  OUT):: elf(:)           !< Total element force (Fd, Fc, Fb)
     INTEGER(IntKi), INTENT(  OUT):: ErrStat          !< Error status of the operation
     CHARACTER(*),   INTENT(  OUT):: ErrMsg           !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                   :: temp_Naaa(dof_node*node_elem)
     REAL(BDKi)                   :: RR0(3,3)
     REAL(BDKi)                   :: kapa(3)
     REAL(BDKi)                   :: E1(3)
     REAL(BDKi)                   :: Stif(6,6)
     REAL(BDKi)                   :: cet
     REAL(BDKi)                   :: uuu(6)
     REAL(BDKi)                   :: uup(3)
     REAL(BDKi)                   :: Fc(6)
     REAL(BDKi)                   :: Fd(6)
     REAL(BDKi)                   :: Oe(6,6)
     REAL(BDKi)                   :: Pe(6,6)
     REAL(BDKi)                   :: Qe(6,6)
     REAL(BDKi)                   :: vvv(6)
     REAL(BDKi)                   :: vvp(6)
     REAL(BDKi)                   :: mmm
     REAL(BDKi)                   :: mEta(3)
     REAL(BDKi)                   :: rho(3,3)
     REAL(BDKi)                   :: Fb(6)
     REAL(BDKi)                   :: Sd(6,6)
     REAL(BDKi)                   :: Od(6,6)
     REAL(BDKi)                   :: Pd(6,6)
     REAL(BDKi)                   :: Qd(6,6)
     REAL(BDKi)                   :: betaC(6,6)
     REAL(BDKi)                   :: Gd(6,6)
     REAL(BDKi)                   :: Xd(6,6)
     REAL(BDKi)                   :: Yd(6,6)
     REAL(BDKi)                   :: temp_aaa(6)
     LOGICAL                      :: fact
     INTEGER(IntKi)               :: igp
     INTEGER(IntKi)               :: i
     INTEGER(IntKi)               :: j
     INTEGER(IntKi)               :: temp_id1
     INTEGER(IntKi)               :: temp_id2
     INTEGER(IntKi)               :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)         :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER      :: RoutineName = 'BD_ElememntMatrixForce'
  
     ErrStat   = ErrID_None
     ErrMsg    = ""
     elf       = 0.0_BDKi
     temp_Naaa = 0.0_BDKi
     
     fact = .FALSE.
  
     DO igp=1,ngp
  
         temp_id1 = (igp-1)*dof_node
         temp_id2 = (igp-1)*dof_node/2
         Stif(:,:) = 0.0_BDKi
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx(:,igp),hpx(:,igp),Jaco(igp),Nuuu,Nrrr,&
                 uu0(temp_id1+1:temp_id1+6),E10(temp_id2+1:temp_id2+3),node_elem,dof_node,&
                 uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         mmm          =  EMass0_GL(1,1,igp)
         mEta(1)      =  0.0_BDKi
         mEta(2)      = -EMass0_GL(1,6,igp)
         mEta(3)      =  EMass0_GL(1,5,igp)
         rho          =  EMass0_GL(4:6,4:6,igp)
         CALL BD_GaussPointDataMass(hhx(:,igp),hpx(:,igp),Jaco(igp),Nvvv,temp_Naaa,RR0,&
               node_elem,dof_node, vvv,temp_aaa,vvp,mEta,rho,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(damp_flag .EQ. 1) THEN
             CALL BD_DissipativeForce(beta,Stif,vvv,vvp,E1,fact,Fc,Fd,Sd,Od,Pd,Qd,&
                                      betaC,Gd,Xd,Yd,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDIF
         CALL BD_GyroForce(mEta,rho,vvv,Fb,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         DO i=1,node_elem
             DO j=1,dof_node
                 temp_id1 = (i-1) * dof_node+j
                 elf(temp_id1) = elf(temp_id1) + hhx(i,igp)*Fb(j)*Jaco(igp)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) + hhx(i,igp)*Fd(j)*Jaco(igp)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) + hpx(i,igp)*Fc(j)*gw(igp)
             ENDDO
         ENDDO
  
         if (ErrStat >= AbortErrLev) then
            return
         end if
  
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_ElementMatrixForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes Global mass matrix and force vector to 
  !! calculate the forces along the beam
  SUBROUTINE BD_GenerateDynamicElementForce(uuN,vvN,aaN, u, p, m, RHS,ErrStat,ErrMsg)
     REAL(BDKi),            INTENT(IN   ):: uuN(:)       !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(IN   ):: vvN(:)       !< Velocity of Mass 1: m/s
     REAL(BDKi),            INTENT(IN   ):: aaN(:)       !< Velocity of Mass 1: m/s
     TYPE(BD_InputType),    INTENT(IN   ):: u            !< Inputs at t
     TYPE(BD_ParameterType),INTENT(IN   ):: p            !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m            !< misc/optimization variables      
     REAL(BDKi),            INTENT(  OUT):: RHS(:)       !< Right hand side of the equation Ax=B
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat      !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg       !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                   :: nelem        ! number of elements
     INTEGER(IntKi)                   :: j            ! Index counter
     INTEGER(IntKi)                   :: temp_id      ! Index counter
     INTEGER(IntKi)                   :: temp_id2
     INTEGER(IntKi)                   :: ErrStat2     ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2      ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_GenerateDynamicElementForce'
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     RHS(:)     = 0.0_BDKi
  
  
     DO nelem=1,p%elem_total
         CALL BD_ElemNodalDisp(uuN,p%node_elem,p%dof_node,nelem,m%Nuuu,ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_NodalRelRot(m%Nuuu,p%node_elem,p%dof_node,m%Nrrr,ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElemNodalDisp(vvN,p%node_elem,p%dof_node,nelem,m%Nvvv,ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElemNodalDisp(aaN,p%node_elem,p%dof_node,nelem,m%Naaa,ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(p%quadrature .EQ. 1) THEN
             temp_id = (nelem-1)*p%ngp + 1
             temp_id2 = (nelem-1)*p%ngp
         ELSEIF(p%quadrature .EQ. 2) THEN
             temp_id = (nelem-1)*p%ngp
             temp_id2= temp_id
         ENDIF
  
         DO j=1,p%ngp
             m%EStif0_GL(1:6,1:6,j) = p%Stif0_GL(1:6,1:6,temp_id2+j)
             m%EMass0_GL(1:6,1:6,j) = p%Mass0_GL(1:6,1:6,temp_id2+j)
             m%DistrLoad_GL(1:3,j) = u%DistrLoad%Force(1:3,temp_id+j)
             m%DistrLoad_GL(4:6,j) = u%DistrLoad%Moment(1:3,temp_id+j)
         ENDDO
  
         CALL BD_ElementMatrixForce(m%Nuuu,m%Nrrr,m%Nvvv,&
                                    m%EStif0_GL,m%EMass0_GL,     &
                                    p%damp_flag,p%beta,          &
                                    p%ngp,p%GLw,p%Shp,p%Der,p%Jacobian(:,nelem),p%uu0(:,nelem),p%E10(:,nelem),&
                                    p%node_elem,p%dof_node,m%elf,&
                                    ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         CALL BD_AssembleRHS(nelem,p%dof_elem,p%node_elem,p%dof_node,m%elf,RHS,ErrStat2,ErrMsg2)
             CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         if (ErrStat >= AbortErrLev) return
  
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_GenerateDynamicElementForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the finite-element nodal forces along the beam
  !! Nodal forces = \f$C \dot{u} + K u\f$
  SUBROUTINE BD_DynamicSolutionForce(uuN,vvN,aaN, u, p, m, Force,ErrStat,ErrMsg)
  
     REAL(BDKi),            INTENT(IN   ):: uuN(:)       !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(IN   ):: vvN(:)       !< Velocity of Mass 1: m/s
     REAL(BDKi),            INTENT(IN   ):: aaN(:)       !< Velocity of Mass 1: m/s
     TYPE(BD_InputType),    INTENT(IN   ):: u            !< Inputs at t   
     TYPE(BD_ParameterType),INTENT(IN   ):: p            !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m            !< misc/optimization variables      
     REAL(BDKi),            INTENT(  OUT):: Force(:)     !< finite-element nodal forces along the beam
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat      !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg       !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                   :: ErrStat2     ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2      ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_DynamicSolutionForce'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
  
     CALL BD_GenerateDynamicElementForce(uuN,vvN,aaN,u, p, m, Force,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     if (ErrStat >= AbortErrLev) RETURN
  
  
  END SUBROUTINE BD_DynamicSolutionForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> 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(np,ns,spts,npts,hhx,hpx,ErrStat,ErrMsg)
     INTEGER(IntKi),INTENT(IN   ):: np            !< polynomial order of basis functions
     INTEGER(IntKi),INTENT(IN   ):: ns            !< number of points at which to eval shape/deriv
     REAL(BDKi),    INTENT(IN   ):: spts(:)       !< spts(ns): location of ns points at which to eval
     REAL(BDKi),    INTENT(IN   ):: npts(:)       !< npts(np+1): location of the (np+1) GLL points
     REAL(BDKi),    INTENT(  OUT):: hhx(:,:)      !< ps(np+1,ns): (np+1) shape functions evaluated at ns points
     REAL(BDKi),    INTENT(  OUT):: hpx(:,:)      !< dPhis(np+1,ns): derivative evaluated at ns points 
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: dPhis(np+1,ns)   ! bjj: why not use hpx  
     REAL(BDKi)                  :: Ps(np+1,ns)      ! bjj: why not use hhx
     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
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     do j = 1,ns  
        do l = 1,np+1
           
         if ((abs(spts(j)-1.).LE.eps).AND.(l.EQ.np+1)) then
           dPhis(l,j) = REAL((np+1)*np, BDKi)/4.0_BDKi
         elseif ((abs(spts(j)+1.).LE.eps).AND.(l.EQ.1)) then
           dPhis(l,j) = -REAL((np+1)*np, BDKi)/4.0_BDKi
         elseif (abs(spts(j)-npts(l)).LE.eps) then
           dPhis(l,j) = 0.0_BDKi
         else
           dPhis(l,j) = 0.0_BDKi
           den = 1.0_BDKi          
           do i = 1,np+1
             if (i.NE.l) then
               den = den*(npts(l)-npts(i))
             endif
             dnum = 1.0_BDKi
             do k = 1,np+1
               if ((k.NE.l).AND.(k.NE.i).AND.(i.NE.l)) then
                 dnum = dnum*(spts(j)-npts(k))
               elseif (i.EQ.l) then
                 dnum = 0.0_BDKi
               endif
             enddo
             dPhis(l,j) = dPhis(l,j) + dnum
           enddo
           dPhis(l,j) = dPhis(l,j)/den
         endif
       enddo
     enddo
  
     do j = 1,ns  
        do l = 1,np+1
           
         if(abs(spts(j)-npts(l)).LE.eps) then
           Ps(l,j) = 1.0_BDKi
         else
           dnum = 1.0_BDKi
           den  = 1.0_BDKi
           do k = 1,np+1
             if (k.NE.l) then
               den  = den *(npts(l) - npts(k))
               dnum = dnum*(spts(j) - npts(k))
             endif
           enddo
           Ps(l,j) = dnum/den
         endif
       enddo
     enddo
  
     hhx(:,:) = Ps(:,:)
     hpx(:,:) = dPhis(:,:)
  
   END SUBROUTINE BD_diffmtc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the segment length, member length, and total length of a beam.
  !! It also computes the ration between the segment/member and total length.
  !! Segment: defined by two adjacent key points
  SUBROUTINE BD_ComputeMemberLength(member_total,kp_member,kp_coordinate,&
                                    Coef,seg_length,member_length,total_length,&
                                    ErrStat,ErrMsg)
     INTEGER(IntKi),INTENT(IN   ):: member_total
     INTEGER(IntKi),INTENT(IN   ):: kp_member(:)
     REAL(BDKi),    INTENT(IN   ):: Coef(:,:,:)
     REAL(BDKi),    INTENT(IN   ):: kp_coordinate(:,:)
     REAL(BDKi),    INTENT(  OUT):: seg_length(:,:)
     REAL(BDKi),    INTENT(  OUT):: member_length(:,:)
     REAL(BDKi),    INTENT(  OUT):: total_length
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: eta0
     REAL(BDKi)                  :: eta1
     REAL(BDKi)                  :: temp_pos0(3)
     REAL(BDKi)                  :: temp_pos1(3)
     REAL(BDKi)                  :: sample_step
     REAL(BDKi)                  :: temp
     INTEGER(IntKi)              :: sample_total
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: k
     INTEGER(IntKi)              :: m
     INTEGER(IntKi)              :: temp_id
     INTEGER(IntKi)              :: id0
     INTEGER(IntKi)              :: id1
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     sample_total = 3 !1001
  
     temp_id = 0
     DO i=1,member_total
         IF(i .EQ. 1) THEN
             id0 = 1
             id1 = kp_member(i)
         ELSE
             id0 = id1
             id1 = id0 + kp_member(i) - 1
         ENDIF
         DO m=1,kp_member(i)-1
             temp_id = temp_id + 1
             sample_step = (kp_coordinate(id0+m,1) - kp_coordinate(id0+m-1,1))/(sample_total-1)
             DO j=1,sample_total-1
                 eta0 = kp_coordinate(temp_id,1) + (j-1)*sample_step
                 eta1 = kp_coordinate(temp_id,1) + j*sample_step
                 DO k=1,3
                     temp_pos0(k) = Coef(temp_id,1,k) + Coef(temp_id,2,k)*eta0 + Coef(temp_id,3,k)*eta0*eta0 + Coef(temp_id,4,k)*eta0*eta0*eta0
                     temp_pos1(k) = Coef(temp_id,1,k) + Coef(temp_id,2,k)*eta1 + Coef(temp_id,3,k)*eta1*eta1 + Coef(temp_id,4,k)*eta1*eta1*eta1
                 ENDDO
                 temp_pos1(:) = temp_pos1(:) - temp_pos0(:)
                 seg_length(temp_id,1) = seg_length(temp_id,1) + SQRT(DOT_PRODUCT(temp_pos1,temp_pos1))
                 member_length(i,1) = member_length(i,1) + SQRT(DOT_PRODUCT(temp_pos1,temp_pos1))
             ENDDO
         ENDDO
         total_length = total_length + member_length(i,1)
     ENDDO
  
     temp_id = 0
     DO i=1,member_total
         temp = 0.0_BDKi
         DO j=1,kp_member(i)-1
             temp_id = temp_id + 1
             IF(j == 1) THEN
                 seg_length(temp_id,2) = 0.0_BDKi
             ELSE
                 seg_length(temp_id,2) = seg_length(temp_id-1,3)
             ENDIF
             temp = temp + seg_length(temp_id,1)
             seg_length(temp_id,3) = temp/member_length(i,1)
         ENDDO
     ENDDO
  
     DO i=1,member_total
         member_length(i,2) = member_length(i,1)/total_length
     ENDDO
  
  
  END SUBROUTINE BD_ComputeMemberLength
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the initial nodal locations given the coefficients for
  !! cubic spline fit. It also computes the unit tangent vector e1 for further use.
  SUBROUTINE BD_ComputeIniNodalPosition(Coef,eta,PosiVec,e1,Twist_Angle,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: Coef(:,:)     !< Coefficients for cubic spline interpolation
     REAL(BDKi),    INTENT(IN   ):: eta           !< Nodal location in [0,1] 
     REAL(BDKi),    INTENT(  OUT):: PosiVec(:)    !< Physical coordinates of GLL points in blade frame
     REAL(BDKi),    INTENT(  OUT):: e1(:)         !< Tangent vector
     REAL(BDKi),    INTENT(  OUT):: Twist_Angle   !< Twist angle at PosiVec
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)              :: i
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     PosiVec = 0.0_BDKi
     e1 = 0.0_BDKi
     DO i=1,3
         PosiVec(i) = Coef(i,1) + Coef(i,2)*eta + Coef(i,3)*eta*eta + Coef(i,4)*eta*eta*eta
         e1(i) = Coef(i,2) + 2.0_BDKi*Coef(i,3)*eta + 3.0_BDKi*Coef(i,4)*eta*eta
     ENDDO
     e1 = e1/TwoNorm(e1)
     Twist_Angle = 0.0_BDKi
     Twist_Angle = Coef(4,1) + Coef(4,2)*eta + Coef(4,3)*eta*eta + Coef(4,4)*eta*eta*eta
  
  END SUBROUTINE BD_ComputeIniNodalPosition
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes initial CRV parameters
  !! given geometry information
  SUBROUTINE BD_ComputeIniNodalCrv(e1,phi,cc,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: e1(:)       !< Unit tangent vector
     REAL(BDKi),    INTENT(IN   ):: phi         !< Initial twist angle
     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)                  :: e3(3)                     ! Unit e3 = e1 * e2, cross-product
     REAL(BDKi)                  :: Rr(3,3)                   ! Initial rotation matrix
     REAL(BDKi)                  :: temp
     REAL(BDKi)                  :: temp2
     REAL(BDKi)                  :: Delta
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_ComputeIniNodalCrv'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     Rr = 0.0_BDKi
     DO i=1,3
         Rr(i,1) = e1(i)
     ENDDO
  
     e2 = 0.0_BDKi
     temp = phi*D2R_D  ! convert to radians
     temp2 = ((e1(2)*COS(temp) + e1(3)*SIN(temp))/e1(1))
     Delta = SQRT(1.0_BDKi + temp2*temp2)
     e2(1) = -(e1(2)*COS(temp)+e1(3)*SIN(temp))/e1(1)
     e2(2) = COS(temp)
     e2(3) = SIN(temp)
     e2 = e2/Delta
     DO i=1,3
         Rr(i,2) = e2(i)
     ENDDO
  
     e3 = 0.0_BDKi
     e3 = Cross_Product(e1,e2)
     DO i=1,3
         Rr(i,3) = e3(i)
     ENDDO
  
     CALL BD_CrvExtractCrv(Rr,cc,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
  END SUBROUTINE BD_ComputeIniNodalCrv
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the coefficients for cubie-spline fit
  !! given key point locations. Clamped conditions are used at the
  !! two end nodes: f''(0) = f''(1) = 0 
  SUBROUTINE BD_ComputeIniCoef(kp_member,kp_coord,Coef,ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: kp_coord(:,:) !< Keypoints coordinates
     INTEGER(IntKi),INTENT(IN   ):: kp_member     !< Number of kps of each member
     REAL(BDKi),    INTENT(  OUT):: Coef(:,:,:)   !< Coefficients for cubie spline interpolation
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi),      ALLOCATABLE:: K(:,:)
     REAL(BDKi),      ALLOCATABLE:: RHS(:)
     INTEGER(IntKi),  ALLOCATABLE:: indx(:)
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: m
     INTEGER(IntKi)              :: temp_id1
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_ComputeIniCoef'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     CALL AllocAry( K, 4*(kp_member-1), 4*(kp_member-1), 'Coefficient matrix', ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry( RHS, 4*(kp_member-1),  'RHS', ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry( indx, 4*(kp_member-1),  'IPIV', ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     if (ErrStat < AbortErrLev) then
        
        DO i=1,4
            K(:,:) = 0.0_BDKi
            RHS(:) = 0.0_BDKi
  
            K(1,3) = 2.0_BDKi
            K(1,4) = 6.0_BDKi*kp_coord(1,1)
            RHS(1) = 0.0_BDKi
            DO j=1,kp_member-2
                temp_id1 = (j-1)*4
                K(temp_id1+2,temp_id1+1) = 1.0_BDKi
                K(temp_id1+2,temp_id1+2) = kp_coord(j,1)
                K(temp_id1+2,temp_id1+3) = kp_coord(j,1)**2
                K(temp_id1+2,temp_id1+4) = kp_coord(j,1)**3
                K(temp_id1+3,temp_id1+1) = 1.0_BDKi
                K(temp_id1+3,temp_id1+2) = kp_coord(j+1,1)
                K(temp_id1+3,temp_id1+3) = kp_coord(j+1,1)**2
                K(temp_id1+3,temp_id1+4) = kp_coord(j+1,1)**3
                K(temp_id1+4,temp_id1+2) = 1.0_BDKi
                K(temp_id1+4,temp_id1+3) = 2.0_BDKi*kp_coord(j+1,1)
                K(temp_id1+4,temp_id1+4) = 3.0_BDKi*kp_coord(j+1,1)**2
                K(temp_id1+4,temp_id1+6) = -1.0_BDKi
                K(temp_id1+4,temp_id1+7) = -2.0_BDKi*kp_coord(j+1,1)
                K(temp_id1+4,temp_id1+8) = -3.0_BDKi*kp_coord(j+1,1)**2
                K(temp_id1+5,temp_id1+3) = 2.0_BDKi
                K(temp_id1+5,temp_id1+4) = 6.0_BDKi*kp_coord(j+1,1)
                K(temp_id1+5,temp_id1+7) = -2.0_BDKi
                K(temp_id1+5,temp_id1+8) = -6.0_BDKi*kp_coord(j+1,1)
                RHS(temp_id1+2) = kp_coord(j,i)
                RHS(temp_id1+3) = kp_coord(j+1,i)
            ENDDO
            temp_id1 = (kp_member-2)*4
            K(temp_id1+2,temp_id1+1) = 1.0_BDKi
            K(temp_id1+2,temp_id1+2) = kp_coord(kp_member-1,1)
            K(temp_id1+2,temp_id1+3) = kp_coord(kp_member-1,1)**2
            K(temp_id1+2,temp_id1+4) = kp_coord(kp_member-1,1)**3
            K(temp_id1+3,temp_id1+1) = 1.0_BDKi
            K(temp_id1+3,temp_id1+2) = kp_coord(kp_member,1)
            K(temp_id1+3,temp_id1+3) = kp_coord(kp_member,1)**2
            K(temp_id1+3,temp_id1+4) = kp_coord(kp_member,1)**3
            K(temp_id1+4,temp_id1+3) = 2.0_BDKi
            K(temp_id1+4,temp_id1+4) = 6.0_BDKi*kp_coord(kp_member,1)
            RHS(temp_id1+2) = kp_coord(kp_member-1,i)
            RHS(temp_id1+3) = kp_coord(kp_member,i)
            RHS(temp_id1+4) = 0.0_BDKi
            CALL LAPACK_getrf( 4*(kp_member-1), 4*(kp_member-1), K,indx, ErrStat2, ErrMsg2) 
               CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            CALL LAPACK_getrs( 'N',4*(kp_member-1), K,indx,RHS, ErrStat2, ErrMsg2) 
               CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            DO j=1,kp_member-1
                DO m=1,4
                    temp_id1 = (j-1)*4+m
                    Coef(j,m,i) = RHS(temp_id1)
                ENDDO
            ENDDO
        ENDDO
        
     end if ! temp arrays are allocated
     
  
     if (allocated(K   )) deallocate(K)
     if (allocated(RHS )) deallocate(RHS)
     if (allocated(indx)) deallocate(indx)
  
  END SUBROUTINE BD_ComputeIniCoef
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_Static(t,n,u,utimes,p,x,xd,z,OtherState,m,ErrStat,ErrMsg)
  
     REAL(DbKi),                      INTENT(IN   ):: t           !< Current simulation time in seconds
     INTEGER(IntKi),                  INTENT(IN   ):: n           !< time step number
     REAL(DbKi),                      INTENT(IN   ):: utimes(:)   !< times of input
     TYPE(BD_ParameterType),          INTENT(IN   ):: p           !< Parameters
     TYPE(BD_DiscreteStateType),      INTENT(IN   ):: xd          !< Discrete states at t
     TYPE(BD_ConstraintStateType),    INTENT(IN   ):: z           !< Constraint states at t (possibly a guess)
     TYPE(BD_OtherStateType),         INTENT(INOUT):: OtherState  !< Other states at t
     TYPE(BD_MiscVarType),            INTENT(INOUT):: m           !< misc/optimization variables
     TYPE(BD_ContinuousStateType),    INTENT(INOUT):: x           !< Continuous states at t on input at t + dt on output
     TYPE(BD_InputType),              INTENT(INOUT):: u(:)        !< Inputs at t
     INTEGER(IntKi),                  INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),                    INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     TYPE(BD_InputType)                            :: u_interp
     TYPE(BD_InputType)                            :: u_temp
     INTEGER(IntKi)                                :: i
     INTEGER(IntKi)                                :: j
     INTEGER(IntKi)                                :: k
     INTEGER(IntKi)                                :: piter
     REAL(BDKi)                                    :: gravity_temp(3)
     INTEGER(IntKi)                                :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                          :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                       :: RoutineName = 'BD_Static'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
        ! allocate space for input type (mainly for meshes)
     CALL BD_CopyInput(u(2),u_interp,MESH_NEWCOPY,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg, RoutineName)
     CALL BD_CopyInput(u(2),u_temp,MESH_NEWCOPY,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg, RoutineName)
     
     if (ErrStat >= AbortErrLev) then
        call cleanup()
        return
     end if
           
     call BD_Input_extrapinterp( u, utimes, u_interp, t, ErrStat2, ErrMsg2 )
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
           
     CALL BD_InputGlobalLocal(p,u_interp,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg, RoutineName)
     ! Incorporate boundary conditions
     CALL BD_BoundaryGA2(x,p,u_interp,t,OtherState,m,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg, RoutineName)
  
     if (ErrStat >= AbortErrLev) then
        call cleanup()
        return
     end if
     i = 1
     piter = 0
     DO WHILE(i .NE. 0)
         k=i
         DO j=1,k
             u_temp%PointLoad%Force(:,:) = u_interp%PointLoad%Force(:,:)/i*j
             u_temp%PointLoad%Moment(:,:) = u_interp%PointLoad%Moment(:,:)/i*j
             u_temp%DistrLoad%Force(:,:) = u_interp%DistrLoad%Force(:,:)/i*j
             u_temp%DistrLoad%Moment(:,:) = u_interp%DistrLoad%Moment(:,:)/i*j
             gravity_temp(:) = p%gravity(:)/i*j
             CALL BD_StaticSolution(x%q, gravity_temp, u_temp, p, m, piter, ErrStat2, ErrMsg2)
                 call SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg, RoutineName)
             IF(p%niter .EQ. piter) EXIT
         ENDDO
         IF(piter .LT. p%niter) THEN
             i=0
         ELSE
             IF(i .EQ. p%niter) THEN
                 call SetErrStat( ErrID_Fatal, "Solution does not converge after the maximum number of load steps.", &
                                  ErrStat,ErrMsg, RoutineName)
                 CALL WrScr( "Maxium number of load steps reached. Exit BeamDyn")
                 EXIT
             ENDIF
             i=i+1
             call WrScr( "Warning: Load may be too large, BeamDyn will attempt to solve with additional steps.")
             call WrScr( "  Load_Step="//trim(num2lstr(i)) )
             x%q = 0.0_BDKi
         ENDIF
     ENDDO
     
     if (ErrStat >= AbortErrLev) then
        call cleanup()
        return
     end if
  
     call cleanup()
     return
     
  contains
     subroutine cleanup()
        CALL BD_DestroyInput(u_interp, ErrStat2, ErrMsg2 )
        CALL BD_DestroyInput(u_temp,   ErrStat2, ErrMsg2 )
     end subroutine cleanup
  END SUBROUTINE BD_Static
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_StaticSolution( uuNf,gravity,u, p, m, piter, ErrStat,ErrMsg)
  
     REAL(BDKi),            INTENT(IN   ):: gravity(:)  !< not the same as p%gravity
     TYPE(BD_InputType),    INTENT(IN   ):: u           !< inputs
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables
        
     REAL(BDKi),            INTENT(INOUT):: uuNf(:)
     INTEGER(IntKi),        INTENT(  OUT):: piter       !< ADDED piter AS OUTPUT
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     ! local variables
     REAL(BDKi),          ALLOCATABLE:: ui(:)
     REAL(BDKi)                      :: Eref
     REAL(BDKi)                      :: Enorm
     INTEGER(IntKi)                  :: i
     INTEGER(IntKi)                  :: j
     INTEGER(IntKi)                  :: k
     INTEGER(IntKi)                  :: temp_id
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER         :: RoutineName = 'BD_StaticSolution'
     
     ErrStat = ErrID_None
     ErrMsg  = ""
     
     CALL AllocAry(ui,      p%dof_total,'Increment vector',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
  
     Eref = 0.0_BDKi
     DO i=1,p%niter
         piter=i 
         CALL BD_GenerateStaticElement(uuNf, gravity, u, p, m, ErrStat2, ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         DO j=1,p%node_total
             temp_id = (j-1)*p%dof_node
             m%RHS(temp_id+1:temp_id+3) = m%RHS(temp_id+1:temp_id+3) + u%Pointload%Force(1:3,j)
             m%RHS(temp_id+4:temp_id+6) = m%RHS(temp_id+4:temp_id+6) + u%Pointload%Moment(1:3,j)
         ENDDO
  
         DO j=1,p%dof_total-6
             m%RHS_LU(j) = m%RHS(j+6)
         ENDDO
         DO k=1,p%dof_total-6
            DO j=1,p%dof_total-6
               m%StifK_LU(j,k) = m%StifK(j+6,k+6)
            ENDDO
         ENDDO
  
         CALL LAPACK_getrf( p%dof_total-6, p%dof_total-6, m%StifK_LU, m%indx, ErrStat2, ErrMsg2) 
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL LAPACK_getrs( 'N',p%dof_total-6, m%StifK_LU, m%indx, m%RHS_LU, ErrStat2, ErrMsg2) 
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         ui(:) = 0.0_BDKi
         DO j=1,p%dof_total-6
             ui(j+6) = m%RHS_LU(j)
         ENDDO
  
         CALL BD_StaticUpdateConfiguration(ui,uuNf,p%node_total,p%dof_node,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         
         IF(i .EQ. 1) THEN
             Eref = SQRT(abs(DOT_PRODUCT(m%RHS_LU, m%RHS(7:p%dof_total))))*p%tol
             IF(Eref .LE. p%tol) THEN
                 CALL Cleanup()
                 RETURN
             ENDIF
         ELSE  !IF(i .GT. 1) THEN
             Enorm = SQRT(abs(DOT_PRODUCT(m%RHS_LU, m%RHS(7:p%dof_total))))
             IF(Enorm .LE. Eref) THEN
                 CALL Cleanup()
                 RETURN
             ENDIF
         ENDIF
  
     ENDDO
  
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
  
     CALL Cleanup()
     RETURN
  
  contains
        subroutine Cleanup()
  
           if (allocated(ui         )) deallocate(ui         )
  
        end subroutine Cleanup
  END SUBROUTINE BD_StaticSolution
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_GenerateStaticElement( uuNf, gravity, u, p, m, ErrStat, ErrMsg)
  
     REAL(BDKi),            INTENT(IN   ):: uuNf(:)
     REAL(BDKi),            INTENT(IN   ):: gravity(:)
     TYPE(BD_InputType),    INTENT(IN   ):: u
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables
     
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                  :: nelem
     INTEGER(IntKi)                  :: j
     INTEGER(IntKi)                  :: temp_id
     INTEGER(IntKi)                  :: temp_id2
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER         :: RoutineName = 'BD_GenerateStaticElement'
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     m%StifK(:,:) = 0.0_BDKi
     m%RHS(:)     = 0.0_BDKi
  
     DO nelem=1,p%elem_total
         CALL BD_ElemNodalDisp(uuNf,p%node_elem,p%dof_node,nelem,m%Nuuu,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_NodalRelRot(m%Nuuu,p%node_elem,p%dof_node,m%Nrrr,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(p%quadrature .EQ. 1) THEN
             temp_id = (nelem-1)*p%ngp + 1
             temp_id2 = (nelem-1)*p%ngp
         ELSEIF(p%quadrature .EQ. 2) THEN
             temp_id = (nelem-1)*p%ngp
             temp_id2= temp_id
         ENDIF
  
         DO j=1,p%ngp
             m%EStif0_GL(   :,:,j) = p%Stif0_GL(1:6,1:6,temp_id2+j)
             m%EMass0_GL(   :,:,j) = p%Mass0_GL(1:6,1:6,temp_id2+j)
             m%DistrLoad_GL(1:3,j) = u%DistrLoad%Force(1:3,temp_id+j)
             m%DistrLoad_GL(4:6,j) = u%DistrLoad%Moment(1:3,temp_id+j)
         ENDDO
         CALL BD_StaticElementMatrix(m%Nuuu,m%Nrrr,&
                 m%DistrLoad_GL,gravity,m%EMass0_GL,m%EStif0_GL,&
                 p%ngp,p%GLw,p%Shp,p%Der,p%Jacobian(:,nelem),p%uu0(:,nelem),p%E10(:,nelem),&
                 p%node_elem,p%dof_node,m%elk,m%elf,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         CALL BD_AssembleStiffK(nelem,p%node_elem,p%dof_elem,p%dof_node,m%elk,m%StifK,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_AssembleRHS(nelem,p%dof_elem,p%node_elem,p%dof_node,m%elf,m%RHS,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ENDDO
  
     !if (ErrStat >= AbortErrLev) return
     RETURN
  
  END SUBROUTINE BD_GenerateStaticElement
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_StaticElementMatrix(Nuuu,Nrrr,Distr_GL,gravity,&
                                    EMass0_GL,EStif0_GL,&
                                    ngp,gw,hhx,hpx,Jaco,uu0,E10,&
                                    node_elem,dof_node,elk,elf,&
                                    ErrStat,ErrMsg)
  
     REAL(BDKi),    INTENT(IN   ):: Nuuu(:)
     REAL(BDKi),    INTENT(IN   ):: Nrrr(:)
     REAL(BDKi),    INTENT(IN   ):: Distr_GL(:,:)
     REAL(BDKi),    INTENT(IN   ):: gravity(:)
     REAL(BDKi),    INTENT(IN   ):: EMass0_GL(:,:,:)
     REAL(BDKi),    INTENT(IN   ):: EStif0_GL(:,:,:)
     INTEGER(IntKi),INTENT(IN   ):: ngp
     REAL(BDKi),    INTENT(IN   ):: gw(:)
     REAL(BDKi),    INTENT(IN   ):: hhx(:,:)
     REAL(BDKi),    INTENT(IN   ):: hpx(:,:)
     REAL(BDKi),    INTENT(IN   ):: Jaco(:)
     REAL(BDKi),    INTENT(IN   ):: uu0(:)
     REAL(BDKi),    INTENT(IN   ):: E10(:)
     INTEGER(IntKi),INTENT(IN   ):: node_elem
     INTEGER(IntKi),INTENT(IN   ):: dof_node
     REAL(BDKi),    INTENT(  OUT):: elk(:,:)
     REAL(BDKi),    INTENT(  OUT):: elf(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: temp_Nvvv(dof_node*node_elem)
     REAL(BDKi)                  :: temp_Naaa(dof_node*node_elem)
     REAL(BDKi)                  :: RR0(3,3)
     REAL(BDKi)                  :: kapa(3)
     REAL(BDKi)                  :: E1(3)
     REAL(BDKi)                  :: Stif(6,6)
     REAL(BDKi)                  :: cet
     REAL(BDKi)                  :: mmm
     REAL(BDKi)                  :: mEta(3)
     REAL(BDKi)                  :: rho(3,3)
     REAL(BDKi)                  :: uuu(6)
     REAL(BDKi)                  :: uup(3)
     REAL(BDKi)                  :: vvv(6)
     REAL(BDKi)                  :: vvp(6)
     REAL(BDKi)                  :: Fc(6)
     REAL(BDKi)                  :: Fd(6)
     REAL(BDKi)                  :: Fg(6)
     REAL(BDKi)                  :: Oe(6,6)
     REAL(BDKi)                  :: Pe(6,6)
     REAL(BDKi)                  :: Qe(6,6)
     REAL(BDKi)                  :: aaa(6)
     LOGICAL                     :: fact
     INTEGER(IntKi)              :: igp
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: m
     INTEGER(IntKi)              :: n
     INTEGER(IntKi)              :: temp_id1
     INTEGER(IntKi)              :: temp_id2
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_StaticElementMatrix'
  
     ErrStat  = ErrID_None
     ErrMsg   = ""
     elk(:,:) = 0.0_BDKi
     elf(:)   = 0.0_BDKi
  
        ! initialize for use in BD_GaussPointDataMass (though they are used to calculate vvv,aaa,vvp, which aren't technically needed)
     temp_Nvvv = 0.0_BDKi
     temp_Naaa = 0.0_BDKi
     
     fact = .TRUE.
  
  
     DO igp=1,ngp
  
         temp_id1 = (igp-1)*dof_node
         temp_id2 = (igp-1)*dof_node/2
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx(:,igp),hpx(:,igp),Jaco(igp),Nuuu,Nrrr,&
               uu0(temp_id1+1:temp_id1+6),E10(temp_id2+1:temp_id2+3),node_elem,dof_node,&
               uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         mmm          = EMass0_GL(1,1,igp)
         mEta(1)      = 0.0_BDKi
         mEta(2)      =-EMass0_GL(1,6,igp)
         mEta(3)      = EMass0_GL(1,5,igp)
         rho(1:3,1:3) = EMass0_GL(4:6,4:6,igp)
         CALL BD_GaussPointDataMass(hhx(:,igp),hpx(:,igp),Jaco(igp),temp_Nvvv,temp_Naaa,RR0,&
               node_elem,dof_node,vvv,aaa,vvp,mEta,rho,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_GravityForce(mmm,mEta,gravity,Fg,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         Fd(:) = Fd(:) - Fg(:) - Distr_GL(:,igp)
  
         DO i=1,node_elem
             DO j=1,node_elem
                 DO m=1,dof_node
                     temp_id1 = (i-1)*dof_node+m
                     DO n=1,dof_node
                         temp_id2 = (j-1)*dof_node+n
                         elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                            hhx(i,igp)*Qe(m,n)*hhx(j,igp)*Jaco(igp)*gw(igp)
                         elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                            hhx(i,igp)*Pe(m,n)*hpx(j,igp)*gw(igp)
                         elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                            hpx(i,igp)*Oe(m,n)*hhx(j,igp)*gw(igp)
                         elk(temp_id1,temp_id2) = elk(temp_id1,temp_id2) + &
                            hpx(i,igp)*Stif(m,n)*hpx(j,igp)*gw(igp)/Jaco(igp)
                     ENDDO
                 ENDDO
             ENDDO
         ENDDO
  
  
         DO i=1,node_elem
             DO j=1,dof_node
                 temp_id1 = (i-1) * dof_node+j
                 elf(temp_id1) = elf(temp_id1) - hhx(i,igp)*Fd(j)*Jaco(igp)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) - hpx(i,igp)*Fc(j)*gw(igp)
             ENDDO
         ENDDO
  
     ENDDO
  
     if (ErrStat >= AbortErrLev) then
         return
     end if
   
     RETURN
  
  END SUBROUTINE BD_StaticElementMatrix
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine updates the static configuration
  !! given incremental value calculated by the 
  !! Newton-Raphson algorithm
  SUBROUTINE BD_StaticUpdateConfiguration(uinc,uf,node_total,dof_node,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: uinc(:)
     INTEGER(IntKi),INTENT(IN   ):: node_total
     INTEGER(IntKi),INTENT(IN   ):: dof_node
     REAL(BDKi),    INTENT(INOUT):: uf(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: rotf_temp(3)
     REAL(BDKi)                  :: roti_temp(3)
     REAL(BDKi)                  :: rot_temp(3)
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: temp_id
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_StaticUpdateConfiguration'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=1, node_total
         temp_id   = (i - 1) * dof_node
         rotf_temp = 0.0_BDKi
         roti_temp = 0.0_BDKi
         rot_temp  = 0.0_BDKi
         DO j=1,3
             uf(temp_id+j) = uf(temp_id+j) + uinc(temp_id+j)
             rotf_temp(j)  = uf(temp_id+3+j)
             roti_temp(j)  = uinc(temp_id+3+j)
         ENDDO
         CALL BD_CrvCompose(rot_temp,roti_temp,rotf_temp,0,ErrStat2,ErrMsg2)
         DO j=1,3
             uf(temp_id+3+j) = rot_temp(j)
         ENDDO
     ENDDO
  
  END SUBROUTINE BD_StaticUpdateConfiguration
  !-----------------------------------------------------------------------------------------------------------------------------------
  ! This subroutine calculates the internal nodal forces at each finite-element 
  ! nodes along beam axis. \n
  ! Nodal forces = K u
  SUBROUTINE BD_StaticSolutionForce(uuN,vvN, p, m, u, Force, ErrStat,ErrMsg)
  
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables
     TYPE(BD_InputType),    INTENT(IN   ):: u           !< Inputs at t
     REAL(BDKi),            INTENT(IN   ):: uuN(:)      !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(IN   ):: vvN(:)      !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(  OUT):: Force(:)
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*),          PARAMETER:: RoutineName = 'BD_StaticSolutionForce'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     CALL BD_GenerateStaticElementForce(uuN,vvN,p, m, u, Force,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     IF(ErrStat >= AbortErrLev) RETURN
  
  END SUBROUTINE BD_StaticSolutionForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  ! This subroutine calculates the internal nodal forces at each finite-element 
  ! nodes along beam axis \n
  ! Nodal forces = K u
  SUBROUTINE BD_GenerateStaticElementForce(uuN,vvN, p, m, u, RHS, ErrStat,ErrMsg)
                                           
     REAL(BDKi),            INTENT(IN   ):: uuN(:)       !< Displacement of Mass 1: m
     REAL(BDKi),            INTENT(IN   ):: vvN(:)       !< Displacement of Mass 1: m
     TYPE(BD_ParameterType),INTENT(IN   ):: p            !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m            !< misc/optimization variables
     TYPE(BD_InputType),    INTENT(IN   ):: u            !< Inputs at t
     REAL(BDKi),            INTENT(  OUT):: RHS(:)       !< Right hand side of the equation Ax=B
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat      !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg       !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                :: nelem ! number of elements
     INTEGER(IntKi)                :: j ! Index counter
     INTEGER(IntKi)                :: temp_id ! Index counter
     INTEGER(IntKi)                :: temp_id2
     INTEGER(IntKi)                :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)          :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*),        PARAMETER:: RoutineName = 'BD_GenerateStaticElementForce'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     RHS(:)  = 0.0_BDKi
  
  
     DO nelem=1,p%elem_total
         CALL BD_ElemNodalDisp(uuN,p%node_elem,p%dof_node,nelem,m%Nuuu,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_NodalRelRot(m%Nuuu,p%node_elem,p%dof_node,m%Nrrr,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElemNodalDisp(vvN,p%node_elem,p%dof_node,nelem,m%Nvvv,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         IF(p%quadrature .EQ. 1) THEN
             temp_id = (nelem-1)*p%ngp + 1
             temp_id2 = (nelem-1)*p%ngp
         ELSEIF(p%quadrature .EQ. 2) THEN
             temp_id = (nelem-1)*p%ngp
             temp_id2= temp_id
         ENDIF
  
         DO j=1,p%ngp
             m%EStif0_GL(:,:,j)    = p%Stif0_GL(1:6,1:6,temp_id2+j)
             m%EMass0_GL(:,:,j)    = p%Mass0_GL(1:6,1:6,temp_id2+j)
             m%DistrLoad_GL(1:3,j) = u%DistrLoad%Force(1:3,temp_id+j)
             m%DistrLoad_GL(4:6,j) = u%DistrLoad%Moment(1:3,temp_id+j)
         ENDDO
         CALL BD_StaticElementMatrixForce(m%Nuuu,m%Nrrr,m%EStif0_GL,p%ngp,p%GLw,p%Shp,p%Der,p%Jacobian(:,nelem),p%uu0(:,nelem),p%E10(:,nelem),&
                 p%node_elem,p%dof_node,m%elf,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         CALL BD_AssembleRHS(nelem,p%dof_elem,p%node_elem,p%dof_node,m%elf,RHS,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_GenerateStaticElementForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates elemental internal node force for static analysis
  SUBROUTINE BD_StaticElementMatrixForce(Nuuu,Nrrr,EStif0_GL,&
                ngp,gw,hhx,hpx,Jaco,uu0,E10,                     &
                node_elem,dof_node,elf,ErrStat,ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: Nuuu(:)          !< Nodal displacement of Mass 1 for each element
     REAL(BDKi),    INTENT(IN   ):: Nrrr(:)          !< Nodal rotation parameters for displacement of Mass 1
     REAL(BDKi),    INTENT(IN   ):: EStif0_GL(:,:,:) !< Nodal material properties for each element
     INTEGER(IntKi),INTENT(IN   ):: ngp              !< Number of Gauss points
     REAL(BDKi),    INTENT(IN   ):: gw(:)
     REAL(BDKi),    INTENT(IN   ):: hhx(:,:)
     REAL(BDKi),    INTENT(IN   ):: hpx(:,:)
     REAL(BDKi),    INTENT(IN   ):: Jaco(:)
     REAL(BDKi),    INTENT(IN   ):: uu0(:)
     REAL(BDKi),    INTENT(IN   ):: E10(:)
     INTEGER(IntKi),INTENT(IN   ):: node_elem        !< Node per element
     INTEGER(IntKi),INTENT(IN   ):: dof_node         !< Degrees of freedom per node
     REAL(BDKi),    INTENT(  OUT):: elf(:)           !< Total element force (Fd, Fc, Fb)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat          !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg           !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: RR0(3,3)
     REAL(BDKi)                  :: kapa(3)
     REAL(BDKi)                  :: E1(3)
     REAL(BDKi)                  :: Stif(6,6)
     REAL(BDKi)                  :: cet
     REAL(BDKi)                  :: uuu(6)
     REAL(BDKi)                  :: uup(3)
     REAL(BDKi)                  :: Fc(6)
     REAL(BDKi)                  :: Fd(6)
     REAL(BDKi)                  :: Oe(6,6)
     REAL(BDKi)                  :: Pe(6,6)
     REAL(BDKi)                  :: Qe(6,6)
     LOGICAL                     :: fact
     INTEGER(IntKi)              :: igp
     INTEGER(IntKi)              :: i
     INTEGER(IntKi)              :: j
     INTEGER(IntKi)              :: temp_id1
     INTEGER(IntKi)              :: temp_id2
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_StaticElementMatrixForce'
  
     ErrStat  = ErrID_None
     ErrMsg   = ""
     elf(:)   = 0.0_BDKi
  
     fact = .FALSE.
  
     DO igp=1,ngp
  
         temp_id1 = (igp - 1) * dof_node
         temp_id2 = (igp - 1) * (dof_node/2)
         Stif(:,:) = 0.0_BDKi
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx(:,igp),hpx(:,igp),Jaco(igp),Nuuu,Nrrr,&
               uu0(temp_id1+1:temp_id1+6),E10(temp_id2+1:temp_id2+3),node_elem,dof_node,&
               uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,fact,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         DO i=1,node_elem
             DO j=1,dof_node
                 temp_id1 = (i-1) * dof_node+j
                 elf(temp_id1) = elf(temp_id1) + hhx(i,igp)*Fd(j)*Jaco(igp)*gw(igp)
                 elf(temp_id1) = elf(temp_id1) + hpx(i,igp)*Fc(j)*gw(igp)
             ENDDO
         ENDDO
     ENDDO
  
     if (ErrStat >= AbortErrLev) then
         return
     end if
  
     RETURN
  
  END SUBROUTINE BD_StaticElementMatrixForce
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine performs time marching from t_i to t_f
  SUBROUTINE BD_GA2(t,n,u,utimes,p,x,xd,z,OtherState,m,ErrStat,ErrMsg)
  
     REAL(DbKi),                        INTENT(IN   )  :: t           !< Current simulation time in seconds
     INTEGER(IntKi),                    INTENT(IN   )  :: n           !< time step number
     TYPE(BD_InputType),                INTENT(INOUT)  :: u(:)        !< Inputs at t
     REAL(DbKi),                        INTENT(IN   )  :: utimes(:)   !< times of input
     TYPE(BD_ParameterType),            INTENT(IN   )  :: p           !< Parameters
     TYPE(BD_ContinuousStateType),      INTENT(INOUT)  :: x           !< Continuous states at t on input at t + dt on output
     TYPE(BD_DiscreteStateType),        INTENT(INOUT)  :: xd          !< Discrete states at t
     TYPE(BD_ConstraintStateType),      INTENT(IN   )  :: z           !< Constraint states at t (possibly a guess)
     TYPE(BD_OtherStateType),           INTENT(INOUT)  :: OtherState  !< Other states at t on input; at t+dt on outputs
     TYPE(BD_MiscVarType),              INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),                    INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                      INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     ! local variables
     TYPE(BD_ContinuousStateType)                       :: x_tmp      ! Holds temporary modification to x
     TYPE(BD_OtherStateType     )                       :: OS_tmp     ! Holds temporary modification to x
     TYPE(BD_InputType)                                 :: u_interp   ! interpolated value of inputs
     INTEGER(IntKi)                                     :: ErrStat2   ! Temporary Error status
     CHARACTER(ErrMsgLen)                               :: ErrMsg2    ! Temporary Error message
     CHARACTER(*), PARAMETER                            :: RoutineName = 'BD_GA2'
  
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     CALL BD_CopyInput(u(1), u_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
           
     CALL BD_CopyContState(x, x_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) 
        
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
     ! initialize accelerations here:
  !   if ( .not. OtherState%InitAcc) then
     if ( n .EQ. 0 .or. .not. OtherState%InitAcc) then
        !Qi, call something to initialize
        call BD_Input_extrapinterp( u, utimes, u_interp, t, ErrStat2, ErrMsg2 )
            call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
        CALL BD_InitAcc( t, u_interp, p, x_tmp, OtherState, m, ErrStat2, ErrMsg2)
            call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
        OtherState%InitAcc = .true. 
  
        if (m%Un_Sum > 0) then
           ! Transform quantities from global frame to local (blade) frame
           CALL BD_InputGlobalLocal(p,u_interp,ErrStat2,ErrMsg2)
              call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
        
           ! Incorporate boundary conditions
           CALL BD_BoundaryGA2(x,p,u_interp,t,OtherState,m,ErrStat2,ErrMsg2)
                 call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
  
           ! find x, acc, and xcc at t+dt
           CALL BD_ComputeMKGA2( x%q,x%dqdt,OtherState%acc,OtherState%xcc,u_interp, p, m, ErrStat2, ErrMsg2)
              call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
  
           WRITE(m%Un_Sum,'()')
           CALL WrMatrix(m%StifK,m%Un_Sum, p%OutFmt, 'Full stiffness matrix')
           WRITE(m%Un_Sum,'()')
           CALL WrMatrix(m%MassM, m%Un_Sum, p%OutFmt, 'Full mass matrix')
            
            CLOSE(m%Un_Sum)
            m%Un_Sum = -1
        end if
        
     end if
  
     CALL BD_CopyOtherState(OtherState, OS_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
  
     call BD_Input_extrapinterp( u, utimes, u_interp, t+p%dt, ErrStat2, ErrMsg2 )
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
  
     CALL BD_UpdateDiscState( t, n, u_interp, p, x, xd, z, OtherState, m, ErrStat2, ErrMsg2 )
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
              
  ! Actuator
     IF( p%UsePitchAct ) THEN
        CALL PitchActuator_SetBC(p, u_interp, xd)      
     ENDIF
  ! END Actuator
  
     ! GA2: prediction        
     CALL BD_TiSchmPredictorStep( x_tmp%q,x_tmp%dqdt,OS_tmp%acc,OS_tmp%xcc,             &
                                  p%coef,p%dt,x%q,x%dqdt,OtherState%acc,OtherState%xcc, &
                                  p%node_total,p%dof_node, ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
     ! Transform quantities from global frame to local (blade) frame
     CALL BD_InputGlobalLocal(p,u_interp,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
     
     ! Incorporate boundary conditions
     CALL BD_BoundaryGA2(x,p,u_interp,t+p%dt,OtherState,m,ErrStat2,ErrMsg2)
           call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
  
     ! find x, acc, and xcc at t+dt
     CALL BD_DynamicSolutionGA2( x%q,x%dqdt,OtherState%acc,OtherState%xcc,u_interp, p, m, ErrStat2, ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
  
     call cleanup()
     return
     
  contains
     subroutine cleanup()
        CALL BD_DestroyInput(u_interp, ErrStat2, ErrMsg2)
        CALL BD_DestroyContState(x_tmp, ErrStat2, ErrMsg2 )
        CALL BD_DestroyOtherState(OS_tmp, ErrStat2, ErrMsg2 )
     end subroutine cleanup   
  END SUBROUTINE BD_GA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the predicted values (initial guess)
  !! of u,v,acc, and xcc in generalized-alpha algorithm
  SUBROUTINE BD_TiSchmPredictorStep(uuNi,vvNi,aaNi,xxNi,coef,deltat,&
                  uuNf,vvNf,aaNf,xxNf,node_total,dof_node, &
                  ErrStat, ErrMsg)
     REAL(BDKi),    INTENT(IN   ):: uuNi(:)
     REAL(BDKi),    INTENT(IN   ):: vvNi(:)
     REAL(BDKi),    INTENT(IN   ):: aaNi(:)
     REAL(BDKi),    INTENT(IN   ):: xxNi(:)
     REAL(DbKi),    INTENT(IN   ):: deltat
     REAL(DbKi),    INTENT(IN   ):: coef(:)
     REAL(BDKi),    INTENT(INOUT):: uuNf(:)
     REAL(BDKi),    INTENT(INOUT):: vvNf(:)
     REAL(BDKi),    INTENT(INOUT):: aaNf(:)
     REAL(BDKi),    INTENT(INOUT):: xxNf(:)
     INTEGER(IntKi),INTENT(IN   ):: node_total
     INTEGER(IntKi),INTENT(IN   ):: dof_node
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  ::vi
     REAL(BDKi)                  ::ai
     REAL(BDKi)                  ::xi
     REAL(BDKi)                  ::tr(6)
     REAL(BDKi)                  ::tr_temp(3)
     REAL(BDKi)                  ::uuNi_temp(3)
     REAL(BDKi)                  ::rot_temp(3)
     INTEGER                     ::i
     INTEGER                     ::j
     INTEGER                     ::temp_id
     INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_TimSchmPredictorStep'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     DO i=1,node_total
  
         DO j=1,6
             temp_id = (i - 1) * dof_node + j
             vi = vvNi(temp_id)
             ai = aaNi(temp_id)
             xi = xxNi(temp_id)
             tr(j) = deltat * vi + coef(1) * ai + coef(2) * xi
             vvNf(temp_id) = vi + coef(3) * ai + coef(4) * xi
             aaNf(temp_id) = 0.0_BDKi
             xxNf(temp_id) = coef(5) * ai + coef(6) * xi
         ENDDO
  
         tr_temp = 0.0_BDKi
         uuNi_temp = 0.0_BDKi
         DO j=1,3
             temp_id = (i - 1) * dof_node + j
             uuNf(temp_id) = uuNi(temp_id) + tr(j)
             tr_temp(j) = tr(j+3)
             uuNi_temp(j) = uuNi(temp_id + 3)
         ENDDO
         rot_temp = 0.0_BDKi
         CALL BD_CrvCompose(rot_temp,tr_temp,uuNi_temp,0,ErrStat2,ErrMsg2)
           call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
         DO j=1,3
             temp_id = (i - 1) * dof_node +j
             uuNf(temp_id + 3) = rot_temp(j)
         ENDDO
  
     ENDDO
  
  END SUBROUTINE BD_TiSchmPredictorStep
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calculates the coefficients used in generalized-alpha
  !! time integrator
  SUBROUTINE BD_TiSchmComputeCoefficients(rhoinf,deltat,coef,ErrStat,ErrMsg)
  
     REAL(DbKi),    INTENT(IN   ):: rhoinf
     REAL(DbKi),    INTENT(IN   ):: deltat
     REAL(DbKi),    INTENT(  OUT):: coef(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     REAL(DbKi)                  :: tr0
     REAL(DbKi)                  :: tr1
     REAL(DbKi)                  :: tr2
     REAL(DbKi)                  :: alfam
     REAL(DbKi)                  :: alfaf
     REAL(DbKi)                  :: gama
     REAL(DbKi)                  :: beta
     REAL(DbKi)                  :: oalfaM
     REAL(DbKi)                  :: deltat2
     CHARACTER(*),      PARAMETER:: RoutineName = 'BD_TiSchmComputeCoefficients'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     tr0 = rhoinf + 1.0_BDKi
     alfam = (2.0_BDKi * rhoinf - 1.0_BDKi) / tr0
     alfaf = rhoinf / tr0
     gama = 0.5_BDKi - alfam + alfaf
     beta = 0.25 * (1.0_BDKi - alfam + alfaf) * (1.0_BDKi - alfam + alfaf)
  
     deltat2 = deltat * deltat
     oalfaM = 1.0_BDKi - alfam
     tr0 = alfaf / oalfaM
     tr1 = alfam / oalfaM
     tr2 = (1.0_BDKi - alfaf) / oalfaM
  
     coef(1) = beta * tr0 * deltat2
     coef(2) = (0.5_BDKi - beta/oalfaM) * deltat2
     coef(3) = gama * tr0 * deltat
     coef(4) = (1.0_BDKi - gama / oalfaM) * deltat
     coef(5) = tr0
     coef(6) = -tr1
     coef(7) = gama * tr2 * deltat
     coef(8) = beta * tr2 * deltat2
     coef(9) = tr2
  
  END SUBROUTINE BD_TiSchmComputeCoefficients
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine applies the prescribed boundary conditions
  !! into states and otherstates at the root finite element node
  SUBROUTINE BD_BoundaryGA2(x,p,u,t,OtherState,m, ErrStat,ErrMsg)
  
     TYPE(BD_InputType),           INTENT(IN   )  :: u           !< Inputs at t
     REAL(DbKi),                   INTENT(IN   )  :: t           !< time (s)
     TYPE(BD_ContinuousStateType), INTENT(INOUT)  :: x           !< Continuous states at t
     TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Inputs at t
     TYPE(BD_OtherStateType),      INTENT(INOUT)  :: OtherState  !< Continuous states at t
     TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< misc/optimization variables
     INTEGER(IntKi),               INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                                   :: temp_cc(3)
     REAL(BDKi)                                   :: temp3(3)
     REAL(BDKi)                                   :: temp33(3,3)
     INTEGER(IntKi)                               :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                         :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                      :: RoutineName = 'BD_BoundaryGA2'
  
     ErrStat = ErrID_None
     ErrMsg = ""
     
     ! Root displacements
     x%q(1:3) = u%RootMotion%TranslationDisp(1:3,1)
     ! Root rotations
     temp33=u%RootMotion%Orientation(:,:,1) !possible type conversion
     CALL BD_CrvExtractCrv(temp33,temp3,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CrvCompose(temp_cc,temp3,p%Glb_crv,2,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     x%q(4:6) = MATMUL(TRANSPOSE(p%GlbRot),temp_cc)
     ! Root velocities/angular velocities and accelerations/angular accelerations
     x%dqdt(1:3) = u%RootMotion%TranslationVel(1:3,1)
     x%dqdt(4:6) = u%Rootmotion%RotationVel(1:3,1)
     OtherState%acc(1:3) = u%RootMotion%TranslationAcc(1:3,1)
     OtherState%acc(4:6) = u%RootMotion%RotationAcc(1:3,1)
  
  END SUBROUTINE BD_BoundaryGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine perform time-marching in one interval
  !! Given states (u,v) and accelerations (acc,xcc) at the initial of a time step (t_i),
  !! it returns the values of states and accelerations at the end of a time step (t_f)
  SUBROUTINE BD_DynamicSolutionGA2( uuNf,vvNf,aaNf,xxNf, u, p, m, ErrStat, ErrMsg)
  
     TYPE(BD_InputType), INTENT(IN   ):: u
     REAL(BDKi),         INTENT(INOUT):: uuNf(:)
     REAL(BDKi),         INTENT(INOUT):: vvNf(:)
     REAL(BDKi),         INTENT(INOUT):: aaNf(:)
     REAL(BDKi),         INTENT(INOUT):: xxNf(:)
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables   
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                   :: ErrStat2    ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2     ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_DynamicSolutionGA2'         
     REAL(BDKi),           ALLOCATABLE:: DampG(:,:)
     REAL(BDKi),           ALLOCATABLE:: ai(:)
     REAL(DbKi)                       :: Eref
     REAL(DbKi)                       :: Enorm
     INTEGER(IntKi)                   :: i
     INTEGER(IntKi)                   :: j
     INTEGER(IntKi)                   :: k
     INTEGER(IntKi)                   :: temp_id
     LOGICAL                          :: fact
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     
  
     CALL AllocAry(DampG,p%dof_total,p%dof_total,'Damping Matrix',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
     CALL AllocAry(ai,p%dof_total,'Increment vector',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
  
     Eref = 0.0_BDKi
     DO i=1,p%niter
  
         IF(MOD(i-1,p%n_fact) .EQ. 0) THEN
             fact = .TRUE.
         ELSE
             fact = .FALSE.
         ENDIF
  
         CALL BD_GenerateDynamicElementGA2(uuNf,vvNf,aaNf, p, m, u,fact,DampG, ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
            if (ErrStat >= AbortErrLev) then
                call Cleanup()
                return
            end if
  
        DO j=1,p%node_total
           temp_id = (j-1)*p%dof_node
           m%F_PointLoad(temp_id+1:temp_id+3) = u%PointLoad%Force(1:3,j)
           m%F_PointLoad(temp_id+4:temp_id+6) = u%PointLoad%Moment(1:3,j)
        ENDDO
        m%RHS(:) = m%RHS(:) + m%F_PointLoad(:)
        DO j=1,p%dof_total-6
           m%RHS_LU(j) = m%RHS(j+6)
        ENDDO
     
        IF(fact) THEN
            m%StifK = m%MassM + p%coef(7) * DampG + p%coef(8) * m%StifK
            
           DO k=1,p%dof_total-6
              DO j=1,p%dof_total-6
                 m%StifK_LU(j,k) = m%StifK(j+6,k+6)
              ENDDO
           ENDDO
            
           CALL LAPACK_getrf( p%dof_total-6, p%dof_total-6, m%StifK_LU, m%indx, ErrStat2, ErrMsg2) ! note m%indx is allocated larger than necessary (to allow us to use it in multiple places)
           CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
               if (ErrStat >= AbortErrLev) then
                   call Cleanup()
                   return
               end if
        ENDIF
        CALL LAPACK_getrs( 'N',p%dof_total-6, m%StifK_LU, m%indx, m%RHS_LU, ErrStat2, ErrMsg2) 
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
        ai = 0.0_BDKi
        DO j=1,p%dof_total-6
           ai(j+6) = m%RHS_LU(j)
        ENDDO
            
            
         Enorm = SQRT(abs(DOT_PRODUCT(m%RHS_LU, m%RHS(7:p%dof_total))))
            
         IF(i==1) THEN
             Eref = Enorm*p%tol
             IF(Enorm .LE. 1.0_DbKi) THEN
                 CALL Cleanup()
                 RETURN
             ENDIF
         ELSE 
             IF(Enorm .LE. Eref) THEN
                 CALL Cleanup()
                 RETURN
             ENDIF
         ENDIF
  
         CALL BD_UpdateDynamicGA2(ai,uuNf,vvNf,aaNf,xxNf,p%coef,p%node_total,p%dof_node,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     ENDDO
     
     CALL setErrStat( ErrID_Fatal, "Solution does not converge after the maximum number of iterations", ErrStat, ErrMsg, RoutineName)
     CALL Cleanup()
     RETURN
  
  contains
        subroutine Cleanup()
  
           if (allocated(DampG      )) deallocate(DampG      )
           if (allocated(ai         )) deallocate(ai         )
  
        end subroutine Cleanup
  END SUBROUTINE BD_DynamicSolutionGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine perform time-marching in one interval
  !! Given states (u,v) and accelerations (acc,xcc) at the initial of a time step (t_i),
  !! it returns the values of states and accelerations at the end of a time step (t_f)
  SUBROUTINE BD_ComputeMKGA2( uuNf,vvNf,aaNf,xxNf, u, p, m, ErrStat, ErrMsg)
  
     TYPE(BD_InputType), INTENT(IN   ):: u
     REAL(BDKi),         INTENT(INOUT):: uuNf(:)
     REAL(BDKi),         INTENT(INOUT):: vvNf(:)
     REAL(BDKi),         INTENT(INOUT):: aaNf(:)
     REAL(BDKi),         INTENT(INOUT):: xxNf(:)
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables   
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                   :: ErrStat2    ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2     ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_ComputeMKGA2'         
     REAL(BDKi),           ALLOCATABLE:: DampG(:,:)
     INTEGER(IntKi)                   :: i
     INTEGER(IntKi)                   :: j
     INTEGER(IntKi)                   :: k
  
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     
  
     CALL AllocAry(DampG,p%dof_total,p%dof_total,'Damping Matrix',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
    
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
  
     CALL BD_GenerateDynamicElementGA2(uuNf,vvNf,aaNf, p, m, u,.TRUE.,DampG, ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
            call Cleanup()
            return
        end if
     CALL Cleanup()
     RETURN
  
  contains
        subroutine Cleanup()
  
           if (allocated(DampG      )) deallocate(DampG      )
  
        end subroutine Cleanup
  END SUBROUTINE BD_ComputeMKGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  ! this routine computes m%MassM, m%RHS, m%StifK
  SUBROUTINE BD_GenerateDynamicElementGA2(uuNf,vvNf,aaNf, p, m,         &
                                          u,fact,DampG, &
                                          ErrStat,ErrMsg)
  
     REAL(BDKi),        INTENT(IN   ):: uuNf(:)
     REAL(BDKi),        INTENT(IN   ):: vvNf(:)
     REAL(BDKi),        INTENT(IN   ):: aaNf(:)
     TYPE(BD_InputType),INTENT(IN   ):: u
     LOGICAL,           INTENT(IN   ):: fact
     REAL(BDKi),        INTENT(  OUT):: DampG(:,:)
     
     TYPE(BD_ParameterType),INTENT(IN   ):: p           !< Parameters
     TYPE(BD_MiscVarType),  INTENT(INOUT):: m           !< misc/optimization variables      
     INTEGER(IntKi),        INTENT(  OUT):: ErrStat     !< Error status of the operation
     CHARACTER(*),          INTENT(  OUT):: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                  :: nelem
     INTEGER(IntKi)                  :: j
     INTEGER(IntKi)                  :: temp_id
     INTEGER(IntKi)                  :: temp_id2
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*),          PARAMETER:: RoutineName = 'BD_GenerateDynamicElementGA2'
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     
        ! must initialize these because BD_AssembleStiffK and BD_AssembleRHS are INOUT
     DampG(:,:) = 0.0_BDKi
     m%StifK = 0.0_BDKi
     m%MassM = 0.0_BDKi
     m%RHS = 0.0_BDKi
  
  
     DO nelem=1,p%elem_total
         CALL BD_ElemNodalDisp(uuNf,p%node_elem,p%dof_node,nelem,m%Nuuu,ErrStat2,ErrMsg2) ! set m%Nuuu
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_NodalRelRot(m%Nuuu,p%node_elem,p%dof_node,m%Nrrr,ErrStat2,ErrMsg2)       ! set m%Nrrr
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )          
         CALL BD_ElemNodalDisp(vvNf,p%node_elem,p%dof_node,nelem,m%Nvvv,ErrStat2,ErrMsg2) ! set m%Nvvv
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         CALL BD_ElemNodalDisp(aaNf,p%node_elem,p%dof_node,nelem,m%Naaa,ErrStat2,ErrMsg2) ! set m%Naaa
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         IF(p%quadrature .EQ. 1) THEN
             temp_id = (nelem-1)*p%ngp + 1
             temp_id2 = (nelem-1)*p%ngp
         ELSEIF(p%quadrature .EQ. 2) THEN
             temp_id = (nelem-1)*p%ngp
             temp_id2= temp_id
         ENDIF
  
         DO j=1,p%ngp
             m%EStif0_GL(:,:,j) = p%Stif0_GL(1:6,1:6,temp_id2+j)
             m%EMass0_GL(:,:,j) = p%Mass0_GL(1:6,1:6,temp_id2+j)
             m%DistrLoad_GL(1:3,j) = u%DistrLoad%Force(1:3,temp_id+j)
             m%DistrLoad_GL(4:6,j) = u%DistrLoad%Moment(1:3,temp_id+j)
         ENDDO
              
           ! compute m%elk,m%elf,m%elm,m%elg:     
         CALL BD_ElementMatrixGA2(m%Nuuu,m%Nrrr,m%Nvvv,m%Naaa,&
                                m%EStif0_GL,m%EMass0_GL,p%gravity,m%DistrLoad_GL,         &
                                p%damp_flag,p%beta,                                   &
                                p%ngp,p%GLw,p%Shp,p%Der,p%Jacobian(:,nelem),p%uu0(:,nelem),p%E10(:,nelem),&
                                p%node_elem,p%dof_node,fact,m%elk,m%elf,m%elm,m%elg,          &
                                ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
         IF(fact) THEN
             CALL BD_AssembleStiffK(nelem,p%node_elem,p%dof_elem,p%dof_node,m%elk,m%StifK,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
             CALL BD_AssembleStiffK(nelem,p%node_elem,p%dof_elem,p%dof_node,m%elm,m%MassM,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
             CALL BD_AssembleStiffK(nelem,p%node_elem,p%dof_elem,p%dof_node,m%elg,DampG,ErrStat2,ErrMsg2)
                CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         ENDIF
         CALL BD_AssembleRHS(nelem,p%dof_elem,p%node_elem,p%dof_node,m%elf,m%RHS,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
        if (ErrStat >= AbortErrLev) return
     ENDDO
  
     RETURN
  
  END SUBROUTINE BD_GenerateDynamicElementGA2
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine tranforms the folloing quantities in Input data structure
  !! from global frame to local (blade) frame:
  !! 1 Displacements; 2 Linear/Angular velocities; 3 Linear/Angular accelerations
  !! 4 Point forces/moments; 5 Distributed forces/moments
  !! It also transforms the DCM to rotation tensor in the input data structure
  SUBROUTINE BD_InputGlobalLocal( p, u, ErrStat, ErrMsg)
  
     TYPE(BD_ParameterType), INTENT(IN   ):: p
     TYPE(BD_InputType),     INTENT(INOUT):: u
     INTEGER(IntKi),         INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),           INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
                                                             ! 1: Blade to Global
     REAL(BDKi)                           :: temp33(3,3)
     REAL(BDKi)                           :: temp_v(3)
     REAL(BDKi)                           :: temp_v2(3)
     INTEGER(IntKi)                       :: i
     INTEGER(IntKi)                       :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                 :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER              :: RoutineName = 'BD_InputGlobalLocal'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     temp_v(:) = u%RootMotion%TranslationDisp(:,1)
     u%RootMotion%TranslationDisp(1,1) = temp_v(3) 
     u%RootMotion%TranslationDisp(2,1) = temp_v(1) 
     u%RootMotion%TranslationDisp(3,1) = temp_v(2) 
     temp_v(:) = u%RootMotion%TranslationVel(:,1)
     u%RootMotion%TranslationVel(1,1) = temp_v(3) 
     u%RootMotion%TranslationVel(2,1) = temp_v(1) 
     u%RootMotion%TranslationVel(3,1) = temp_v(2) 
     temp_v(:) = u%RootMotion%RotationVel(:,1)
     u%RootMotion%RotationVel(1,1) = temp_v(3) 
     u%RootMotion%RotationVel(2,1) = temp_v(1) 
     u%RootMotion%RotationVel(3,1) = temp_v(2) 
     temp_v(:) = u%RootMotion%TranslationAcc(:,1)
     u%RootMotion%TranslationAcc(1,1) = temp_v(3) 
     u%RootMotion%TranslationAcc(2,1) = temp_v(1) 
     u%RootMotion%TranslationAcc(3,1) = temp_v(2) 
     temp_v(:) = u%RootMotion%RotationAcc(:,1)
     u%RootMotion%RotationAcc(1,1) = temp_v(3) 
     u%RootMotion%RotationAcc(2,1) = temp_v(1) 
     u%RootMotion%RotationAcc(3,1) = temp_v(2) 
     ! Transform Root Motion from Global to Local (Blade) frame
     u%RootMotion%TranslationDisp(:,1) = MATMUL(u%RootMotion%TranslationDisp(:,1),p%GlbRot)  ! = MATMUL(TRANSPOSE(p%GlbRot),u%RootMotion%TranslationDisp(:,1))
     u%RootMotion%TranslationVel(:,1)  = MATMUL(u%RootMotion%TranslationVel( :,1),p%GlbRot)  ! = MATMUL(TRANSPOSE(p%GlbRot),u%RootMotion%TranslationVel(:,1))
     u%RootMotion%RotationVel(:,1)     = MATMUL(u%RootMotion%RotationVel(    :,1),p%GlbRot)  ! = MATMUL(TRANSPOSE(p%GlbRot),u%RootMotion%RotationVel(:,1))
     u%RootMotion%TranslationAcc(:,1)  = MATMUL(u%RootMotion%TranslationAcc( :,1),p%GlbRot)  ! = MATMUL(TRANSPOSE(p%GlbRot),u%RootMotion%TranslationAcc(:,1))
     u%RootMotion%RotationAcc(:,1)     = MATMUL(u%RootMotion%RotationAcc(    :,1),p%GlbRot)  ! = MATMUL(TRANSPOSE(p%GlbRot),u%RootMotion%RotationAcc(:,1))
     ! Transform DCM to Rotation Tensor (RT)
     temp33 = TRANSPOSE(u%RootMotion%Orientation(:,:,1)) !possible type conversion
     CALL BD_CrvExtractCrv(temp33,temp_v,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     temp_v2(1) = temp_v(3)
     temp_v2(2) = temp_v(1)
     temp_v2(3) = temp_v(2)
     CALL BD_CrvMatrixR(temp_v2,temp33,ErrStat2,ErrMsg2)
     u%RootMotion%Orientation(:,:,1)=temp33 !possible type conversion
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     ! Transform Applied Forces from Global to Local (Blade) frame
     DO i=1,p%node_total
         temp_v(:) = u%PointLoad%Force(1:3,i)
         u%PointLoad%Force(1,i) = temp_v(3)
         u%PointLoad%Force(2,i) = temp_v(1)
         u%PointLoad%Force(3,i) = temp_v(2)
         u%PointLoad%Force(1:3,i)  = MATMUL(u%PointLoad%Force(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%PointLoad%Force(:,i))
         temp_v(:) = u%PointLoad%Moment(1:3,i)
         u%PointLoad%Moment(1,i) = temp_v(3)
         u%PointLoad%Moment(2,i) = temp_v(1)
         u%PointLoad%Moment(3,i) = temp_v(2)
         u%PointLoad%Moment(1:3,i) = MATMUL(u%PointLoad%Moment(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%PointLoad%Moment(:,i))
     ENDDO
     IF(p%quadrature .EQ. 1) THEN
         DO i=1,p%ngp * p%elem_total + 2
             temp_v(:) = u%DistrLoad%Force(1:3,i)
             u%DistrLoad%Force(1,i) = temp_v(3)
             u%DistrLoad%Force(2,i) = temp_v(1)
             u%DistrLoad%Force(3,i) = temp_v(2)
             u%DistrLoad%Force(1:3,i)  = MATMUL(u%DistrLoad%Force(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%DistrLoad%Force(:,i))
             temp_v(:) = u%DistrLoad%Moment(1:3,i)
             u%DistrLoad%Moment(1,i) = temp_v(3)
             u%DistrLoad%Moment(2,i) = temp_v(1)
             u%DistrLoad%Moment(3,i) = temp_v(2)
             u%DistrLoad%Moment(1:3,i) = MATMUL(u%DistrLoad%Moment(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%DistrLoad%Moment(:,i))
         ENDDO
     ELSEIF(p%quadrature .EQ. 2) THEN
         DO i=1,p%ngp
             temp_v(:) = u%DistrLoad%Force(1:3,i)
             u%DistrLoad%Force(1,i) = temp_v(3)
             u%DistrLoad%Force(2,i) = temp_v(1)
             u%DistrLoad%Force(3,i) = temp_v(2)
             u%DistrLoad%Force(1:3,i)  = MATMUL(u%DistrLoad%Force(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%DistrLoad%Force(:,i))
             temp_v(:) = u%DistrLoad%Moment(1:3,i)
             u%DistrLoad%Moment(1,i) = temp_v(3)
             u%DistrLoad%Moment(2,i) = temp_v(1)
             u%DistrLoad%Moment(3,i) = temp_v(2)
             u%DistrLoad%Moment(1:3,i) = MATMUL(u%DistrLoad%Moment(:,i),p%GlbRot) !=MATMUL(TRANSPOSE(p%GlbRot),u%DistrLoad%Moment(:,i)) 
         ENDDO
     ENDIF
  
  END SUBROUTINE BD_InputGlobalLocal
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes the initial states
  !! Rigid body assumption is used in initialization of the states.
  !! The initial displacements/rotations and linear velocities are 
  !! set to the root value; the angular velocities over the beam 
  !! are computed based on rigid body rotation: \omega = v_{root} \times r_{pos}
  SUBROUTINE BD_CalcIC( u, p, x, ErrStat, ErrMsg)
  
     TYPE(BD_InputType),           INTENT(INOUT):: u             !< Inputs at t
     TYPE(BD_ParameterType),       INTENT(IN   ):: p             !< Parameters
     TYPE(BD_ContinuousStateType), INTENT(INOUT):: x             !< Continuous states at t
  
     INTEGER(IntKi),               INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                             :: i
     INTEGER(IntKi)                             :: j
     INTEGER(IntKi)                             :: k
     INTEGER(IntKi)                             :: temp_id
     INTEGER(IntKi)                             :: temp_id2
     REAL(BDKi)                                 :: temp3(3)
     REAL(BDKi)                                 :: temp_p0(3)
     REAL(BDKi)                                 :: temp_rv(3)
     REAL(BDKi)                                 :: temp_R(3,3)
     REAL(BDKi)                                 :: GlbRot_TransVel(3)           ! = MATMUL(p%GlbRot,u%RootMotion%TranslationVel(:,1))
     REAL(BDKi)                                 :: GlbRot_RotVel_tilde(3,3)     ! = BD_Tilde(MATMUL(p%GlbRot,u%RootMotion%RotationVel(:,1)))
     REAL(BDKi)                                 :: temp33(3,3)
     INTEGER(IntKi)                             :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                       :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                    :: RoutineName = 'BD_CalcIC'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     temp33=u%RootMotion%Orientation(:,:,1) ! possible type conversion
     CALL BD_CrvExtractCrv(temp33,temp3,ErrStat2,ErrMsg2) !returns temp3
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CrvCompose(temp_rv,temp3,p%Glb_crv,2,ErrStat2,ErrMsg2)  !returns temp_rv
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CrvMatrixR(temp_rv,temp_R,ErrStat2,ErrMsg2) !returns temp_R
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        
        
     !Initialize displacements and rotations
     k = 1 !when i=1, k=1
     DO i=1,p%elem_total
        DO j=k,p%node_elem
           temp_id = (j-1)*p%dof_node
           temp_p0 = MATMUL(p%GlbRot,p%uuN0(temp_id+1:temp_id+3,i))
           temp_p0 = MATMUL(temp_R,temp_p0) - temp_p0
           temp_p0 = MATMUL(temp_p0,p%GlbRot) != transpose(MATMUL(transpose(temp_p0),p%GlbRot)) = MATMUL(TRANSPOSE(p%GlbRot),temp_p0)  [transpose of a 1-d array temp_p0 is temp_p0]
           
           temp_id = ((i-1)*(p%node_elem-1)+j-1)*p%dof_node
           x%q(temp_id+1:temp_id+3) = u%RootMotion%TranslationDisp(1:3,1) + temp_p0
        ENDDO
        k = 2 ! start j loop at k=2 for remaining elements (i>1)
     ENDDO
     
     k = 1 !when i=1, k=1
     DO i=1,p%elem_total
        DO j=k,p%node_elem
           temp_id = ((i-1)*(p%node_elem-1)+j-1)*p%dof_node
           x%q(temp_id+4:temp_id+6) = MATMUL(temp_rv,p%GlbRot) != transpose(MATMUL(TRANSPOSE(temp_rv),p%GlbRot) = MATMUL(TRANSPOSE(p%GlbRot),temp_rv) because temp_rv is 1-dimension
        ENDDO
        k = 2 ! start j loop at k=2 for remaining elements (i>1)
     ENDDO
  
     !Initialize velocities and angular velocities
     x%dqdt(:) = 0.0_BDKi
     
     ! these values don't change in the loop:
     GlbRot_TransVel     = MATMUL(p%GlbRot,u%RootMotion%TranslationVel(:,1))
     GlbRot_RotVel_tilde = BD_Tilde(MATMUL(p%GlbRot,u%RootMotion%RotationVel(:,1)))
     k=1 !when i=1, k=1
     DO i=1,p%elem_total
        DO j=k,p%node_elem
           temp_id = (j-1)*p%dof_node
           temp_id2 = ((i-1)*(p%node_elem-1)+j-1)*p%dof_node
           
           temp3 = MATMUL(p%GlbRot, p%uuN0(temp_id+1:temp_id+3,i) + x%q(temp_id2+1:temp_id2+3) )
           temp3 = GlbRot_TransVel + MATMUL(GlbRot_RotVel_tilde,temp3)
           
           temp_id = ((i-1)*(p%node_elem-1)+j-1)*p%dof_node
           x%dqdt(temp_id2+1:temp_id2+3) = MATMUL(temp3,p%GlbRot) ! = transpose(MATMUL(transpose(temp3),p%GlbRot)) = MATMUL(TRANSPOSE(p%GlbRot),temp3)  because temp3 is 1-dimension
           x%dqdt(temp_id2+4:temp_id2+6) = u%RootMotion%RotationVel(1:3,1)
        ENDDO
        k = 2 ! start j loop at k=2 for remaining elements (i>1)
     ENDDO
  
  END SUBROUTINE BD_CalcIC
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> Routine for computing derivatives of continuous states.
  !! This subroutine calculates the reaction forces/moments at the root
  !! and the accelerations at all other FE points along the beam
  SUBROUTINE BD_CalcForceAcc( u, p, m, x, OtherState, ErrStat, ErrMsg)
  
     TYPE(BD_InputType),           INTENT(IN   ):: u             !< Inputs at t
     TYPE(BD_ParameterType),       INTENT(IN   ):: p             !< Parameters
     TYPE(BD_MiscVarType),         INTENT(INOUT):: m             !< Misc/optimization variables
     TYPE(BD_ContinuousStateType), INTENT(IN   ):: x             !< Continuous states at t
     TYPE(BD_OtherStateType),      INTENT(INOUT):: OtherState    !< Other states at t
     INTEGER(IntKi),               INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                             :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                       :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                    :: RoutineName = 'BD_CalcForceAcc'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     CALL BD_SolutionForceAcc(x%q,x%dqdt,u, p, m, OtherState%Acc,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     if (ErrStat >= AbortErrLev) RETURN
  
  END SUBROUTINE BD_CalcForceAcc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine calls other subroutines to apply the force, build the beam element
  !! stiffness and mass matrices, build nodal force vector.  The output of this subroutine
  !! is the second time derivative of state "q".
  SUBROUTINE BD_SolutionForceAcc(uuN,vvN,u, p, m, Acc,ErrStat,ErrMsg)
  
     TYPE(BD_InputType),           INTENT(IN   ):: u             !< Inputs at t
     TYPE(BD_ParameterType),       INTENT(IN   ):: p             !< Parameters
     TYPE(BD_MiscVarType),         INTENT(INOUT):: m             !< Misc/optimization variables
     REAL(BDKi),                   INTENT(IN   ):: uuN(:)        !< Displacement of Mass 1: m
     REAL(BDKi),                   INTENT(IN   ):: vvN(:)        !< Velocity of Mass 1: m/s
     REAL(BDKi),                   INTENT(  OUT):: Acc(:)
     INTEGER(IntKi),               INTENT(  OUT):: ErrStat       !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT):: ErrMsg        !< Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                             :: j
     INTEGER(IntKi)                             :: k
     INTEGER(IntKi)                             :: temp_id
     REAL(BDKi)                                 :: temp6(6)
     INTEGER(IntKi)                             :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                       :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                    :: RoutineName = 'BD_SolutionForceAcc'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
  
     CALL BD_GenerateDynamicElementAcc(uuN,vvN,u, p, m, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     DO j=1,p%node_total
         temp_id = (j-1)*p%dof_node
         m%F_PointLoad(temp_id+1:temp_id+3) = u%PointLoad%Force(1:3,j)
         m%F_PointLoad(temp_id+4:temp_id+6) = u%PointLoad%Moment(1:3,j)
     ENDDO
     temp6(1:3) = u%RootMotion%TranslationAcc(1:3,1)
     temp6(4:6) = u%RootMotion%RotationAcc(1:3,1)
     m%RHS(:)   = m%RHS(:) + m%F_PointLoad(:)
     m%RHS(1:6) = m%RHS(1:6) - MATMUL(m%MassM(1:6,1:6),temp6)
     m%RHS(7:p%dof_total) = m%RHS(7:p%dof_total) - MATMUL(m%MassM(7:p%dof_total,1:6),temp6)
     DO j=1,p%dof_total
         DO k=1,6
             m%MassM(j,k) = 0.0_BDKi
         ENDDO
     ENDDO
     DO j=1,6
         m%MassM(j,j) = -1.0_BDKi
     ENDDO
  
     CALL LAPACK_getrf( p%dof_total, p%dof_total, m%MassM, m%indx, ErrStat2, ErrMsg2) 
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
      CALL LAPACK_getrs( 'N',p%dof_total, m%MassM, m%indx, m%RHS,ErrStat2, ErrMsg2) 
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     if (ErrStat >= AbortErrLev) return
  
     Acc(:) = m%RHS(:)
  
     RETURN
  END SUBROUTINE BD_SolutionForceAcc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !! Routine for computing outputs, used in both loose and tight coupling.
  SUBROUTINE BD_InitAcc( t, u, p, x, OtherState, m, ErrStat, ErrMsg )
  
     REAL(DbKi),                   INTENT(IN   )  :: t           !< Current simulation time in seconds
     TYPE(BD_InputType),           INTENT(INOUT)  :: u           !< Inputs at t
     TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
     TYPE(BD_ContinuousStateType), INTENT(IN   )  :: x           !< Continuous states at t
     TYPE(BD_OtherStateType),      INTENT(INOUT)  :: OtherState  !< Other states at t
     TYPE(BD_MiscVarType),         INTENT(INOUT)  :: m           !< Misc/optimization variables
     INTEGER(IntKi),               INTENT(  OUT)  :: ErrStat     !< Error status of the operation
     CHARACTER(*),                 INTENT(  OUT)  :: ErrMsg      !< Error message if ErrStat /= ErrID_None
  
     TYPE(BD_OtherStateType)                      :: OS_tmp
     TYPE(BD_ContinuousStateType)                 :: x_tmp
     INTEGER(IntKi)                               :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)                         :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER                      :: RoutineName = 'BD_InitAcc'
     
     ! Initialize ErrStat
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
     CALL BD_CopyContState(x, x_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_CopyOtherState(OtherState, OS_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
     CALL BD_CopyInput(u, m%u, MESH_UPDATECOPY, ErrStat2, ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
        if (ErrStat >= AbortErrLev) then
           call cleanup()
           return
        end if
        
     CALL BD_InputGlobalLocal(p,m%u,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL BD_BoundaryGA2(x_tmp,p,m%u,t,OS_tmp,m,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     CALL BD_CalcForceAcc(m%u,p,m,x_tmp,OS_tmp,ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         
     OtherState%Acc(:)   = OS_tmp%Acc(:)
     OtherState%Acc(1:3) = m%u%RootMotion%TranslationAcc(1:3,1)
     OtherState%Acc(4:6) = m%u%RootMotion%RotationAcc(1:3,1)
     OtherState%Xcc(:)   = OtherState%Acc(:)
  
     call cleanup()
     return
     
  contains
     subroutine cleanup()
        CALL BD_DestroyContState(x_tmp, ErrStat2, ErrMsg2 )
        CALL BD_DestroyOtherState(OS_tmp, ErrStat2, ErrMsg2 )
     end subroutine cleanup 
  END SUBROUTINE BD_InitAcc
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine computes Global mass matrix and force vector for the beam.
  SUBROUTINE BD_ComputeBladeMassNew(Mass0,GaussPos,         &
                                    elem_total,node_elem,dof_node,&
                                    quadrature,ngp,gw,Jaco,&
                                    blade_mass,blade_CG,blade_IN,ErrStat,ErrMsg)
  
     REAL(BDKi),        INTENT(IN   ):: Mass0(:,:,:)    !< Element stiffness matrix
     REAL(BDKi),        INTENT(IN   ):: GaussPos(:,:)   !< Initial position vector
     INTEGER(IntKi),    INTENT(IN   ):: elem_total      !< Total number of elements
     INTEGER(IntKi),    INTENT(IN   ):: node_elem       !< Node per element
     INTEGER(IntKi),    INTENT(IN   ):: dof_node        !< Degrees of freedom per node
     INTEGER(IntKi),    INTENT(IN   ):: quadrature
     INTEGER(IntKi),    INTENT(IN   ):: ngp             !< Number of Gauss points
     REAL(BDKi),        INTENT(IN   ):: gw(:) 
     REAL(BDKi),        INTENT(IN   ):: Jaco(:,:) 
     REAL(BDKi),        INTENT(  OUT):: blade_mass      !< Mass matrix
     REAL(BDKi),        INTENT(  OUT):: blade_CG(:)     !< Mass matrix
     REAL(BDKi),        INTENT(  OUT):: blade_IN(:,:)   !< Mass matrix
     INTEGER(IntKi),    INTENT(  OUT):: ErrStat         !< Error status of the operation
     CHARACTER(*),      INTENT(  OUT):: ErrMsg          !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi),          ALLOCATABLE:: NGPpos(:,:)
     REAL(BDKi)                      :: elem_mass
     REAL(BDKi)                      :: elem_CG(3)
     REAL(BDKi)                      :: elem_IN(3,3)
     REAL(BDKi),          ALLOCATABLE:: EMass0_GL(:,:,:)
     INTEGER(IntKi)                  :: dof_elem ! Degree of freedom per node
     INTEGER(IntKi)                  :: nelem ! number of elements
     INTEGER(IntKi)                  :: j ! Index counter
     INTEGER(IntKi)                  :: temp_id ! Index counter
     INTEGER(IntKi)                  :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)            :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER         :: RoutineName = 'BD_ComputeBladeMassNew'
  
     ErrStat    = ErrID_None
     ErrMsg     = ""
     blade_mass = 0.0_BDKi
  
     dof_elem = dof_node * node_elem
  
     CALL AllocAry(NGPpos,3,ngp,'NGPpos',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     CALL AllocAry(EMass0_GL,6,6,ngp,'EMass0_GL',ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
     NGPpos(:,:)  = 0.0_BDKi
     EMass0_GL(:,:,:)  = 0.0_BDKi
     elem_mass= 0.0_BDKi
     elem_CG(:)= 0.0_BDKi
     elem_IN(:,:)= 0.0_BDKi
  
     DO nelem=1,elem_total
  
         temp_id = (nelem-1)*ngp
         DO j=1,ngp
             EMass0_GL(1:6,1:6,j) = Mass0(1:6,1:6,temp_id+j)
             IF(quadrature .EQ. 1) THEN
                 NGPpos(1:3,j) = GaussPos(1:3,temp_id+j+1)
             ELSEIF(quadrature .EQ. 2) THEN
                 NGPpos(1:3,j) = GaussPos(1:3,temp_id+j)
             ENDIF 
         ENDDO
  
         CALL BD_ComputeElementMass(NGPpos,EMass0_GL,&
                                    ngp,gw,Jaco(:,nelem),&
                                    elem_mass,elem_CG,elem_IN,ErrStat2,ErrMsg2)
            CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
         blade_mass = blade_mass + elem_mass
         blade_CG(:) = blade_CG(:) + elem_CG(:)
         blade_IN(:,:) = blade_IN(:,:) + elem_IN(:,:)
  
     ENDDO
  
     if (ErrStat >= AbortErrLev) then
         call Cleanup()
         return
     end if
  
     blade_CG(:) = blade_CG(:) / blade_mass
  
     CALL Cleanup()
     RETURN
  
  contains
        subroutine Cleanup()
  
           if (allocated(NGPpos      )) deallocate(NGPpos      )
           if (allocated(EMass0_GL   )) deallocate(EMass0_GL   )
  
        end subroutine Cleanup
  
  END SUBROUTINE BD_ComputeBladeMassNew
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine total element forces and mass matrices
  SUBROUTINE BD_ComputeElementMass(NGPpos,EMass0_GL,&
                                   ngp,gw,Jaco,&
                                   elem_mass,elem_CG,elem_IN,ErrStat,ErrMsg)
  
     REAL(BDKi),INTENT(IN   )    :: NGPpos(:,:)
     REAL(BDKi),INTENT(IN   )    :: EMass0_GL(:,:,:) !< Nodal material properties for each element
     REAL(BDKi),INTENT(  OUT)    :: elem_mass        !< Total element force (Fd, Fc, Fb)
     REAL(BDKi),INTENT(  OUT)    :: elem_CG(:)
     REAL(BDKi),INTENT(  OUT)    :: elem_IN(:,:)
     INTEGER(IntKi),INTENT(IN   ):: ngp              !< Number of Gauss points
     REAL(BDKi),    INTENT(IN   ):: gw(:)
     REAL(BDKi),    INTENT(IN   ):: Jaco(:)
     INTEGER(IntKi),INTENT(  OUT):: ErrStat          !< Error status of the operation
     CHARACTER(*),  INTENT(  OUT):: ErrMsg           !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                  :: mmm
     INTEGER(IntKi)              :: igp
  !   INTEGER(IntKi)              :: ErrStat2                     ! Temporary Error status
  !   CHARACTER(ErrMsgLen)        :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER     :: RoutineName = 'BD_ComputeElementMass'
  
     ErrStat  = ErrID_None
     ErrMsg   = ""
     elem_mass  = 0.0_BDKi
     elem_CG(:) = 0.0_BDKi
     elem_IN(:,:) = 0.0_BDKi
  
  
     DO igp=1,ngp
  
         mmm  = 0.0_BDKi
         mmm  = EMass0_GL(1,1,igp)
  
         elem_mass = elem_mass + gw(igp) * Jaco(igp) * mmm
         elem_CG(1:3) = elem_CG(1:3) + gw(igp) * Jaco(igp) * mmm * NGPpos(1:3,igp)
         elem_IN(1:3,1:3) = elem_IN(1:3,1:3) + gw(igp) * Jaco(igp) * mmm * &
                            MATMUL(BD_Tilde(NGPpos(1:3,igp)),TRANSPOSE(BD_Tilde(NGPpos(1:3,igp))))
  
     ENDDO
  
  
     RETURN
  
  END SUBROUTINE BD_ComputeElementMass
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_InitShpDerJaco(quadrature,GL,GLL,uuN0,&
                 node_elem,elem_total,dof_node,ngp,&
                 refine,kp_member,&
                 hhx,hpx,TZw,Jacobian,&
                 ErrStat,ErrMsg)
  
     REAL(BDKi),         INTENT(INOUT):: GL(:)          !< GL(Gauss) point locations
     REAL(BDKi),         INTENT(IN   ):: GLL(:)         !< GLL point locations
     REAL(BDKi),         INTENT(IN   ):: uuN0(:,:)      !< Initial position vector
     INTEGER(IntKi),     INTENT(IN   ):: quadrature     !< Quadrature method
     INTEGER(IntKi),     INTENT(IN   ):: node_elem      !< Nodes per element
     INTEGER(IntKi),     INTENT(IN   ):: elem_total     !< Total number of elements
     INTEGER(IntKi),     INTENT(IN   ):: dof_node
     INTEGER(IntKi),     INTENT(IN   ):: ngp            !< Number of quadrature points
     INTEGER(IntKi),     INTENT(IN   ):: refine         !< TZ refinement parameter
     INTEGER(IntKi),     INTENT(IN   ):: kp_member(:)   !< Number of key points in each member
     REAL(BDKi),         INTENT(  OUT):: hhx(:,:)       !< Shape function matrix
     REAL(BDKi),         INTENT(  OUT):: hpx(:,:)       !< Derivative of shape function matrix
     REAL(BDKi),         INTENT(  OUT):: TZw(:)         !< TZ weight functions 
     REAL(BDKi),         INTENT(  OUT):: Jacobian(:,:)  !< Jacobians at each quadrature point
     INTEGER(IntKi),     INTENT(  OUT):: ErrStat        !< Error status of the operation
     CHARACTER(*),       INTENT(  OUT):: ErrMsg         !< Error message if ErrStat /= ErrID_None
  
     REAL(BDKi)                       :: temp1
     REAL(BDKi)                       :: temp2
     REAL(BDKi)                       :: Gup0(3)
     INTEGER(IntKi)                   :: temp_id
     INTEGER(IntKi)                   :: temp_id0
     INTEGER(IntKi)                   :: temp_id1
     INTEGER(IntKi)                   :: id0
     INTEGER(IntKi)                   :: id1
     INTEGER(IntKi)                   :: inode
     INTEGER(IntKi)                   :: i
     INTEGER(IntKi)                   :: j
     INTEGER(IntKi)                   :: m
  
     INTEGER(IntKi)                   :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_InitShpDerJaco'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
     
     IF(quadrature .EQ. 2) THEN
         DO j=1,ngp
             temp_id = 0
             id0 = 1
             id1 = kp_member(1)
             temp_id0 = (id0 - 1)*refine + 1
             temp_id1 = (id1 - 1)*refine + 1
             IF(j .EQ. 1) THEN
                 temp1 = -1.0_BDKi + (GL(temp_id0+j-1) - GL(temp_id0))*2.0_BDKi/ &
                                         (GL(temp_id1) - GL(temp_id0))
                 temp2 = -1.0_BDKi + (GL(temp_id0+j) - GL(temp_id0))*2.0_BDKi/ &
                                       (GL(temp_id1) - GL(temp_id0))
                 TZw(j) = 0.5_BDKi * (temp2 - temp1)
             ELSEIF(j .EQ. ngp) THEN
                 temp1 = -1.0_BDKi + (GL(temp_id1-1) - GL(temp_id0))*2.0_BDKi/ &
                                     (GL(temp_id1  ) - GL(temp_id0))
                 temp2 = -1.0_BDKi + (GL(temp_id1) - GL(temp_id0))*2.0_BDKi/ &
                                     (GL(temp_id1) - GL(temp_id0))
                 TZw(j) = 0.5_BDKi * (temp2 - temp1)
             ELSE 
                 temp1 = -1.0_BDKi + (GL(temp_id0+j-2) - GL(temp_id0))*2.0_BDKi/ &
                                     (GL(temp_id1) - GL(temp_id0))
                 temp2 = -1.0_BDKi + (GL(temp_id0+j) - GL(temp_id0))*2.0_BDKi/ &
                                     (GL(temp_id1) - GL(temp_id0))
                 TZw(j) = 0.5_BDKi * (temp2 - temp1)
             ENDIF
         ENDDO
         GL(:) = 2.0_BDKi*GL(:) - 1.0_BDKi
     ENDIF
  
     CALL BD_diffmtc(node_elem-1,ngp,GL,GLL,hhx,hpx,ErrStat2,ErrMsg2)
        call SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName)
  
     DO i = 1,elem_total
         DO j = 1, ngp
             Gup0(:) = 0.0_BDKi
             DO inode=1,node_elem
                 temp_id = (inode-1)*dof_node
                 DO m=1,3
                     Gup0(m) = Gup0(m) + hpx(inode,j)*uuN0(temp_id+m,i)
                 ENDDO
             ENDDO
  
             Jacobian(j,i) = SQRT(DOT_PRODUCT(Gup0,Gup0))
  
         ENDDO
     ENDDO
  
  END SUBROUTINE BD_InitshpDerJaco
  !-----------------------------------------------------------------------------------------------------------------------------------
  !> This routine alters the RootMotion inputs based on the pitch-actuator parameters and discrete states
  SUBROUTINE PitchActuator_SetBC(p, u, xd, AllOuts)
  
     TYPE(BD_ParameterType),    INTENT(IN   )  :: p                                 !< The module parameters
     TYPE(BD_InputType),        INTENT(INOUT)  :: u                                 !< inputs
     TYPE(BD_DiscreteStateType),INTENT(IN   )  :: xd                                !< The module discrete states
     REAL(ReKi),       OPTIONAL,INTENT(INOUT)  :: AllOuts(0:)                       !< all output array for writing to file
     ! local variables
     REAL(BDKi)                                :: temp_R(3,3)
     REAL(BDKi)                                :: temp_cc(3)
     REAL(BDKi)                                :: u_theta_pitch
     REAL(BDKi)                                :: thetaP
     REAL(BDKi)                                :: omegaP
     REAL(BDKi)                                :: alphaP
     
     
     temp_R = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
     temp_cc = EulerExtract(temp_R)   != Hub_theta_Root
     u_theta_pitch = -temp_cc(3)
         
     thetaP = xd%thetaP
     omegaP = xd%thetaPD
     alphaP = -(p%pitchK/p%pitchJ) * xd%thetaP - (p%pitchC/p%pitchJ) * xd%thetaPD + (p%pitchK/p%pitchJ) * u_theta_pitch
         
     ! Calculate new root motions for use as BeamDyn boundary conditions:       
     !note: we alter the orientation last because we need the input (before actuator) root orientation for the rotational velocity and accelerations
     u%RootMotion%RotationVel(:,1) = u%RootMotion%RotationVel(:,1) - omegaP * u%RootMotion%Orientation(3,:,1)
     u%RootMotion%RotationAcc(:,1) = u%RootMotion%RotationAcc(:,1) - alphaP * u%RootMotion%Orientation(3,:,1)
  
     temp_cc(3) = -thetaP
     temp_R = EulerConstruct(temp_cc)       
     u%RootMotion%Orientation(:,:,1) = MATMUL(temp_R,u%HubMotion%Orientation(:,:,1))
         
     if (present(AllOuts)) then
        AllOuts(PAngInp) = u_theta_pitch
        AllOuts(PAngAct) = thetaP
        AllOuts(PRatAct) = omegaP
        AllOuts(PAccAct) = alphaP
     end if
     
     
     
  END SUBROUTINE PitchActuator_SetBC
  !-----------------------------------------------------------------------------------------------------------------------------------
  END MODULE BeamDyn
  