      SUBROUTINE AB05RD( FBTYPE, JOBD, N, M, P, MV, PZ, ALPHA, BETA, A,
     $                   LDA, B, LDB, C, LDC, D, LDD, F, LDF, K, LDK,
     $                   G, LDG, H, LDH, RCOND, BC, LDBC, CC, LDCC,
     $                   DC, LDDC, IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To construct for a given state space system (A,B,C,D) the closed-
C     loop system (Ac,Bc,Cc,Dc) corresponding to the mixed output and
C     state feedback control law
C
C          u = alpha*F*y + beta*K*x + G*v
C          z = H*y.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     FBTYPE  CHARACTER*1
C             Specifies the type of the feedback law as follows:
C             = 'I':  Unitary output feedback (F = I);
C             = 'O':  General output feedback.
C
C     JOBD    CHARACTER*1
C             Specifies whether or not a non-zero matrix D appears
C             in the given state space model:
C             = 'D':  D is present;
C             = 'Z':  D is assumed a zero matrix.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The dimension of state vector x, i.e. the order of the
C             matrix A, the number of rows of B and the number of
C             columns of C.  N >= 0.
C
C     M       (input) INTEGER
C             The dimension of input vector u, i.e. the number of
C             columns of matrices B and D, and the number of rows of F.
C             M >= 0.
C
C     P       (input) INTEGER
C             The dimension of output vector y, i.e. the number of rows
C             of matrices C and D, and the number of columns of F.
C             P >= 0 and P = M if FBTYPE = 'I'.
C
C     MV      (input) INTEGER
C             The dimension of the new input vector v, i.e. the number
C             of columns of matrix G.  MV >= 0.
C
C     PZ      (input) INTEGER.
C             The dimension of the new output vector z, i.e. the number
C             of rows of matrix H.  PZ >= 0.
C
C     ALPHA   (input) DOUBLE PRECISION
C             The coefficient alpha in the output feedback law.
C
C     BETA    (input) DOUBLE PRECISION.
C             The coefficient beta in the state feedback law.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the system state transition matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the state matrix Ac of the closed-loop system.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the system input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the intermediary input matrix B1 (see METHOD).
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the system output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the intermediary output matrix C1+BETA*D1*K (see METHOD).
C
C     LDC     INTEGER
C             The leading dimension of array C.
C             LDC >= MAX(1,P) if N > 0.
C             LDC >= 1 if N = 0.
C
C     D       (input/output) DOUBLE PRECISION array, dimension (LDD,M)
C             On entry, if JOBD = 'D', the leading P-by-M part of this
C             array must contain the system direct input/output
C             transmission matrix D.
C             On exit, the leading P-by-M part of this array contains
C             the intermediary direct input/output transmission matrix
C             D1 (see METHOD).
C             The array D is not referenced if JOBD = 'Z'.
C
C     LDD     INTEGER
C             The leading dimension of array D.
C             LDD >= MAX(1,P) if JOBD = 'D'.
C             LDD >= 1 if JOBD = 'Z'.
C
C     F       (input) DOUBLE PRECISION array, dimension (LDF,P)
C             If FBTYPE = 'O', the leading M-by-P part of this array
C             must contain the output feedback matrix F.
C             If FBTYPE = 'I', then the feedback matrix is assumed to be
C             an M x M order identity matrix.
C             The array F is not referenced if FBTYPE = 'I'  or
C             ALPHA = 0.
C
C     LDF     INTEGER
C             The leading dimension of array F.
C             LDF >= MAX(1,M) if FBTYPE = 'O' and ALPHA <> 0.
C             LDF >= 1 if FBTYPE = 'I' or ALPHA = 0.
C
C     K       (input) DOUBLE PRECISION array, dimension (LDK,N)
C             The leading M-by-N part of this array must contain the
C             state feedback matrix K.
C             The array K is not referenced if BETA = 0.
C
C     LDK     INTEGER
C             The leading dimension of the array K.
C             LDK >= MAX(1,M) if BETA <> 0.
C             LDK >= 1 if BETA = 0.
C
C     G       (input) DOUBLE PRECISION array, dimension (LDG,MV)
C             The leading M-by-MV part of this array must contain the
C             system input scaling matrix G.
C
C     LDG     INTEGER
C             The leading dimension of the array G.  LDG >= MAX(1,M).
C
C     H       (input) DOUBLE PRECISION array, dimension (LDH,P)
C             The leading PZ-by-P part of this array must contain the
C             system output scaling matrix H.
C
C     LDH     INTEGER
C             The leading dimension of the array H.  LDH >= MAX(1,PZ).
C
C     RCOND   (output) DOUBLE PRECISION
C             The reciprocal condition number of the matrix
C             I - alpha*D*F.
C
C     BC      (output) DOUBLE PRECISION array, dimension (LDBC,MV)
C             The leading N-by-MV part of this array contains the input
C             matrix Bc of the closed-loop system.
C
C     LDBC    INTEGER
C             The leading dimension of array BC.  LDBC >= MAX(1,N).
C
C     CC      (output) DOUBLE PRECISION array, dimension (LDCC,N)
C             The leading PZ-by-N part of this array contains the
C             system output matrix Cc of the closed-loop system.
C
C     LDCC    INTEGER
C             The leading dimension of array CC.
C             LDCC >= MAX(1,PZ) if N > 0.
C             LDCC >= 1 if N = 0.
C
C     DC      (output) DOUBLE PRECISION array, dimension (LDDC,MV)
C             If JOBD = 'D', the leading PZ-by-MV part of this array
C             contains the direct input/output transmission matrix Dc
C             of the closed-loop system.
C             The array DC is not referenced if JOBD = 'Z'.
C
C     LDDC    INTEGER
C             The leading dimension of array DC.
C             LDDC >= MAX(1,PZ) if JOBD = 'D'.
C             LDDC >= 1 if JOBD = 'Z'.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (LIWORK)
C             LIWORK >= MAX(1,2*P) if JOBD = 'D'.
C             LIWORK >= 1 if JOBD = 'Z'.
C             IWORK is not referenced if JOBD = 'Z'.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= wspace, where
C                   wspace = MAX( 1, M, P*MV, P*P + 4*P ) if JOBD = 'D',
C                   wspace = MAX( 1, M ) if JOBD = 'Z'.
C             For best performance, LDWORK >= MAX( wspace, N*M, N*P ).
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:  if the matrix I - alpha*D*F is numerically singular.
C
C     METHOD
C
C     The matrices of the closed-loop system have the expressions:
C
C     Ac = A1 + beta*B1*K,      Bc = B1*G,
C     Cc = H*(C1 + beta*D1*K),  Dc = H*D1*G,
C
C     where
C
C     A1 = A + alpha*B*F*E*C,   B1 = B + alpha*B*F*E*D,
C     C1 = E*C,                 D1 = E*D,
C
C     with E = (I - alpha*D*F)**-1.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of computations basically depends on the conditioning
C     of the matrix I - alpha*D*F. If RCOND is very small, it is likely
C     that the computed results are inaccurate.
C
C     CONTRIBUTORS
C
C     A. Varga, German Aerospace Research Establishment,
C     Oberpfaffenhofen, Germany, and V. Sima, Katholieke Univ. Leuven,
C     Belgium, Nov. 1996.
C
C     REVISIONS
C
C     January 14, 1997, February 18, 1998.
C     V. Sima, Research Institute for Informatics, Bucharest, July 2003,
C     Jan. 2005.
C
C     KEYWORDS
C
C     Multivariable system, state-space model, state-space
C     representation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         FBTYPE, JOBD
      INTEGER           INFO, LDA, LDB, LDBC, LDC, LDCC, LDD, LDDC,
     $                  LDF, LDG, LDH, LDK, LDWORK, M, MV, N, P, PZ
      DOUBLE PRECISION  ALPHA, BETA, RCOND
C     .. Array Arguments ..
      INTEGER           IWORK(*)
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), BC(LDBC,*), C(LDC,*),
     $                  CC(LDCC,*), D(LDD,*), DC(LDDC,*), DWORK(*),
     $                  F(LDF,*), G(LDG,*), H(LDH,*), K(LDK,*)
C     .. Local Scalars ..
      LOGICAL           LJOBD, OUTPF, UNITF
      INTEGER           LDWP
C     .. External functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External subroutines ..
      EXTERNAL          AB05SD, DGEMM, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX, MIN
C
C     .. Executable Statements ..
C
C     Check the input scalar arguments.
C
      UNITF = LSAME( FBTYPE, 'I' )
      OUTPF = LSAME( FBTYPE, 'O' )
      LJOBD = LSAME( JOBD, 'D' )
C
      INFO = 0
C
      IF( .NOT.UNITF .AND. .NOT.OUTPF ) THEN
         INFO = -1
      ELSE IF( .NOT.LJOBD .AND. .NOT.LSAME( JOBD, 'Z' )  ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( M.LT.0 ) THEN
         INFO = -4
      ELSE IF( P.LT.0 .OR. UNITF.AND.P.NE.M ) THEN
         INFO = -5
      ELSE IF( MV.LT.0 ) THEN
         INFO = -6
      ELSE IF( PZ.LT.0 ) THEN
         INFO = -7
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -11
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -13
      ELSE IF( ( N.GT.0 .AND. LDC.LT.MAX( 1, P ) ) .OR.
     $         ( N.EQ.0 .AND. LDC.LT.1 ) ) THEN
         INFO = -15
      ELSE IF( ( LJOBD .AND. LDD.LT.MAX( 1, P ) ) .OR.
     $    ( .NOT.LJOBD .AND. LDD.LT.1 ) ) THEN
         INFO = -17
      ELSE IF( ( OUTPF .AND. ALPHA.NE.ZERO .AND. LDF.LT.MAX( 1, M ) )
     $  .OR. ( ( UNITF .OR.  ALPHA.EQ.ZERO ) .AND. LDF.LT.1 ) ) THEN
         INFO = -19
      ELSE IF( ( BETA.NE.ZERO .AND. LDK.LT.MAX( 1, M ) ) .OR.
     $         ( BETA.EQ.ZERO .AND. LDK.LT.1 ) ) THEN
         INFO = -21
      ELSE IF( LDG.LT.MAX( 1, M ) ) THEN
         INFO = -23
      ELSE IF( LDH.LT.MAX( 1, PZ ) ) THEN
         INFO = -25
      ELSE IF( LDBC.LT.MAX( 1, N ) ) THEN
         INFO = -28
      ELSE IF( ( N.GT.0 .AND. LDCC.LT.MAX( 1, PZ ) ) .OR.
     $         ( N.EQ.0 .AND. LDCC.LT.1 ) ) THEN
         INFO = -30
      ELSE IF( ( ( LJOBD .AND. LDDC.LT.MAX( 1, PZ ) ) .OR.
     $      ( .NOT.LJOBD .AND. LDDC.LT.1 ) ) ) THEN
         INFO = -32
      ELSE IF( ( LJOBD .AND. LDWORK.LT.MAX( 1, M, P*MV, P*P + 4*P ) )
     $   .OR. (  .NOT.LJOBD .AND. LDWORK.LT.MAX( 1, M ) ) ) THEN
         INFO = -35
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'AB05RD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( MAX( N, MIN( M, P ), MIN( MV, PZ ) ).EQ.0 ) THEN
         RCOND = ONE
         RETURN
      END IF
C
C     Apply the partial output feedback u = alpha*F*y + v1
C
      CALL AB05SD( FBTYPE, JOBD, N, M, P, ALPHA, A, LDA, B, LDB, C,
     $             LDC, D, LDD, F, LDF, RCOND, IWORK, DWORK, LDWORK,
     $             INFO )
      IF ( INFO.NE.0 ) RETURN
C
C     Apply the partial state feedback v1 = beta*K*x + v2.
C
C     Compute Ac = A1 + beta*B1*K and C1 <- C1 + beta*D1*K.
C
      IF( BETA.NE.ZERO .AND. N.GT.0 ) THEN
         CALL DGEMM( 'N', 'N', N, N, M, BETA, B, LDB, K, LDK, ONE, A,
     $               LDA )
         IF( LJOBD )
     $       CALL DGEMM( 'N', 'N', P, N, M, BETA, D, LDD, K, LDK, ONE,
     $                   C, LDC )
      END IF
C
C     Apply the input and output conversions v2 = G*v, z = H*y.
C
C     Compute Bc = B1*G.
C
      CALL DGEMM( 'N', 'N', N, MV, M, ONE, B, LDB, G, LDG, ZERO, BC,
     $            LDBC )
C
C     Compute Cc = H*C1.
C
      IF( N.GT.0 )
     $   CALL DGEMM( 'N', 'N', PZ, N, P, ONE, H, LDH, C, LDC, ZERO, CC,
     $               LDCC )
C
C     Compute Dc = H*D1*G.
C
      IF( LJOBD ) THEN
         LDWP = MAX( 1, P )
         CALL DGEMM( 'N', 'N', P, MV, M, ONE, D, LDD, G, LDG, ZERO,
     $               DWORK, LDWP )
         CALL DGEMM( 'N', 'N', PZ, MV, P, ONE, H, LDH, DWORK, LDWP,
     $               ZERO, DC, LDDC )
      END IF
C
      RETURN
C *** Last line of AB05RD ***
      END


!==========================================================================
elemental function gsw_sound_speed (sa, ct, p)
!==========================================================================
!
!  Calculates the speed of sound in seawater.  This function has inputs of
!  Absolute Salinity and Conservative Temperature.  This function uses the
!  computationally-efficient expression for specific volume in terms of SA,
!  CT and p (Roquet et al., 2014).
!
!  SA  =  Absolute Salinity                                        [ g/kg ]
!  CT  =  Conservative Temperature (ITS-90)                       [ deg C ]
!  p   =  sea pressure                                             [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  sound_speed  =  speed of sound in seawater                       [ m/s ]
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_sfac, offset

use gsw_mod_specvol_coefficients

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, ct, p

real (r8) :: gsw_sound_speed

real (r8) :: v, v_p, xs, ys, z

xs = sqrt(gsw_sfac*sa + offset)
ys = ct*0.025_r8
z = p*1e-4_r8

v = v000 + xs*(v010 + xs*(v020 + xs*(v030 + xs*(v040 + xs*(v050 &
    + v060*xs))))) + ys*(v100 + xs*(v110 + xs*(v120 + xs*(v130 + xs*(v140 &
    + v150*xs)))) + ys*(v200 + xs*(v210 + xs*(v220 + xs*(v230 + v240*xs))) &
    + ys*(v300 + xs*(v310 + xs*(v320 + v330*xs)) + ys*(v400 + xs*(v410 &
    + v420*xs) + ys*(v500 + v510*xs + v600*ys))))) + z*(v001 + xs*(v011 &
    + xs*(v021 + xs*(v031 + xs*(v041 + v051*xs)))) + ys*(v101 + xs*(v111 &
    + xs*(v121 + xs*(v131 + v141*xs))) + ys*(v201 + xs*(v211 + xs*(v221 &
    + v231*xs)) + ys*(v301 + xs*(v311 + v321*xs) + ys*(v401 + v411*xs &
    + v501*ys)))) + z*(v002 + xs*(v012 + xs*(v022 + xs*(v032 + v042*xs))) &
    + ys*(v102 + xs*(v112 + xs*(v122 + v132*xs)) + ys*(v202 + xs*(v212 &
    + v222*xs) + ys*(v302 + v312*xs + v402*ys))) + z*(v003 + xs*(v013 &
    + v023*xs) + ys*(v103 + v113*xs + v203*ys) + z*(v004 + v014*xs + v104*ys &
    + z*(v005 + v006*z)))))

v_p = c000 + xs*(c100 + xs*(c200 + xs*(c300 + xs*(c400 + c500*xs)))) &
    + ys*(c010 + xs*(c110 + xs*(c210 + xs*(c310 + c410*xs))) + ys*(c020 &
    + xs*(c120 + xs*(c220 + c320*xs)) + ys*(c030 + xs*(c130 + c230*xs) &
    + ys*(c040 + c140*xs + c050*ys)))) + z*(c001 + xs*(c101 + xs*(c201 &
    + xs*(c301 + c401*xs))) + ys*(c011 + xs*(c111 + xs*(c211 + c311*xs)) &
    + ys*(c021 + xs*(c121 + c221*xs) + ys*(c031 + c131*xs + c041*ys))) &
    + z*( c002 + xs*(c102 + c202*xs) + ys*(c012 + c112*xs + c022*ys) &
    + z*(c003 + c103*xs + c013*ys + z*(c004 + c005*z))))

gsw_sound_speed = 10000.0_r8*sqrt(-v**2/v_p)

return
end function

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


      SUBROUTINE NF01BX( N, IPAR, LIPAR, DPAR, LDPAR, J, LDJ, X, INCX,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute (J'*J + c*I)*x, where J is an m-by-n real matrix, c is
C     a real scalar, I is the n-by-n identity matrix, and x is a real
C     n-vector.
C
C     NOTE: this routine must have the same arguments as SLICOT Library
C     routine NF01BW.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The number of columns of the Jacobian matrix J.  N >= 0.
C
C     IPAR    (input) INTEGER array, dimension (LIPAR)
C             The integer parameters describing the structure of the
C             matrix J, as follows:
C             IPAR(1) must contain the number of rows M of the Jacobian
C                     matrix J.  M >= 0.
C             IPAR is provided for compatibility with SLICOT Library
C             routine MD03AD.
C
C     LIPAR   (input) INTEGER
C             The length of the array IPAR.  LIPAR >= 1.
C
C     DPAR    (input) DOUBLE PRECISION array, dimension (LDPAR)
C             The real parameters needed for solving the problem.
C             The entry DPAR(1) must contain the real scalar c.
C
C     LDPAR   (input) INTEGER
C             The length of the array DPAR.  LDPAR >= 1.
C
C     J       (input) DOUBLE PRECISION array, dimension (LDJ,N)
C             The leading M-by-N part of this array must contain the
C             Jacobian matrix J.
C
C     LDJ     INTEGER
C             The leading dimension of the array J.  LDJ >= MAX(1,M).
C
C     X       (input/output) DOUBLE PRECISION array, dimension
C             (1+(N-1)*abs(INCX))
C             On entry, this incremented array must contain the
C             vector x.
C             On exit, this incremented array contains the value of the
C             matrix-vector product (J'*J + c*I)*x.
C
C     INCX    (input) INTEGER
C             The increment for the elements of X.  INCX <> 0.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C
C     LDWORK  INTEGER
C             The length of the array DWORK.  LDWORK >= M.
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
C     METHOD
C
C     The associativity of matrix multiplications is used; the result
C     is obtained as:  x_out = J'*( J*x ) + c*x.
C
C     CONTRIBUTORS
C
C     A. Riedel, R. Schneider, Chemnitz University of Technology,
C     Oct. 2000, during a stay at University of Twente, NL.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Mar. 2001,
C     Mar. 2002, Oct. 2004.
C
C     KEYWORDS
C
C     Elementary matrix operations, matrix algebra, matrix operations.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      INTEGER           INCX, INFO, LDJ, LDPAR, LDWORK, LIPAR, N
C     .. Array Arguments ..
      INTEGER           IPAR(*)
      DOUBLE PRECISION  DPAR(*), DWORK(*), J(LDJ,*), X(*)
C     .. Local Scalars ..
      INTEGER           M
      DOUBLE PRECISION  C
C     .. External Subroutines ..
      EXTERNAL          DGEMV, DSCAL, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         MAX
C     ..
C     .. Executable Statements ..
C
      INFO = 0
      IF ( N.LT.0 ) THEN
         INFO = -1
      ELSEIF ( LIPAR.LT.1 ) THEN
         INFO = -3
      ELSEIF ( LDPAR.LT.1 ) THEN
         INFO = -5
      ELSEIF ( INCX.EQ.0 ) THEN
         INFO = -9
      ELSE
         M = IPAR(1)
         IF ( M.LT.0 ) THEN
            INFO = -2
         ELSEIF ( LDJ.LT.MAX( 1, M ) ) THEN
            INFO = -7
         ELSEIF ( LDWORK.LT.M ) THEN
            INFO = -11
         ENDIF
      ENDIF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'NF01BX', -INFO )
         RETURN
      ENDIF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 )
     $   RETURN
C
      C = DPAR(1)
      IF ( M.EQ.0 ) THEN
C
C        Special case, void J: x <-- c*x.
C
         CALL DSCAL( N, C, X, INCX )
         RETURN
      END IF
C
      CALL DGEMV( 'NoTranspose', M, N, ONE, J, LDJ, X, INCX, ZERO,
     $            DWORK, 1 )
      CALL DGEMV( 'Transpose', M, N, ONE, J, LDJ, DWORK, 1, C, X, INCX )
      RETURN
C
C *** Last line of NF01BX ***
      END