!> @anchor      astroConvert
!!
!> @brief       Provides simple conversion functions for astronomical applications
!!
!> @author      Vitali Braun (VB)
!> @author      Christopher Kebschull (CHK)
!!
!> @date

!!
VB:  06.06.2014 (initial design)

!!
CHK: 13.11.2015 (moved to neptune from the lib and updated to use libslam)

!!
VB:  15.05.2016 (updated to have only statements for all use statements)

!!
CHK: 04.01.2018 (Using Solarsystem_class)

!!

!!
!> @details     This module contains parameters, subroutines and functions related to the
!!              simple conversion of different quantities as listed below:
!!
!> @copyright   Institute of Space Systems / TU Braunschweig
!!
!> @anchor      astroConvert
!!
!!------------------------------------------------------------------------------------------------
module astroConvert

  use slam_math,              only: deg2rad, twopi, mag, rad2deg, undefined
  use slam_error_handling,    only: isControlled, hasToReturn, hasFailed, checkIn, checkOut
  use solarsystem,            only: Solarsystem_class, ID_SUN
  use slam_types,             only: dp

  implicit none

  private

  public :: raan2solarTime
  public :: solarTime2Raan

contains

  !=========================================================================
  !
  !>  @brief  Converts an angle given as solar time to RAAN
  !!
  !>  @author Vitali Braun
  !!
  !>  @param[inout] solarsystem_model  The solar system model
  !>  @param[in]    mjd   Modified Julian Day
  !>  @param[in]    lst   Local solar time
  !!
  !>  @return RAAN
  !!
  !>  @date

  !!
06.06.2014 (initial design)

  !!

  !!
  !!------------------------------------------------------------
  real(dp) function solarTime2Raan(solarsystem_model, mjd, lst) result(raan)

    type(Solarsystem_class),intent(inout)   :: solarsystem_model
    real(dp), intent(in)                    :: mjd
    real(dp), intent(in)                    :: lst

    character(len=*), parameter :: csubid = 'solarTime2Raan'

    real(dp), dimension(3) :: rsun

    raan = undefined

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    ! get Sun's position
    rsun = solarsystem_model%getBodyPosition(mjd, ID_SUN)

    if(hasFailed()) return

    ! compute RAAN
    raan = atan2(rsun(2), rsun(1)) + 15.d0*(lst - 12.d0)*deg2rad

    ! convert to angle between 0...360 deg
    if(raan < 0.d0) then
      raan = raan + twopi
    end if

    ! done
    if(isControlled()) then
      call checkOut(csubid)
    end if

    return

  end function solarTime2Raan

  !=========================================================================
  !
  !>  @brief  Converts a given RAAN to solar time in hours
  !!
  !!  @author Vitali Braun
  !!
  !>  @param[inout] solarsystem_model  The solar system model
  !>  @param[in]    mjd   Modified Julian Day
  !>  @param[in]    raan  Right ascension of ascending node / rad
  !!
  !>  @return RAAN
  !!
  !!  @date

  !!
06.06.2014 (initial design)

  !!

  !!
  !!------------------------------------------------------------
  real(dp) function raan2solarTime(solarsystem_model, mjd, raan) result(lst)

    type(Solarsystem_class),intent(inout)   :: solarsystem_model
    real(dp), intent(in)                    :: mjd
    real(dp), intent(in)                    :: raan

    character(len=*), parameter :: csubid = 'raan2solarTime'

    real(dp), dimension(3) :: rsun

    lst = undefined

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    ! get Sun's position
    rsun = solarsystem_model%getBodyPosition(mjd, ID_SUN)

    if(hasFailed()) return

    ! compute RAAN
    lst = (raan - atan2(rsun(2), rsun(1)))*rad2deg/15.d0 + 12.d0

    ! convert to hours between 0...24 h
    if(lst < 0.d0) then
      lst = lst + 24.d0
    else if(lst >= 24.d0) then
      lst = lst - 24.d0
    end if

    ! done
    if(isControlled()) then
      call checkOut(csubid)
    end if

    return

  end function raan2solarTime

end module astroConvert


module TimeEvolution

implicit none
public rigid_hammer_strike

public time_evolution
public time_evolution_bridge

contains

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

subroutine time_evolution(y,initY,g,N,nTimeSteps,Ms, &
 bL1,bL2,bL3,bL4,bLF,a1,a2,a3,a4,a5,bR1,bR2,bR3,bR4,bRF,deltaT)

  real*8, intent(in)  :: initY(:), g(:), Ms, deltaT

  real*8, intent(in)  :: bL1,bL2,bL3,bL4,bLF
  real*8, intent(in)  :: a1,a2,a3,a4,a5
  real*8, intent(in)  :: bR1,bR2,bR3,bR4,bRF
  integer, intent(in) :: N, nTimeSteps

  real*8, intent(out) :: y(N+1,nTimeSteps+1)

  real*8              :: F
  integer             :: i, t

  !!! First step 1: !!!
  t = 1
  F = 0

    y(1,1) = bL1*initY(1)+bL2*initY(2)+bL3*initY(3)+bL4*initY(1)+bLF*0
    y(2,1) = a1*(initY(4)-initY(2)+2*initY(1)) + a2*(initY(3)+initY(1)) &
 + a3*initY(2) + a4*initY(2) + a5*(initY(3)+initY(1))
    y(N-1,1) = a1*(2*initY(N)-initY(N-1)+initY(N-3)) &
 + a2*(initY(N)+initY(N-2)) + a3*initY(N-1) + a4*initY(N-1) &
 + a5*(initY(N)+initY(N-2))
    y(N,1) = bR1*initY(N) + bR2*initY(N-1) + bR3*initY(N-1) + bR4*initY(N) &
 + bRF*0

    do i=3,(N-1)
      y(i,t) = a1*(initY(i+2)+initY(i-2)) + a2*(initY(i+1) + initY(i-1)) &
 + a3*initY(i) + a4*initY(i) + a5*(initY(i+1)+initY(i-1)) &
 + deltaT*deltaT*N*F*g(i)/Ms
    end do

 ! Now the second step:
 t = 2

    y(1,t) = bL1*y(1,t-1)+bL2*y(2,t-1)+bL3*y(3,t-1)+bL4*initY(1)+bLF*0
    y(2,t) = a1*(y(4,t-1)-y(2,t-1)+2*y(1,t-1)) + a2*(y(3,t-1)+y(1,t-1)) &
 + a3*y(2,t-1) + a4*initY(2) + a5*(initY(3)+initY(1))
    y(N-1,t) = a1*(2*y(N,t-1)-y(N-1,t-1)+y(N-3,t-1)) &
 + a2*(y(N,t-1)+y(N-2,t-1)) + a3*y(N-1,t-1) + a4*initY(N-1) &
 + a5*(initY(N)+initY(N-2))
    y(N,t) = bR1*y(N,t-1) + br2*y(N-1,t-1) + bR3*y(N-1,t-1) + bR4*initY(N) &
 + bRF*0

    do i=3,(N-1)
      y(i,t) = a1*(y(i+2,t-1)+y(i-2,t-1)) + a2*(y(i+1,t-1) + y(i-1,t-1)) &
 + a3*y(i,t-1) + a4*initY(i) + a5*(initY(i+1)+initY(i-1)) &
 + deltaT*deltaT*N*F*g(i)/Ms
    end do

! Finally the Great Time Loop:

do t=3,(nTimeSteps+1)
    y(1,t) = bL1*y(1,t-1)+bL2*y(2,t-1)+bL3*y(3,t-1)+bL4*y(1,t-2)+bLF*0
    y(2,t) = a1*(y(4,t-1)-y(2,t-1)+2*y(1,t-1)) + a2*(y(3,t-1)+y(1,t-1)) &
 + a3*y(2,t-1) + a4*y(2,t-2) + a5*(y(3,t-2)+y(1,t-2))
    y(N-1,t) = a1*(2*y(N,t-1)-y(N-1,t-1)+y(N-3,t-1)) &
 + a2*(y(N,t-1)+y(N-2,t-1)) + a3*y(N-1,t-1) + a4*y(N-1,t-2) &
 + a5*(y(N,t-2)+y(N-2,t-2))
    y(N,t) = bR1*y(N,t-1) + br2*y(N-1,t-1) + bR3*y(N-1,t-1) + bR4*y(N,t-2) &
 + bRF*0

    do i=3,(N-1)
      y(i,t) = a1*(y(i+2,t-1)+y(i-2,t-1)) + a2*(y(i+1,t-1) + y(i-1,t-1)) &
 + a3*y(i,t-1) + a4*y(i,t-2) + a5*(y(i+1,t-2)+y(i-1,t-2)) &
 + deltaT*deltaT*N*F*g(i)/Ms
    end do

  if (t==1) then
    F = 1.5d1
  else if (t==100) then
    F = 0d0
  end if

end do

end subroutine

!!!!!!!!!!!!

subroutine time_evolution_bridge(bridgeY,initY,g,N,nTimeSteps,Ms,bridgePosition, &
     deltaT,inputF,fduration,b1,b2,deltaX,r,mu,zetaB,zetaL,rho,b1Damped,b2Damped,dampT, &
     initialHammerheight,initialHammerVelocity,k,b,hammerMass)

  real*8, intent(in)  :: initY(:), g(:), Ms, deltaT, bridgePosition, inputF, fduration, k, b
  real*8, intent(in)  :: b1, b2, deltaX, r, mu, zetaB, zetaL, rho, b1Damped, b2Damped, dampT, initialHammerHeight, &
       initialHammerVelocity, hammerMass

  real*8              :: bL1,bL2,bL3,bL4,bLF
  real*8              :: a1,a2,a3,a4,a5
  real*8              :: bR1,bR2,bR3,bR4,bRF

  real*8              :: hammerHeight(size(initY,1)), hammerVelocity(size(initY,1)), hammerForce(size(initY,1)), &
       lastHammerVelocity, compression, &
                              stringForce(size(initY,1)), bendingPrefactor, tension
  integer, intent(in) :: N, nTimeSteps
  logical             :: hammerDone

  real*8, intent(out) :: bridgeY(3,nTimeSteps+1)

  real*8              :: F, y(N+1,3), vy(N+1)
  integer             :: i, t, localt, bridgeElement

  real*8              :: D, nu, b_R_denom, b_L_denom

  y = 0
  hammerHeight = initialHammerHeight
  hammerVelocity = initialHammerVelocity
  hammerForce = 0
  stringForce = 0
  hammerDone = .FALSE.

  !!! Constants

  D = 1+b1*deltaT
  nu = (2*b2*deltaT)/(deltaX**2)

  a1 = (-(r**2)*mu)/D
  a2 = (r**2+4*(r**2)*mu+nu)/D
  a3 = (2-2*(r**2)-6*(r**2)*mu-2*nu)/D
  a4 = (-1+b1*deltaT+2*nu)/D
  a5 = -nu/D

  b_R_denom = 1 + b1*deltaT+zetaB*r
  bR1 = (2-2*r**2*mu - 2*r**2)/b_R_denom
  bR2 = (4*r**2*mu - 2*r**2)/b_R_denom
  bR3 = (-2*r**2*mu)/b_R_denom
  bR4 = (-1+b1*deltaT + zetaB*r)/b_R_denom
  bRF = (deltaT**2/rho)/b_R_denom

  b_L_denom = 1 + b1*deltaT+zetaL*r
  bL1 = (2-2*r**2*mu - 2*r**2)/b_L_denom
  bL2 = (4*r**2*mu - 2*r**2)/b_L_denom
  bL3 = (-2*r**2*mu)/b_L_denom
  bL4 = (-1+b1*deltaT + zetaL*r)/b_L_denom
  bLF = (deltaT**2/rho)/b_L_denom

  tension = (k*deltaX*rho/deltaT)**2
  bendingPrefactor = mu*tension*deltaX*deltaX

  bridgeElement = nint(bridgePosition*N)
  !!! First step 1: !!!
  t = 1
  localt = 3
!!!!!!!!!!!!!!!!! Magic numbers are highly discouraged, hence
!!!!!!!!!!!!!!!!! localt having a constant value of 3 (which
!!!!!!!!!!!!!!!!! helps continuity as well.)
  F = 0
  vy = 0

    y(1,localt) = bL1*initY(1)+bL2*initY(2)+bL3*initY(3)+bL4*initY(1)+bLF*0
    y(2,localt) = a1*(initY(4)-initY(2)+2*initY(1)) + a2*(initY(3)+initY(1)) &
 + a3*initY(2) + a4*initY(2) + a5*(initY(3)+initY(1))
    y(N-1,localt) = a1*(2*initY(N)-initY(N-1)+initY(N-3)) &
 + a2*(initY(N)+initY(N-2)) + a3*initY(N-1) + a4*initY(N-1) &
 + a5*(initY(N)+initY(N-2))
    y(N,localt) = bR1*initY(N) + bR2*initY(N-1) + bR3*initY(N-1) + bR4*initY(N) &
 + bRF*0

    do i=3,(N-1)
      y(i,localt) = a1*(initY(i+2)+initY(i-2)) + a2*(initY(i+1) + initY(i-1)) &
 + a3*initY(i) + a4*initY(i) + a5*(initY(i+1)+initY(i-1)) &
 + deltaT*deltaT*N*stringForce(i)/Ms
    end do

! Transfer the bridge element string amplitudes to bridgeY:
  do i=1,3
    bridgeY(i,t) = y(bridgeElement-2+i,localt)
  end do

  y(:,localt-2) = y(:,localt-1)
  y(:,localt-1) = y(:,localt)

 ! Now the second step:
 t = 2

    y(1,localt) = bL1*y(1,localt-1)+bL2*y(2,localt-1)+bL3*y(3,localt-1)+bL4*initY(1)+bLF*0
    y(2,localt) = a1*(y(4,localt-1)-y(2,localt-1)+2*y(1,localt-1)) + a2*(y(3,localt-1)+y(1,localt-1)) &
 + a3*y(2,localt-1) + a4*initY(2) + a5*(initY(3)+initY(1))
    y(N-1,localt) = a1*(2*y(N,localt-1)-y(N-1,localt-1)+y(N-3,localt-1)) &
 + a2*(y(N,localt-1)+y(N-2,localt-1)) + a3*y(N-1,localt-1) + a4*initY(N-1) &
 + a5*(initY(N)+initY(N-2))
    y(N,localt) = bR1*y(N,localt-1) + br2*y(N-1,localt-1) + bR3*y(N-1,localt-1) + bR4*initY(N) &
 + bRF*0

    do i=3,(N-1)
      y(i,localt) = a1*(y(i+2,localt-1)+y(i-2,localt-1)) + a2*(y(i+1,localt-1) + y(i-1,localt-1)) &
 + a3*y(i,localt-1) + a4*initY(i) + a5*(initY(i+1)+initY(i-1)) &
 + deltaT*deltaT*N*stringForce(i)/Ms
    end do

! Transfer the bridge element string amplitudes to bridgeY:
  do i=1,3
    bridgeY(i,t) = y(bridgeElement-2+i,localt)
  end do

  y(:,localt-2) = y(:,localt-1)
  y(:,localt-1) = y(:,localt)

! Finally the Great Time Loop:

do t=3,(nTimeSteps+1)
    y(1,localt) = bL1*y(1,localt-1)+bL2*y(2,localt-1)+bL3*y(3,localt-1)+bL4*y(1,localt-2)+bLF*0
    y(2,localt) = a1*(y(4,localt-1)-y(2,localt-1)+2*y(1,localt-1)) + a2*(y(3,localt-1)+y(1,localt-1)) &
 + a3*y(2,localt-1) + a4*y(2,localt-2) + a5*(y(3,localt-2)+y(1,localt-2))
    y(N-1,localt) = a1*(2*y(N,localt-1)-y(N-1,localt-1)+y(N-3,localt-1)) &
 + a2*(y(N,localt-1)+y(N-2,localt-1)) + a3*y(N-1,localt-1) + a4*y(N-1,localt-2) &
 + a5*(y(N,localt-2)+y(N-2,localt-2))
    y(N,localt) = bR1*y(N,localt-1) + br2*y(N-1,localt-1) + bR3*y(N-1,localt-1) + bR4*y(N,localt-2) &
 + bRF*0

    do i=3,(N-1)
      y(i,localt) = a1*(y(i+2,localt-1)+y(i-2,localt-1)) + a2*(y(i+1,localt-1) + y(i-1,localt-1)) &
 + a3*y(i,localt-1) + a4*y(i,localt-2) + a5*(y(i+1,localt-2)+y(i-1,localt-2)) &
 + deltaT*deltaT*N*stringForce(i)/Ms
    end do

    if (hammerDone.eqv..FALSE.) then
      call rigid_hammer(y(:,localt),hammerHeight,hammerMass,hammerVelocity,hammerForce, &
         deltaT,hammerDone,tension,bendingPrefactor, &
         k,b,rho,deltaX,vy,stringForce)
    end if

  if (t==dampT) then
    !!! Damped constants
    D = 1+b1Damped*deltaT
    nu = (2*b2Damped*deltaT)/(deltaX**2)

    a1 = (-(r**2)*mu)/D
    a2 = (r**2+4*(r**2)*mu+nu)/D
    a3 = (2-2*(r**2)-6*(r**2)*mu-2*nu)/D
    a4 = (-1+b1Damped*deltaT+2*nu)/D
    a5 = -nu/D

    b_R_denom = 1 + b1Damped*deltaT+zetaB*r
    bR1 = (2-2*r**2*mu - 2*r**2)/b_R_denom
    bR2 = (4*r**2*mu - 2*r**2)/b_R_denom
    bR3 = (-2*r**2*mu)/b_R_denom
    bR4 = (-1+b1Damped*deltaT + zetaB*r)/b_R_denom
    bRF = (deltaT**2/rho)/b_R_denom

    b_L_denom = 1 + b1Damped*deltaT+zetaL*r
    bL1 = (2-2*r**2*mu - 2*r**2)/b_L_denom
    bL2 = (4*r**2*mu - 2*r**2)/b_L_denom
    bL3 = (-2*r**2*mu)/b_L_denom
    bL4 = (-1+b1Damped*deltaT + zetaL*r)/b_L_denom
    bLF = (deltaT**2/rho)/b_L_denom
  end if

! Transfer the bridge element string amplitudes to bridgeY:
  do i=1,3
    bridgeY(i,t) = y(bridgeElement-2+i,localt)
  end do

  y(:,localt-2) = y(:,localt-1)
  y(:,localt-1) = y(:,localt)

end do

end subroutine

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

subroutine rigid_hammer(y,hammerHeight,hammerMass,hammerVelocity,hammerForce,deltaT, &
       hammerDone,tension,bendingPrefactor,k,b,rho,deltaX,vy,stringForce)

    real*8, intent(inout) :: y(:), vy(:), hammerHeight(:), hammerVelocity(:)
    real*8, intent(inout) :: hammerForce(:),stringForce(:)
    real*8, intent(in)    :: hammerMass, bendingPrefactor, rho, deltaT, deltaX, tension, k, b
    real*8                :: lastCompression(size(hammerHeight,1)), compression(size(hammerHeight,1)), &
         oldStringForce(size(y,1))
    logical, intent(out)  :: hammerDone
    logical               :: contact(size(y,1))
    integer               :: i, j, nContactElements

    nContactElements = size(y,1)
    hammerDone = .FALSE.
    contact = .TRUE.

    lastCompression = hammerHeight - y
    stringforce = 0

    forall (i=1:size(y,1),lastCompression(i).le.0)
       contact(i) = .FALSE.
       lastCompression(i) = 0
       nContactElements = nContactElements - 1
    end forall
    if (nContactElements.le.1) then
       hammerDone = .TRUE.
       return
    end if
    lastCompression = lastCompression**b

    hammerHeight = hammerHeight + hammerVelocity*deltaT - k*nContactElements*lastCompression*deltaT**2/(2*hammerMass)

    forall (j=1:size(y,1),contact(i).eqv..TRUE.)
       stringforce(i) = -(y(j-2)-4*y(j-1)-2*y(j)-4*y(j+1)+y(j+2))*(1+(y(j-1)-y(j+1))**2)
       stringforce(j) = -stringforce(j)*bendingPrefactor
       stringforce(j) = stringforce(j) - tension*(y(j-1)-2*y(j)+y(j+1))
    end forall

    y = y - vy*deltaT + (k*lastCompression + stringforce)*deltaT**2/(2*rho*deltaX)

!CALCULATEFORCES

    oldstringforce = stringforce
    compression = hammerHeight - y

    forall (i=1:size(y,1),compression(i).le.0)
       contact(i) = .FALSE.
       compression(i) = 0
       nContactElements = nContactElements - 1
    end forall
    if (nContactElements.le.1) then
       hammerDone = .TRUE.
       return
    end if
    compression = compression**b

    forall (i=1:size(y,1),contact(i).eqv..TRUE.)
       stringforce(j) = -(y(j-2)-4*y(j-1)-2*y(j)-4*y(j+1)+y(j+2))*(1+(y(j-1)-y(j+1))**2)
       stringforce(j) = -stringforce(j)*bendingPrefactor
       stringforce(j) = stringforce(j) - tension*(y(j-1)-2*y(j)+y(j+1))
    end forall

    hammerVelocity = hammerVelocity + k*nContactElements*deltaT*(lastcompression+compression)/(2*hammerMass)
    vy = vy + (k*(lastcompression+compression) + oldstringforce+stringforce)*deltaT/(2*rho*deltaX)
    stringforce = k*compression
    print *,maxval(hammerHeight),maxval(hammerVelocity)
  end subroutine rigid_hammer

  subroutine rigid_hammer_strike(y,hammerHeight,hammerMass,hammerVelocity,hammerForce,lastHammerVelocity,compression,deltaT, &
       profile,force,hammerDone,tension,bendingPrefactor,k,b,rho,deltaX)

    real*8, intent(inout) :: y(:), hammerHeight, hammerVelocity, lastHammerVelocity, compression, force(:)
    real*8, intent(inout) :: hammerForce
    real*8, intent(in)    :: profile(:), hammerMass, bendingPrefactor, rho, deltaT, deltaX, tension, k, b
    real*8                :: surface, oldforce(size(force,1)), oldcompression, comppowbp1, oldcomppowbp1, netforce, &
         oldHammerHeight, pstring, pstringmin1
    logical, intent(out)  :: hammerDone
    logical               :: contact(size(y,1))
    integer               :: i, j, nContactElements

    pstring = 0
    pstringmin1 = 0
    nContactElements = 0
    hammerDone = .FALSE.
    oldHammerHeight = hammerHeight
    hammerHeight = hammerHeight + hammerVelocity*deltaT + hammerForce*deltaT**2/(2*hammerMass)
    forall (i=1:size(y,1),y(i)<hammerHeight+profile(i)-compression)
       y(i) = hammerHeight+profile(i)-compression
       contact(i) = .TRUE.
       nContactElements = nContactElements+1
    end forall
    if (all(contact.eqv..FALSE., 1).eqv..TRUE.) then
       hammerDone = .TRUE.
       return
    end if

    ! Now calculate new forces:
    oldforce = force
    forall(j=1:size(y,1),contact(j).eqv..TRUE.)
       force(j) = -(y(j-2)-4*y(j-1)-2*y(j)-4*y(j+1)+y(j+2))*(1+(y(j-1)-y(j+1))**2)
       force(j) = -force(j)*bendingPrefactor
       force(j) = force(j) - tension*(y(j-1)-2*y(j)+y(j+1))
       netforce = netforce - force(j)
       hammerVelocity = hammerVelocity + (oldforce(j)+force(j))/(hammerMass*2)
    end forall
    forall(j=1:size(y,1),contact(j).eqv..TRUE.)
       pstring = pstring + rho * deltaX * hammerVelocity
    end forall

    oldcompression = compression
    oldcomppowbp1 = oldcompression**b

    comppowbp1 = (netforce+hammerForce)*(hammerHeight-oldHammerHeight)/2 &
         + (-pstring**2+pstringmin1**2)/(2*rho*deltaX*nContactElements) &
         - (pstring-pstringmin1)**2/(2*hammerMass)
    comppowbp1 = comppowbp1 + oldcomppowbp1*(b+1)/k
    compression = comppowbp1**(1/(b+1))

    hammerHeight = hammerHeight + oldcompression - compression
    hammerForce = netforce - k*comppowbp1/compression

!    print *, hammerHeight, hammerVelocity, hammerForce, compression
  end subroutine rigid_hammer_strike

end module


!! Print leading zeros for real, integer in Fortran
use, intrinsic :: iso_fortran_env, only: stdout=>output_unit, dp=>real64

implicit none

real(dp):: x
integer :: i

x = 1234.56_dp
i = 123456

write(stdout,'(a5,f12.7)') 'hello',x
write(stdout,'(i0.12)')  i

print *,'there should be leading zeros and no blanks for the real and integer above.'

end program


      double complex function D4six(k1,k2,k3,k4,k5,k6)
      implicit none
C----- six dimensional box corresponding to
C----- qlI4(s56,0d0,s34,0d0,s156,s134,mtsq,0d0,0d0,mtsq,musq,e)
C----- multiplied by a factor of -C(0)/2
      include 'constants.f'
      include 'sprods_com.f'
      include 'masses.f'
      include 'scale.f'
      double complex qlI4,qlI3,IntC(4),IntD
      double precision s12,s34,s56,s134,s156,C(0:4),mtsq,Delta
      integer e,k1,k2,k3,k4,k5,k6
      mtsq=mt**2
      s12=s(k1,k2)
      s34=s(k3,k4)
      s56=s(k5,k6)
      s134=s(k1,k3)+s(k1,k4)+s(k3,k4)
      s156=s(k1,k5)+s(k1,k6)+s(k5,k6)
      Delta=(s12*mtsq-s34*s56+s134*s156)
      C(1)=2d0*(s34-s134)/Delta
      C(2)=-2d0*(s34-s156)*(2d0*s12*mtsq-Delta)/Delta**2
      C(3)=-2d0*(s56-s134)*(2d0*s12*mtsq-Delta)/Delta**2
      C(4)=2d0*(s56-s156)/Delta
      C(0)=-4d0*s12*(s34*s56-s134*s156)/Delta**2
      do e=-2,0
      IntC(1)=qlI3(0d0,s134,s34,0d0,0d0,mtsq,musq,e)  !C2
      IntC(2)=qlI3(s34,0d0,s156,0d0,mtsq,mtsq,musq,e) !C10
      IntC(3)=qlI3(s56,0d0,s134,0d0,mtsq,mtsq,musq,e) !C8
      IntC(4)=qlI3(0d0,s156,s56,0d0,0d0,mtsq,musq,e)  !C4
      IntD=qlI4(s56,0d0,s34,0d0,s156,s134,mtsq,0d0,0d0,mtsq,musq,e)
      D4six=0.5d0*(C(1)*IntC(1)+C(2)*IntC(2)+C(3)*IntC(3)+C(4)*IntC(4)
     & +2d0*IntD)

c--debug
c--real sixdim box
c      D4six=-(C(1)*IntC(1)+C(2)*IntC(2)+C(3)*IntC(3)+C(4)*IntC(4)
c     & +2d0*IntD)/C(0)
c--debug

      enddo
      return
      end


      subroutine initcld

!     Legacy Mars GCM v24
!     Mars Climate Modeling Center
!     NASA Ames Research Center

C  Water ice tracer
C  GCM2.0  Feb 2003
C
C  Initialize the variables used in the cloud scheme

      use grid_h
      use constants_h, only: PI
      use dtcommon_h
      use cldcommon_h

      implicit none

      integer i,j,l

      real*8 :: rmin  = 0.1e-6
      real*8 :: rmax  = 10.e-6
      real*8 :: rbmin = 0.0001e-6
      real*8 :: rbmax = 1.e-2

      real*8 vrat_rt

      real*8 factor

      real*8 :: vistoir = 2.75

      real*8 a0

C======================================================================C

C  Set the various tracer index

c  For dust: Mass and Number
      iMa_dt  = 1
      iNb_dt  = 2
c  For water ice: Mass and Number
      iMa_cld = 3
      iNb_cld = 4
c  For dust core: only Mass
      iMa_cor = 5
c  For water vapor: only Mass
      iMa_vap = 6

C  Some general constants - fill variables that will be passed to the
C  cloud scheme via cldcommon.h.

c  1/3 is computed here once for all

      athird  = (1. / 3.)

C  Water ice particle density

      dpden_ice = 917.

c  Avogadro number
      nav    = 6.02e23
c  Perfect gas constant
      rgp    = 8.3143
c  Boltzman constant
      kbz    = rgp / nav
c  Molecular weight of water  (kg)
      mh2o   = 18.01e-3
c  Weight of a water molecule (kg)
      m0     = mh2o / nav
c  Volume of a water molecule (m3)
      vo1    = mh2o / dpden_ice
c  Activation energy for desorption of water on a dust-like substrate (J/molecule)
      desorp = 0.288e-19
c  Estimated activation energy for surface diffusion of water molecules (J/molecule)
      surfdif= desorp / 10.
c  Jump frequency of a water molecule (s-1)
      nus    = 1.e+13
c
      a0     = 4.52E-10
c
      d_nuc  = a0 / sqrt(2.)

c  Contact parameter of water ice on dust ( m=cos(theta) )
!     mteta  = 0.95        !Franck's number
      mteta  = 0.975       !Franck recommends - newer value

c  nu_eff = exp( dev^2) - 1
c  Standard deviation of the dust distribution
      dev_dt = 0.63676    ! gives an effective variance of 0.5  for dust
c  Standard deviation of the water ice distribution
      dev_ice= 0.3087     ! gives an effective variance of 0.1  for water ice
!      dev_ice= 0.05     ! gives an effective variance of 0.02  for water ice

      aerdens(iMa_dt) = dpden_dt
      aerdens(iNb_dt) = dpden_dt
      aerdens(iMa_cld)= dpden_ice
      aerdens(iNb_cld)= dpden_ice
      aerdens(iMa_cor)= dpden_ice

      stdv(iMa_dt) = dev_dt
      stdv(iNb_dt) = dev_dt
      stdv(iMa_cld)= dev_ice
      stdv(iNb_cld)= dev_ice
      stdv(iMa_cor)= dev_ice

c     Definition of the size grid
c     ~~~~~~~~~~~~~~~~~~~~~~~~~~~
c     aerad is the primary radius grid used for microphysics computation.
c     the grid spacing is based on a volume ratio between two
c     consecutive bins; i.e. vrat.
c     rb defines the boundary values for each aerad bin.

c     Volume ratio between two adjacent bins
      vrat = log(rmax/rmin) / float(nbin-1) *3.
      vrat = exp(vrat)

      rb(1)      = rbmin
      aerad(1)   = rmin
      vol(1)     = 4./3. * pi * rmin**3.

      do i=1,nbin-1
        aerad(i+1)  = aerad(i) * vrat**(athird)
        vol(i+1)    = vol(i) * vrat
      enddo

      do i=1,nbin
        rb(i+1)= ( (2.*vrat) / (vrat+1.) )**(athird) * aerad(i)
        dr(i)  = rb(i+1) - rb(i)
      enddo
      rb(nbin+1) = rbmax
      dr(nbin)   = rb(nbin+1) - rb(nbin)

      print*, ' '
      print*, ' Here are the size bins (lower bnd, center values & dr):'
      print*, ' -------------------------------------------------------'
      do i=1,nbin
         write(*,'(i2,3x,3(e12.6,4x))') i,rb(i), aerad(i),dr(i)
      enddo
       write(*,'(i2,3x,e12.6)') nbin+1,rb(nbin+1)
      print*, ' ----------------------------------------------------'

c     Now, initialize the water ice cloud radiative properties.
c     Here, we use a different size grid (more refined) called rad_rt.

      rad_rt(1)      = 1.e-7
      rad_rt(nbin_rt)= 50.e-6
      radb_rt(1)     = rbmin
      radb_rt(nbin_rt+1)= rbmax

      vrat_rt = log(rad_rt(nbin_rt)/rad_rt(1)) / float(nbin_rt-1) *3.
      vrat_rt = exp(vrat_rt)

      do i = 1, nbin_rt-1
        rad_rt(i+1)  = rad_rt(i) * vrat_rt**(athird)
        radb_rt(i+1)=((2.*vrat_rt) / (vrat_rt+1.))**(athird) * rad_rt(i)
      enddo

c     Read in the data files the Qext,Qscat and g values for each
c     size bins and for each spectral interval.

      open(60,file='data/waterCoated_vis_JD_12bands.dat')
      open(61,file='data/waterCoated_ir_JD_12bands.dat')
      open(62,file='data/Dust_vis_wolff2010_JD_12bands.dat')
      open(63,file='data/Dust_ir_wolff2010_JD_12bands.dat')

      do j = 1, nratio
      do i = 1, nbin_rt
        read(60,'(7(e12.7,x))') (qextv_cld(j,i,l), l=1,nlonv)
        read(60,'(7(e12.7,x))') (qscatv_cld(j,i,l), l=1,nlonv)
        read(60,'(7(e12.7,x))') (gv_cld(j,i,l), l=1,nlonv)
        read(61,'(5(e12.7,x))') (qexti_cld(j,i,l), l=1,nloni)
        read(61,'(5(e12.7,x))') (qscati_cld(j,i,l), l=1,nloni)
        read(61,'(5(e12.7,x))') (gi_cld(j,i,l), l=1,nloni)
      enddo
      enddo

      do i = 1, nbin_rt
        read(62,'(7(e11.5,x))') (qextv_dst(i,l), l=1,nlonv)
        read(62,'(7(e11.5,x))') (qscatv_dst(i,l), l=1,nlonv)
        read(62,'(7(e11.5,x))') (gv_dst(i,l), l=1,nlonv)
        read(63,'(5(e11.5,x))') (qexti_dst(i,l), l=1,nloni)
        read(63,'(5(e11.5,x))') (qscati_dst(i,l), l=1,nloni)
        read(63,'(5(e11.5,x))') (gi_dst(i,l), l=1,nloni)
!        factor  = qextv_dst(i,6) / (vistoir*qexti_dst(i,4))
        factor  = 1.0
        do l = 1, nloni
          qexti_dst(i,l)  = qexti_dst(i,l)  * factor
          qscati_dst(i,l) = qscati_dst(i,l) * factor
        enddo
      enddo

      close(60)
      close(61)
      close(62)
      close(63)

      return
      end


      SUBROUTINE SB10KD( N, M, NP, A, LDA, B, LDB, C, LDC, FACTOR,
     $                   AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK, RCOND,
     $                   IWORK, DWORK, LDWORK, BWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrices of the positive feedback controller
C
C              | Ak | Bk |
C          K = |----|----|
C              | Ck | Dk |
C
C     for the shaped plant
C
C              | A | B |
C          G = |---|---|
C              | C | 0 |
C
C     in the Discrete-Time Loop Shaping Design Procedure.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the plant.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain the
C             system state matrix A of the shaped plant.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= max(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain the
C             system input matrix B of the shaped plant.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= max(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading NP-by-N part of this array must contain the
C             system output matrix C of the shaped plant.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= max(1,NP).
C
C     FACTOR  (input) DOUBLE PRECISION
C             = 1  implies that an optimal controller is required;
C             > 1  implies that a suboptimal controller is required
C                  achieving a performance FACTOR less than optimal.
C             FACTOR >= 1.
C
C     AK      (output) DOUBLE PRECISION array, dimension (LDAK,N)
C             The leading N-by-N part of this array contains the
C             controller state matrix Ak.
C
C     LDAK    INTEGER
C             The leading dimension of the array AK.  LDAK >= max(1,N).
C
C     BK      (output) DOUBLE PRECISION array, dimension (LDBK,NP)
C             The leading N-by-NP part of this array contains the
C             controller input matrix Bk.
C
C     LDBK    INTEGER
C             The leading dimension of the array BK.  LDBK >= max(1,N).
C
C     CK      (output) DOUBLE PRECISION array, dimension (LDCK,N)
C             The leading M-by-N part of this array contains the
C             controller output matrix Ck.
C
C     LDCK    INTEGER
C             The leading dimension of the array CK.  LDCK >= max(1,M).
C
C     DK      (output) DOUBLE PRECISION array, dimension (LDDK,NP)
C             The leading M-by-NP part of this array contains the
C             controller matrix Dk.
C
C     LDDK    INTEGER
C             The leading dimension of the array DK.  LDDK >= max(1,M).
C
C     RCOND   (output) DOUBLE PRECISION array, dimension (4)
C             RCOND(1) contains an estimate of the reciprocal condition
C                      number of the linear system of equations from
C                      which the solution of the P-Riccati equation is
C                      obtained;
C             RCOND(2) contains an estimate of the reciprocal condition
C                      number of the linear system of equations from
C                      which the solution of the Q-Riccati equation is
C                      obtained;
C             RCOND(3) contains an estimate of the reciprocal condition
C                      number of the linear system of equations from
C                      which the solution of the X-Riccati equation is
C                      obtained;
C             RCOND(4) contains an estimate of the reciprocal condition
C                      number of the matrix Rx + Bx'*X*Bx (see the
C                      comments in the code).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*max(N,NP+M))
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) contains the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= 15*N*N + 6*N +
C                       max( 14*N+23, 16*N, 2*N+NP+M, 3*(NP+M) ) +
C                       max( N*N, 11*N*NP + 2*M*M + 8*NP*NP + 8*M*N +
C                                 4*M*NP + NP ).
C             For good performance, LDWORK must generally be larger.
C
C     BWORK   LOGICAL array, dimension (2*N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  the P-Riccati equation is not solved successfully;
C             = 2:  the Q-Riccati equation is not solved successfully;
C             = 3:  the X-Riccati equation is not solved successfully;
C             = 4:  the iteration to compute eigenvalues failed to
C                   converge;
C             = 5:  the matrix Rx + Bx'*X*Bx is singular;
C             = 6:  the closed-loop system is unstable.
C
C     METHOD
C
C     The routine implements the method presented in [1].
C
C     REFERENCES
C
C     [1] McFarlane, D. and Glover, K.
C         A loop shaping design procedure using H_infinity synthesis.
C         IEEE Trans. Automat. Control, vol. AC-37, no. 6, pp. 759-769,
C         1992.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of the results depends on the conditioning of the
C     two Riccati equations solved in the controller design. For
C     better conditioning it is advised to take FACTOR > 1.
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 2000.
C
C     REVISIONS
C
C     V. Sima, Katholieke University Leuven, January 2001,
C     February 2001.
C
C     KEYWORDS
C
C     H_infinity control, Loop-shaping design, Robust control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDDK,
     $                   LDWORK, M, N, NP
      DOUBLE PRECISION   FACTOR
C     ..
C     .. Array Arguments ..
      INTEGER            IWORK( * )
      LOGICAL            BWORK( * )
      DOUBLE PRECISION   A( LDA, * ), AK( LDAK, * ), B( LDB, * ),
     $                   BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ),
     $                   DK( LDDK, * ), DWORK( * ), RCOND( 4 )
C     ..
C     .. Local Scalars ..
      INTEGER            I, I1, I2, I3, I4, I5, I6, I7, I8, I9, I10,
     $                   I11, I12, I13, I14, I15, I16, I17, I18, I19,
     $                   I20, I21, I22, I23, I24, I25, I26, INFO2,
     $                   IWRK, J, LWA, LWAMAX, MINWRK, N2, NS, SDIM
      DOUBLE PRECISION   GAMMA, RNORM
C     ..
C     .. External Functions ..
      LOGICAL            SELECT
      DOUBLE PRECISION   DLANSY, DLAPY2
      EXTERNAL           DLANSY, DLAPY2, SELECT
C     ..
C     .. External Subroutines ..
      EXTERNAL           DGEMM, DGEES, DLACPY, DLASET, DPOTRF, DPOTRS,
     $                   DSYCON, DSYEV, DSYRK, DSYTRF, DSYTRS, SB02OD,
     $                   XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          DBLE, INT, MAX, SQRT
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -5
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
         INFO = -9
      ELSE IF( FACTOR.LT.ONE ) THEN
         INFO = -10
      ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
         INFO = -14
      ELSE IF( LDCK.LT.MAX( 1, M ) ) THEN
         INFO = -16
      ELSE IF( LDDK.LT.MAX( 1, M ) ) THEN
         INFO = -18
      END IF
C
C     Compute workspace.
C
      MINWRK = 15*N*N + 6*N + MAX( 14*N+23, 16*N, 2*N+NP+M, 3*(NP+M) ) +
     $         MAX( N*N, 11*N*NP + 2*M*M + 8*NP*NP + 8*M*N +
     $                   4*M*NP + NP )
      IF( LDWORK.LT.MINWRK ) THEN
         INFO = -22
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10KD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 ) THEN
         RCOND( 1 ) = ONE
         RCOND( 2 ) = ONE
         RCOND( 3 ) = ONE
         RCOND( 4 ) = ONE
         DWORK( 1 ) = ONE
         RETURN
      END IF
C
C     Workspace usage.
C
      N2 = 2*N
      I1 = N*N
      I2 = I1 + N*N
      I3 = I2 + N*N
      I4 = I3 + N*N
      I5 = I4 + N2
      I6 = I5 + N2
      I7 = I6 + N2
      I8 = I7 + N2*N2
      I9 = I8 + N2*N2
C
      IWRK = I9 + N2*N2
      LWAMAX = 0
C
C     Compute Cr = C'*C .
C
      CALL DSYRK( 'U', 'T', N, NP, ONE, C, LDC, ZERO, DWORK( I2+1 ), N )
C
C     Compute Dr = B*B' .
C
      CALL DSYRK( 'U', 'N', N, M, ONE, B, LDB, ZERO, DWORK( I3+1 ), N )
C                                                     -1
C     Solution of the Riccati equation A'*P*(In + Dr*P) *A - P + Cr = 0.
C
      CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, A, LDA,
     $             DWORK( I3+1 ), N, DWORK( I2+1 ), N, DWORK, M, DWORK,
     $             N, RCOND( 1 ), DWORK, N, DWORK( I4+1 ),
     $             DWORK( I5+1 ), DWORK( I6+1 ), DWORK( I7+1 ), N2,
     $             DWORK( I8+1 ), N2, DWORK( I9+1 ), N2, -ONE, IWORK,
     $             DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 1
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
C
C     Transpose A in AK (used as workspace).
C
      DO 40 J = 1, N
         DO 30 I = 1, N
            AK( I,J ) = A( J,I )
   30    CONTINUE
   40 CONTINUE
C                                                    -1
C     Solution of the Riccati equation A*Q*(In + Cr*Q) *A' - Q + Dr = 0.
C
      CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, AK, LDAK,
     $             DWORK( I2+1 ), N, DWORK( I3+1 ), N, DWORK, M, DWORK,
     $             N, RCOND( 2 ), DWORK( I1+1 ), N, DWORK( I4+1 ),
     $             DWORK( I5+1 ), DWORK( I6+1 ), DWORK( I7+1 ), N2,
     $             DWORK( I8+1 ), N2, DWORK( I9+1 ), N2, -ONE, IWORK,
     $             DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 2
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
C
C     Compute gamma.
C
      CALL DGEMM( 'N', 'N', N, N, N, ONE, DWORK( I1+1 ), N, DWORK, N,
     $            ZERO, AK, LDAK )
      CALL DGEES( 'N', 'N', SELECT, N, AK, LDAK, SDIM, DWORK( I6+1 ),
     $            DWORK( I7+1 ), DWORK( IWRK+1 ), N, DWORK( IWRK+1 ),
     $            LDWORK-IWRK, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 4
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
      GAMMA = ZERO
      DO 50 I = 1, N
         GAMMA = MAX( GAMMA, DWORK( I6+I ) )
   50 CONTINUE
      GAMMA = FACTOR*SQRT( ONE + GAMMA )
C
C     Workspace usage.
C
      I3  = I2  + N*NP
      I4  = I3  + NP*NP
      I5  = I4  + NP*NP
      I6  = I5  + NP*NP
      I7  = I6  + NP
      I8  = I7  + NP*NP
      I9  = I8  + NP*NP
      I10 = I9  + NP*NP
      I11 = I10 + N*NP
      I12 = I11 + N*NP
      I13 = I12 + ( NP+M )*( NP+M )
      I14 = I13 + N*( NP+M )
      I15 = I14 + N*( NP+M )
      I16 = I15 + N*N
      I17 = I16 + N2
      I18 = I17 + N2
      I19 = I18 + N2
      I20 = I19 + ( N2+NP+M )*( N2+NP+M )
      I21 = I20 + ( N2+NP+M )*N2
C
      IWRK = I21 + N2*N2
C
C     Compute Q*C' .
C
      CALL DGEMM( 'N', 'T', N, NP, N, ONE, DWORK( I1+1 ), N, C, LDC,
     $            ZERO, DWORK( I2+1 ), N )
C
C     Compute Ip + C*Q*C' .
C
      CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK( I3+1 ), NP )
      CALL DGEMM( 'N', 'N', NP, NP, N, ONE, C, LDC, DWORK( I2+1 ), N,
     $            ONE, DWORK( I3+1 ), NP )
C
C     Compute the eigenvalues and eigenvectors of Ip + C'*Q*C
C
      CALL DLACPY( 'U', NP, NP, DWORK( I3+1 ), NP, DWORK( I5+1 ), NP )
      CALL DSYEV( 'V', 'U', NP, DWORK( I5+1 ), NP, DWORK( I6+1 ),
     $            DWORK( IWRK+1 ), LDWORK-IWRK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 4
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
C                            -1
C     Compute ( Ip + C'*Q*C )  .
C
      DO 70 J = 1, NP
         DO 60 I = 1, NP
            DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP ) /
     $                               DWORK( I6+I )
   60    CONTINUE
   70 CONTINUE
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP,
     $            DWORK( I9+1 ), NP, ZERO, DWORK( I4+1 ), NP )
C
C     Compute Z2 .
C
      DO 90 J = 1, NP
         DO 80 I = 1, NP
            DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP ) /
     $                               SQRT( DWORK( I6+I ) )
   80    CONTINUE
   90 CONTINUE
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP,
     $            DWORK( I9+1 ), NP, ZERO, DWORK( I7+1 ), NP )
C               -1
C     Compute Z2  .
C
      DO 110 J = 1, NP
         DO 100 I = 1, NP
            DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP )*
     $                               SQRT( DWORK( I6+I ) )
  100    CONTINUE
  110 CONTINUE
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP,
     $            DWORK( I9+1 ), NP, ZERO, DWORK( I8+1 ), NP )
C
C     Compute A*Q*C' .
C
      CALL DGEMM( 'N', 'N', N, NP, N, ONE, A, LDA, DWORK( I2+1 ), N,
     $            ZERO, DWORK( I10+1 ), N )
C                                        -1
C     Compute H = -A*Q*C'*( Ip + C*Q*C' )  .
C
      CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I10+1 ), N,
     $            DWORK( I4+1 ), NP, ZERO, DWORK( I11+1 ), N )
C
C     Compute Rx .
C
      CALL DLASET( 'F', NP+M, NP+M, ZERO, ONE, DWORK( I12+1 ), NP+M )
      DO 130 J = 1, NP
         DO 120 I = 1, NP
            DWORK( I12+I+(J-1)*(NP+M) ) = DWORK( I3+I+(J-1)*NP )
  120    CONTINUE
         DWORK( I12+J+(J-1)*(NP+M) ) = DWORK( I3+J+(J-1)*NP ) -
     $                                 GAMMA*GAMMA
  130 CONTINUE
C
C     Compute Bx .
C
      CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I11+1 ), N,
     $            DWORK( I8+1 ), NP, ZERO, DWORK( I13+1 ), N )
      DO 150 J = 1, M
         DO 140 I = 1, N
            DWORK( I13+N*NP+I+(J-1)*N ) = B( I, J )
  140    CONTINUE
  150 CONTINUE
C
C     Compute Sx .
C
      CALL DGEMM( 'T', 'N', N, NP, NP, ONE, C, LDC, DWORK( I8+1 ), NP,
     $            ZERO, DWORK( I14+1 ), N )
      CALL DLASET( 'F', N, M, ZERO, ZERO, DWORK( I14+N*NP+1 ), N )
C
C     Solve the Riccati equation
C                                                      -1
C       X = A'*X*A + Cx - (Sx + A'*X*Bx)*(Rx + Bx'*X*B ) *(Sx'+Bx'*X*A).
C
      CALL SB02OD( 'D', 'B', 'C', 'U', 'N', 'S', N, NP+M, NP, A, LDA,
     $             DWORK( I13+1 ), N, C, LDC, DWORK( I12+1 ), NP+M,
     $             DWORK( I14+1 ), N, RCOND( 3 ), DWORK( I15+1 ), N,
     $             DWORK( I16+1 ), DWORK( I17+1 ), DWORK( I18+1 ),
     $             DWORK( I19+1 ), N2+NP+M, DWORK( I20+1 ), N2+NP+M,
     $             DWORK( I21+1 ), N2, -ONE, IWORK, DWORK( IWRK+1 ),
     $             LDWORK-IWRK, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
C
      I22 = I16
      I23 = I22 + ( NP+M )*N
      I24 = I23 + ( NP+M )*( NP+M )
      I25 = I24 + ( NP+M )*N
      I26 = I25 + M*N
C
      IWRK = I25
C
C     Compute Bx'*X .
C
      CALL DGEMM( 'T', 'N', NP+M, N, N, ONE, DWORK( I13+1 ), N,
     $            DWORK( I15+1 ), N, ZERO, DWORK( I22+1 ), NP+M )
C
C     Compute Rx + Bx'*X*Bx .
C
      CALL DLACPY( 'F', NP+M, NP+M, DWORK( I12+1 ), NP+M,
     $             DWORK( I23+1 ), NP+M )
      CALL DGEMM( 'N', 'N', NP+M, NP+M, N, ONE, DWORK( I22+1 ), NP+M,
     $            DWORK( I13+1 ), N, ONE, DWORK( I23+1 ), NP+M )
C
C     Compute -( Sx' + Bx'*X*A ) .
C
      DO 170 J = 1, N
         DO 160 I = 1, NP+M
            DWORK( I24+I+(J-1)*(NP+M) ) = DWORK( I14+J+(I-1)*N )
 160     CONTINUE
 170  CONTINUE
      CALL DGEMM( 'N', 'N', NP+M, N, N, -ONE, DWORK( I22+1 ), NP+M,
     $            A, LDA, -ONE, DWORK( I24+1 ), NP+M )
C
C     Factorize Rx + Bx'*X*Bx .
C
      RNORM = DLANSY( '1', 'U', NP+M, DWORK( I23+1 ), NP+M,
     $                DWORK( IWRK+1 ) )
      CALL DSYTRF( 'U', NP+M, DWORK( I23+1 ), NP+M, IWORK,
     $             DWORK( IWRK+1 ), LDWORK-IWRK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 5
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
      CALL DSYCON( 'U', NP+M, DWORK( I23+1 ), NP+M, IWORK, RNORM,
     $             RCOND( 4 ), DWORK( IWRK+1 ), IWORK( NP+M+1), INFO2 )
C                                   -1
C     Compute F = -( Rx + Bx'*X*Bx )  ( Sx' + Bx'*X*A ) .
C
      CALL DSYTRS( 'U', NP+M, N, DWORK( I23+1 ), NP+M, IWORK,
     $             DWORK( I24+1 ), NP+M, INFO2 )
C
C     Compute B'*X .
C
      CALL DGEMM( 'T', 'N', M, N, N, ONE, B, LDB, DWORK( I15+1 ), N,
     $            ZERO, DWORK( I25+1 ), M )
C
C     Compute Im + B'*X*B .
C
      CALL DLASET( 'F', M, M, ZERO, ONE, DWORK( I23+1 ), M )
      CALL DGEMM( 'N', 'N', M, M, N, ONE, DWORK( I25+1 ), M, B, LDB,
     $            ONE, DWORK( I23+1 ), M )
C
C     Factorize Im + B'*X*B .
C
      CALL DPOTRF( 'U', M, DWORK( I23+1 ), M, INFO2 )
C                            -1
C     Compute ( Im + B'*X*B )  B'*X .
C
      CALL DPOTRS( 'U', M, N, DWORK( I23+1 ), M, DWORK( I25+1 ), M,
     $             INFO2 )
C                                 -1
C     Compute Dk = ( Im + B'*X*B )  B'*X*H .
C
      CALL DGEMM( 'N', 'N', M, NP, N, ONE, DWORK( I25+1 ), M,
     $            DWORK( I11+1 ), N, ZERO, DK, LDDK )
C
C     Compute Bk = -H + B*Dk .
C
      CALL DLACPY( 'F', N, NP, DWORK( I11+1 ), N, BK, LDBK )
      CALL DGEMM( 'N', 'N', N, NP, M, ONE, B, LDB, DK, LDDK, -ONE,
     $            BK, LDBK )
C                  -1
C     Compute Dk*Z2  .
C
      CALL DGEMM( 'N', 'N', M, NP, NP, ONE, DK, LDDK, DWORK( I8+1 ),
     $            NP, ZERO, DWORK( I26+1 ), M )
C
C     Compute F1 + Z2*C .
C
      CALL DLACPY( 'F', NP, N, DWORK( I24+1 ), NP+M, DWORK( I12+1 ),
     $             NP )
      CALL DGEMM( 'N', 'N', NP, N, NP, ONE, DWORK( I7+1 ), NP, C, LDC,
     $            ONE, DWORK( I12+1 ), NP )
C                            -1
C     Compute Ck = F2 - Dk*Z2  *( F1 + Z2*C ) .
C
      CALL DLACPY( 'F', M, N, DWORK( I24+NP+1 ), NP+M, CK, LDCK )
      CALL DGEMM( 'N', 'N', M, N, NP, -ONE, DWORK( I26+1 ), M,
     $            DWORK( I12+1 ), NP, ONE, CK, LDCK )
C
C     Compute Ak = A + H*C + B*Ck .
C
      CALL DLACPY( 'F', N, N, A, LDA, AK, LDAK )
      CALL DGEMM( 'N', 'N', N, N, NP, ONE, DWORK( I11+1 ), N, C, LDC,
     $            ONE, AK, LDAK )
      CALL DGEMM( 'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK, ONE, AK,
     $            LDAK )
C
C     Workspace usage.
C
      I1 = M*N
      I2 = I1 + N2*N2
      I3 = I2 + N2
C
      IWRK = I3 + N2
C
C     Compute Dk*C .
C
      CALL DGEMM( 'N', 'N', M, N, NP, ONE, DK, LDDK, C, LDC, ZERO,
     $            DWORK, M )
C
C     Compute the closed-loop state matrix.
C
      CALL DLACPY( 'F', N, N, A, LDA, DWORK( I1+1 ), N2 )
      CALL DGEMM( 'N', 'N', N, N, M, -ONE, B, LDB, DWORK, M, ONE,
     $            DWORK( I1+1 ), N2 )
      CALL DGEMM( 'N', 'N', N, N, NP, -ONE, BK, LDBK, C, LDC, ZERO,
     $            DWORK( I1+N+1 ), N2 )
      CALL DGEMM( 'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK, ZERO,
     $            DWORK( I1+N2*N+1 ), N2 )
      CALL DLACPY( 'F', N, N, AK, LDAK, DWORK( I1+N2*N+N+1 ), N2 )
C
C     Compute the closed-loop poles.
C
      CALL DGEES( 'N', 'N', SELECT, N2, DWORK( I1+1 ), N2, SDIM,
     $            DWORK( I2+1 ), DWORK( I3+1 ), DWORK( IWRK+1 ), N,
     $            DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 4
         RETURN
      END IF
      LWA = INT( DWORK( IWRK+1 ) ) + IWRK
      LWAMAX = MAX( LWA, LWAMAX )
C
C     Check the stability of the closed-loop system.
C
      NS = 0
      DO 180 I = 1, N2
        IF( DLAPY2( DWORK( I2+I ), DWORK( I3+I ) ).GT.ONE ) NS = NS + 1
  180 CONTINUE
      IF( NS.GT.0 ) THEN
         INFO = 6
         RETURN
      END IF
C
      DWORK( 1 ) = DBLE( LWAMAX )
      RETURN
C *** Last line of SB10KD ***
      END


      subroutine von_Mises_elastoPlasticity
     *            (matData,F,stre,cc,Cp,xi,finite,err)

      implicit none

      integer          finite, err,
     *                 i, j, P(7), elTyp, mxi, iter, nip,
     *                 round

      double precision matData(*), F(3,*), stre(*), cc(6,*), Cp(*), xi,
     *                 C(6), S(6), invC(6), diC(6,6), invCp(6),
     *                 diCp(6,6), Cpn(6), invCpn(6), xin, Phi,
     *                 dRdCp(6,6), dRdfp(6,6), dRdDg(6),
     *                 dSdC(6,6), dSdCp(6,6), dfpdDg(6), dfpdCp(6,6),
     *                 CS(3,3), gp(3,3), fp(6), n, m1dn, r2dn, Dg,
     *                 Rnorm, R(7), K(7,7), upd(7), dndCp(6),
     *                 h21(9), h22(6), h41(6,6), h42(7,7), h43(6,7),
     *                 h44(7,6), sigY, sigInf, H, Dsig, mHdDsig,
     *                 tol, expFact, fact, fact2, bulk, Hlin,
     *                 r2d3, sr2d3, r0, r1d2, r1, r2,
     *                 norm2_s, norm2s_u, det_u

      logical          outp

      data r0, r1d2, r1, r2 / 0.d0, 0.5d0, 1.d0, 2.d0 /
      r2d3  = r2 / 3.d0
      sr2d3 = sqrt(r2d3)

c
c     3D MATERIAL SUBROUTINE
c
c     elasto-plasticity
c     with isotropic hardening
c     ----------------------------------

c...  small strains .........

      if (finite.eq.0)

     *  call prgError(1,"von_Mises_elastoPlasticity",
     *                  "no small strain version implemented!")

      !write(*,*) finite

c...  finite strains .........
c
c    bulk, elTyp, tol, mxi, nip, outp, mu,                        sigY, H, sigInfty, Hlin
c    bulk, elTyp, tol, mxi, nip, outp, nOgd, mu1, ..., alp1, ..., sigY, H, sigInfty, Hlin
c
      bulk      = matData(1)
      elTyp     = round(matData(2))
      tol       = matData(3)
      mxi       = round(matData(4))
      nip       = round(matData(5))
      outp      = (round(matData(6)).ne.0)

      i = 7
      if (elTyp.eq.4) i = i + round(matData(7))*2

      sigY      = matData(i+1)
      H         = matData(i+2)
      sigInf    = matData(i+3)
      Hlin      = matData(i+4)

      !write(*,*) Hlin

      if (nip.lt.0)      nip =  4
      if (mxi.le.0)      mxi = 10
      if (tol.lt.1.d-14) tol = 1.d-14

      Dsig      = sigInf - sigY

      !write(*,*) "Dsig =", Dsig

      if (Dsig .lt. 1.d-10) then
        H = r0
        mHdDsig = r0
      else
        mHdDsig = -H/Dsig
      endif

      expFact   = exp(mHdDsig * xi)

c...  stress update  -> S, Cp, xi

      call mult_s_uTu_p(C,F,F)
      call inverse_s (invC,C)
      call diff_ATA_m(diC,invC)

c     compute trial state

      call inverse_s (invCp,Cp)                  ! -> invCp
      call diff_ATA_m(diCp,invCp)                ! -> diCp
      call computeElasticResponse(S,dSdC,dSdCp,  !
     *         C,invC,diC,Cp,invCp,diCp,         !
     *         bulk,matData(7),elTyp)            ! -> S, dSdC, dSdCp

      call mult_u_ss_p(CS,C,S)                   ! -> CS
      call set_u_p    (gp,CS)                    !
      call dev_u_r    (gp)                       ! -> gp
      n    = norm2s_u(gp)                        ! -> n
      m1dn = - r1 / n                            ! -> m1dn
      r2dn = - m1dn - m1dn                       ! -> r2dn
      call mult_s_us_f(fp,gp,Cp,r2dn)            ! -> fp

      Phi  = n - sr2d3 * (sigInf - Dsig*expFact + Hlin*xi) !

c      Phi  = n - sr2d3 * (sigY + Dsig*(1.0-expFact) + Hlin*xi) !

      !write(*,*) Phi

      Dg = r0

      if (Phi.gt.tol) then

        call pmove(Cp,    Cpn,    6   )
        call pmove(invCp, invCpn, 6   )
        xin = xi

        if (outp) write(*,'(/5x,g12.5)') abs(Phi)

        iter = 0
        do while ((Rnorm.gt.tol.or.Phi.gt.tol).and.iter.le.mxi)

          iter = iter + 1

c         partial derivatives dfp/dCp, dfp/dDg, dn/dCp, dn/dDg
          call diff_norm2sATd_p(h21,C,CS,n)             ! now: h21 = d n / d S
          call mult_s_s4_p     (dndCp,h21,dSdCp)        ! now: dndCp = d n / d Cp
          call diff_UT_p       (h41,gp)                 ! now: h41 = d dev(CS)T / d T
          call diff_ATdB_p     (h42,C,Cp)               ! now: h42 = d dev(CS)Cp / d S
          call mult_4_44_ap    (h41,h42,dSdCp)          ! now: h41 = d dev(CS)Cp / d Cp
          call set_4_f         (dfpdCp,h41,r2dn)        !
          call mult_4_ss_af    (dfpdCp,fp,dndCp,m1dn)   ! now: dfpdCp = d fp / d Cp

c         R, dR/dCp, dR/dfp, dR/dDg
          call comp_R1111(R,dRdCp,dRdfp,dRdDg,                !
     *                    fp,Cp,invCp,diCp,Cpn,invCpn,Dg,nip) ! -> R
          call mult_4_44_ap(dRdCp,dRdfp,dfpdCp)               !

          do i=1, 6
            do j=1, 6
              K(i,j) = dRdCp(i,j)
            enddo
            K(i,7) = dRdDg(i)
            K(7,i) = dndCp(i)
          enddo
          K(7,7) = - r2d3 * (H * expFact + Hlin)

c         solve and update Cp, Dg
          R(7) = Phi
          call decompLR_matrix(K,P,7,0)
          call solve_matrix   (K,P,R,upd,7)
          Dg = Dg - upd(7)
          xi = xin + sr2d3 * Dg
          expFact = exp(mHdDsig * xi)
          do i=4, 6
            upd(i) = r1d2 * upd(i)
          enddo
          call set_s_am(Cp,upd)

c         update S, fp, Phi
          call inverse_s (invCp,Cp)                  ! -> invCp
          call diff_ATA_m(diCp,invCp)                ! -> diCp
          call computeElasticResponse(S,dSdC,dSdCp,  !
     *         C,invC,diC,Cp,invCp,diCp,             !
     *         bulk,matData(7),elTyp)                ! -> S, dSdC, dSdCp

          call mult_u_ss_p(CS,C,S)                   ! -> CS
          call set_u_p    (gp,CS)                    !
          call dev_u_r    (gp)                       ! -> gp
          n    = norm2s_u(gp)                        ! -> n
          m1dn = - r1 / n                            ! -> m1dn
          r2dn = - m1dn - m1dn                       ! -> r2dn
          call mult_s_us_f(fp,gp,Cp,r2dn)            ! -> fp

          Phi  = n - sr2d3 * (sigInf - Dsig*expFact + Hlin*xi) ! -> Phi

c          Phi  = n - sr2d3 * (sigY + Dsig*(1.0-expFact) + Hlin*xi) !

          call comp_R1000(R,fp,Cp,invCp,Cpn,invCpn,Dg,nip)

          Rnorm = norm2_s(R)

          if (outp) write(*,'(5x,g12.5)') sqrt(Rnorm*Rnorm + Phi*Phi)

        enddo

        if (iter.gt.mxi) then
          write(*,'(/a/)')' strangely, it did not converge (Cp,Dg)!'
          err = 1
          return
        endif

      endif

c...  consistent tangent = dS/dC

      if (Dg.gt.tol) then

c       dR/dC, dPhi/dC
        call diff_norm2sATd_p (h21,C,CS,n)         ! now: h21 = d n / d S
        call mult_s_s4_p      (h22,h21,dSdC)       ! now: h22 = d n / d S * d S / d C
        call transpose_r      (CS)                 !
        call diff_norm2sATd_ap(h22,S,CS,n)         ! now: h22 = d n / d C
        call diff_ATdB_f      (h41,C,Cp,r2dn)      !
        call mult_4_44_p      (h42,h41,dSdC)       !
        call diff_TAdB_af     (h42,S,Cp,r2dn)      !
        call mult_4_ss_af     (h42,fp,h22,m1dn)    ! now: h42 = d fp / d C
        call mult_4_44_p      (h41,dRdfp,h42)      ! now: h41 = d R / d C
                                                   !
        call inverse_matrix(K,h42,P,7)             ! now: h42 = K^{-1}
                                                   !
        do i=1, 6
          do j=1, 6
            h43(i,j) = h42(i,j)
            h44(i,j) = h41(i,j)
          enddo
          h43(i,7) = h42(i,7)  !now: h43 = d Cp / d(R,Phi)   -> 6x7-matrix
          h44(7,i) = h22(i)    !now: h44 = d(R,Phi) / d C    -> 7x6-matrix
        enddo

        call mult_matrix_m (h41, h43,  h44,6,7,6)
        call mult_matrix_ap(dSdC,dSdCp,h41,6,6,6)

      endif

c...  push forward of stre and tangent tensors

      call pushfwrd_s(stre,F,S)

      call pushfwrd_4(cc,F,dSdC)

      fact  = r1 / det_u(F)
      fact2 = fact + fact

      do i=1, 6
        stre(i) = stre(i) * fact
        do j=1, 6
          cc(i,j) = cc(i,j) * fact2
        enddo
      enddo

      return

 100  continue

      end


C SIDENT.F - Gateway function for computation of a discrete-time
C            state-space realization (A,B,C,D) and Kalman gain
C            using SLICOT routine IB01BD.
C
C RELEASE 2.0 of SLICOT System Identification Toolbox.
C Based on SLICOT RELEASE 5.7. Copyright (c) 2002-2020 NICONET e.V.
C
C Matlab call:
C   [(A,C)(,B(,D))(,K,Q,Ry,S,rcnd)] = sident(meth,job,s,n,l,R(,tol,t,A,
C                                            C,printw))
C
C Purpose:
C   To compute a state-space realization (A,B,C,D) and the Kalman
C   predictor gain K of a discrete-time system, given the system
C   order and the relevant part of the R factor of the concatenated
C   block-Hankel matrices, using subspace identification techniques
C   (MOESP and N4SID).
C
C Input parameters:
C   meth  - integer option to determine the method to use:
C           = 1 : MOESP method with past inputs and outputs;
C           = 2 : N4SID method;
C           = 3 : combined method: A and C via MOESP, B and D via N4SID.
C   job   - integer option to determine the calculation to be performed:
C           = 1 : compute all system matrices, A, B, C, D;
C           = 2 : compute the matrices A and C only;
C           = 3 : compute the matrix B only;
C           = 4 : compute the matrices B and D only.
C   s     - the number of block rows in the processed input and output
C           block Hankel matrices.  s > 0.
C   n     - the order of the system.
C   l     - the number of system outputs.
C   R     - the 2*(m+l)*s-by-2*(m+l)*s part of  R  contains the
C           processed upper triangular factor  R  from the
C           QR factorization of the concatenated block-Hankel matrices,
C           and further details needed for computing system matrices.
C           (Above, m denotes the number of system inputs, determined
C           by s, l, and the size of R.)
C   tol   - (optional) tolerance used for estimating the rank of
C           matrices. If  tol > 0,  then the given value of  tol  is
C           used as a lower bound for the reciprocal condition number;
C           an m-by-n matrix whose estimated condition number is less
C           than  1/tol  is considered to be of full rank.
C           Default:    m*n*epsilon_machine where epsilon_machine is
C           the relative machine precision.
C   t     - (optional) the total number of samples used for calculating
C           the covariance matrices.  Either  t = 0, or  t >= 2*(m+l)*s.
C           This parameter is not needed if the covariance matrices
C           and/or the Kalman predictor gain matrix are not desired.
C           If t = 0, then K, Q, Ry, and S are not computed.
C           Default:    t = 0.
C   A     - (optional) the n-by-n system state matrix A.
C           This parameter is needed if meth >= 2 and job >= 3.
C   C     - (optional) the l-by-n system output matrix C.
C           This parameter is needed if meth >= 2 and job >= 3.
C   printw- (optional) switch for printing the warning messages.
C           = 1:  print warning messages;
C           = 0:  do not print warning messages.
C           Default:    printw = 0.
C
C Output parameters:
C   A     - if job <= 2, the n-by-n system state matrix A.
C   C     - if job <= 2, the l-by-n system output matrix C.
C   B     - if job <> 2, the n-by-m system input matrix B.
C   D     - if job = 1 or 4, the l-by-m system matrix D.
C   K     - (optional) the n-by-l Kalman predictor gain matrix K.
C   Q     - (optional) the n-by-n positive semidefinite state covariance
C           matrix used as state weighting matrix when computing the
C           Kalman gain.
C   Ry    - (optional) the l-by-l positive (semi)definite output
C           covariance matrix used as output weighting matrix when
C           computing the Kalman gain.
C   S     - (optional) the n-by-l state-output cross-covariance matrix
C           used as cross-weighting matrix when computing the Kalman
C           gain.
C   rcnd  - (optional) vector of length lr, containing estimates of the
C           reciprocal condition numbers of the matrices involved in
C           rank decisions, least squares or Riccati equation solutions,
C           where lr = 4,  if Kalman gain matrix K is not required, and
C                 lr = 12, if Kalman gain matrix K is required.
C
C Contributor:
C   V. Sima, Research Institute for Informatics, Bucharest, Oct. 1999.
C
C Revisions:
C   V. Sima, May 2000, July 2000, March 2005, Apr. 2009, Dec. 2012.
C
C **********************************************************************
C
      SUBROUTINE MEXFUNCTION( NLHS, PLHS, NRHS, PRHS )
C
C .. Parameters ..
      DOUBLE PRECISION  ZERO
      PARAMETER         ( ZERO = 0.0D0 )
C
C .. Mex-file interface parameters ..
      INTEGER           PLHS(*), PRHS(*)
      INTEGER*4         NLHS, NRHS
C
C .. Mex-file integer functions ..
      INTEGER           mxCreateDoubleMatrix, mxGetPr
      INTEGER*4         mxGetM, mxGetN, mxIsNumeric, mxIsComplex
C
C .. Scalar parameters used by SLICOT subroutines ..
      DOUBLE PRECISION  TOL
      INTEGER           INFO, IWARN, L, LDA, LDB, LDC, LDD, LDK, LDO,
     $                  LDQ, LDR, LDRY, LDS, LDWORK, M, N, NOBR, NSMPL
      CHARACTER         JOB, JOBCK, METH
C
C .. Allocatable arrays ..
C !Fortran 90/95 (Fixed dimensions should be used with Fortran 77.)
      INTEGER,          ALLOCATABLE :: IWORK(:)
      LOGICAL,          ALLOCATABLE :: BWORK(:)
      DOUBLE PRECISION, ALLOCATABLE :: A(:,:), B(:,:), C(:,:), D(:,:),
     $                                 DWORK(:), K(:,:), Q(:,:), R(:,:),
     $                                 RY(:,:), S(:,:)
C
C .. Local variables and constant dimension arrays ..
      DOUBLE PRECISION  TEMP
      CHARACTER*120     TEXT
      INTEGER           ID, IJOB, IP, ITMP, LBWORK, LDUNN, LIWORK, LL,
     $                  LNOBR, MA, MNOBR, MNOBRN, N2, NA, NCOL, NL, NN,
     $                  NPL, NR, NRC, TASK
      LOGICAL           PRINTW
C
C .. External subroutines ..
      EXTERNAL          IB01BD
C
C ..Intrinsic functions..
      INTRINSIC         MAX
C
C Check for proper number of arguments.
C
      IF ( NRHS.LT.6 ) THEN
         CALL mexErrMsgTxt
     $        ( 'SIDENT requires at least 6 input arguments' )
      ELSE IF ( NLHS.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 'SIDENT requires at least 1 output arguments' )
      END IF
C
C Check dimensions of input parameters and read/set scalar parameters.
C
C   meth
C
      IF ( mxGetM( PRHS(1) ).NE.1 .OR. mxGetN( PRHS(1) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'METH must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(1) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(1) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'METH must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(1) ), TEMP, 1 )
      TASK = TEMP
      IF ( TASK.LT.1 .OR. TASK.GT.3 ) THEN
         CALL mexErrMsgTxt
     $        ( 'METH has 1, 2, or 3 the only admissible values' )
      END IF
C
      IF ( TASK.EQ.1 ) THEN
         METH = 'M'
      ELSE IF ( TASK.EQ.2 ) THEN
         METH = 'N'
      ELSE
         METH = 'C'
      END IF
C
C   job
C
      IF ( mxGetM( PRHS(2) ).NE.1 .OR. mxGetN( PRHS(2) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'JOB must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(2) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(2) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'JOB must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(2) ), TEMP, 1 )
      IJOB = TEMP
      IF ( IJOB.LT.1 .OR. IJOB.GT.4 ) THEN
         CALL mexErrMsgTxt
     $        ( 'JOB has 1, 2, 3 or 4 the only admissible values' )
      END IF
C
      IF ( IJOB.EQ.1 ) THEN
         JOB = 'A'
      ELSE IF ( IJOB.EQ.2 ) THEN
         JOB = 'C'
      ELSE IF ( IJOB.EQ.3 ) THEN
         JOB = 'B'
      ELSE
         JOB = 'D'
      END IF
C
C   s
C
      IF ( mxGetM( PRHS(3) ).NE.1 .OR. mxGetN( PRHS(3) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 's must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(3) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(3) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 's must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(3) ), TEMP, 1 )
      NOBR = TEMP
      IF ( NOBR.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 's must be a positive integer' )
      END IF
C
C   n
C
      IF ( mxGetM( PRHS(4) ).NE.1 .OR. mxGetN( PRHS(4) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'n must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(4) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(4) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'n must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(4) ), TEMP, 1 )
      N = TEMP
      IF ( N.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 'n must be a positive integer' )
      END IF
      IF ( N.GE.NOBR ) THEN
         WRITE( TEXT, '('' The order should be at most '', I6)' )
     $          NOBR - 1
         CALL mexErrMsgTxt( TEXT )
      END IF
C
C   l
C
      IF ( mxGetM( PRHS(5) ).NE.1 .OR. mxGetN( PRHS(5) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'l must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(5) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(5) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'l must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(5) ), TEMP, 1 )
      L = TEMP
      IF ( L.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 'The system has no outputs' )
      END IF
C
C   R(nr,nr)
C
      NR   = mxGetM( PRHS(6) )
      NCOL = mxGetN( PRHS(6) )
      IF ( NR.LT.2*L ) THEN
         WRITE( TEXT, '(''R must have at least '',I5,'' rows'')' ) 2*L
         CALL mexErrMsgTxt( TEXT )
      END IF
      IF ( NCOL.LT.NR ) THEN
         WRITE( TEXT, '(''R must have at least '',I5,'' columns'')' )
     $          NCOL
         CALL mexErrMsgTxt( TEXT )
      END IF
      IF ( mxIsNumeric( PRHS(6) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(6) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'R must be a real matrix' )
      END IF
C
C   m
C
      M = NR / ( 2*NOBR ) - L
C
C   tol
C
      TOL = ZERO
      IF ( NRHS.GT.6 ) THEN
         IF ( mxGetM( PRHS(7) ).NE.1 .OR. mxGetN( PRHS(7) ).NE.1 ) THEN
            CALL mexErrMsgTxt( 'TOL must be a scalar' )
         END IF
         IF ( mxIsNumeric( PRHS(7) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(7) ).EQ.1 ) THEN
              CALL mexErrMsgTxt( 'TOL must be a real scalar' )
         END IF
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(7) ), TOL, 1 )
      END IF
C
C   t
C
      NSMPL = 0
      JOBCK = 'N'
      IF ( NRHS.GT.7 ) THEN
         JOBCK = 'K'
         IF ( mxGetM( PRHS(8) ).NE.1 .OR. mxGetN( PRHS(8) ).NE.1 ) THEN
            CALL mexErrMsgTxt( 't must be a scalar' )
         END IF
         IF ( mxIsNumeric( PRHS(8) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(8) ).EQ.1 ) THEN
              CALL mexErrMsgTxt( 't must be a real scalar' )
         END IF
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(8) ), TEMP, 1 )
         NSMPL = TEMP
C
         IF ( NSMPL.NE.0 .AND. NSMPL.LT.NR ) THEN
            WRITE( TEXT,
     $             '('' The number of samples should be at least '',
     $                  I10)' ) NR
            CALL mexErrMsgTxt( TEXT )
         ELSE IF ( NSMPL.EQ.0 ) THEN
            JOBCK = 'N'
         END IF
      END IF
C
C   A(n,n)
C
      IF ( TASK.GE.2 .AND. IJOB.GE.3 ) THEN
         MA = mxGetM( PRHS(9) )
         NA = mxGetN( PRHS(9) )
         IF ( MA.NE.N .OR. NA.NE.N ) THEN
            WRITE( TEXT, '(''A must have '',I5,'' rows and columns'')' )
     $             N
            CALL mexErrMsgTxt( TEXT )
         END IF
         IF ( mxIsNumeric( PRHS(9) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(9) ).EQ.1 ) THEN
            CALL mexErrMsgTxt( 'A must be a real matrix' )
         END IF
C
C   C(l,n)
C
         MA = mxGetM( PRHS(10) )
         NA = mxGetN( PRHS(10) )
         IF ( MA.NE.L ) THEN
            WRITE( TEXT, '(''C must have '',I5,'' rows'')' ) L
            CALL mexErrMsgTxt( TEXT )
         END IF
         IF ( NA.NE.N ) THEN
            WRITE( TEXT, '(''C must have '',I5,'' columns'')' ) N
            CALL mexErrMsgTxt( TEXT )
         END IF
         IF ( mxIsNumeric( PRHS(10) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(10) ).EQ.1 ) THEN
            CALL mexErrMsgTxt( 'C must be a real matrix' )
         END IF
      END IF
C
C     printw
C
      PRINTW = .FALSE.
      IP = 11
      IF ( NRHS.GE.IP ) THEN
         IF ( mxGetM( PRHS(IP) ).NE.1 .OR.
     $        mxGetN( PRHS(IP) ).NE.1 ) THEN
            CALL mexErrMsgTxt( 'PRINTW must be a scalar' )
         END IF
         IF ( mxIsNumeric( PRHS(IP) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(IP) ).EQ.1 ) THEN
            CALL mexErrMsgTxt( 'PRINTW must be an integer scalar' )
         END IF
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(IP) ), TEMP, 1 )
         ITMP = TEMP
         IF ( ITMP.LT.0 .OR. ITMP.GT.1 ) THEN
            CALL mexErrMsgTxt
     $           ( 'PRINTW has 0 or 1 the only admissible values' )
         END IF
         PRINTW = ITMP.EQ.1
      END IF
C
C Determine the lenghts of working arrays.
C The value for LDWORK is the minimum value needed by IB01BD for each
C method and algorithm implemented.  Using a larger value could
C increase the efficiency.
C
      MNOBR  = M*NOBR
      LNOBR  = L*NOBR
      MNOBRN = MNOBR + N
      LDUNN  = ( LNOBR - L )*N
      NPL    = N + L
      N2     = N + N
      NN     = N*N
      NL     = N*L
      LL     = L*L
C
      LDA = MAX( 1, N )
      LDB = LDA
      LDC = MAX( 1, L )
      LDD = LDC
      LDO = LNOBR
      LDR = NR
      IF ( NSMPL.NE.0 ) THEN
         LDK    = LDA
         LDQ    = LDA
         LDS    = LDA
         LDRY   = LDC
         LBWORK = N2
      ELSE
         LDK    = 1
         LDQ    = 1
         LDS    = 1
         LDRY   = 1
         LBWORK = 1
      END IF
C
      LIWORK = MNOBR + N
      IF ( TASK.EQ.1 ) THEN
         LIWORK = MAX( LIWORK, LNOBR )
      ELSE IF ( TASK.EQ.2 ) THEN
         LIWORK = MAX( LIWORK, M*NPL )
      ELSE
         LIWORK = MAX( LIWORK, LNOBR, M*NPL )
      END IF
      IF ( NSMPL.GT.0 )
     $   LIWORK = MAX( LIWORK, NN )
C
      IAW    = 0
      LDWORK = LDUNN + 4*N
      IF ( TASK.EQ.1 ) THEN
         ID = 0
      ELSE
         ID = N
      END IF
C
      IF ( TASK.NE.2 ) THEN
         IF ( IJOB.LE.2 ) THEN
            LDWORK = MAX( LDWORK, 2*LDUNN + N2, LDUNN + NN + 7*N )
         END IF
      END IF
C
      IF ( ( M.GT.0 .AND. IJOB.NE.2 ) .OR. TASK.GE.2 ) THEN
         LDWORK = MAX( LDWORK, 2*LDUNN + NN + ID + 7*N )
         IF ( TASK.EQ.1 )
     $      LDWORK = MAX( LDWORK, LDUNN + N + 6*MNOBR, LDUNN + N +
     $                    MAX( L + MNOBR, LNOBR +
     $                                    MAX( 3*LNOBR + 1, M ) ) )
      ELSE
         IF ( TASK.NE.2 ) THEN
     $      IAW = N + NN
      END IF
C
      IF ( TASK.NE.1 .OR. NSMPL.GT.0 ) THEN
         LDWORK = MAX( LDWORK, LDUNN + IAW + N2 +
     $                 MAX( 5*N, LNOBR + 2*MNOBR + L ),
     $                 ID + 4*MNOBRN + 1, ID + MNOBRN + NPL )
         IF ( TASK.NE.1 .AND. M.GT.0 .AND. IJOB.NE.2 )
     $      LDWORK = MAX( LDWORK, MNOBR*NPL*( M*NPL + 1 ) +
     $                    MAX( NPL**2, 4*M*NPL + 1 ) )
         LDWORK = LNOBR*N + LDWORK
      END IF
C
      IF ( NSMPL.GT.0 )
     $   LDWORK = MAX( LDWORK, 4*NN + 2*NL + LL + MAX( 3*L, NL ),
     $                 14*NN + 12*N + 5 )
C
C Allocate variable dimension local arrays.
C !Fortran 90/95
C
      ALLOCATE ( A( LDA, N ), B( LDB, M ), C( LDC, N ), D( LDD, M ),
     $           DWORK( LDWORK ), IWORK( LIWORK ), Q( LDQ, N ),
     $           R( LDR, NCOL ), RY( LDRY, L ), S( LDS, L ) )
      ALLOCATE ( BWORK( LBWORK ), K( LDK, L ) )
C
C Copy inputs from MATLAB workspace to locally allocated arrays.
C
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(6) ), R, LDR*NCOL )
      IF ( TASK.GE.2 .AND. IJOB.GE.3 ) THEN
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(9) ),  A, LDA*N )
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(10) ), C, LDC*N )
      END IF
C
C Do the actual computations.
C
      CALL IB01BD( METH, JOB, JOBCK, NOBR, N, M, L, NSMPL, R, LDR, A,
     $             LDA, C, LDC, B, LDB, D, LDD, Q, LDQ, RY, LDRY, S,
     $             LDS, K, LDK, TOL, IWORK, DWORK, LDWORK, BWORK, IWARN,
     $             INFO )
      IF ( IWARN.NE.0 .AND. PRINTW ) THEN
         WRITE( TEXT, '(''  IWARN = '',I4,'' ON EXIT FROM IB01BD'')'
     $        ) IWARN
      END IF
      IF ( INFO.NE.0 ) THEN
         WRITE( TEXT, '(''INFO = '',I4,'' ON EXIT FROM IB01BD'')'
     $        ) INFO
      ELSE
C
C Copy output to MATLAB workspace.
C
         IF ( IJOB.LE.2 ) THEN
            PLHS(1) = mxCreateDoubleMatrix( N, N, 0 )
            CALL mxCopyReal8ToPtr( A, mxGetPr( PLHS(1) ), NN )
            IP = 1
            IF ( NLHS.GT.1 ) THEN
               PLHS(2) = mxCreateDoubleMatrix( L, N, 0 )
               CALL mxCopyReal8ToPtr( C, mxGetPr( PLHS(2) ), NL )
               IP = 2
            END IF
         ELSE
            IP = 0
         END IF
C
         IF ( NLHS.GT.IP ) THEN
            IF ( IJOB.EQ.1 .OR. IJOB.GE.3 ) THEN
               IP = IP + 1
               PLHS(IP) = mxCreateDoubleMatrix( N, M, 0 )
               CALL mxCopyReal8ToPtr( B, mxGetPr( PLHS(IP) ), N*M )
            END IF
            IF ( NLHS.GT.IP ) THEN
               IF ( IJOB.EQ.1 .OR. IJOB.EQ.4 ) THEN
                  IP = IP + 1
                  PLHS(IP) = mxCreateDoubleMatrix( L, M, 0 )
                  CALL mxCopyReal8ToPtr( D, mxGetPr( PLHS(IP) ), L*M )
               END IF
            END IF
         END IF
C
         IF ( NSMPL.GT.0 .AND. NLHS.GT.IP ) THEN
            IP = IP + 1
            PLHS(IP) = mxCreateDoubleMatrix( N, L, 0 )
            CALL mxCopyReal8ToPtr( K, mxGetPr( PLHS(IP) ), NL )
         END IF
C
         IF ( NSMPL.GT.0 .AND. NLHS.GT.IP ) THEN
            IP = IP + 1
            PLHS(IP) = mxCreateDoubleMatrix( N, N, 0 )
            CALL mxCopyReal8ToPtr( Q, mxGetPr( PLHS(IP) ), NN )
            IP = IP + 1
            PLHS(IP) = mxCreateDoubleMatrix( L, L, 0 )
            CALL mxCopyReal8ToPtr( RY, mxGetPr( PLHS(IP) ), LL )
            IP = IP + 1
            PLHS(IP) = mxCreateDoubleMatrix( N, L, 0 )
            CALL mxCopyReal8ToPtr( S, mxGetPr( PLHS(IP) ), NL )
         END IF
C
         IF ( NLHS.GT.IP ) THEN
            IP = IP + 1
            IF ( NSMPL.EQ.0 ) THEN
               NRC = 4
            ELSE
               NRC = 12
            END IF
            PLHS(IP) = mxCreateDoubleMatrix( NRC, 1, 0 )
            CALL mxCopyReal8ToPtr( DWORK(2), mxGetPr( PLHS(IP) ), NRC )
         END IF
      END IF
C
C Deallocate local arrays.
C !Fortran 90/95
C
      DEALLOCATE( A, B, C, D, R, Q, RY, S, IWORK, DWORK )
      DEALLOCATE( K, BWORK )
C
C Error and warning handling.
C
      IF ( IWARN.NE.0 .AND. PRINTW ) THEN
         CALL mexPrintf( TEXT )
      END IF
C
      IF ( INFO.NE.0 ) THEN
         CALL mexErrMsgTxt( TEXT )
      END IF
C
      RETURN
C *** Last line of SIDENT ***
      END


MODULE Controllers

	USE, INTRINSIC	:: ISO_C_Binding
	USE Functions
	USE Filters

	IMPLICIT NONE

CONTAINS
	SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst)

		USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

	   ! Local Variables:
		REAL(C_FLOAT), INTENT(INOUT)	:: avrSWAP(*)	! The swap array, used to pass data to, and receive data from the DLL controller.
		INTEGER(4)						:: K			! Index used for looping through blades.

		TYPE(ControlParameters), INTENT(INOUT)	:: CntrPar
		TYPE(LocalVariables),    INTENT(INOUT)	:: LocalVar
		TYPE(ObjectInstances),   INTENT(INOUT)	:: objInst

		!..............................................................................................................................
		! Pitch control
		!..............................................................................................................................
		! Set the pitch override to yes
		avrSWAP(55) = 0.0						! Pitch override: 0=yes

		IF (LocalVar%PC_State >= 1) THEN
			LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit
		ELSE
			LocalVar%PC_MaxPitVar = CntrPar%PC_FinePit
		END IF

		! Compute the gain scheduling correction factor based on the previously
		! commanded pitch angle for blade 1:
		LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KP, LocalVar%PC_PitComT)
		LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KI, LocalVar%PC_PitComT)
		LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KD, LocalVar%PC_PitComT)
		LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_TF, LocalVar%PC_PitComT)

		! Compute the current speed error and its integral w.r.t. time; saturate the
		! integral term using the pitch angle limits:
		LocalVar%PC_SpdErr = CntrPar%PC_RefSpd - LocalVar%GenSpeedF					! Speed error
		LocalVar%PC_PwrErr = CntrPar%VS_RtPwr - LocalVar%VS_GenPwr					! Power error
		LocalVar%Y_MErr = LocalVar%Y_M + CntrPar%Y_MErrSet							! Yaw-alignment error

		! Compute the pitch commands associated with the proportional and integral
		! gains:
		IF (LocalVar%iStatus == 0) THEN
			LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPitVar, LocalVar%DT, LocalVar%PitCom(1), .TRUE., objInst%instPI)
		ELSE
			LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_FinePit, .FALSE., objInst%instPI)
		END IF

		! Individual pitch control
		IF ((CntrPar%IPC_ControlMode >= 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN
			CALL IPC(CntrPar, LocalVar, objInst)
		ELSE
			LocalVar%IPC_PitComF = 0.0 ! THIS IS AN ARRAY!!
		END IF

        ! Fore-aft tower vibration damping control
        IF ((CntrPar%FA_KI > 0.0) .OR. (CntrPar%Y_ControlMode == 2)) THEN
            CALL ForeAftDamping(CntrPar, LocalVar, objInst)
        ELSE
            LocalVar%FA_PitCom = 0.0 ! THIS IS AN ARRAY!!
        END IF

		! Combine and saturate all pitch commands:
		DO K = 1,LocalVar%NumBl ! Loop through all blades, add IPC contribution and limit pitch rate
			! PitCom(K) = ratelimit(LocalVar%PC_PitComT_IPC(K), LocalVar%BlPitch(K), PC_MinRat, PC_MaxRat, LocalVar%DT)	! Saturate the overall command of blade K using the pitch rate limit
			LocalVar%PitCom(K) = saturate(LocalVar%PC_PitComT, CntrPar%PC_MinPit, CntrPar%PC_MaxPit)					! Saturate the overall command using the pitch angle limits
			LocalVar%PitCom(K) = LocalVar%PitCom(K) + LocalVar%IPC_PitComF(K) + LocalVar%FA_PitCom(K)
		END DO

		! Command the pitch demanded from the last
		! call to the controller (See Appendix A of Bladed User's Guide):
			avrSWAP(42) = LocalVar%PitCom(1)		! Use the command angles of all blades if using individual pitch
			avrSWAP(43) = LocalVar%PitCom(2)		! "
			avrSWAP(44) = LocalVar%PitCom(3)		! "
			avrSWAP(45) = LocalVar%PitCom(1)		! Use the command angle of blade 1 if using collective pitch

	END SUBROUTINE PitchControl

	SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)

		USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

		REAL(C_FLOAT), INTENT(INOUT)				:: avrSWAP(*)	! The swap array, used to pass data to, and receive data from, the DLL controller.

		TYPE(ControlParameters), INTENT(INOUT)	:: CntrPar
		TYPE(LocalVariables), INTENT(INOUT)	:: LocalVar
		TYPE(ObjectInstances), INTENT(INOUT)	:: objInst

		!..............................................................................................................................
		! VARIABLE-SPEED TORQUE CONTROL:
		!..............................................................................................................................
		avrSWAP(35) = 1.0          ! Generator contactor status: 1=main (high speed) variable-speed generator
		avrSWAP(56) = 0.0          ! Torque override: 0=yes

		! Filter the HSS (generator) speed measurement:
		! Apply Low-Pass Filter (choice between first- and second-order low-pass filter)
		IF (CntrPar%F_FilterType == 1) THEN
            LocalVar%GenSpeedF = LPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_CornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
		ELSEIF (CntrPar%F_FilterType == 2) THEN
            LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_CornerFreq, CntrPar%F_Damping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Second-order low-pass filter on generator speed
        END IF

		! Compute the generator torque, which depends on which region we are in:
		LocalVar%VS_SpdErrAr = CntrPar%VS_RefSpd - LocalVar%GenSpeedF		! Current speed error - Above-rated PI-control
		LocalVar%VS_SpdErrBr = CntrPar%VS_MinOMSpd - LocalVar%GenSpeedF		! Current speed error - Below-rated PI-control
		IF (LocalVar%VS_State >= 4) THEN
			LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_ArSatTq, .TRUE., objInst%instPI)
			LocalVar%GenBrTq = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_MinOMTq, LocalVar%DT, CntrPar%VS_MinOMTq, .TRUE., objInst%instPI)
			IF (LocalVar%VS_State == 4) THEN
				LocalVar%GenTq = CntrPar%VS_RtTq
			ELSEIF (LocalVar%VS_State == 5) THEN
				LocalVar%GenTq = (CntrPar%VS_RtPwr/CntrPar%VS_GenEff)/LocalVar%GenSpeedF
			END IF
		ELSE
			LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_MaxOMTq, .FALSE., objInst%instPI)
			LocalVar%GenBrTq = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_MinOMTq, LocalVar%DT, CntrPar%VS_MinOMTq, .FALSE., objInst%instPI)
			IF (LocalVar%VS_State == 3) THEN
				LocalVar%GenTq = LocalVar%GenArTq
			ELSEIF (LocalVar%VS_State == 1) THEN
				LocalVar%GenTq = LocalVar%GenBrTq
			ELSEIF (LocalVar%VS_State == 2) THEN
				LocalVar%GenTq = CntrPar%VS_Rgn2K*LocalVar%GenSpeedF*LocalVar%GenSpeedF
			ELSE
				LocalVar%GenTq = CntrPar%VS_MaxOMTq
			END IF
		END IF

		! Saturate the commanded torque using the maximum torque limit:
		LocalVar%GenTq = MIN(LocalVar%GenTq, CntrPar%VS_MaxTq)						! Saturate the command using the maximum torque limit

		! Saturate the commanded torque using the torque rate limit:
		IF (LocalVar%iStatus == 0)  LocalVar%VS_LastGenTrq = LocalVar%GenTq				! Initialize the value of LocalVar%VS_LastGenTrq on the first pass only
		LocalVar%GenTq = ratelimit(LocalVar%GenTq, LocalVar%VS_LastGenTrq, -CntrPar%VS_MaxRat, CntrPar%VS_MaxRat, LocalVar%DT)	! Saturate the command using the torque rate limit

		! Reset the value of LocalVar%VS_LastGenTrq to the current values:
		LocalVar%VS_LastGenTrq = LocalVar%GenTq

		! Set the generator contactor status, avrSWAP(35), to main (high speed)
		! variable-speed generator, the torque override to yes, and command the
		! generator torque (See Appendix A of Bladed User's Guide):
		avrSWAP(47) = LocalVar%VS_LastGenTrq   ! Demanded generator torque
	END SUBROUTINE VariableSpeedControl

	SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst)

		USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

		REAL(C_FLOAT), INTENT(INOUT)				:: avrSWAP(*)	! The swap array, used to pass data to, and receive data from, the DLL controller.

		TYPE(ControlParameters), INTENT(INOUT)	:: CntrPar
		TYPE(LocalVariables), INTENT(INOUT)	:: LocalVar
		TYPE(ObjectInstances), INTENT(INOUT)	:: objInst

		!..............................................................................................................................
		! Yaw control
		!..............................................................................................................................

		IF (CntrPar%Y_ControlMode == 1) THEN
			avrSWAP(29) = 0									! Yaw control parameter: 0 = yaw rate control
			IF (LocalVar%Time >= LocalVar%Y_YawEndT) THEN											! Check if the turbine is currently yawing
				avrSWAP(48) = 0.0													! Set yaw rate to zero

				LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .FALSE., objInst%instLPF)		! Fast low pass filtered yaw error with a frequency of 1
				LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .FALSE., objInst%instLPF)		! Slow low pass filtered yaw error with a frequency of 1/60

				LocalVar%Y_AccErr = LocalVar%Y_AccErr + LocalVar%DT*SIGN(LocalVar%Y_ErrLPFFast**2, LocalVar%Y_ErrLPFFast)	! Integral of the fast low pass filtered yaw error

				IF (ABS(LocalVar%Y_AccErr) >= CntrPar%Y_ErrThresh) THEN								! Check if accumulated error surpasses the threshold
					LocalVar%Y_YawEndT = ABS(LocalVar%Y_ErrLPFSlow/CntrPar%Y_Rate) + LocalVar%Time					! Yaw to compensate for the slow low pass filtered error
				END IF
			ELSE
				avrSWAP(48) = SIGN(CntrPar%Y_Rate, LocalVar%Y_MErr)		! Set yaw rate to predefined yaw rate, the sign of the error is copied to the rate
				LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .TRUE., objInst%instLPF)		! Fast low pass filtered yaw error with a frequency of 1
				LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .TRUE., objInst%instLPF)		! Slow low pass filtered yaw error with a frequency of 1/60
				LocalVar%Y_AccErr = 0.0								! "
			END IF
		END IF
	END SUBROUTINE YawRateControl

	SUBROUTINE IPC(CntrPar, LocalVar, objInst)
		!-------------------------------------------------------------------------------------------------------------------------------
		! Individual pitch control subroutine
		! Calculates the commanded pitch angles for IPC employed for blade fatigue load reductions at 1P and 2P
        !
		! Variable declaration and initialization
		!------------------------------------------------------------------------------------------------------------------------------
		USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

		! Local variables
		REAL(4)					:: PitComIPC(3), PitComIPC_1P(3), PitComIPC_2P(3)
		INTEGER(4)				:: K								    ! Integer used to loop through turbine blades
		REAL(4)					:: axisTilt_1P, axisYaw_1P, axisYawF_1P ! Direct axis and quadrature axis outputted by Coleman transform, 1P
		REAL(4), SAVE			:: IntAxisTilt_1P, IntAxisYaw_1P		! Integral of the direct axis and quadrature axis, 1P
        REAL(4)					:: axisTilt_2P, axisYaw_2P, axisYawF_2P ! Direct axis and quadrature axis outputted by Coleman transform, 1P
		REAL(4), SAVE			:: IntAxisTilt_2P, IntAxisYaw_2P		! Integral of the direct axis and quadrature axis, 1P
		REAL(4)					:: IntAxisYawIPC_1P					    ! IPC contribution with yaw-by-IPC component
		REAL(4)					:: Y_MErrF, Y_MErrF_IPC				    ! Unfiltered and filtered yaw alignment error [rad]

		TYPE(ControlParameters), INTENT(INOUT)	:: CntrPar
		TYPE(LocalVariables), INTENT(INOUT)		:: LocalVar
		TYPE(ObjectInstances), INTENT(INOUT)	:: objInst

		! Body
		! Initialization
			! Set integrals to be 0 in the first time step
		IF (LocalVar%iStatus==0) THEN
			IntAxisTilt_1P = 0.0
			IntAxisYaw_1P = 0.0
            IntAxisTilt_2P = 0.0
			IntAxisYaw_2P = 0.0
		END IF

		! Pass rootMOOPs through the Coleman transform to get the tilt and yaw moment axis
		CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, NP_1, axisTilt_1P, axisYaw_1P)
        CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, NP_2, axisTilt_2P, axisYaw_2P)

		! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution
		IF (CntrPar%Y_ControlMode == 2) THEN
			Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%iStatus, .FALSE., objInst%instSecLPF)
			Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
		ELSE
			axisYawF_1P = axisYaw_1P
			Y_MErrF = 0.0
			Y_MErrF_IPC = 0.0
		END IF

		! Integrate the signal and multiply with the IPC gain
		IF ((CntrPar%IPC_ControlMode >= 1) .AND. (CntrPar%Y_ControlMode /= 2)) THEN
			IntAxisTilt_1P	= IntAxisTilt_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisTilt_1P
			IntAxisYaw_1P = IntAxisYaw_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisYawF_1P
			IntAxisTilt_1P = saturate(IntAxisTilt_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
			IntAxisYaw_1P = saturate(IntAxisYaw_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)

            IF (CntrPar%IPC_ControlMode >= 2) THEN
                IntAxisTilt_2P	= IntAxisTilt_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisTilt_2P
			    IntAxisYaw_2P = IntAxisYaw_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisYawF_2P
			    IntAxisTilt_2P = saturate(IntAxisTilt_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
			    IntAxisYaw_2P = saturate(IntAxisYaw_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
            END IF
		ELSE
			IntAxisTilt_1P = 0.0
			IntAxisYaw_1P = 0.0
            IntAxisTilt_2P = 0.0
			IntAxisYaw_2P = 0.0
		END IF

		! Add the yaw-by-IPC contribution
		IntAxisYawIPC_1P = IntAxisYaw_1P + Y_MErrF_IPC

		! Pass direct and quadrature axis through the inverse Coleman transform to get the commanded pitch angles
		CALL ColemanTransformInverse(IntAxisTilt_1P, IntAxisYawIPC_1P, LocalVar%Azimuth, NP_1, CntrPar%IPC_aziOffset(1), PitComIPC_1P)
        CALL ColemanTransformInverse(IntAxisTilt_2P, IntAxisYaw_2P, LocalVar%Azimuth, NP_2, CntrPar%IPC_aziOffset(2), PitComIPC_2P)

		! Sum nP IPC contrubutions and store to LocalVar data type
		DO K = 1,LocalVar%NumBl
            PitComIPC(K) = PitComIPC_1P(K) + PitComIPC_2P(K)
			LocalVar%IPC_PitComF(K) = PitComIPC(K)
		END DO
	END SUBROUTINE IPC

    SUBROUTINE ForeAftDamping(CntrPar, LocalVar, objInst)
		!-------------------------------------------------------------------------------------------------------------------------------
		! Fore-aft damping controller, reducing the tower fore-aft vibrations using pitch
		!
		! Variable declaration and initialization
		!------------------------------------------------------------------------------------------------------------------------------
		USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

		! Local variables
		INTEGER(4)				:: K								    ! Integer used to loop through turbine blades

		TYPE(ControlParameters), INTENT(INOUT)	:: CntrPar
		TYPE(LocalVariables), INTENT(INOUT)		:: LocalVar
		TYPE(ObjectInstances), INTENT(INOUT)	:: objInst

		! Body
		LocalVar%FA_AccHPF = HPFilter( LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPF_CornerFreq, LocalVar%iStatus, .FALSE., objInst%instHPF )
        LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0, .FALSE., objInst%instPI)

        ! Store the fore-aft pitch contribution to LocalVar data type
		DO K = 1,LocalVar%NumBl
			LocalVar%FA_PitCom(K) = LocalVar%FA_AccHPFI
		END DO

    END SUBROUTINE ForeAftDamping
END MODULE Controllers


!  This file contains:
!  1.  SUBROUTINE fftc2c
!  2.  SUBROUTINE ifftc2c
!  3.  SUBROUTINE fftc2c_inplace
!  4.  SUBROUTINE ifftc2c_inplace
!
!  CREATOR:  Yu Yu @ SHAO 2011.10.29
!  LAST MODIFIER:  Yu Yu @ SHAO 2014.06.18

!======================================
SUBROUTINE fftc2c(indata1d,outdata1d,L)
!  This SUBROUTINE compute forward FFT from COMPLEX to COMPLEX
!  with forward scale factor 1.0
!  by using Intel MKL.
!  'mkl_dfti.f90' should be included

Use MKL_DFTI

INTEGER::L(3)
COMPLEX(4)::indata1d(L(1)*L(2)*L(3))
COMPLEX(4)::outdata1d(L(1)*L(2)*L(3))
type(DFTI_DESCRIPTOR),POINTER::my_desc_handle
INTEGER::status

print*,'begin fftc2c'

status = DftiCreateDescriptor( my_desc_handle, DFTI_SINGLE, DFTI_COMPLEX, 3, L )
status = DftiSetValue( my_desc_handle, DFTI_PLACEMENT, DFTI_NOT_INPLACE )
status = DftiCommitDescriptor(my_desc_handle)
status = DftiComputeForward( my_desc_handle, indata1d, outdata1d)
status = DftiFreeDescriptor(my_desc_handle)

print*,'fftc2c success'

END SUBROUTINE

!=======================================
SUBROUTINE ifftc2c(indata1d,outdata1d,L)
!  This SUBROUTINE compute backward FFT from COMPLEX to COMPLEX
!  with forward scale factor 1.0/N_grid
!  by using Intel MKL.
!  'mkl_dfti.f90' should be included

Use MKL_DFTI

INTEGER::L(3)
COMPLEX(4)::indata1d(L(1)*L(2)*L(3))
COMPLEX(4)::outdata1d(L(1)*L(2)*L(3))
REAL(4)::factor
type(DFTI_DESCRIPTOR),POINTER::my_desc_handle
INTEGER::status

print*,'begin ifftc2c'

factor=1.0/(float(L(1))*float(L(2))*float(L(3)))

status = DftiCreateDescriptor( my_desc_handle, DFTI_SINGLE, DFTI_COMPLEX, 3, L )
status = DftiSetValue( my_desc_handle, DFTI_PLACEMENT, DFTI_NOT_INPLACE )
status = DftiSetValue( my_desc_handle, DFTI_BACKWARD_SCALE, factor )
status = DftiCommitDescriptor(my_desc_handle)
status = DftiComputeBackward( my_desc_handle, indata1d, outdata1d)
status = DftiFreeDescriptor(my_desc_handle)

print*,'ifftc2c success'

END SUBROUTINE

!======================================
SUBROUTINE fftc2c_inplace(indata1d,L)
!  This SUBROUTINE compute forward FFT from COMPLEX to COMPLEX
!  with forward scale factor 1.0
!  by using Intel MKL.
!  'mkl_dfti.f90' should be included

Use MKL_DFTI

INTEGER::L(3)
COMPLEX(4)::indata1d(L(1)*L(2)*L(3))
type(DFTI_DESCRIPTOR),POINTER::my_desc_handle
INTEGER::status

print*,'begin fftc2c'

status = DftiCreateDescriptor( my_desc_handle, DFTI_SINGLE, DFTI_COMPLEX, 3, L )
status = DftiSetValue( my_desc_handle, DFTI_PLACEMENT, DFTI_INPLACE )
status = DftiCommitDescriptor(my_desc_handle)
status = DftiComputeForward( my_desc_handle, indata1d)
status = DftiFreeDescriptor(my_desc_handle)

print*,'fftc2c success'

END SUBROUTINE

!=======================================
SUBROUTINE ifftc2c_inplace(indata1d,L)
!  This SUBROUTINE compute backward FFT from COMPLEX to COMPLEX
!  with forward scale factor 1.0/N_grid
!  by using Intel MKL.
!  'mkl_dfti.f90' should be included

Use MKL_DFTI

INTEGER::L(3)
COMPLEX(4)::indata1d(L(1)*L(2)*L(3))
REAL(4)::factor
type(DFTI_DESCRIPTOR),POINTER::my_desc_handle
INTEGER::status

print*,'begin ifftc2c'

factor=1.0/(float(L(1))*float(L(2))*float(L(3)))

status = DftiCreateDescriptor( my_desc_handle, DFTI_SINGLE, DFTI_COMPLEX, 3, L )
status = DftiSetValue( my_desc_handle, DFTI_PLACEMENT, DFTI_INPLACE )
status = DftiSetValue( my_desc_handle, DFTI_BACKWARD_SCALE, factor )
status = DftiCommitDescriptor(my_desc_handle)
status = DftiComputeBackward( my_desc_handle, indata1d)
status = DftiFreeDescriptor(my_desc_handle)

print*,'ifftc2c success'

END SUBROUTINE

!EOF


program array1
implicit none
real :: x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,average
read*,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10
average = (x1+x2+x3+x4+x5+x6+x7+x8+x9+x10)/10
print*, "the average is ",average
print*, "the numbers are"
print*,x1
print*,x2
print*,x3
print*,x4
print*,x5
print*,x6
print*,x7
print*,x8
print*,x9
print*,x10
end program array1