anchor.diff:        IF (ErrStat >= AbortErrLev) RETURN
   
      p%DT  = Interval
  -   p%Gravity = InitInp%Gravity      ! Gravity vector pointed in negative (/0,0,-g/)
  +   p%Gravity = InitInp%Gravity      ! Gravity vector pointed in negative global Z-axis (/0,0,-g/)
   
         !............................................................................................
         ! Define initial system states here:
  
positive.code:       IF ( p%TMD_DOF_MODE == DOFMode_Prescribed ) THEN
           do i=1,3
              ! Get interpolated force   -- this is not in any particular coordinate system yet
              PrescribedForce(i)   = InterpStp( real(Time,ReKi), p%TMD_PrescribedForce(1,:),p%TMD_PrescribedForce(i+1,:),m%PrescribedInterpIdx, size(p%TMD_PrescribedForce,2))
              ! Get interpolated moment  -- this is not in any particular coordinate system yet
              PrescribedMoment(i)  = InterpStp( real(Time,ReKi), p%TMD_PrescribedForce(1,:),p%TMD_PrescribedForce(i+4,:),m%PrescribedInterpIdx, size(p%TMD_PrescribedForce,2))
           enddo
  !         IF (.not. p%TMD_On_Blade) THEN
  !            if (p%PrescribedForcesCoordSys == 0_IntKi) then
  !               ! Global coords
  !               y%Mesh%Force(1:3,1)  =  PrescribedForce
  !               y%Mesh%Moment(1:3,1) =  PrescribedMoment
  !            elseif (p%PrescribedForcesCoordSys == 1_IntKi) then
  !               ! local coords
  !               y%Mesh%Force(1:3,1)  =  matmul(transpose(u%Mesh%Orientation(:,:,1)), PrescribedForce)
  !               y%Mesh%Moment(1:3,1) =  matmul(transpose(u%Mesh%Orientation(:,:,1)), PrescribedMoment)
  !            endif
  !         ELSE if (p%TMD_On_Blade) then
              if (p%PrescribedForcesCoordSys == 0_IntKi) then
                 ! Global coords
                 do i_pt=1,p%NumMeshPts
                    y%Mesh(i_pt)%Force(1:3,1)  =  PrescribedForce
                    y%Mesh(i_pt)%Moment(1:3,1) =  PrescribedMoment
                 enddo
              elseif (p%PrescribedForcesCoordSys == 1_IntKi) then
                 ! local coords
                 do i_pt=1,p%NumMeshPts
                    y%Mesh(i_pt)%Force(1:3,1)  =  matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)), PrescribedForce)
                    y%Mesh(i_pt)%Moment(1:3,1) =  matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)), PrescribedMoment)
                 enddo
              endif
  !         ENDIF
        END IF
  CONTAINS
     subroutine CleanUp()
        if (allocated(a_G_B        )) deallocate(a_G_B        )
        if (allocated(F_P_B        )) deallocate(F_P_B        )
        if (allocated(M_P_B        )) deallocate(M_P_B        )
        if (allocated(r_ddot_P_B   )) deallocate(r_ddot_P_B   )
        if (allocated(omega_B_O_B  )) deallocate(omega_B_O_B  )
        if (allocated(alpha_B_O_B  )) deallocate(alpha_B_O_B  )
        if (allocated(F_x_btmdY_P_B)) deallocate(F_x_btmdY_P_B)
        if (allocated(F_z_btmdY_P_B)) deallocate(F_z_btmdY_P_B)
        if (allocated(F_y_btmdX_P_B)) deallocate(F_y_btmdX_P_B)
        if (allocated(F_z_btmdX_P_B)) deallocate(F_z_btmdX_P_B)
        call TMD_DestroyContState(dxdt,ErrStat2,ErrMsg2)    !Ignore error status
     end subroutine CleanUp
     logical function Failed()
  
negative.code: 
           ! tmd external forces of dependent degrees:
           do i_pt=1,p%NumMeshPts
              F_x_tmdXY_P_N(i_pt) = 0
              F_y_tmdXY_P_N(i_pt) = 0
              F_z_tmdXY_P_N(i_pt) = - p%M_XY * (a_G(3,i_pt) - rddot_P(3,i_pt) - (alpha_P(1,i_pt) + omega_P(2,i_pt)*omega_P(3,i_pt))*x%tmd_x(3,i_pt) + (alpha_P(2,i_pt) - omega_P(1,i_pt)*omega_P(3,i_pt))*x%tmd_x(1,i_pt) - 2*omega_P(1,i_pt)*x%tmd_x(4,i_pt) + 2*omega_P(2,i_pt)*x%tmd_x(2,i_pt))
           enddo
  
           ! forces in local coordinates
           do i_pt=1,p%NumMeshPts
              F_P(1,i_pt) =  p%K_X * x%tmd_x(1,i_pt) + m%C_ctrl(1) * x%tmd_x(2,i_pt) + m%C_Brake(1) * x%tmd_x(2,i_pt) - m%F_stop(1) - m%F_ext(1) - m%F_fr(1) - F_x_tmdXY_P_N(i_pt) + m%F_table(1)*(m%F_k_x)
              F_P(2,i_pt) =  p%K_Y * x%tmd_x(3,i_pt) + m%C_ctrl(2) * x%tmd_x(4,i_pt) + m%C_Brake(2) * x%tmd_x(4,i_pt) - m%F_stop(2) - m%F_ext(2) - m%F_fr(2) - F_y_tmdXY_P_N(i_pt) + m%F_table(2)*(m%F_k_y)
              F_P(3,i_pt) = - F_z_tmdXY_P_N(i_pt)
  
              ! inertial contributions from mass of TMDs and acceleration of nacelle
              ! Moments on nacelle in local coordinates
              M_P(1,i_pt) = - F_z_tmdXY_P_N(i_pt) * x%tmd_x(3,i_pt)
              M_P(2,i_pt) =   F_z_tmdXY_P_N(i_pt) * x%tmd_x(1,i_pt)
              M_P(3,i_pt) = (- F_x_tmdXY_P_N(i_pt)) * x%tmd_x(3,i_pt) + (F_y_tmdXY_P_N(i_pt)) * x%tmd_x(1,i_pt)
           enddo
  
           do i_pt=1,p%NumMeshPts
              ! forces in global coordinates
              y%Mesh(i_pt)%Force(:,1) =  matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)),F_P(1:3,i_pt))
              ! moments in global coordinates
              y%Mesh(i_pt)%Moment(:,1) = matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)),M_P(1:3,i_pt))
           enddo
  
        ELSE IF (p%TMD_DOF_MODE == DOFMode_TLCD) THEN
  
           do i_pt=1,p%NumMeshPts
              !fore-aft TLCD external forces of dependent degrees
              F_x_tlcd_WR_N(i_pt) = p%rho_FA*p%area_FA*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))*(                      &
                                         rddot_P(1,i_pt)                                                          &
                                      +2*omega_P(2,i_pt)*x%tmd_x(2,i_pt)                                          &
                                        +alpha_P(2,i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))                      &
                                        -omega_P(2,i_pt)*omega_P(2,i_pt)*p%B_FA*.5                                &
                                        -omega_P(3,i_pt)*omega_P(3,i_pt)*p%B_FA*.5                                &
                                        +omega_P(3,i_pt)*omega_P(1,i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))      &
                                        -a_G(1,i_pt)  )
              F_y_tlcd_WR_N(i_pt) = p%rho_FA*p%area_FA*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))*(                      &
                                         rddot_P(2,i_pt)                                                          &
                                      -2*omega_P(1,i_pt)*x%tmd_x(2,i_pt)                                          &
                                        +alpha_P(3,i_pt)*p%B_FA*.5                                                &
                                        -alpha_P(1,i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))                      &
                                        +omega_P(3,i_pt)*omega_P(2,i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt))      &
                                        +omega_P(1,i_pt)*omega_P(2,i_pt)*p%B_FA*.5                                &
                                        -a_G(2,i_pt)  )
              F_x_tlcd_WL_N(i_pt) = p%rho_FA*p%area_FA*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))*(                      &
                                         rddot_P(1,i_pt)                                                          &
                                      -2*omega_P(2,i_pt)*x%tmd_x(2,i_pt)                                          &
                                        +alpha_P(2,i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))                      &
                                        +omega_P(2,i_pt)*omega_P(2,i_pt)*p%B_FA*.5                                &
                                        +omega_P(3,i_pt)*omega_P(3,i_pt)*p%B_FA*.5                                &
                                        +omega_P(3,i_pt)*omega_P(1,i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))      &
                                        -a_G(1,i_pt)  )
              F_y_tlcd_WL_N(i_pt) = p%rho_FA*p%area_FA*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))*(                      &
                                         rddot_P(2,i_pt)                                                          &
                                      +2*omega_P(1,i_pt)*x%tmd_x(2,i_pt)                                          &
                                        -alpha_P(3,i_pt)*p%B_FA*.5                                                &
                                        -alpha_P(1,i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))                      &
                                        +omega_P(3,i_pt)*omega_P(2,i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt))      &
                                        -omega_P(1,i_pt)*omega_P(2,i_pt)*p%B_FA*.5                                &
                                        -a_G(2,i_pt)  )
              F_y_tlcd_WH_N(i_pt) = p%rho_FA*p%area_FA/p%area_ratio_FA*p%B_FA*(            &
                                         rddot_P(2,i_pt)                                   &
                                      +2*omega_P(3,i_pt)*p%area_ratio_FA*x%tmd_x(2,i_pt)   &
                                        -a_G(2,i_pt)  )
              F_z_tlcd_WH_N(i_pt) = p%rho_FA*p%area_FA/p%area_ratio_FA*p%B_FA*(            &
                                         rddot_P(3,i_pt)                                   &
                                      -2*omega_P(2,i_pt)*p%area_ratio_FA*x%tmd_x(2,i_pt)   &
                                        -a_G(3,i_pt)  )
  
              !side-to-side TLCD external forces of dependent degrees
              F_x_otlcd_WB_N(i_pt) = p%rho_SS*p%area_SS*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))*(                    &
                                           rddot_P(1,i_pt)                                                       &
                                        +2*omega_P(2,i_pt)*x%tmd_x(4,i_pt)                                       &
                                          +alpha_P(2,i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))                   &
                                          +alpha_P(3,i_pt)*p%B_SS/2-omega_P(2,i_pt)*omega_P(1,i_pt)*p%B_SS/2     &
                                          +omega_P(3,i_pt)*omega_P(1,i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))   &
                                          -a_G(1,i_pt)   )
              F_y_otlcd_WB_N(i_pt) = p%rho_SS*p%area_SS*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))*(                    &
                                           rddot_P(2,i_pt)                                                       &
                                        -2*omega_P(1,i_pt)*x%tmd_x(4,i_pt)                                       &
                                          -alpha_P(1,i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))                   &
                                          +omega_P(3,i_pt)*omega_P(2,i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt))   &
                                          +omega_P(3,i_pt)*omega_P(3,i_pt)*p%B_SS/2                              &
                                          +omega_P(1,i_pt)*omega_P(1,i_pt)*p%B_SS/2                              &
                                          -a_G(2,i_pt)   )
              F_x_otlcd_WF_N(i_pt) = p%rho_SS*p%area_SS*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))*(                    &
                                           rddot_P(1,i_pt)                                                       &
                                        -2*omega_P(2,i_pt)*x%tmd_x(4,i_pt)                                       &
                                          +alpha_P(2,i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))                   &
                                          -alpha_P(2,i_pt)*p%B_SS/2                                              &
                                          +omega_P(2,i_pt)*omega_P(1,i_pt)*p%B_SS/2                              &
                                          +omega_P(3,i_pt)*omega_P(1,i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))   &
                                          -a_G(1,i_pt)   )
              F_y_otlcd_WF_N(i_pt) = p%rho_SS*p%area_SS*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))*(                    &
                                           rddot_P(2,i_pt)                                                       &
                                        +2*omega_P(1,i_pt)*x%tmd_x(4,i_pt)                                       &
                                          -alpha_P(1,i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))                   &
                                          +omega_P(3,i_pt)*omega_P(2,i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))   &
                                          -omega_P(3,i_pt)*omega_P(3,i_pt)*p%B_SS/2                              &
                                          -omega_P(1,i_pt)*omega_P(1,i_pt)*p%B_SS/2                              &
                                          -a_G(2,i_pt)   )
              F_x_otlcd_WH_N(i_pt) = p%rho_SS*p%area_SS/p%area_ratio_SS*p%B_SS*(              &
                                            rddot_P(1,i_pt)                                   &
                                         -2*omega_P(3,i_pt)*p%area_ratio_SS*x%tmd_x(4,i_pt)   &
                                           -a_G(1,i_pt)  )
              F_z_otlcd_WH_N(i_pt) = p%rho_SS*p%area_SS/p%area_ratio_SS*p%B_SS*(              &
                                            rddot_P(3,i_pt)                                   &
                                         +2*omega_P(1,i_pt)*p%area_ratio_SS*x%tmd_x(4,i_pt)   &
                                           -a_G(3,i_pt)  )
           enddo
  
           !forces in local coordinates (from fore-aft and side-to-side TLCDs)
           do i_pt=1,p%NumMeshPts
              F_P(1,i_pt) = -F_x_tlcd_WR_N(i_pt) - F_x_tlcd_WL_N(i_pt) - p%rho_FA*(p%area_FA/p%area_ratio_FA)*p%B_FA*dxdt%tmd_x(2,i_pt)*p%area_ratio_FA + F_x_otlcd_WB_N(i_pt) + F_x_otlcd_WF_N(i_pt) + F_x_otlcd_WH_N(i_pt)
              F_P(2,i_pt) = +F_y_tlcd_WR_N(i_pt) + F_y_tlcd_WL_N(i_pt) - p%rho_SS*(p%area_SS/p%area_ratio_SS)*p%B_SS*dxdt%tmd_x(4,i_pt)*p%area_ratio_SS + F_y_tlcd_WH_N(i_pt)  - F_y_otlcd_WB_N(i_pt) - F_y_otlcd_WF_N(i_pt)
              F_P(3,i_pt) = -F_z_tlcd_WH_N(i_pt) - F_z_otlcd_WH_N(i_pt)
           enddo
  
           !moments in local coordinates (from fore-aft and side-to-side TLCDs)
           do i_pt=1,p%NumMeshPts
              M_P(1,i_pt) =  F_y_tlcd_WR_N(i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt)) + F_y_tlcd_WL_N(i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt)) - F_y_otlcd_WB_N(i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt)) - F_y_otlcd_WF_N(i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))
              M_P(2,i_pt) = -F_x_tlcd_WR_N(i_pt)*((p%L_FA-p%B_FA)/2+x%tmd_x(1,i_pt)) - F_x_tlcd_WL_N(i_pt)*((p%L_FA-p%B_FA)/2-x%tmd_x(1,i_pt)) + F_x_otlcd_WB_N(i_pt)*((p%L_SS-p%B_SS)/2+x%tmd_x(3,i_pt)) + F_x_otlcd_WF_N(i_pt)*((p%L_SS-p%B_SS)/2-x%tmd_x(3,i_pt))
              M_P(3,i_pt) =  F_y_tlcd_WR_N(i_pt)*p%B_FA*.5 - F_y_tlcd_WL_N(i_pt)*p%B_FA*.5 + F_x_otlcd_WB_N(i_pt)*p%B_SS*.5 - F_x_otlcd_WF_N(i_pt)*p%B_SS*.5
           enddo
  
           do i_pt=1,p%NumMeshPts
              !forces in global coordinates
              y%Mesh(i_pt)%Force(:,1)  = matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)), F_P(1:3,i_pt))
              !moments in global coordinates
              y%Mesh(i_pt)%Moment(:,1) = matmul(transpose(u%Mesh(i_pt)%Orientation(:,:,1)), M_P(1:3,i_pt))
           enddo
        ENDIF
     ENDIF
  