      subroutine utmul(a,n1a,m,b,n1b,n,wk)
c-----------------------------------------------------------------
c        calculates matrix product of upper triangular matrix
c  stored in array a and matrix stored in array b.  product
c  actually calculated is
c    a(transpose)b
c  procedure is a space economizer as the product overwrites b.
c  lower triangle of a is never referenced.
c
c  a  - m by m upper triangular array to multiply b by.
c  n1a- first dimension of a in calling program
c  m  - number of rows and columns in a and number of rows in
c       matrix b.
c  b  - m by n matrix to be multiplied.  on output b is overwritten
c       by the product  a(transpose)b
c  n  - number of columns in b.
c  wk - work space vector of length at least m.
c
c  required subprograms
c     sdot and scopy  -  two routine of blas
c  language - 1977 ansi standard fortran
c  author   - gary l. pavlis
c  written  - may 1981
c-----------------------------------------------------------------
      real a(n1a,m),b(n1b,n)
      real wk(m)
      do 200 j=1,n
            do 100 i=1,m
                  wk(i) = sdot(i,a(1,i),1,b(1,j),1)
  100       continue
            call scopy(m,wk,1,b(1,j),1)
  200 continue
      return
      end

c $Id$


module autogrid

use, intrinsic :: iso_fortran_env, only : stderr=>error_unit
implicit none (type, external)

private
public :: grid_auto, test_process_number, max_mpi

contains

subroutine grid_auto(lx2all,lx3all, lid, lid2, lid3)
!! Automatically determine the PROCESS GRID
!! sets value of lid2,lid3 globally
!!
!! lid: total number of MPI processes, set by mpi_comm_size (from CLI mpiexec -n).
!!      Consider lid read-only or weird behavior can result (deadlock, crash)

integer, intent(in) :: lx2all,lx3all,lid
integer, intent(out) :: lid2, lid3

integer :: inds(2), i,j,N

if(lid < 1) error stop "MPI image count must be positive"
if(lid == 1) then
  lid2 = 1
  lid3 = 1
  return
endif

if (lx3all==1) then
  !! 2D simulation in x2, SWAP x2 to x3
  lid3 = gcd(lid, lx2all)
  lid2 = 1
elseif (lx2all==1) then
  !! 2D simulation in x3
  lid3 = gcd(lid, lx3all)
  lid2 = 1
else
  ! print *, 'lx2all,lx3all,lid', lx2all,lx3all,lid
  !! 3D simulation
  !! in 3D sims, must have at least 2 images on each axis to avoid MPI/MUMPS memory / vader errors
  lid2 = 0
  lid3 = huge(0)
  N = 0

  do i = gcd(lid, lx2all), 1, -1
    do j = gcd(lid, lx3all), 1, -1
      if (i*j /= lid) cycle
      if (i*j < N) cycle
      if (modulo(lx2all, i) /= 0) cycle
      if (modulo(lx3all, j) /= 0) cycle
      if (lx2all / i == 1 .or. lx3all / j == 1) cycle
      if (abs(i-j) > abs(lid2-lid3)) cycle

      N = i*j
      lid2 = i
      lid3 = j
      ! print *, i,j
    enddo
  enddo
  if (N==0) then
    write(stderr,'(A,3I4)') "grid_auto: no non-singular factorization found for MPI image partition. lx2all,lx3all,lid: ",&
      lx2all,lx3all,lid
    error stop
  endif
end if

call check_partition(lx2all, lx3all, lid, lid2, lid3)

end subroutine grid_auto

subroutine check_partition(lx2all, lx3all, lid, lid2, lid3)
!! checks grid partitioning for MPI
integer, intent(in) :: lx2all,lx3all,lid, lid2, lid3

character(6) :: s1,s2
integer :: lx2, lx3

if (lid < 1 .or. lid2 < 1 .or. lid3 < 1) error stop "MPI image count must be positive"

if (lid2*lid3 /= lid) then
  write(s1, '(I6)') lid
  write(s2, '(I6)') lid2*lid3
  error stop "autogrid:check_partition MPI image count " // s1 // " not a factor of x2*x3 " // s2
endif

if (lx2all > 1 .and. lx3all > 1) then
  lx2 = lx2all / lid2
  lx3 = lx3all / lid3
  if(lx2 == 1 .or. lx3 == 1) then
    write(stderr,'(A,/,A,8I4)') "ERROR: 3-D grid cannot have singular MPI axis:","lx2,lx3,lx2all,lx3all,lid2,lid3,lid: ", &
      lx2, lx3,lx2all,lx3all,lid2,lid3,lid
    error stop
  endif
endif

if (lx2all > 1) then
  if (lid2 > lx2all) error stop "lid2 cannot be greater than lx2"

  if (modulo(lx2all, lid2) /= 0) then
    write(s1, '(I6)') lid2
    write(s2, '(I6)') lx2all
    error stop 'autogrid:check_partition MPI x2 image count ' // s1 // ' not a factor of lx2 ' // s2
  endif
endif

if (lx3all > 1) then
  if (lid3 > lx3all) error stop "lid3 cannot be greater than lx3"

  if (modulo(lx3all, lid3) /= 0) then
    write(s1, '(I6)') lid3
    write(s2, '(I6)') lx3all
    error stop 'autogrid:check_partition MPI x3 image count ' // s1 // ' not a factor of lx3 ' // s2
  endif
endif

if (modulo(lid, lid2) /= 0) then
  write(s1, '(I6)') lid2
  write(s2, '(I6)') lid
  error stop "autogrid:check_partition MPI x2 image count " // s1 // " not a factor of lid " // s2
endif

if (modulo(lid, lid3) /= 0)  then
  write(s1, '(I6)') lid3
  write(s2, '(I6)') lid
  error stop "autogrid:check_partition MPI x3 image count " // s1 // " not a factor of lid " // s2
endif

end subroutine check_partition

integer function gcd(a, b)

integer, intent(in) :: a, b
integer :: x,y,z

if (a < 1 .or. b < 1) error stop "autogrid:gcd positive integers only"

x = a
y = b
z = modulo(x, y)
do while (z /= 0)
  x = y
  y = z
  z = modulo(x, y)
end do
gcd = y

end function gcd

integer function max_mpi(lx2, lx3, max_cpu)
!! goal is to find the highest x2 + x3 to maximum CPU core count
integer, intent(in) :: lx2, lx3, max_cpu

!> divide by 2 to ensure MPI partition has at least 2 per axis
if (lx3 == 1) then
  max_mpi = max_gcd(lx2/2, max_cpu)
elseif (lx2 == 1) then
  max_mpi = max_gcd(lx3/2, max_cpu)
else
  max_mpi = max_gcd2(lx2, lx3, max_cpu)
end if

end function max_mpi

integer function max_gcd(L, M)
!! find the Greatest Common Factor to evenly partition the simulation grid
!! Output range is [M, 1]

integer, intent(in) :: L,M
integer :: i

if (M < 1) error stop "autogrid:max_gcd CPU count must be at least one"

max_gcd = 1
do i = M, 2, -1
  max_gcd = max(gcd(L, i), max_gcd)
  if (i < max_gcd) exit
end do

end function max_gcd

integer function max_gcd2(lx2, lx3, M)
!! find the Greatest Common Factor to evenly partition the simulation grid
!! Output range is [M, 1]
!!
!!    1. find factors of each dimension
!!    2. choose partition that yields highest CPU count usage

integer, intent(in) :: lx2, lx3, M
integer :: f2, f3, i,j, t2, t3

if (M < 1) error stop "autogrid:max_gcd2 CPU count must be at least one"

max_gcd2 = 1
t2 = 1
t3 = huge(0)
x2 : do i = M,2,-1
  x3 : do j = M,2,-1
    f2 = max_gcd(lx2, i)
    f3 = max_gcd(lx3, j)
    if (M < f2 * f3) cycle x3 !< too big for CPU count
    if (f2 * f3 < max_gcd2) exit x3 !< too small for max CPU count
    if (lx2 / i == 1) cycle x2
    if (lx3 / j == 1) cycle x3
    if (abs(f2-f3) > abs(t2-t3)) cycle x3

    t2 = f2
    t3 = f3
    ! print *, "lid2, lid3: ", f2, f3
    max_gcd2 = f2 * f3
  end do x3
  if (i*M < max_gcd2) exit x2
end do x2

! print *, "final: lid2, lid3: ", t2, t3

end function max_gcd2

logical function test_process_number(N, lx2all, lx3all, rx2, rx3) result (ok)
!! this is only for testing
integer, intent(in) :: N, rx2, rx3, lx2all, lx3all

integer :: lid, lid2, lid3

lid = N
call grid_auto(lx2all, lx3all, lid, lid2, lid3)

ok = (lid2 == rx2 .and. lid3 == rx3)

if (.not. ok) then
  write(stderr,'(A,I0,A1,I0,A1,I0,A1,I0)') 'failed: lx2all,lx3all,lid,Nimg: ', lx2all,' ', lx3all,' ', lid,' ',N
  write(stderr,'(A,I0,A1,I0,A,I0,A1,I0)') 'expected lid2,lid3 ', rx2,' ', rx3, ' but got: ', lid2,' ', lid3
end if

end function test_process_number

end module autogrid


      subroutine factlog(n)
      implicit real*8(a-h,o-z)
      common/factoriall/factl(0:200),ifactl
      factl(0)=0.0d0
      do i=1,n
        factl(i)=factl(i-1)+dlog(dfloat(i))
      enddo
      ifactl=1
      end


*> \brief \b DSTT21
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE DSTT21( N, KBAND, AD, AE, SD, SE, U, LDU, WORK,
*                          RESULT )
*
*       .. Scalar Arguments ..
*       INTEGER            KBAND, LDU, N
*       ..
*       .. Array Arguments ..
*       DOUBLE PRECISION   AD( * ), AE( * ), RESULT( 2 ), SD( * ),
*      $                   SE( * ), U( LDU, * ), WORK( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> DSTT21 checks a decomposition of the form
*>
*>    A = U S U'
*>
*> where ' means transpose, A is symmetric tridiagonal, U is orthogonal,
*> and S is diagonal (if KBAND=0) or symmetric tridiagonal (if KBAND=1).
*> Two tests are performed:
*>
*>    RESULT(1) = | A - U S U' | / ( |A| n ulp )
*>
*>    RESULT(2) = | I - UU' | / ( n ulp )
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The size of the matrix.  If it is zero, DSTT21 does nothing.
*>          It must be at least zero.
*> \endverbatim
*>
*> \param[in] KBAND
*> \verbatim
*>          KBAND is INTEGER
*>          The bandwidth of the matrix S.  It may only be zero or one.
*>          If zero, then S is diagonal, and SE is not referenced.  If
*>          one, then S is symmetric tri-diagonal.
*> \endverbatim
*>
*> \param[in] AD
*> \verbatim
*>          AD is DOUBLE PRECISION array, dimension (N)
*>          The diagonal of the original (unfactored) matrix A.  A is
*>          assumed to be symmetric tridiagonal.
*> \endverbatim
*>
*> \param[in] AE
*> \verbatim
*>          AE is DOUBLE PRECISION array, dimension (N-1)
*>          The off-diagonal of the original (unfactored) matrix A.  A
*>          is assumed to be symmetric tridiagonal.  AE(1) is the (1,2)
*>          and (2,1) element, AE(2) is the (2,3) and (3,2) element, etc.
*> \endverbatim
*>
*> \param[in] SD
*> \verbatim
*>          SD is DOUBLE PRECISION array, dimension (N)
*>          The diagonal of the (symmetric tri-) diagonal matrix S.
*> \endverbatim
*>
*> \param[in] SE
*> \verbatim
*>          SE is DOUBLE PRECISION array, dimension (N-1)
*>          The off-diagonal of the (symmetric tri-) diagonal matrix S.
*>          Not referenced if KBSND=0.  If KBAND=1, then AE(1) is the
*>          (1,2) and (2,1) element, SE(2) is the (2,3) and (3,2)
*>          element, etc.
*> \endverbatim
*>
*> \param[in] U
*> \verbatim
*>          U is DOUBLE PRECISION array, dimension (LDU, N)
*>          The orthogonal matrix in the decomposition.
*> \endverbatim
*>
*> \param[in] LDU
*> \verbatim
*>          LDU is INTEGER
*>          The leading dimension of U.  LDU must be at least N.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is DOUBLE PRECISION array, dimension (N*(N+1))
*> \endverbatim
*>
*> \param[out] RESULT
*> \verbatim
*>          RESULT is DOUBLE PRECISION array, dimension (2)
*>          The values computed by the two tests described above.  The
*>          values are currently limited to 1/ulp, to avoid overflow.
*>          RESULT(1) is always modified.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date December 2016
*
*> \ingroup double_eig
*
*  =====================================================================
      SUBROUTINE DSTT21( N, KBAND, AD, AE, SD, SE, U, LDU, WORK,
     $                   RESULT )
*
*  -- LAPACK test routine (version 3.7.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     December 2016
*
*     .. Scalar Arguments ..
      INTEGER            KBAND, LDU, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   AD( * ), AE( * ), RESULT( 2 ), SD( * ),
     $                   SE( * ), U( LDU, * ), WORK( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D0, ONE = 1.0D0 )
*     ..
*     .. Local Scalars ..
      INTEGER            J
      DOUBLE PRECISION   ANORM, TEMP1, TEMP2, ULP, UNFL, WNORM
*     ..
*     .. External Functions ..
      DOUBLE PRECISION   DLAMCH, DLANGE, DLANSY
      EXTERNAL           DLAMCH, DLANGE, DLANSY
*     ..
*     .. External Subroutines ..
      EXTERNAL           DGEMM, DLASET, DSYR, DSYR2
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, DBLE, MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     1)      Constants
*
      RESULT( 1 ) = ZERO
      RESULT( 2 ) = ZERO
      IF( N.LE.0 )
     $   RETURN
*
      UNFL = DLAMCH( 'Safe minimum' )
      ULP = DLAMCH( 'Precision' )
*
*     Do Test 1
*
*     Copy A & Compute its 1-Norm:
*
      CALL DLASET( 'Full', N, N, ZERO, ZERO, WORK, N )
*
      ANORM = ZERO
      TEMP1 = ZERO
*
      DO 10 J = 1, N - 1
         WORK( ( N+1 )*( J-1 )+1 ) = AD( J )
         WORK( ( N+1 )*( J-1 )+2 ) = AE( J )
         TEMP2 = ABS( AE( J ) )
         ANORM = MAX( ANORM, ABS( AD( J ) )+TEMP1+TEMP2 )
         TEMP1 = TEMP2
   10 CONTINUE
*
      WORK( N**2 ) = AD( N )
      ANORM = MAX( ANORM, ABS( AD( N ) )+TEMP1, UNFL )
*
*     Norm of A - USU'
*
      DO 20 J = 1, N
         CALL DSYR( 'L', N, -SD( J ), U( 1, J ), 1, WORK, N )
   20 CONTINUE
*
      IF( N.GT.1 .AND. KBAND.EQ.1 ) THEN
         DO 30 J = 1, N - 1
            CALL DSYR2( 'L', N, -SE( J ), U( 1, J ), 1, U( 1, J+1 ), 1,
     $                  WORK, N )
   30    CONTINUE
      END IF
*
      WNORM = DLANSY( '1', 'L', N, WORK, N, WORK( N**2+1 ) )
*
      IF( ANORM.GT.WNORM ) THEN
         RESULT( 1 ) = ( WNORM / ANORM ) / ( N*ULP )
      ELSE
         IF( ANORM.LT.ONE ) THEN
            RESULT( 1 ) = ( MIN( WNORM, N*ANORM ) / ANORM ) / ( N*ULP )
         ELSE
            RESULT( 1 ) = MIN( WNORM / ANORM, DBLE( N ) ) / ( N*ULP )
         END IF
      END IF
*
*     Do Test 2
*
*     Compute  UU' - I
*
      CALL DGEMM( 'N', 'C', N, N, N, ONE, U, LDU, U, LDU, ZERO, WORK,
     $            N )
*
      DO 40 J = 1, N
         WORK( ( N+1 )*( J-1 )+1 ) = WORK( ( N+1 )*( J-1 )+1 ) - ONE
   40 CONTINUE
*
      RESULT( 2 ) = MIN( DBLE( N ), DLANGE( '1', N, N, WORK, N,
     $              WORK( N**2+1 ) ) ) / ( N*ULP )
*
      RETURN
*
*     End of DSTT21
*
      END


$ Declare
   SUBROUTINE StressFact (DetailType, c, Sfact)
!
! Stress intensity correction factor Y according to BS 7910:2005 M.3 Flat plates
!
! Parameters
	IMPLICIT NONE
   INTEGER,          INTENT(IN)    :: DetailType   ! 1 = Detail 11b
                                                   ! 2 = Detail 12
												   ! 3 = Detail 4a
   DOUBLE PRECISION, INTENT(IN)    :: c            ! half crack width
   DOUBLE PRECISION, INTENT(OUT)   :: Sfact        ! Factor with which the stress needs to be multiplied
!
! Declarations
   DOUBLE PRECISION                :: SCF
!
! Set stress concentration factor for flaw type 4 (detail 12)
   SCF = 1.5D0
!
! Calculate stress factor
   IF (DetailType .EQ. 2) THEN
	  Sfact = 1.D0 / SCF + (1.D0 - 1.D0 / SCF) * DEXP(-1.D0 * c)
   ELSE
      Sfact = 1.D0
   END IF
!
! End subroutine
   RETURN
   END


      SUBROUTINE AB01ND( JOBZ, N, M, A, LDA, B, LDB, NCONT, INDCON,
     $                   NBLK, Z, LDZ, TAU, TOL, 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 find a controllable realization for the linear time-invariant
C     multi-input system
C
C             dX/dt = A * X + B * U,
C
C     where A and B are N-by-N and N-by-M matrices, respectively,
C     which are reduced by this routine to orthogonal canonical form
C     using (and optionally accumulating) orthogonal similarity
C     transformations.  Specifically, the pair (A, B) is reduced to
C     the pair (Ac, Bc),  Ac = Z' * A * Z,  Bc = Z' * B,  given by
C
C             [ Acont     *    ]         [ Bcont ]
C        Ac = [                ],   Bc = [       ],
C             [   0    Auncont ]         [   0   ]
C
C        and
C
C                [ A11 A12  . . .  A1,p-1 A1p ]         [ B1 ]
C                [ A21 A22  . . .  A2,p-1 A2p ]         [ 0  ]
C                [  0  A32  . . .  A3,p-1 A3p ]         [ 0  ]
C        Acont = [  .   .   . . .    .     .  ],   Bc = [ .  ],
C                [  .   .     . .    .     .  ]         [ .  ]
C                [  .   .       .    .     .  ]         [ .  ]
C                [  0   0   . . .  Ap,p-1 App ]         [ 0  ]
C
C     where the blocks  B1, A21, ..., Ap,p-1  have full row ranks and
C     p is the controllability index of the pair.  The size of the
C     block  Auncont is equal to the dimension of the uncontrollable
C     subspace of the pair (A, B).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBZ    CHARACTER*1
C             Indicates whether the user wishes to accumulate in a
C             matrix Z the orthogonal similarity transformations for
C             reducing the system, as follows:
C             = 'N':  Do not form Z and do not store the orthogonal
C                     transformations;
C             = 'F':  Do not form Z, but store the orthogonal
C                     transformations in the factored form;
C             = 'I':  Z is initialized to the unit matrix and the
C                     orthogonal transformation matrix Z is returned.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the original state-space representation,
C             i.e. the order of the matrix A.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs, or of columns of B.  M >= 0.
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 original state dynamics matrix A.
C             On exit, the leading NCONT-by-NCONT part contains the
C             upper block Hessenberg state dynamics matrix Acont in Ac,
C             given by Z' * A * Z, of a controllable realization for
C             the original system. The elements below the first block-
C             subdiagonal are set to zero.
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 input matrix B.
C             On exit, the leading NCONT-by-M part of this array
C             contains the transformed input matrix Bcont in Bc, given
C             by Z' * B, with all elements but the first block set to
C             zero.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     NCONT   (output) INTEGER
C             The order of the controllable state-space representation.
C
C     INDCON  (output) INTEGER
C             The controllability index of the controllable part of the
C             system representation.
C
C     NBLK    (output) INTEGER array, dimension (N)
C             The leading INDCON elements of this array contain the
C             the orders of the diagonal blocks of Acont.
C
C     Z       (output) DOUBLE PRECISION array, dimension (LDZ,N)
C             If JOBZ = 'I', then the leading N-by-N part of this
C             array contains the matrix of accumulated orthogonal
C             similarity transformations which reduces the given system
C             to orthogonal canonical form.
C             If JOBZ = 'F', the elements below the diagonal, with the
C             array TAU, represent the orthogonal transformation matrix
C             as a product of elementary reflectors. The transformation
C             matrix can then be obtained by calling the LAPACK Library
C             routine DORGQR.
C             If JOBZ = 'N', the array Z is not referenced and can be
C             supplied as a dummy array (i.e. set parameter LDZ = 1 and
C             declare this array to be Z(1,1) in the calling program).
C
C     LDZ     INTEGER
C             The leading dimension of array Z. If JOBZ = 'I' or
C             JOBZ = 'F', LDZ >= MAX(1,N); if JOBZ = 'N', LDZ >= 1.
C
C     TAU     (output) DOUBLE PRECISION array, dimension (N)
C             The elements of TAU contain the scalar factors of the
C             elementary reflectors used in the reduction of B and A.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determination when
C             transforming (A, B). If the user sets TOL > 0, then
C             the given value of TOL is used as a lower bound for the
C             reciprocal condition number (see the description of the
C             argument RCOND in the SLICOT routine MB03OD);  a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by  TOLDEF = N*N*EPS,  is used instead, where EPS
C             is the machine precision (see LAPACK Library routine
C             DLAMCH).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (M)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(1, N, 3*M).
C             For optimum performance LDWORK should be larger.
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     Matrix B is first QR-decomposed and the appropriate orthogonal
C     similarity transformation applied to the matrix A. Leaving the
C     first rank(B) states unchanged, the remaining lower left block
C     of A is then QR-decomposed and the new orthogonal matrix, Q1,
C     is also applied to the right of A to complete the similarity
C     transformation. By continuing in this manner, a completely
C     controllable state-space pair (Acont, Bcont) is found for the
C     given (A, B), where Acont is upper block Hessenberg with each
C     subdiagonal block of full row rank, and Bcont is zero apart from
C     its (independent) first rank(B) rows.
C     NOTE that the system controllability indices are easily
C     calculated from the dimensions of the blocks of Acont.
C
C     REFERENCES
C
C     [1] Konstantinov, M.M., Petkov, P.Hr. and Christov, N.D.
C         Orthogonal Invariants and Canonical Forms for Linear
C         Controllable Systems.
C         Proc. 8th IFAC World Congress, Kyoto, 1, pp. 49-54, 1981.
C
C     [2] Paige, C.C.
C         Properties of numerical algorithms related to computing
C         controllablity.
C         IEEE Trans. Auto. Contr., AC-26, pp. 130-138, 1981.
C
C     [3] Petkov, P.Hr., Konstantinov, M.M., Gu, D.W. and
C         Postlethwaite, I.
C         Optimal Pole Assignment Design of Linear Multi-Input Systems.
C         Leicester University, Report 99-11, May 1996.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations and is backward stable.
C
C     FURTHER COMMENTS
C
C     If the system matrices A and B are badly scaled, it would be
C     useful to scale them with SLICOT routine TB01ID, before calling
C     the routine.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Nov. 1996.
C     Supersedes Release 2.0 routine AB01BD by P.Hr. Petkov.
C
C     REVISIONS
C
C     January 14, 1997, June 4, 1997, February 13, 1998,
C     September 22, 2003, February 29, 2004.
C
C     KEYWORDS
C
C     Controllability, minimal realization, orthogonal canonical form,
C     orthogonal transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOBZ
      INTEGER           INDCON, INFO, LDA, LDB, LDWORK, LDZ, M, N, NCONT
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), DWORK(*), TAU(*), Z(LDZ,*)
      INTEGER           IWORK(*), NBLK(*)
C     .. Local Scalars ..
      LOGICAL           LJOBF, LJOBI, LJOBZ
      INTEGER           IQR, ITAU, J, MCRT, NBL, NCRT, NI, NJ, RANK,
     $                  WRKOPT
      DOUBLE PRECISION  ANORM, BNORM, FNRM, TOLDEF
C     .. Local Arrays ..
      DOUBLE PRECISION  SVAL(3)
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANGE, DLAPY2
      EXTERNAL          DLAMCH, DLANGE, DLAPY2, LSAME
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DLACPY, DLAPMT, DLASET, DORGQR, DORMQR,
     $                  MB01PD, MB03OY, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, INT, MAX, MIN
C     ..
C     .. Executable Statements ..
C
      INFO = 0
      LJOBF = LSAME( JOBZ, 'F' )
      LJOBI = LSAME( JOBZ, 'I' )
      LJOBZ = LJOBF.OR.LJOBI
C
C     Test the input scalar arguments.
C
      IF( .NOT.LJOBZ .AND. .NOT.LSAME( JOBZ, 'N' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.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( LDZ.LT.1 .OR. ( LJOBZ .AND. LDZ.LT.N ) ) THEN
         INFO = -12
      ELSE IF( LDWORK.LT.MAX( 1, N, 3*M ) ) THEN
         INFO = -17
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'AB01ND', -INFO )
         RETURN
      END IF
C
      NCONT  = 0
      INDCON = 0
C
C     Quick return if possible.
C
      IF ( MIN( N, M ).EQ.0 ) THEN
         IF( N.GT.0 ) THEN
            IF ( LJOBI ) THEN
               CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ )
            ELSE IF ( LJOBF ) THEN
               CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ )
               CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N )
            END IF
         END IF
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Calculate the absolute norms of A and B (used for scaling).
C
      ANORM = DLANGE( 'M', N, N, A, LDA, DWORK )
      BNORM = DLANGE( 'M', N, M, B, LDB, DWORK )
C
C     Return if matrix B is zero.
C
      IF( BNORM.EQ.ZERO ) THEN
         IF ( LJOBI ) THEN
            CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ )
         ELSE IF ( LJOBF ) THEN
            CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ )
            CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N )
         END IF
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Scale (if needed) the matrices A and B.
C
      CALL MB01PD( 'Scale', 'G', N, N, 0, 0, ANORM, 0, NBLK, A, LDA,
     $             INFO )
      CALL MB01PD( 'Scale', 'G', N, M, 0, 0, BNORM, 0, NBLK, B, LDB,
     $             INFO )
C
C     Compute the Frobenius norm of [ B  A ] (used for rank estimation).
C
      FNRM = DLAPY2( DLANGE( 'F', N, M, B, LDB, DWORK ),
     $               DLANGE( 'F', N, N, A, LDA, DWORK ) )
C
      TOLDEF = TOL
      IF ( TOLDEF.LE.ZERO ) THEN
C
C        Use the default tolerance in controllability determination.
C
         TOLDEF = DBLE( N*N )*DLAMCH( 'EPSILON' )
      END IF
C
      WRKOPT = 1
      NI = 0
      ITAU = 1
      NCRT = N
      MCRT = M
      IQR  = 1
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
   10 CONTINUE
C
C        Rank-revealing QR decomposition with column pivoting.
C        The calculation is performed in NCRT rows of B starting from
C        the row IQR (initialized to 1 and then set to rank(B)+1).
C        Workspace: 3*MCRT.
C
         CALL MB03OY( NCRT, MCRT, B(IQR,1), LDB, TOLDEF, FNRM, RANK,
     $                SVAL, IWORK, TAU(ITAU), DWORK, INFO )
C
         IF ( RANK.NE.0 ) THEN
            NJ = NI
            NI = NCONT
            NCONT = NCONT + RANK
            INDCON = INDCON + 1
            NBLK(INDCON) = RANK
C
C           Premultiply and postmultiply the appropriate block row
C           and block column of A by Q' and Q, respectively.
C           Workspace: need   NCRT;
C                      prefer NCRT*NB.
C
            CALL DORMQR( 'Left', 'Transpose', NCRT, NCRT, RANK,
     $                   B(IQR,1), LDB, TAU(ITAU), A(NI+1,NI+1), LDA,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           Workspace: need   N;
C                      prefer N*NB.
C
            CALL DORMQR( 'Right', 'No transpose', N, NCRT, RANK,
     $                   B(IQR,1), LDB, TAU(ITAU), A(1,NI+1), LDA,
     $                   DWORK, LDWORK, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C           If required, save transformations.
C
            IF ( LJOBZ.AND.NCRT.GT.1 ) THEN
               CALL DLACPY( 'L', NCRT-1, MIN( RANK, NCRT-1 ),
     $                      B(IQR+1,1), LDB, Z(NI+2,ITAU), LDZ )
            END IF
C
C           Zero the subdiagonal elements of the current matrix.
C
            IF ( RANK.GT.1 )
     $         CALL DLASET( 'L', RANK-1, RANK-1, ZERO, ZERO, B(IQR+1,1),
     $                      LDB )
C
C           Backward permutation of the columns of B or A.
C
            IF ( INDCON.EQ.1 ) THEN
               CALL DLAPMT( .FALSE., RANK, M, B(IQR,1), LDB, IWORK )
               IQR = RANK + 1
            ELSE
               DO 20 J = 1, MCRT
                  CALL DCOPY( RANK, B(IQR,J), 1, A(NI+1,NJ+IWORK(J)),
     $                        1 )
   20          CONTINUE
            END IF
C
            ITAU = ITAU + RANK
            IF ( RANK.NE.NCRT ) THEN
               MCRT = RANK
               NCRT = NCRT - RANK
               CALL DLACPY( 'G', NCRT, MCRT, A(NCONT+1,NI+1), LDA,
     $                      B(IQR,1), LDB )
               CALL DLASET( 'G', NCRT, MCRT, ZERO, ZERO,
     $                      A(NCONT+1,NI+1), LDA )
               GO TO 10
            END IF
         END IF
C
C     If required, accumulate transformations.
C     Workspace: need N;  prefer N*NB.
C
      IF ( LJOBI ) THEN
         CALL DORGQR( N, N, MAX( 1, ITAU-1 ), Z, LDZ, TAU, DWORK,
     $                LDWORK, INFO )
         WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
      END IF
C
C     Annihilate the trailing blocks of B.
C
      IF ( N.GE.IQR )
     $   CALL DLASET( 'G', N-IQR+1, M, ZERO, ZERO, B(IQR,1), LDB )
C
C     Annihilate the trailing elements of TAU, if JOBZ = 'F'.
C
      IF ( LJOBF ) THEN
         DO 30 J = ITAU, N
            TAU(J) = ZERO
   30    CONTINUE
      END IF
C
C     Undo scaling of A and B.
C
      IF ( INDCON.LT.N ) THEN
         NBL = INDCON + 1
         NBLK(NBL) = N - NCONT
      ELSE
         NBL = 0
      END IF
      CALL MB01PD( 'Undo', 'H', N, N, 0, 0, ANORM, NBL, NBLK, A,
     $             LDA, INFO )
      CALL MB01PD( 'Undo', 'G', NBLK(1), M, 0, 0, BNORM, 0, NBLK, B,
     $             LDB, INFO )
C
C     Set optimal workspace dimension.
C
      DWORK(1)  = WRKOPT
      RETURN
C *** Last line of AB01ND ***
      END


! ============================ Module Description ===========================
! etdmp_mod Implementation of ETDMP PESE method. Contains a 2 subroutines:
!   1. etdmp    - ETDMP timestepping code
!   2. initW    - initializes ETD coefficients
! ===========================================================================

module etdab_mod

    use tools_mod,  only: dp, tic, toc, weights, linspace, constvec_r
    use phi_mod,    only: phi

    implicit none
    ! Custom Settings Data Type
    type etdab_settings
        integer               :: n
    end type etdab_settings

    contains

    ! =======================================================================
    ! SOLVE   Implements ETDPBM Numerical Time Integrator with PMFO_mS AII
    !
    ! Arguments
    !
    !   L       (input) COMPLEX*16 array (square)
    !           vector cooresponding to PDE linear operator
    !
    !   N       (input) SUBROUTINE:
    !           cooresponds to PDE nonlinear operator. Must be of the form:
    !               subroutine N(t,y_in,N_out)
    !                   real(kind=8),    intent(in)  :: t
    !                   complex(kind=8), intent(in)  :: y_in(:)
    !                   complex(kind=8), intent(out) :: N_out(:)
    !               end subroutine N
    !
    !   y_in    (input) COMPLEX*16 array, dimension (N)
    !           Initial condition.
    !
    !   tspan   (input) DOUBLE array, dimension(2)
    !           contains left and right integration bounds
    !
    !   options (input) ETDMP_SETTINGS
    !           derived data type that contains one field:
    !               options%z : DOUBLE(:) (nodes)
    !               options%b : DOUBLE(:) (expansion points)
    !               options%alpha : DOUBLE (extrapolation parameter)
    !               options%mS : DOUBLE(:) setminus
    !               options%m : DOUBLE number of correction iterations
    !
    !   y_out   (output) COMPLEX*16 array, dimension (N)
    !           Initial condition.
    !
    !   times   (output) DOUBLE array, dimension(4)
    !           times(1:2) = [cputime,clocktime] for timestepping loop
    !           times(3:4) = [cputime,clocktime] for initializing coefficients,
    ! =======================================================================

    subroutine etdab(L, N, tspan, y_in, Nt, settings, y_out, times)

        ! Arguments
        complex(dp),           intent(in)  :: L(:)
        real(dp),              intent(in)  :: tspan(2)
        complex(dp),           intent(in)  :: y_in(:)
        integer,               intent(in)  :: Nt
        type(etdab_settings), intent(in)  :: settings
        complex(dp),           intent(out) :: y_out(:)
        real(dp),              intent(out) :: times(4)
        ! Define interface subroutine function N
        interface
            subroutine N(t,yh_in, N_out, thread_id)
            import :: dp
            complex(dp), intent(in)  :: t
            complex(dp), intent(in)  :: yh_in(:)
            complex(dp), intent(out) :: N_out(:)
            integer, intent(in), optional :: thread_id
            end subroutine N
        end interface

        ! Local Variables
        integer  :: i, j, k, m, q, nL, LN_AII_len, lAII
        real(dp) :: r, h
        complex(dp) :: t
        complex(dp), allocatable :: W_m(:,:), WA_m(:,:,:), W_i(:,:,:)
        complex(dp), allocatable :: E_m(:), EA_m(:,:), E_i(:,:)
        complex(dp), allocatable :: yA(:,:), y_n(:), N_n(:,:)
        real(dp),    allocatable :: z(:)

        ! Initialize Method Parameters
        q  = settings%n
        z  = linspace(0.0_dp, real(q-1,dp), q)
        h  = (tspan(2) - tspan(1)) / (real(Nt + q - 1, dp))
        nL = size(L)

        allocate(E_m(nL), W_m(nl, q), EA_m(nL, 1), WA_m(nL, q, 1))
        allocate(E_i(nL, q), W_i(nL, q, q))
        allocate(y_n(nL), yA(nL, q), N_n(nL, q))

        ! === Initial ETD Coefficients =======================================================================================================================================
        call tic()
        call initWMP(L, h, z, constvec_r(1, real(q - 1, dp)), constvec_r(1, real(q, dp)), WA_m, EA_m)
        call initWMP(L, h, z, constvec_r(q, 0.0_dp), z,  W_i, E_i)
        E_m(:) = EA_m(:, 1)  ! eliminate extra dimension
        W_m(:,:) = WA_m(:,:,1) ! eliminate extra dimension
        call toc(times(3:4))
        deallocate(EA_m)
        deallocate(WA_m)

        ! === Initial Conditions : Iterator ==================================================================================================================================
        t = tspan(1)
        do i = 1, q
            yA(:, i) = y_in;
        enddo
        do i = 1, q
            do j = 1, q
                call N(t + h * z(j), yA(:, j), N_n(:, j))
            enddo
            do j = 2, q
                yA(:,j) = E_i(:, j) * yA(:, 1) + sum(W_i(:, :, j) * N_n, 2)
            enddo
        enddo

        ! === Time-Stepping Loop =============================================================================================================================================
        call tic()
        do j = 1, q
            call N(t + h * z(j), yA(:, j), N_n(:, j))
        enddo
        y_n = yA(:, q)
        t   = h * q
        do i = 1, Nt
            y_n = E_m * y_n + sum(W_m * N_n, 2)
            N_n(:, 1 : q - 1) = N_n(:, 2 : q)
            call N(t + h, y_n, N_n(:, q))
            t = t + h
        enddo
        call toc(times(1:2))
        y_out = y_n;

    end subroutine etdab

    ! =======================================================================
    ! INITW   Initializes ETDMP W functions for matrix L using weights
    !         function by Fornberg.
    !
    ! Arguments
    !
    !   L   (input) COMPLEX*16 array, dimension(M)
    !       vector cooresponding to PDE linear operator
    !
    !   h   (input) DOUBLE
    !       timestep
    !
    !   tau (input) DOUBLE array, dimensions(n,1)
    !       normalized quadrature points
    !
    !   a   (input) DOUBLE array, dimension(n,1)
    !       left integration bounds
    !
    !   b   (input) DOUBLE array, dimension(n,1)
    !       right integration bounds
    !
    !   W   (output) COMPLEX*16 array, dimension(size(tau), size(tau), size(L))
    !       contains the ETD Coefficients so that W(:,:,j) is the ETD integration
    !       matrix cooresponding to lambda=L(j)
    !
    !   E   (output) COMPLEX*16 array, dimension(size(tau),size(L))
    !       contains Matrix Exponentials where E(i,:) = exp(h_i L)
    ! =======================================================================

    subroutine initWMP(L, h, z, a, b, W, E)

        ! Argument
        complex(dp), intent(in)  :: L(:)
        real(dp),    intent(in)  :: h
        real(dp),    intent(in)  :: z(:), a(:), b(:)
        complex(dp), intent(out) :: W(size(L), size(z), size(a))
        complex(dp), intent(out) :: E(size(L), size(a))

        ! Declare variables
        real(dp) :: eta(size(a))
        real(dp) :: sz(size(z))
        real(dp) :: FD(size(z), size(z))
        complex(dp), allocatable :: p(:, :)
        integer :: i, n, q

        q     = size(z)
        n     = size(a)
        eta   = b - a
        allocate(p(q + 1, size(L)))

        ! Form W Functions
        do i = 1, n
            if(eta(i) .ne. 0.0_dp) then
                sz = (z - a(i)) / eta(i)                                        ! scaled nodes
                call weights(0.0_dp, sz, q - 1, FD)                             ! finite difference matrix
                call phi(eta(i) * h * L, q, p)                                  ! phi functions 0 to n
                W(:, :, i) = transpose(h * eta(i) * matmul(FD, p(2:q + 1,:)))   ! store ith row of W matrix
                E(:, i)    = p(1, :)                                            ! store exp(h_i L)
            else
                W(:, :, i) = 0.0_dp
                E(:, i)    = 1.0_dp
            endif
        enddo
        deallocate(p)

    end subroutine initWMP

end module etdab_mod


      SUBROUTINE TB04BW( ORDER, P, M, MD, IGN, LDIGN, IGD, LDIGD, GN,
     $                   GD, D, LDD, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the sum of an P-by-M rational matrix G and a real
C     P-by-M matrix D.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     ORDER   CHARACTER*1
C             Specifies the order in which the polynomial coefficients
C             of the rational matrix are stored, as follows:
C             = 'I':  Increasing order of powers of the indeterminate;
C             = 'D':  Decreasing order of powers of the indeterminate.
C
C     Input/Output Parameters
C
C     P       (input) INTEGER
C             The number of the system outputs.  P >= 0.
C
C     M       (input) INTEGER
C             The number of the system inputs.  M >= 0.
C
C     MD      (input) INTEGER
C             The maximum degree of the polynomials in G, plus 1, i.e.,
C             MD = MAX(IGN(I,J),IGD(I,J)) + 1.
C                  I,J
C
C     IGN     (input/output) INTEGER array, dimension (LDIGN,M)
C             On entry, the leading P-by-M part of this array must
C             contain the degrees of the numerator polynomials in G:
C             the (i,j) element of IGN must contain the degree of the
C             numerator polynomial of the polynomial ratio G(i,j).
C             On exit, the leading P-by-M part of this array contains
C             the degrees of the numerator polynomials in G + D.
C
C     LDIGN   INTEGER
C             The leading dimension of array IGN.  LDIGN >= max(1,P).
C
C     IGD     (input) INTEGER array, dimension (LDIGD,M)
C             The leading P-by-M part of this array must contain the
C             degrees of the denominator polynomials in G (and G + D):
C             the (i,j) element of IGD contains the degree of the
C             denominator polynomial of the polynomial ratio G(i,j).
C
C     LDIGD   INTEGER
C             The leading dimension of array IGD.  LDIGD >= max(1,P).
C
C     GN      (input/output) DOUBLE PRECISION array, dimension (P*M*MD)
C             On entry, this array must contain the coefficients of the
C             numerator polynomials, Num(i,j), of the rational matrix G.
C             The polynomials are stored in a column-wise order, i.e.,
C             Num(1,1), Num(2,1), ..., Num(P,1), Num(1,2), Num(2,2),
C             ..., Num(P,2), ..., Num(1,M), Num(2,M), ..., Num(P,M);
C             MD memory locations are reserved for each polynomial,
C             hence, the (i,j) polynomial is stored starting from the
C             location ((j-1)*P+i-1)*MD+1. The coefficients appear in
C             increasing or decreasing order of the powers of the
C             indeterminate, according to ORDER.
C             On exit, this array contains the coefficients of the
C             numerator polynomials of the rational matrix G + D,
C             stored similarly.
C
C     GD      (input) DOUBLE PRECISION array, dimension (P*M*MD)
C             This array must contain the coefficients of the
C             denominator polynomials, Den(i,j), of the rational
C             matrix G. The polynomials are stored as for the
C             numerator polynomials.
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading P-by-M part of this array must contain the
C             matrix D.
C
C     LDD     INTEGER
C             The leading dimension of array D.  LDD >= max(1,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
C     METHOD
C
C     The (i,j) entry of the real matrix D is added to the (i,j) entry
C     of the matrix G, g(i,j), which is a ratio of two polynomials,
C     for i = 1 : P, and for j = 1 : M. If g(i,j) = 0, it is assumed
C     that its denominator is 1.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically stable.
C
C     FURTHER COMMENTS
C
C     Often, the rational matrix G is found from a state-space
C     representation (A,B,C), and D corresponds to the direct
C     feedthrough matrix of the system. The sum G + D gives the
C     transfer function matrix of the system (A,B,C,D).
C     For maximum efficiency of index calculations, GN and GD are
C     implemented as one-dimensional arrays.
C
C     CONTRIBUTORS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 2002.
C     Based on the BIMASC Library routine TMCADD by A. Varga.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Feb. 2004.
C
C     KEYWORDS
C
C     State-space representation, transfer function.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO
      PARAMETER          ( ZERO = 0.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          ORDER
      INTEGER            INFO, LDD, LDIGD, LDIGN, M, MD, P
C     .. Array Arguments ..
      DOUBLE PRECISION   D(LDD,*), GD(*), GN(*)
      INTEGER            IGD(LDIGD,*), IGN(LDIGN,*)
C     .. Local Scalars ..
      LOGICAL            ASCEND
      INTEGER            I, II, J, K, KK, KM, ND, NN
      DOUBLE PRECISION   DIJ
C     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
C     .. External Subroutines ..
      EXTERNAL           DAXPY, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
C     ..
C     .. Executable Statements ..
C
C     Test the input scalar parameters.
C
      INFO   = 0
      ASCEND = LSAME( ORDER, 'I' )
      IF( .NOT.ASCEND .AND. .NOT.LSAME( ORDER, 'D' ) ) THEN
         INFO = -1
      ELSE IF( P.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( MD.LT.1 ) THEN
         INFO = -4
      ELSE IF( LDIGN.LT.MAX( 1, P ) ) THEN
         INFO = -6
      ELSE IF( LDIGD.LT.MAX( 1, P ) ) THEN
         INFO = -8
      ELSE IF( LDD.LT.MAX( 1, P ) ) THEN
         INFO = -12
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'TB04BW', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( MIN( P, M ).EQ.0 )
     $   RETURN
C
      K = 1
C
      IF ( ASCEND ) THEN
C
C        Polynomial coefficients are stored in increasing order.
C
         DO 30 J = 1, M
C
            DO 20 I = 1, P
               DIJ = D(I,J)
               IF ( DIJ.NE.ZERO ) THEN
                  NN = IGN(I,J)
                  ND = IGD(I,J)
                  IF ( NN.EQ.0 .AND. ND.EQ.0 ) THEN
                     IF ( GN(K).EQ.ZERO ) THEN
                        GN(K) = DIJ
                     ELSE
                        GN(K) = GN(K) + DIJ*GD(K)
                     ENDIF
                  ELSE
                     KM = MIN( NN, ND ) + 1
                     CALL DAXPY( KM, DIJ, GD(K), 1, GN(K), 1 )
                     IF ( NN.LT.ND ) THEN
C
                        DO 10 II = K + KM, K + ND
                           GN(II) = DIJ*GD(II)
   10                   CONTINUE
C
                        IGN(I,J) = ND
                      ENDIF
                  ENDIF
               ENDIF
               K = K + MD
   20       CONTINUE
C
   30    CONTINUE
C
      ELSE
C
C        Polynomial coefficients are stored in decreasing order.
C
         DO 60 J = 1, M
C
            DO 50 I = 1, P
               DIJ = D(I,J)
               IF ( DIJ.NE.ZERO ) THEN
                  NN = IGN(I,J)
                  ND = IGD(I,J)
                  IF ( NN.EQ.0 .AND. ND.EQ.0 ) THEN
                     IF ( GN(K).EQ.ZERO ) THEN
                        GN(K) = DIJ
                     ELSE
                        GN(K) = GN(K) + DIJ*GD(K)
                     ENDIF
                  ELSE
                     KM = MIN( NN, ND ) + 1
                     IF ( NN.LT.ND ) THEN
                        KK = K + ND - NN
C
                        DO 35 II = K + NN, K, -1
                           GN(II+ND-NN) = GN(II)
   35                   CONTINUE
C
                        DO 40 II = K, KK - 1
                           GN(II) = DIJ*GD(II)
   40                   CONTINUE
C
                        IGN(I,J) = ND
                        CALL DAXPY( KM, DIJ, GD(KK), 1, GN(KK), 1 )
                     ELSE
                        KK = K + NN - ND
                        CALL DAXPY( KM, DIJ, GD(K), 1, GN(KK), 1 )
                     ENDIF
                  ENDIF
               ENDIF
               K = K + MD
   50       CONTINUE
C
   60    CONTINUE
C
      ENDIF
C
      RETURN
C *** Last line of TB04BW ***
      END


      SUBROUTINE TB01VD( APPLY, N, M, L, A, LDA, B, LDB, C, LDC, D, LDD,
     $                   X0, THETA, LTHETA, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To convert the linear discrete-time system given as (A, B, C, D),
C     with initial state x0, into the output normal form [1], with
C     parameter vector THETA. The matrix A is assumed to be stable.
C     The matrices A, B, C, D and the vector x0 are converted, so that
C     on exit they correspond to the system defined by THETA.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     APPLY   CHARACTER*1
C             Specifies whether or not the parameter vector should be
C             transformed using a bijective mapping, as follows:
C             = 'A' : apply the bijective mapping to the N vectors in
C                     THETA corresponding to the matrices A and C;
C             = 'N' : do not apply the bijective mapping.
C             The transformation performed when APPLY = 'A' allows
C             to get rid of the constraints norm(THETAi) < 1, i = 1:N.
C             A call of the SLICOT Library routine TB01VY associated to
C             a call of TB01VD must use the same value of APPLY.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs.  M >= 0.
C
C     L       (input) INTEGER
C             The number of system outputs.  L >= 0.
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 matrix A, assumed to be stable.
C             On exit, the leading N-by-N part of this array contains
C             the transformed system state matrix corresponding to the
C             output normal form with parameter vector THETA.
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 transformed system input matrix corresponding to the
C             output normal form with parameter vector THETA.
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 L-by-N part of this array must
C             contain the system output matrix C.
C             On exit, the leading L-by-N part of this array contains
C             the transformed system output matrix corresponding to the
C             output normal form with parameter vector THETA.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,L).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading L-by-M part of this array must contain the
C             system input/output matrix D.
C
C     LDD     INTEGER
C             The leading dimension of array D.  LDD >= MAX(1,L).
C
C     X0      (input/output) DOUBLE PRECISION array, dimension (N)
C             On entry, this array must contain the initial state of the
C             system, x0.
C             On exit, this array contains the transformed initial state
C             of the system, corresponding to the output normal form
C             with parameter vector THETA.
C
C     THETA   (output) DOUBLE PRECISION array, dimension (LTHETA)
C             The leading N*(L+M+1)+L*M part of this array contains the
C             parameter vector that defines a system (A, B, C, D, x0)
C             which is equivalent up to a similarity transformation to
C             the system given on entry. The parameters are:
C
C             THETA(1:N*L)                      : parameters for A, C;
C             THETA(N*L+1:N*(L+M))              : parameters for B;
C             THETA(N*(L+M)+1:N*(L+M)+L*M)      : parameters for D;
C             THETA(N*(L+M)+L*M+1:N*(L+M+1)+L*M): parameters for x0.
C
C     LTHETA  INTEGER
C             The length of array THETA.  LTHETA >= N*(L+M+1)+L*M.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(1, N*N*L + N*L + N,
C                           N*N + MAX(N*N + N*MAX(N,L) + 6*N + MIN(N,L),
C                                     N*M)).
C             For optimum performance LDWORK should be larger.
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 Lyapunov equation A'*Q*A - Q = -scale^2*C'*C
C                   could only be solved with scale = 0;
C             = 2:  if matrix A is not discrete-time stable;
C             = 3:  if the QR algorithm failed to converge for
C                   matrix A.
C
C     METHOD
C
C     The matrices A and C are converted to output normal form.
C     First, the Lyapunov equation
C
C        A'*Q*A - Q = -scale^2*C'*C,
C
C     is solved in the Cholesky factor T, T'*T = Q, and then T is used
C     to get the transformation matrix.
C
C     The matrix B and the initial state x0 are transformed accordingly.
C
C     Then, the QR factorization of the transposed observability matrix
C     is computed, and the matrix Q is used to further transform the
C     system matrices. The parameters characterizing A and C are finally
C     obtained by applying a set of N orthogonal transformations.
C
C     REFERENCES
C
C     [1] Peeters, R.L.M., Hanzon, B., and Olivi, M.
C         Balanced realizations of discrete-time stable all-pass
C         systems and the tangential Schur algorithm.
C         Proceedings of the European Control Conference,
C         31 August - 3 September 1999, Karlsruhe, Germany.
C         Session CP-6, Discrete-time Systems, 1999.
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     Feb. 2002, Feb. 2004.
C
C     KEYWORDS
C
C     Asymptotically stable, Lyapunov equation, output normal form,
C     parameter estimation, similarity transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO, HALF
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0,
     $                    HALF = 0.5D0 )
C     .. Scalar Arguments ..
      CHARACTER         APPLY
      INTEGER           INFO, L, LDA, LDB, LDC, LDD, LDWORK, LTHETA, M,
     $                  N
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), D(LDD,*),
     $                  DWORK(*), THETA(*), X0(*)
C     .. Local Scalars ..
      DOUBLE PRECISION  PIBY2, RI, SCALE, TI
      INTEGER           CA, I, IA, IN, IQ, IR, IT, ITAU, IU, IWI, IWR,
     $                  J, JWORK, K, LDCA, LDT, WRKOPT
      LOGICAL           LAPPLY
C     .. External Functions ..
      EXTERNAL          DNRM2, LSAME
      DOUBLE PRECISION  DNRM2
      LOGICAL           LSAME
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DGEMM, DGEMV, DGEQRF, DGER,
     $                  DLACPY, DLASET, DORMQR, DSCAL, DTRMM, DTRMV,
     $                  DTRSM, MA02AD, SB03OD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ATAN, INT, MAX, MIN, SQRT, TAN
C     ..
C     .. Executable Statements ..
C
C     Check the scalar input parameters.
C
      LAPPLY = LSAME( APPLY, 'A' )
C
      INFO = 0
      IF ( .NOT.( LAPPLY .OR. LSAME( APPLY, 'N' ) ) ) THEN
         INFO = -1
      ELSEIF ( N.LT.0 ) THEN
         INFO = -2
      ELSEIF ( M.LT.0 ) THEN
         INFO = -3
      ELSEIF ( L.LT.0 ) THEN
         INFO = -4
      ELSEIF ( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSEIF ( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSEIF ( LDC.LT.MAX( 1, L ) ) THEN
         INFO = -10
      ELSEIF ( LDD.LT.MAX( 1, L ) ) THEN
         INFO = -12
      ELSEIF ( LTHETA.LT.( N*( M + L + 1 ) + L*M ) ) THEN
         INFO = -15
      ELSEIF ( LDWORK.LT.MAX( 1, N*N*L + N*L + N, N*N +
     $                        MAX( N*( N + MAX( N, L ) + 6 ) +
     $                             MIN( N, L ), N*M ) ) ) THEN
         INFO = -17
      ENDIF
C
C     Return if there are illegal arguments.
C
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'TB01VD', -INFO )
         RETURN
      ENDIF
C
C     Quick return if possible.
C
      IF ( MAX( N, M, L ).EQ.0 ) THEN
         DWORK(1) = ONE
         RETURN
      ELSE IF ( N.EQ.0 ) THEN
         CALL DLACPY( 'Full', L, M, D, LDD, THETA, MAX( 1, L ) )
         DWORK(1) = ONE
         RETURN
      ELSE IF ( L.EQ.0 ) THEN
         CALL DLACPY( 'Full', N, M, B, LDB, THETA, N )
         CALL DCOPY( N, X0, 1, THETA(N*M+1), 1 )
         DWORK(1) = ONE
         RETURN
      ENDIF
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      WRKOPT = 1
      PIBY2  = TWO*ATAN( ONE )
C
C     Convert A and C to output normal form.
C     First, solve the Lyapunov equation
C        A'*Q*A - Q = -scale^2*C'*C,
C     in the Cholesky factor T, T'*T = Q, and use T to get the
C     transformation matrix. Copy A and C, to preserve them.
C
C     Workspace: need   N*(2*N + MAX(N,L) + 6) + MIN(N,L).
C                prefer larger.
C
C     Initialize the indices in the workspace.
C
      LDT = MAX( N, L )
      CA  = 1
      IA  = 1
      IT  = IA  + N*N
      IU  = IT  + LDT*N
      IWR = IU  + N*N
      IWI = IWR + N
C
      JWORK = IWI + N
C
      CALL DLACPY( 'Full', N, N, A, LDA, DWORK(IA), N )
      CALL DLACPY( 'Full', L, N, C, LDC, DWORK(IT), LDT )
C
      CALL SB03OD( 'Discrete', 'NotFactored', 'NoTranspose', N, L,
     $             DWORK(IA), N, DWORK(IU), N, DWORK(IT), LDT, SCALE,
     $             DWORK(IWR), DWORK(IWI), DWORK(JWORK), LDWORK-JWORK+1,
     $             INFO )
      IF ( INFO.NE.0 ) THEN
         IF ( INFO.EQ.6 ) THEN
            INFO = 3
         ELSE
            INFO = 2
         ENDIF
         RETURN
      ENDIF
      WRKOPT = INT( DWORK(JWORK) ) + JWORK - 1
C
      IF ( SCALE.EQ.ZERO ) THEN
         INFO = 1
         RETURN
      ENDIF
C
C     Compute A = T*A*T^(-1).
C
      CALL DTRMM( 'Left', 'Upper', 'NoTranspose', 'NonUnit', N, N, ONE,
     $            DWORK(IT), LDT, A, LDA )
C
      CALL DTRSM( 'Right', 'Upper', 'NoTranspose', 'NonUnit', N, N, ONE,
     $            DWORK(IT), LDT, A, LDA )
      IF ( M.GT.0 ) THEN
C
C        Compute B = (1/scale)*T*B.
C
         CALL DTRMM( 'Left', 'Upper', 'NoTranspose', 'NonUnit', N, M,
     $               ONE/SCALE, DWORK(IT), LDT, B, LDB )
      ENDIF
C
C     Compute x0 = (1/scale)*T*x0.
C
      CALL DTRMV( 'Upper', 'NoTranspose', 'NonUnit', N, DWORK(IT), LDT,
     $            X0, 1 )
      CALL DSCAL( N, ONE/SCALE, X0, 1 )
C
C     Compute C = scale*C*T^(-1).
C
      CALL DTRSM( 'Right', 'Upper', 'NoTranspose', 'NonUnit', L, N,
     $            SCALE, DWORK(IT), LDT, C, LDC )
C
C     Now, the system has been transformed to the output normal form.
C     Build the transposed observability matrix in DWORK(CA) and compute
C     its QR factorization.
C
      CALL MA02AD( 'Full', L, N, C, LDC, DWORK(CA), N )
C
      DO 10 I = 1, N - 1
         CALL DGEMM( 'Transpose', 'NoTranspose', N, L, N, ONE, A, LDA,
     $               DWORK(CA+(I-1)*N*L), N, ZERO, DWORK(CA+I*N*L), N )
   10  CONTINUE
C
C     Compute the QR factorization.
C
C     Workspace: need   N*N*L + N + L*N.
C                prefer N*N*L + N + NB*L*N.
C
      ITAU  = CA   + N*N*L
      JWORK = ITAU + N
      CALL DGEQRF( N, L*N, DWORK(CA), N, DWORK(ITAU), DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
C
C     Compute Q such that R has all diagonal elements nonnegative.
C     Only the first N*N part of R is needed. Move the details
C     of the QR factorization process, to gain memory and efficiency.
C
C     Workspace: need   2*N*N + 2*N.
C                prefer 2*N*N + N + NB*N.
C
      IR = N*N + 1
      IF ( L.NE.2 )
     $   CALL DCOPY( N, DWORK(ITAU), 1, DWORK(IR+N*N), 1 )
      CALL DLACPY( 'Lower', N, N, DWORK(CA), N, DWORK(IR), N )
      ITAU  = IR + N*N
      JWORK = ITAU + N
C
      IQ = 1
      CALL DLASET( 'Full', N, N, ZERO, ONE, DWORK(IQ), N )
C
      DO 20 I = 1, N
         IF ( DWORK(IR+(I-1)*(N+1)).LT.ZERO )
     $        DWORK(IQ+(I-1)*(N+1))= -ONE
   20 CONTINUE
C
      CALL DORMQR( 'Left', 'NoTranspose', N, N, N, DWORK(IR), N,
     $             DWORK(ITAU), DWORK(IQ), N, DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
      JWORK  = IR
C
C     Now, the transformation matrix Q is in DWORK(IQ).
C
C     Compute A = Q'*A*Q.
C
      CALL DGEMM( 'Transpose', 'NoTranspose', N, N, N, ONE, DWORK(IQ),
     $            N, A, LDA, ZERO, DWORK(JWORK), N )
      CALL DGEMM( 'NoTranspose', 'NoTranspose', N, N, N, ONE,
     $            DWORK(JWORK), N, DWORK(IQ), N, ZERO, A, LDA )
C
      IF ( M.GT.0 ) THEN
C
C        Compute B = Q'*B.
C        Workspace: need   N*N + N*M.
C
         CALL DLACPY( 'Full', N, M, B, LDB, DWORK(JWORK), N )
         CALL DGEMM(  'Transpose', 'NoTranspose', N, M, N, ONE,
     $                DWORK(IQ), N, DWORK(JWORK), N, ZERO, B, LDB )
      ENDIF
C
C     Compute C = C*Q.
C     Workspace: need   N*N + N*L.
C
      CALL DLACPY( 'Full', L, N, C, LDC, DWORK(JWORK), L )
      CALL DGEMM(  'NoTranspose', 'NoTranspose', L, N, N, ONE,
     $             DWORK(JWORK), L, DWORK(IQ), N, ZERO, C, LDC )
C
C     Compute x0 = Q'*x0.
C
      CALL DCOPY( N, X0, 1, DWORK(JWORK), 1 )
      CALL DGEMV( 'Transpose', N, N, ONE, DWORK(IQ), N, DWORK(JWORK),
     $            1, ZERO, X0, 1 )
C
C     Now, copy C and A into the workspace to make it easier to read out
C     the corresponding part of THETA, and to apply the transformations.
C
      LDCA = N + L
C
      DO 30 I = 1, N
         CALL DCOPY( L, C(1,I), 1, DWORK(CA+(I-1)*LDCA), 1 )
         CALL DCOPY( N, A(1,I), 1, DWORK(CA+L+(I-1)*LDCA), 1 )
   30 CONTINUE
C
      JWORK = CA + LDCA*N
C
C     The parameters characterizing A and C are extracted in this loop.
C     Workspace: need   N*(N + L + 1).
C
      DO 60 I = 1, N
         CALL DCOPY( L, DWORK(CA+1+(N-I)*(LDCA+1)), 1, THETA((I-1)*L+1),
     $               1 )
         RI = DWORK(CA+(N-I)*(LDCA+1))
         TI = DNRM2( L, THETA((I-1)*L+1), 1 )
C
C        Multiply the part of [C; A] which will be currently transformed
C        with Ui = [ -THETAi, Si; RI, THETAi' ] from the left, without
C        storing Ui. Ui has the size (L+1)-by-(L+1).
C
         CALL DGEMV( 'Transpose', L, N, ONE, DWORK(CA+N-I+1), LDCA,
     $               THETA((I-1)*L+1), 1, ZERO, DWORK(JWORK), 1 )
C
         IF ( TI.GT.ZERO ) THEN
            CALL DGER( L, N, (RI-ONE)/TI/TI, THETA((I-1)*L+1), 1,
     $                 DWORK(JWORK), 1, DWORK(CA+N-I+1), LDCA )
         ELSE
C
C           The call below is for the limiting case.
C
            CALL DGER( L, N, -HALF, THETA((I-1)*L+1), 1,
     $                 DWORK(JWORK), 1, DWORK(CA+N-I+1), LDCA )
         ENDIF
C
         CALL DGER( L, N, -ONE, THETA((I-1)*L+1), 1, DWORK(CA+N-I),
     $              LDCA, DWORK(CA+N-I+1), LDCA )
         CALL DAXPY( N, RI, DWORK(CA+N-I), LDCA, DWORK(JWORK), 1 )
C
C        Move these results to their appropriate locations.
C
         DO 50 J = 1, N
            IN = CA + N - I + ( J - 1 )*LDCA
            DO 40 K = IN + 1, IN + L
               DWORK(K-1) = DWORK(K)
   40       CONTINUE
            DWORK(IN+L) = DWORK(JWORK+J-1)
   50    CONTINUE
C
C        Now, apply the bijective mapping, which allows to get rid
C        of the constraint norm(THETAi) < 1.
C
         IF ( LAPPLY .AND. TI.NE.ZERO )
     $     CALL DSCAL( L, TAN( TI*PIBY2 )/TI, THETA((I-1)*L+1), 1 )
C
   60 CONTINUE
C
      IF ( M.GT.0 ) THEN
C
C        The next part of THETA is B.
C
         CALL DLACPY( 'Full', N, M, B, LDB, THETA(N*L+1), N )
C
C        Copy the matrix D.
C
         CALL DLACPY( 'Full', L, M, D, LDD, THETA(N*(L+M)+1), L )
      ENDIF
C
C     Copy the initial state x0.
C
      CALL DCOPY( N, X0, 1, THETA(N*(L+M)+L*M+1), 1 )
C
      DWORK(1) = WRKOPT
      RETURN
C
C *** Last line of TB01VD ***
      END