      SUBROUTINE SB10ZD( N, M, NP, A, LDA, B, LDB, C, LDC, D, LDD,
     $                   FACTOR, AK, LDAK, BK, LDBK, CK, LDCK, DK,
     $                   LDDK, RCOND, TOL, 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 | D |
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     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading NP-by-M part of this array must contain the
C             system input/output matrix D of the shaped plant.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     FACTOR  (input) DOUBLE PRECISION
C             = 1  implies that an optimal controller is required
C                  (not recommended);
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 (6)
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 matrix (gamma^2-1)*In - P*Q;
C             RCOND(4) contains an estimate of the reciprocal condition
C                      number of the matrix Rx + Bx'*X*Bx;
C             RCOND(5) contains an estimate of the reciprocal condition
C                                                  ^
C                      number of the matrix Ip + D*Dk;
C             RCOND(6) contains an estimate of the reciprocal condition
C                                                ^
C                      number of the matrix Im + Dk*D.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             Tolerance used for checking the nonsingularity of the
C             matrices to be inverted. If TOL <= 0, then a default value
C             equal to sqrt(EPS) is used, where EPS is the relative
C             machine precision.  TOL < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*max(N,M+NP))
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 >= 16*N*N + 5*M*M + 7*NP*NP + 6*M*N + 7*M*NP +
C                        7*N*NP + 6*N + 2*(M + NP) +
C                        max(14*N+23,16*N,2*M-1,2*NP-1).
C             For good performance, LDWORK must generally be larger.
C
C     BWORK   LOGICAL array, dimension (2*N)
C
C     Error Indicator
C
C     INFO    (output) 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 iteration to compute eigenvalues or singular
C                    values failed to converge;
C             =  4:  the matrix (gamma^2-1)*In - P*Q is singular;
C             =  5:  the matrix Rx + Bx'*X*Bx is singular;
C                                      ^
C             =  6:  the matrix Ip + D*Dk is singular;
C                                    ^
C             =  7:  the matrix Im + Dk*D is singular;
C             =  8:  the matrix Ip - D*Dk is singular;
C             =  9:  the matrix Im - Dk*D is singular;
C             = 10:  the closed-loop system is unstable.
C
C     METHOD
C
C     The routine implements the formulas given in [1].
C
C     REFERENCES
C
C     [1] Gu, D.-W., Petkov, P.H., and Konstantinov, M.M.
C         On discrete H-infinity loop shaping design procedure routines.
C         Technical Report 00-6, Dept. of Engineering, Univ. of
C         Leicester, UK, 2000.
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, May 2001.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, July 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, LDD,
     $                   LDDK, LDWORK, M, N, NP
      DOUBLE PRECISION   FACTOR, TOL
C     ..
C     .. Array Arguments ..
      INTEGER            IWORK( * )
      LOGICAL            BWORK( * )
      DOUBLE PRECISION   A ( LDA,  * ), AK( LDAK, * ), B ( LDB,  * ),
     $                   BK( LDBK, * ), C ( LDC,  * ), CK( LDCK, * ),
     $                   D ( LDD,  * ), DK( LDDK, * ), DWORK( * ),
     $                   RCOND( 6 )
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, LWAMAX, MINWRK, N2, NS, SDIM
      DOUBLE PRECISION   ANORM, GAMMA, TOLL
C     ..
C     .. External Functions ..
      LOGICAL            SELECT
      DOUBLE PRECISION   DLAMCH, DLANGE, DLANSY, DLAPY2
      EXTERNAL           DLAMCH, DLANGE, DLANSY, DLAPY2, SELECT
C     ..
C     .. External Subroutines ..
      EXTERNAL           DCOPY, DGECON, DGEES, DGEMM, DGETRF, DGETRS,
     $                   DLACPY, DLASCL, DLASET, DPOTRF, DPOTRS, DSWAP,
     $                   DSYCON, DSYEV, DSYRK, DSYTRF, DSYTRS, DTRSM,
     $                   DTRTRS, MA02AD, MB01RX, MB02VD, SB02OD, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          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( LDD.LT.MAX( 1, NP ) ) THEN
         INFO = -11
      ELSE IF( FACTOR.LT.ONE ) THEN
         INFO = -12
      ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
         INFO = -14
      ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
         INFO = -16
      ELSE IF( LDCK.LT.MAX( 1, M ) ) THEN
         INFO = -18
      ELSE IF( LDDK.LT.MAX( 1, M ) ) THEN
         INFO = -20
      ELSE IF( TOL.GE.ONE ) THEN
         INFO = -22
      END IF
C
C     Compute workspace.
C
      MINWRK = 16*N*N + 5*M*M + 7*NP*NP + 6*M*N + 7*M*NP + 7*N*NP +
     $         6*N + 2*(M + NP) + MAX( 14*N+23, 16*N, 2*M-1, 2*NP-1 )
      IF( LDWORK.LT.MINWRK ) THEN
         INFO = -25
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10ZD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C     Note that some computation could be made if one or two of the
C     dimension parameters N, M, and P are zero, but the results are
C     not so meaningful.
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
         RCOND( 5 ) = ONE
         RCOND( 6 ) = ONE
         DWORK( 1 ) = ONE
         RETURN
      END IF
C
C     Set the default tolerance, if needed.
C
      IF( TOL.LE.ZERO ) THEN
         TOLL = SQRT( DLAMCH( 'Epsilon' ) )
      ELSE
         TOLL = TOL
      END IF
C
C     Workspace usage.
C
      N2  = 2*N
      I1  = 1   + N*N
      I2  = I1  + N*N
      I3  = I2  + NP*NP
      I4  = I3  + M*M
      I5  = I4  + NP*NP
      I6  = I5  + M*M
      I7  = I6  + M*N
      I8  = I7  + M*N
      I9  = I8  + N*N
      I10 = I9  + N*N
      I11 = I10 + N2
      I12 = I11 + N2
      I13 = I12 + N2
      I14 = I13 + N2*N2
      I15 = I14 + N2*N2
C
      IWRK   = I15 + N2*N2
      LWAMAX = 0
C
C     Compute R1 = Ip + D*D' .
C
      CALL DLASET( 'U', NP, NP, ZERO, ONE, DWORK( I2 ), NP )
      CALL DSYRK(  'U', 'N', NP, M, ONE, D, LDD, ONE, DWORK( I2 ), NP )
      CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I4 ), NP )
C
C     Factorize R1 = R'*R .
C
      CALL DPOTRF( 'U', NP, DWORK( I4 ), NP, INFO2 )
C                 -1
C     Compute C'*R   in BK .
C
      CALL MA02AD( 'F', NP, N, C, LDC, BK, LDBK )
      CALL DTRSM(  'R', 'U', 'N', 'N', N, NP, ONE, DWORK( I4 ), NP, BK,
     $             LDBK )
C
C     Compute R2 = Im + D'*D .
C
      CALL DLASET( 'U', M, M, ZERO, ONE, DWORK( I3 ), M )
      CALL DSYRK(  'U', 'T', M, NP, ONE, D, LDD, ONE, DWORK( I3 ), M )
      CALL DLACPY( 'U', M, M, DWORK( I3 ), M, DWORK( I5 ), M )
C
C     Factorize R2 = U'*U .
C
      CALL DPOTRF( 'U', M, DWORK( I5 ), M, INFO2 )
C               -1
C     Compute (U  )'*B' .
C
      CALL MA02AD( 'F', N, M, B, LDB, DWORK( I6 ), M )
      CALL DTRTRS( 'U', 'T', 'N', M, N, DWORK( I5 ), M, DWORK( I6 ), M,
     $             INFO2 )
C
C     Compute D'*C .
C
      CALL DGEMM( 'T', 'N', M, N, NP, ONE, D, LDD, C, LDC, ZERO,
     $            DWORK( I7 ), M )
C               -1
C     Compute (U  )'*D'*C .
C
      CALL DTRTRS( 'U', 'T', 'N', M, N, DWORK( I5 ), M, DWORK( I7 ), M,
     $             INFO2 )
C                          -1
C     Compute Ar = A - B*R2  D'*C .
C
      CALL DLACPY( 'F', N, N, A, LDA, DWORK( I8 ), N )
      CALL DGEMM(  'T', 'N', N, N, M, -ONE, DWORK( I6 ), M, DWORK( I7 ),
     $             M, ONE, DWORK( I8 ), N )
C                       -1
C     Compute Cr = C'*R1  *C .
C
      CALL DSYRK( 'U', 'N', N, NP, ONE, BK, LDBK, ZERO, DWORK( I9 ), N )
C                      -1
C     Compute Dr = B*R2  B' in AK .
C
      CALL DSYRK( 'U', 'T', N, M, ONE, DWORK( I6 ), M, ZERO, AK, LDAK )
C                                                       -1
C     Solution of the Riccati equation Ar'*P*(In + Dr*P)  Ar - P +
C                                              Cr = 0 .
      CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, DWORK( I8 ),
     $             N, AK, LDAK, DWORK( I9 ), N, DWORK, M, DWORK, N,
     $             RCOND( 1 ), DWORK, N, DWORK( I10 ), DWORK( I11 ),
     $             DWORK( I12 ), DWORK( I13 ), N2, DWORK( I14 ), N2,
     $             DWORK( I15 ), N2, -ONE, IWORK, DWORK( IWRK ),
     $             LDWORK-IWRK+1, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 1
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C     Transpose Ar .
C
      DO 10 J = 1, N - 1
         CALL DSWAP( J, DWORK( I8+J ), N, DWORK( I8+J*N ), 1 )
   10 CONTINUE
C                                                      -1
C     Solution of the Riccati equation Ar*Q*(In + Cr*Q)  *Ar' - Q +
C                                             Dr = 0 .
      CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, DWORK( I8 ),
     $             N, DWORK( I9 ), N, AK, LDAK, DWORK, M, DWORK, N,
     $             RCOND( 2 ), DWORK( I1 ), N, DWORK( I10 ),
     $             DWORK( I11 ), DWORK( I12 ), DWORK( I13 ), N2,
     $             DWORK( I14 ), N2, DWORK( I15 ), N2, -ONE, IWORK,
     $             DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 2
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C     Compute gamma.
C
      CALL DGEMM( 'N', 'N', N, N, N, ONE, DWORK( I1 ), N, DWORK, N,
     $            ZERO, DWORK( I8 ), N )
      CALL DGEES( 'N', 'N', SELECT, N, DWORK( I8 ), N, SDIM,
     $            DWORK( I10 ), DWORK( I11 ), DWORK( IWRK ), N,
     $            DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
      GAMMA  = ZERO
C
      DO 20 I = 0, N - 1
         GAMMA = MAX( GAMMA, DWORK( I10+I ) )
   20 CONTINUE
C
      GAMMA = FACTOR*SQRT( ONE + GAMMA )
C
C     Workspace usage.
C
      I5  = I4  + NP*NP
      I6  = I5  + M*M
      I7  = I6  + NP*NP
      I8  = I7  + NP*NP
      I9  = I8  + NP*NP
      I10 = I9  + NP
      I11 = I10 + NP*NP
      I12 = I11 + M*M
      I13 = I12 + M
C
      IWRK = I13 + M*M
C
C     Compute the eigenvalues and eigenvectors of R1 .
C
      CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I8 ), NP )
      CALL DSYEV(  'V', 'U', NP, DWORK( I8 ), NP, DWORK( I9 ),
     $             DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C               -1/2
C     Compute R1     .
C
      DO 40 J = 1, NP
         DO 30 I = 1, NP
            DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP ) /
     $                                  SQRT( DWORK( I9+I-1 ) )
   30    CONTINUE
   40 CONTINUE
C
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP,
     $            DWORK( I10 ), NP, ZERO, DWORK( I4 ), NP )
C
C     Compute the eigenvalues and eigenvectors of R2 .
C
      CALL DLACPY( 'U', M, M, DWORK( I3 ), M, DWORK( I11 ), M )
      CALL DSYEV(  'V', 'U', M, DWORK( I11 ), M, DWORK( I12 ),
     $             DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C               -1/2
C     Compute R2     .
C
      DO 60 J = 1, M
         DO 50 I = 1, M
            DWORK( I13-1+I+(J-1)*M ) = DWORK( I11-1+J+(I-1)*M ) /
     $                                 SQRT( DWORK( I12+I-1 ) )
   50    CONTINUE
   60 CONTINUE
C
      CALL DGEMM( 'N', 'N', M, M, M, ONE, DWORK( I11 ), M, DWORK( I13 ),
     $            M, ZERO, DWORK( I5 ), M )
C
C     Compute R1 + C*Q*C' .
C
      CALL DGEMM(  'N', 'T', N, NP, N, ONE, DWORK( I1 ), N, C, LDC,
     $             ZERO, BK, LDBK )
      CALL MB01RX( 'L', 'U', 'N', NP, N, ONE, ONE, DWORK( I2 ), NP,
     $             C, LDC, BK, LDBK, INFO2 )
      CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I8 ), NP )
C
C     Compute the eigenvalues and eigenvectors of R1 + C*Q*C' .
C
      CALL DSYEV( 'V', 'U', NP, DWORK( I8 ), NP, DWORK( I9 ),
     $            DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C                            -1
C     Compute ( R1 + C*Q*C' )   .
C
      DO 80 J = 1, NP
         DO 70 I = 1, NP
            DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP ) /
     $                                  DWORK( I9+I-1 )
   70    CONTINUE
   80 CONTINUE
C
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP,
     $            DWORK( I10 ), NP, ZERO, DWORK( I6 ), NP )
C               -1
C     Compute Z2  .
C
      DO 100 J = 1, NP
         DO 90 I = 1, NP
            DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP )*
     $                                  SQRT( DWORK( I9+I-1 ) )
   90    CONTINUE
  100 CONTINUE
C
      CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP,
     $            DWORK( I10 ), NP, ZERO, DWORK( I7 ), NP )
C
C     Workspace usage.
C
      I9  = I8  + N*NP
      I10 = I9  + N*NP
      I11 = I10 + NP*M
      I12 = I11 + ( NP + M )*( NP + M )
      I13 = I12 + N*( NP + M )
      I14 = I13 + N*( NP + M )
      I15 = I14 + N*N
      I16 = I15 + N*N
      I17 = I16 + ( NP + M )*N
      I18 = I17 + ( NP + M )*( NP + M )
      I19 = I18 + ( NP + M )*N
      I20 = I19 + M*N
      I21 = I20 + M*NP
      I22 = I21 + NP*N
      I23 = I22 + N*N
      I24 = I23 + N*NP
      I25 = I24 + NP*NP
      I26 = I25 + M*M
C
      IWRK = I26 + N*M
C
C     Compute A*Q*C' + B*D' .
C
      CALL DGEMM( 'N', 'T', N, NP, M, ONE, B, LDB, D, LDD, ZERO,
     $            DWORK( I8 ), N )
      CALL DGEMM( 'N', 'N', N, NP, N, ONE, A, LDA, BK, LDBK,
     $            ONE, DWORK( I8 ), N )
C                                                 -1
C     Compute H = -( A*Q*C'+B*D' )*( R1 + C*Q*C' )   .
C
      CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I8 ), N,
     $            DWORK( I6 ), NP, ZERO, DWORK( I9 ), N )
C               -1/2
C     Compute R1    D .
C
      CALL DGEMM( 'N', 'N', NP, M, NP, ONE, DWORK( I4 ), NP, D, LDD,
     $            ZERO, DWORK( I10 ), NP )
C
C     Compute Rx .
C
      DO 110 J = 1, NP
         CALL DCOPY( J, DWORK( I2+(J-1)*NP ), 1,
     $                  DWORK( I11+(J-1)*(NP+M) ), 1 )
         DWORK( I11-1+J+(J-1)*(NP+M) ) = DWORK( I2-1+J+(J-1)*NP ) -
     $                                   GAMMA*GAMMA
  110 CONTINUE
C
      CALL DGEMM(  'N', 'N', NP, M, NP, ONE, DWORK( I7 ), NP,
     $             DWORK( I10 ), NP, ZERO, DWORK( I11+(NP+M)*NP ),
     $             NP+M )
      CALL DLASET( 'U', M, M, ZERO, ONE, DWORK( I11+(NP+M)*NP+NP ),
     $             NP+M )
C
C     Compute Bx .
C
      CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I9 ), N,
     $            DWORK( I7 ), NP, ZERO, DWORK( I12 ), N )
      CALL DGEMM( 'N', 'N', N, M, M, ONE, B, LDB, DWORK( I5 ), M,
     $            ZERO, DWORK( I12+N*NP ), N )
C
C     Compute Sx .
C
      CALL DGEMM( 'T', 'N', N, NP, NP, ONE, C, LDC, DWORK( I7 ), NP,
     $            ZERO, DWORK( I13 ), N )
      CALL DGEMM( 'T', 'N', N, M, NP, ONE, C, LDC, DWORK( I10 ), NP,
     $            ZERO, DWORK( I13+N*NP ), N )
C
C     Compute  (gamma^2 - 1)*In - P*Q .
C
      CALL DLASET( 'F', N, N, ZERO, GAMMA*GAMMA-ONE, DWORK( I14 ), N )
      CALL DGEMM(  'N', 'N', N, N, N, -ONE, DWORK, N, DWORK( I1 ), N,
     $             ONE, DWORK( I14 ), N )
C                                          -1
C     Compute X =  ((gamma^2 - 1)*In - P*Q)  *gamma^2*P .
C
      CALL DLACPY( 'F', N, N, DWORK, N, DWORK( I15 ), N )
      CALL DLASCL( 'G', 0, 0, ONE, GAMMA*GAMMA, N, N, DWORK( I15 ), N,
     $             INFO )
      ANORM = DLANGE( '1', N, N, DWORK( I14 ), N, DWORK( IWRK ) )
      CALL DGETRF( N, N, DWORK( I14 ), N, IWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = 4
         RETURN
      END IF
      CALL DGECON( '1', N, DWORK( I14 ), N, ANORM, RCOND( 3 ),
     $             DWORK( IWRK ), IWORK( N+1 ), INFO2 )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND( 3 ).LT.TOLL ) THEN
         INFO = 4
         RETURN
      END IF
      CALL DGETRS( 'N', N, N, DWORK( I14 ), N, IWORK, DWORK( I15 ),
     $             N, INFO2 )
C
C     Compute Bx'*X .
C
      CALL DGEMM( 'T', 'N', NP+M, N, N, ONE, DWORK( I12 ), N,
     $            DWORK( I15 ), N, ZERO, DWORK( I16 ), NP+M )
C
C     Compute Rx + Bx'*X*Bx .
C
      CALL DLACPY( 'U', NP+M, NP+M, DWORK( I11 ), NP+M, DWORK( I17 ),
     $             NP+M )
      CALL MB01RX( 'L', 'U', 'N', NP+M, N, ONE, ONE, DWORK( I17 ), NP+M,
     $             DWORK( I16 ), NP+M, DWORK( I12 ), N, INFO2 )
C
C     Compute  -( Sx' + Bx'*X*A ) .
C
      CALL MA02AD( 'F', N, NP+M, DWORK( I13 ), N, DWORK( I18 ), NP+M )
      CALL DGEMM(  'N', 'N', NP+M, N, N, -ONE, DWORK( I16 ), NP+M,
     $             A, LDA, -ONE, DWORK( I18 ), NP+M )
C
C     Factorize Rx + Bx'*X*Bx .
C
      ANORM = DLANSY( '1', 'U', NP+M, DWORK( I17 ), NP+M,
     $                DWORK( IWRK ) )
      CALL DSYTRF( 'U', NP+M, DWORK( I17 ), NP+M, IWORK,
     $             DWORK( IWRK ), LDWORK-IWRK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 5
         RETURN
      END IF
      CALL DSYCON( 'U', NP+M, DWORK( I17 ), NP+M, IWORK, ANORM,
     $             RCOND( 4 ), DWORK( IWRK ), IWORK( NP+M+1), INFO2 )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND( 4 ).LT.TOLL ) THEN
         INFO = 5
         RETURN
      END IF
C                                   -1
C     Compute F = -( Rx + Bx'*X*Bx )  ( Sx' + Bx'*X*A ) .
C
      CALL DSYTRS( 'U', NP+M, N, DWORK( I17 ), NP+M, IWORK,
     $             DWORK( I18 ), NP+M, INFO2 )
C
C     Compute B'*X .
C
      CALL DGEMM( 'T', 'N', M, N, N, ONE, B, LDB, DWORK( I15 ), N,
     $            ZERO, DWORK( I19 ), M )
C
C     Compute  -( D' - B'*X*H ) .
C
      DO 130 J = 1, NP
         DO 120 I = 1, M
            DWORK( I20-1+I+(J-1)*M ) = -D( J, I )
  120    CONTINUE
  130 CONTINUE
C
      CALL DGEMM( 'N', 'N', M, NP, N, ONE, DWORK( I19 ), M,
     $            DWORK( I9 ), N, ONE, DWORK( I20 ), M )
C                   -1
C     Compute C + Z2  *F1 .
C
      CALL DLACPY( 'F', NP, N, C, LDC, DWORK( I21 ), NP )
      CALL DGEMM(  'N', 'N', NP, N, NP, ONE, DWORK( I7 ), NP,
     $             DWORK( I18 ), NP+M, ONE, DWORK( I21 ), NP )
C
C     Compute R2 + B'*X*B .
C
      CALL MB01RX( 'L', 'U', 'N', M, N, ONE, ONE, DWORK( I3 ), M,
     $             DWORK( I19 ), M, B, LDB, INFO2 )
C
C     Factorize R2 + B'*X*B .
C
      CALL DPOTRF( 'U', M, DWORK( I3 ), M, INFO2 )
C             ^                    -1
C     Compute Dk = -( R2 + B'*X*B )  (D' - B'*X*H) .
C
      CALL DLACPY( 'F', M, NP, DWORK( I20 ), M, DK, LDDK )
      CALL DPOTRS( 'U', M, NP, DWORK( I3 ), M, DK, LDDK, INFO2 )
C             ^           ^
C     Compute Bk = -H + B*Dk .
C
      CALL DLACPY( 'F', N, NP, DWORK( I9 ), N, DWORK( I23 ), N )
      CALL DGEMM(  'N', 'N', N, NP, M, ONE, B, LDB, DK, LDDK,
     $             -ONE, DWORK( I23 ), N )
C               -1/2
C     Compute R2    *F2  .
C
      CALL DGEMM( 'N', 'N', M, N, M, ONE, DWORK( I5 ), M,
     $            DWORK( I18+NP ), NP+M, ZERO, CK, LDCK )
C             ^      -1/2      ^          -1
C     Compute Ck = R2    *F2 - Dk*( C + Z2  *F1 ) .
C
      CALL DGEMM( 'N', 'N', M, N, NP, -ONE, DK, LDDK,
     $            DWORK( I21 ), 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( I9 ), N, C, LDC,
     $             ONE, AK, LDAK )
      CALL DGEMM(  'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK,
     $             ONE, AK, LDAK )
C                    ^
C     Compute Ip + D*Dk .
C
      CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK( I24 ), NP )
      CALL DGEMM(  'N', 'N', NP, NP, M, ONE, D, LDD, DK, LDDK,
     $             ONE, DWORK( I24 ), NP )
C                   ^
C     Compute  Im + Dk*D .
C
      CALL DLASET( 'Full', M, M, ZERO, ONE, DWORK( I25 ), M )
      CALL DGEMM(  'N', 'N', M, M, NP, ONE, DK, LDDK, D, LDD,
     $             ONE, DWORK( I25 ), M )
C                  ^ ^    ^         ^    -1
C     Compute Ck = M*Ck,  M = (Im + Dk*D)   .
C
      ANORM = DLANGE( '1', M, M, DWORK( I25 ), M, DWORK( IWRK ) )
      CALL DGETRF( M, M, DWORK( I25 ), M, IWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 7
         RETURN
      END IF
      CALL DGECON( '1', M, DWORK( I25 ), M, ANORM, RCOND( 6 ),
     $             DWORK( IWRK ), IWORK( M+1 ), INFO2 )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND( 6 ).LT.TOLL ) THEN
         INFO = 7
         RETURN
      END IF
      CALL DGETRS( 'N', M, N, DWORK( I25 ), M, IWORK, CK, LDCK, INFO2 )
C                  ^ ^
C     Compute Dk = M*Dk .
C
      CALL DGETRS( 'N', M, NP, DWORK( I25 ), M, IWORK, DK, LDDK, INFO2 )
C             ^
C     Compute Bk*D .
C
      CALL DGEMM( 'N', 'N', N, M, NP, ONE, DWORK( I23 ), N, D, LDD,
     $            ZERO, DWORK( I26 ), N )
C                  ^    ^
C     Compute Ak = Ak - Bk*D*Ck.
C
      CALL DGEMM( 'N', 'N', N, N, M, -ONE, DWORK( I26 ), N, CK, LDCK,
     $            ONE, AK, LDAK )
C                  ^          ^  -1
C     Compute Bk = Bk*(Ip + D*Dk)   .
C
      ANORM = DLANGE( '1', NP, NP, DWORK( I24 ), NP, DWORK( IWRK ) )
      CALL DLACPY( 'Full', N, NP, DWORK( I23 ), N, BK, LDBK )
      CALL MB02VD( 'N', N, NP, DWORK( I24 ), NP, IWORK, BK, LDBK,
     $             INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 6
         RETURN
      END IF
      CALL DGECON( '1', NP, DWORK( I24 ), NP, ANORM, RCOND( 5 ),
     $             DWORK( IWRK ), IWORK( NP+1 ), INFO2 )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND( 5 ).LT.TOLL ) THEN
         INFO = 6
         RETURN
      END IF
C
C     Workspace usage.
C
      I2 = 1  + NP*NP
      I3 = I2 + N*NP
      I4 = I3 + M*M
      I5 = I4 + N*M
      I6 = I5 + NP*N
      I7 = I6 + M*N
      I8 = I7 + N2*N2
      I9 = I8 + N2
C
      IWRK = I9 + N2
C
C     Compute Ip - D*Dk .
C
      CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK, NP )
      CALL DGEMM(  'N', 'N', NP, NP, M, -ONE, D, LDD, DK, LDDK, ONE,
     $              DWORK, NP )
C                         -1
C     Compute Bk*(Ip-D*Dk)   .
C
      CALL DLACPY( 'Full', N, NP, BK, LDBK, DWORK( I2 ), N )
      CALL MB02VD( 'N', N, NP, DWORK, NP, IWORK, DWORK( I2 ), N, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 8
         RETURN
      END IF
C
C     Compute Im - Dk*D .
C
      CALL DLASET( 'Full', M, M, ZERO, ONE, DWORK( I3 ), M )
      CALL DGEMM(  'N', 'N', M, M, NP, -ONE, DK, LDDK, D, LDD, ONE,
     $              DWORK( I3 ), M )
C                        -1
C     Compute B*(Im-Dk*D)    .
C
      CALL DLACPY( 'Full', N, M, B, LDB, DWORK( I4 ), N )
      CALL MB02VD( 'N', N, M, DWORK( I3 ), M, IWORK, DWORK( I4 ), N,
     $             INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 9
         RETURN
      END IF
C
C     Compute D*Ck .
C
      CALL DGEMM( 'N', 'N', NP, N, M, ONE, D, LDD, CK, LDCK, ZERO,
     $            DWORK( I5 ), NP )
C
C     Compute Dk*C .
C
      CALL DGEMM( 'N', 'N', M, N, NP, ONE, DK, LDDK, C, LDC, ZERO,
     $            DWORK( I6 ), M )
C
C     Compute the closed-loop state matrix.
C
      CALL DLACPY( 'F', N, N, A, LDA, DWORK( I7 ), N2 )
      CALL DGEMM(  'N', 'N', N, N, M, ONE, DWORK( I4 ), N,
     $             DWORK( I6 ), M, ONE, DWORK( I7 ), N2 )
      CALL DGEMM(  'N', 'N', N, N, M, ONE, DWORK( I4 ), N, CK, LDCK,
     $             ZERO, DWORK( I7+N2*N ), N2 )
      CALL DGEMM(  'N', 'N', N, N, NP, ONE, DWORK( I2 ), N, C, LDC,
     $             ZERO, DWORK( I7+N ), N2 )
      CALL DLACPY( 'F', N, N, AK, LDAK, DWORK( I7+N2*N+N ), N2 )
      CALL DGEMM(  'N', 'N', N, N, NP, ONE, DWORK( I2 ), N,
     $             DWORK( I5 ), NP, ONE, DWORK( I7+N2*N+N ), N2 )
C
C     Compute the closed-loop poles.
C
      CALL DGEES( 'N', 'N', SELECT, N2, DWORK( I7 ), N2, SDIM,
     $            DWORK( I8 ), DWORK( I9 ), DWORK( IWRK ), N,
     $            DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 )
      IF( INFO2.NE.0 ) THEN
         INFO = 3
         RETURN
      END IF
      LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C     Check the stability of the closed-loop system.
C
      NS = 0
C
      DO 140 I = 0, N2 - 1
         IF( DLAPY2( DWORK( I8+I ), DWORK( I9+I ) ).GT.ONE )
     $      NS = NS + 1
  140 CONTINUE
C
      IF( NS.GT.0 ) THEN
         INFO = 10
         RETURN
      END IF
C
      DWORK( 1 ) = DBLE( LWAMAX )
      RETURN
C *** Last line of SB10ZD ***
      END


c  Estimate sh2o & frz states from smc and tsoil

      subroutine scale_sh2o(smc,tsoil,fgpm,nsoil,nupl,zsoil,
     +                         state,smax,brt,tbot,fgco,sh2o)
      real smc(nsoil),sh2o(5),tsoil(nsoil),fgpm(7)
      real state(5),zsoil(nsoil),fgco(5)
      PARAMETER (T0 = 273.16)

      TBUP=TBND(TSOIL(1)+T0,TSOIL(2)+T0,ZSOIL,FGPM(3),1,NSOIL)
      IF(SH2O(1) .LT. 0.0) SH2O(1)=SMC(1)
      SUZ=STATE(1)+STATE(2)
      SLZ=STATE(3)+STATE(4)+STATE(5)
      SUP=0.
      SLW=0.
      DO I=2,NSOIL
C  CALCULATE AVERAGE SOIL TEMPERATURE OF I-TH LAYER
       IF(I .NE. NSOIL) THEN
        TBDN=TBND(TSOIL(I)+T0,TSOIL(I+1)+T0,ZSOIL,FGPM(3),
     +            I,NSOIL)
       ELSE
        TBDN=TBND(TSOIL(I)+T0,TBOT,ZSOIL,FGPM(3),I,NSOIL)
       ENDIF
       DZ=ZSOIL(I-1)-ZSOIL(I)
       TS=ST_AVG1(TBUP,TSOIL(I)+T0,TBDN,DZ)
       TBUP=TBDN

C  CALCULATE POTENTIAL UNFROZEN WATER CONTENT
       IF(TS .LE. T0) THEN
        SH2O(I)=FRH2O_356(TS,SMC(I),SMC(I),SMAX,BRT,FGPM(6),FGPM(2))
       ELSE
        SH2O(I)=SMC(I)
       ENDIF
       DSW=1000*(SH2O(I)-FGPM(7))*(ZSOIL(I-1)-ZSOIL(I))/FGPM(4)
       IF(DSW .GT. 0.) THEN
        IF(I .LE. NUPL)THEN
         SUP=SUP+DSW
        ELSE
         SLW=SLW+DSW
        ENDIF
       ENDIF
      ENDDO

C  ESIMATE FROZEN SAC STATES
      L=0
      DO I=1,NSOIL
       IF(TSOIL(I) .LT. 0.0) L=1
      ENDDO
      IF(L .EQ. 1) THEN
       IF(SUP .GT. SUZ) SUP=SUZ
       IF(SLW .GT. SLZ) SLW=SLZ
       ALP=STATE(1)/SUZ
       FGCO(1)=SUP*ALP
       FGCO(2)=SUP*(1-ALP)
       ALP=STATE(3)/SLZ
       FGCO(3)=SLW*ALP
       ALP1=STATE(4)/SLZ
       FGCO(4)=SLW*ALP1
       FGCO(5)=SLW*(1-ALP-ALP1)
      ELSE
       DO I=1,5
        FGCO(I) = STATE(I)
       ENDDO
      ENDIF

      RETURN
      END


SUBROUTINE c2num
!======================================
! 2nd Neighbour Number
!======================================
! Calculates the second nearest neighbour number for each atom in the cluster.
! Calculates the derivatives.
! Use the c2num light version disregard the derivative loop and improve performance.
!======================================
   USE PARACLUSTER
   USE CLUSTER
   USE POTENTIAL
   USE ENFORCE
   USE DISTANCE
   USE META

   IMPLICIT NONE

   integer               :: i, j, mminusn_pwr
   double precision      :: ratio_ton, ratio_tom, rnp, rmp, num
   double precision      :: ratio, comfactor
   real(8), dimension(3) :: rij0 ! 3 possible cases: atoms A-A, B-B, A-B
   real(8), dimension(3) :: dij0 ! 3 possible cases: atoms A-A, B-B, A-B
   real(8), DIMENSION(nsiz,nsiz) :: dij, df_dx, df_dy, df_dz

   if (ipas.eq.1) write(*,*) '> c2num: first calculation of window function 1 (C2N)'

   !------------------------------------------------------------------------
   mminusn_pwr = m2n_pwr - n2n_pwr

   ! Evaluating the values of rij0 parameter in dependence of atom pair kind
   rij0 = dist * r2n * dsqrt(2.d0) ! rij0 and dist are 3 element vectors

   ! Evaluating the ref distance for the couple of atoms (THIS IS DIFFERENT)
   dij0 = dist * d2n * dsqrt(2.d0)
   !------------------------------------------------------------------------

!$OMP PARALLEL DEFAULT(SHARED)
!$OMP DO PRIVATE(i, j), SCHEDULE(STATIC)
   DO i = 1, natom
      DO j = 1, natom
         ! Calculating the distance between atom i and atom j
         dij(j,i) = pair_dist (j,i) - dij0(pairkindmat(j,i))
      END DO
   END DO
!$OMP END DO

!$OMP DO PRIVATE(i), SCHEDULE(STATIC)
   ! Elements on the diagonal
   DO i = 1, natom
      s2n(i,i) = 0.d0
   END DO
!$OMP END DO

!$OMP DO PRIVATE(i, j, ratio, ratio_ton, ratio_tom, rnp, rmp, num), SCHEDULE(DYNAMIC,20)
   DO i = 1, natom-1
      DO j = i+1, natom
         ! Calculating the analytic C2N
         ratio = dij(j,i)/rij0(pairkindmat(j,i))
         ! I need to rise to (X_pwr-1) for the derivative
         ratio_ton = ratio**(n2n_pwr-1)
         ratio_tom = ratio_ton*(ratio**(mminusn_pwr))

         ! Calculating some pieces of derivative
         rnp = n2n_pwr * ratio_ton
         rmp = m2n_pwr * ratio_tom

         ! Now ratio_ton becomes really ratio to n_pwr
         ratio_ton = ratio_ton * ratio

         ! Another piece of derivative
         num = rmp - rnp - mminusn_pwr*ratio_ton*ratio_tom
         ! same as:
         ! num = rmp - rnp + ( n_pwr - m_pwr )*ratio**(n_pwr + m_pwr - 1)

         ! Now ratio_tom becomes really ratio to m_pwr
         ratio_tom = ratio_tom * ratio

         ! The sum of these s(i,j) is the analytic coord no.
         s2n(j,i) = (1.d0-ratio_ton)/(1.d0-ratio_tom)  ! this is f(rij)
         s2n(i,j) = s2n(j,i)

         ! Derivative of s
         dS(j,i) = num/( rij0(pairkindmat(j,i))* (1.d0 - ratio_tom)**2)
         dS(i,j) = dS(j,i)
      END DO ! on j
   END DO ! on i
!$OMP END DO

!$OMP DO PRIVATE(i), SCHEDULE(STATIC)
   ! LP: Calculating the sum of s2n(i,j), which is C2Natom(i), global.
   ! This is the 2nd neigh num of any single atom i
   ! No problem because s2n(i,i) is always zero
   DO i = 1, natom
      C2Natom(i) = SUM(s2n(:,i))
   END DO
!$OMP END DO

!$OMP DO PRIVATE(i,j,comfactor), SCHEDULE(STATIC)
   !-------------------------------------------------------------------------
   ! Calculating the sum on j of derivatives of s
   ! with respect to coordinates of every atoms
   DO i = 1, natom
      DO j = 1, natom
         IF (j .NE.i ) THEN
            ! Calculating the derivative of coordination number with
            ! respect to distance between atoms i and j, multiplied for
            ! the derivative of distance between atoms i and j
            ! with respect to position of atom i
            comfactor  = dS(j,i)/ pair_dist(j,i)    ! Common factor for the following...
            df_dx(j,i) = comfactor* xyz_dist(1,j,i) ! order of indexes depends on the definition of xyz_dist
            df_dy(j,i) = comfactor* xyz_dist(2,j,i)
            df_dz(j,i) = comfactor* xyz_dist(3,j,i)
         ELSE
            df_dx(j,i) = 0.d0
            df_dy(j,i) = 0.d0
            df_dz(j,i) = 0.d0
         END IF
      END DO
   END DO
!$OMP END DO

!$OMP SECTIONS PRIVATE(i)
!$OMP SECTION
   DO i = 1, natom
      dS2n_dx(i) = 2.d0 * SUM(df_dx(1:natom, i))
   END DO
!$OMP SECTION
   DO i = 1, natom
      dS2n_dy(i) = 2.d0 * SUM(df_dy(1:natom, i))
   END DO
!$OMP SECTION
   DO i = 1, natom
      dS2n_dz(i) = 2.d0 * SUM(df_dz(1:natom, i))
   END DO
!$OMP END SECTIONS
!$OMP END PARALLEL
   !---------------------------------------------------------------------------
END SUBROUTINE c2num


c***********************************************************************
c	                    COORD_B2J.F
c***********************************************************************
*     PURPOSE: Converts from Barycentric to Jacobi coords.
*     ARGUMENTS:  Input is
*                    nbod ==> number of bodies (must be less than NBMAX)
*                             (integer)
*	             mass(*) ==>  masses (real array)
*		     xb(*),yb(*),zb(*) ==> Barycentric particle coords
*                                          (real array)
*		     vxb(*),vyb(*),vzb(*) ==> Barycentric particle velocities
*                                             (real array)
*                 Returned are
*                    xj(*),yj(*),zj(*) ==> jacobi particle positions
*                                          (real array)
*                    vxj(*),vyj(*),vzj(*) ==> jacobi particle velocities
*                                            (real array)
*
*     ALGORITHM:  See e.g. Wisdom and Holman
*     Authors:  Martin Duncan
*     WRITTEN:  Jan 29/93
*     REVISIONS: 2/17/95  HFL

	subroutine coord_b2j(nbod,mass,xb,yb,zb,vxb,vyb,vzb,
     &      xj,yj,zj,vxj,vyj,vzj)

      include '../swift.inc'

c...  Inputs:
	integer nbod
	real*8 mass(NPLMAX)
	real*8 xb(NPLMAX),yb(NPLMAX),zb(NPLMAX)
	real*8 vxb(NPLMAX),vyb(NPLMAX),vzb(NPLMAX)

c...  Outputs:
	real*8 xj(NPLMAX),yj(NPLMAX),zj(NPLMAX)
	real*8 vxj(NPLMAX),vyj(NPLMAX),vzj(NPLMAX)

c...  Internals:
	real*8 eta(NPLMAX)
	real*8 sumx,sumy,sumz,sumvx,sumvy,sumvz
	real*8 capx,capy,capz,capvx,capvy,capvz
	integer n

c----
c...  Executable code

c First calc. the array eta(*) then convert to jacobi coords

	eta(1) = mass(1)
	do n = 2,nbod
	  eta(n) = eta(n-1) + mass(n)
        enddo

	xj(1) = 0.d0
	yj(1) = 0.d0
	zj(1) = 0.d0
	vxj(1) = 0.d0
	vyj(1) = 0.d0
	vzj(1) = 0.d0

	sumx = mass(1)*xb(1)
	sumy = mass(1)*yb(1)
	sumz = mass(1)*zb(1)
	sumvx = mass(1)*vxb(1)
	sumvy = mass(1)*vyb(1)
	sumvz = mass(1)*vzb(1)

	capx = xb(1)
	capy = yb(1)
	capz = zb(1)
	capvx = vxb(1)
	capvy = vyb(1)
	capvz = vzb(1)

	do n=2,nbod - 1
	  xj(n) = xb(n) - capx
	  yj(n) = yb(n) - capy
	  zj(n) = zb(n) - capz
	  vxj(n) = vxb(n) - capvx
	  vyj(n) = vyb(n) - capvy
	  vzj(n) = vzb(n) - capvz

	  sumx = sumx + mass(n)*xb(n)
	  sumy = sumy + mass(n)*yb(n)
	  sumz = sumz + mass(n)*zb(n)
	  sumvx = sumvx + mass(n)*vxb(n)
	  sumvy = sumvy + mass(n)*vyb(n)
	  sumvz = sumvz + mass(n)*vzb(n)

	  capx = sumx/eta(n)
	  capy = sumy/eta(n)
	  capz = sumz/eta(n)
	  capvx =sumvx/eta(n)
	  capvy =sumvy/eta(n)
	  capvz =sumvz/eta(n)

	enddo

	  xj(nbod) = xb(nbod) - capx
	  yj(nbod) = yb(nbod) - capy
	  zj(nbod) = zb(nbod) - capz
	  vxj(nbod) = vxb(nbod) - capvx
	  vyj(nbod) = vyb(nbod) - capvy
	  vzj(nbod) = vzb(nbod) - capvz

	return
	end   ! coord_b2j

c--------------------------------------------------------------------------


!----------------------------------------------------------------------
!                           BHMIE code
!----------------------------------------------------------------------
! This is a modified version of the BHMIE code written by Bruce Draine.
!  https://www.astro.princeton.edu/~draine/scattering.html
!
! R.T. modified following points.
!- A big static array is now defined as a dynamic array.
!- Implemented the Lentz's continued fraction method to estimate
!  the logarithmic derivative of Riccati Bessel function,
!  which is safe for very large grains.
!- Implemented adjustable real and complex precision
!- Slightly modified xstop to match the Wiscombe (1980)'s criterion.
!
!                                              Ryo Tazaki (2020.12.16)
!----------------------------------------------------------------------
module types
implicit none
integer, parameter      :: dp = selected_real_kind(P=15)
end module types

subroutine bhmie_rt(x,refrel,nang,S1,S2,Qext,Qsca,Qback,gsca)
use types
implicit none
integer::nang,j,jj,n,nn,nstop,nmx
complex(kind=dp)::refrel,y,dini
complex(kind=dp),dimension(1:2*nang-1)::S1,S2
complex(kind=dp),allocatable,dimension(:)::d,ajy
complex(kind=dp)::an,bn,an1,bn1
complex(kind=dp)::xi1,xi,dpcx
real(kind=dp)::x,qext,qsca,gsca,qback
real(kind=dp)::en,fn,p,xstop,ymod,nu
real(kind=dp)::psi0,psi1,psi,chi0,chi1,chi
real(kind=dp)::dang,pii,theta
real(kind=dp),dimension(1:nang)::amu,pi0,pi1,pi,tau
real(kind=dp) realpart
real(kind=dp) imagpart
realpart(dpcx)=(dble(dpcx))
imagpart(dpcx)=(dimag(dpcx))

! some checks...
y     = refrel*x
ymod  = abs(y)
xstop = x + 4.05_dp * x ** (1.0/3.0) + 2.0_dp
nstop = nint(xstop)

!*** require nang.ge.1 in order to calculate scattering intensities
pii  = 4.0_dp*atan(1.0_dp)
dang = 0.0_dp
if(nang.gt.1)dang=0.5_dp*pii/dble(nang-1)
do j=1,nang
        theta =real(j-1,kind=dp)*dang
        amu(j)=cos(theta)
enddo
do j=1,nang
        pi0(j)=0.0_dp
        pi1(j)=1.0_dp
enddo
nn=2*nang-1
do j=1,nn
        S1(j)=cmplx(0.0_dp,0.0_dp)
        S2(j)=cmplx(0.0_dp,0.0_dp)
enddo

!----------------------------------------------------------------------
!       logarithmic derivative
!----------------------------------------------------------------------
! 1. Standard BHMIE algorithm ( Draine's code as well )
! Warming-up terms of 15 may not be sufficient for very large spheres.
!----------------------------------------------------------------------
!nmx = nint(max(xstop,ymod)) + 15
!nmx = nint(max(xstop,ymod)) + 100
!allocate(d(1:nmx))
!if(nmx .gt. 150000) then !1.5e5 is from BTD's code.
!        write(*,*) "Error: nmx > nmxx=",150000,"for |m|x=",ymod
!        stop
!endif
!calculate D_n(mx) by downward recurrence.
!Initial value is set as D(mx)=0+0i at n=NMX.
!d(nmx)=cmplx(0.0_dp,0.0_dp)
!do n=1,nmx-1
!        en=real(nmx-n+1,kind=dp)
!        d(nmx-n) = en/y - (1.0_dp/(d(nmx-n+1) + en/y))
!enddo

!----------------------------------------------------------------------
! 2. The Lentz's continued fraction method
!    Reference: W. J. Lentz, Appl. Opt. 15. 668. (1976)
!----------------------------------------------------------------------
! Finding initial value of the logarithmic derivative "d" at
! the starting order of downward recurrence (n=nmx).
!----------------------------------------------------------------------
nmx = nint(max(xstop,ymod)) + 15
allocate(d(1:nmx),ajy(1:nmx))
! Equation (5) in Lentz (1976):
nu = real(nmx,kind=dp)+0.5_dp
ajy(1) = 2.0_dp * nu / y
do j=2,nmx
       ajy(j) = (-1.0_dp)**real(j-1,kind=dp)*2.0_dp*(nu+real(j-1,kind=dp))/y
enddo
dini = ajy(nmx)
do j=1,nmx-1
        dini = ajy(nmx-j) + 1.0_dp / dini
enddo
deallocate(ajy)
! Equation (3) in Lentz (1976):
d(nmx)=-real(nmx,kind=dp)/y+dini
! usual downward recurrence:
do n=1,nmx-1
        en=real(nmx-n+1,kind=dp)
        d(nmx-n) = (en/y) - (1.0_dp/(d(nmx-n+1) + en/y))
enddo
!----------------------------------------------------------------------

psi0 = cos(x)
psi1 = sin(x)
chi0 =-sin(x)
chi1 = cos(x)
xi1  = cmplx(psi1,-chi1)
!
qsca = 0.0_dp
gsca = 0.0_dp
p    =-1.0_dp
do n=1,nstop
        en = real(n,kind=dp)
        fn = (2.0_dp*en+1.0_dp)/(en*(en+1.0_dp))

        !Calculate psi and chi via upward recurrence:
        psi = (2.0_dp*en-1.0_dp)*psi1/x - psi0
        chi = (2.0_dp*en-1.0_dp)*chi1/x - chi0
        xi = cmplx(psi,-chi)

        !*** store previous values of an and bn for use
        !    in computation of g=<cos(theta)>
        if(n.gt.1)then
                an1=an
                bn1=bn
        endif

        !*** compute an and bn:
        an = (d(n)/refrel+en/x)*psi-psi1
        an = an/((d(n)/refrel+en/x)*xi-xi1)
        bn = (refrel*d(n)+en/x)*psi-psi1
        bn = bn/((refrel*d(n)+en/x)*xi-xi1)

        !*** augment sums for qsca and g=<cos(theta)>
        qsca=qsca+(2.0_dp*en+1.0_dp)*(abs(an)**2.0+abs(bn)**2.0)
        gsca=gsca+((2.0_dp*en+1.0_dp)/(en*(en+1.0_dp)))*&
        &        (realpart(an)*realpart(bn)+imagpart(an)*imagpart(bn))
        if(n.gt.1)then
            gsca=gsca+((en-1.0_dp)*(en+1.0_dp)/en)*&
        &      (realpart(an1)*realpart(an)+imagpart(an1)*imagpart(an)+&
        &      realpart(bn1)*realpart(bn)+imagpart(bn1)*imagpart(bn))
        endif

        !c*** now calculate scattering intensity pattern
        !    first do angles from 0 to 90
        do j=1,nang
                jj=2*nang-j
                pi(j)=pi1(j)
                tau(j)=en*amu(j)*pi(j)-(en+1.0_dp)*pi0(j)
                S1(j)=S1(j)+fn*(an*pi(j)+bn*tau(j))
                S2(j)=S2(j)+fn*(an*tau(j)+bn*pi(j))
        enddo

        !*** now do angles greater than 90 using pi and tau from
        !    angles less than 90.
        !    p=1 for n=1,3,...; p=-1 for n=2,4,...
        p=-p
        do j=1,nang-1
                jj=2*nang-j
                S1(jj)=S1(jj)+fn*p*(an*pi(j)-bn*tau(j))
                S2(jj)=S2(jj)+fn*p*(bn*pi(j)-an*tau(j))
        enddo

        !Preparation for next step
        psi0 = psi1
        psi1 = psi
        chi0 = chi1
        chi1 = chi
        xi1 = cmplx(psi1,-chi1)

        !*** compute pi_n for next value of n
        !    for each angle j, compute pi_n+1
        !    from pi = pi_n , pi0 = pi_n-1
        do j=1,nang
                pi1(j)=((2.0_dp*en+1.0_dp)*amu(j)*pi(j)-(en+1.0_dp)*pi0(j))/en
                pi0(j)=pi(j)
        enddo
enddo

!*** have summed sufficient terms.
!    now compute qsca,qext,qback,and gsca
gsca=2.0_dp*gsca/qsca
qsca=(2.0_dp/(x*x))*qsca
qext=(4.0_dp/(x*x))*realpart(S1(1))
qback=(4.0_dp*(ABS(S1(2*NANG-1))/x)**2._dp)

deallocate(d)

return
end subroutine bhmie_rt


      SUBROUTINE SCOND (A,B,C,KING)
C     TO OBTAIN PARABOLIC DERIVATIVE OF CURVE (UNEQUALLY SPACED POINTS)
      IMPLICIT REAL*8(A-H,O-Z)
      DIMENSION A(150), B(150), C(150)
      N=KING-1
      DO 1 K=2,N
      S=A(K)-A(K-1)
      T=A(K+1)-A(K)
1     C(K)=((B(K+1)-B(K))*S*S+(B(K)-B(K-1))*T*T)/(S*S*T+S*T*T)
      SO=A(2)-A(1)
      T0=A(3)-A(2)
      QO=SO+T0
      C(1)=(-T0*(QO+SO)*B(1)+QO*QO*B(2)-SO*SO*B(3))/QO/SO/T0
      SF=A(KING-1)-A(KING-2)
      TF=A(KING)-A(KING-1)
      QF=SF+TF
      QST=QF*SF*TF
      C(KING)=(SF*(QF+TF)*B(KING)-QF*QF*B(KING-1)+TF*TF*B(KING-2))/QST
      RETURN
      END


      subroutine bndcond(nstep,tg,vk,rlnzz,z0,ustar,
     *                   thstar,ric,dphi,w,z,psi,cdh,cdm)

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

! New routine calculates ustar and thstar from rib,
! based on Savijarvi, Icarus Vol 117 p121, section 2.1 (rib < 0)
! and Hourdin et al, JGR Vol 100 p5505, equations 5 and 6 (rib > 0)

      use grid_h
      use constants_h, only: grav
      use pbl_defines_h

      implicit none

!     global variables:

      real*8 z(2*n+1), psi(2*n+1,nvar)

      integer :: nstep
      real*8  :: tg, vk, rlnzz, z0, ustar, thstar, ric, dphi, w, cdh
      real*8  :: cdm, rib, frih, frim

!======================================================================C

      w  = sqrt( psi(2*n,1)*psi(2*n,1) + psi(2*n,2)*psi(2*n,2) )

!     For now use psi at midpoint of bottom layer.
!     Later interpolate to surface

      dphi = (psi(2*n,3) - tg)
      rib  = ((grav * z(2*n)) / ( psi(2*n,3)*w*w + 1.0e-09))*dphi

      if ( rib .ge. 0.0 ) then

        frih   = 1.0/(1.0+(15.0*rib/sqrt(1.0+5.0*rib)))
        frim   = 1.0/(1.0+(10.0*rib/sqrt(1.0+5.0*rib)))

      else if ( rib .lt. 0.0 ) then

! LMD formulation

!           frih   = 1.0 - 15.0*rib/(1+75.0*((vk/rlnzz)**2)*
!     &              sqrt(-rib*exp(rlnzz)))
!           frim   = 1.0 - 15.0*rib/(1+75.0*((vk/rlnzz)**2)*
!     &              sqrt(-rib*(1.0+exp(rlnzz))))

! Original, incorrect version

!       frih   = sqrt(1.0-16.0*rib)
!       frim   = sqrt(1.0-64.0*rib)

! Corrected 5-30-06

        frih = sqrt(1.0-64.0*rib)
        frim = sqrt(1.0-16.0*rib)

      end if

      cdh    = sqrt(frih)*vk/rlnzz
      cdm    = frim*((vk/rlnzz)**2)

      ustar  = sqrt(cdm)*w
      thstar = cdh*dphi

      return
      end


!------------------------------------------------------------------------------
! MODULE: opt_stiffness
!------------------------------------------------------------------------------
!> @author Johannes Gebert - HLRS - NUM - gebert@hlrs.de
!
! @description:
!> Module for optimizing R6x6 anisotropic tensors.
!------------------------------------------------------------------------------
MODULE opt_stiffness

USE global_std
USE mechanical
USE math
USE user_interaction

IMPLICIT NONE

!------------------------------------------------------------------------------
! Following variables are declared globally, but mustn't be send via mpi(!)
! Declaring them globally makes calling functions and exporting the computed
! criteria space of a tensor easier.
!------------------------------------------------------------------------------
! tin            Input tensor
! dig            Input angles (Often = 0)
! pm_steps       +/- steps of the angle to check with.
! intervall      stps +/- intervall = range of optimization of this call.
! tout           Output tensor
! dog            Output angles
!------------------------------------------------------------------------------
TYPE(tensor_2nd_rank_R66) :: tin, tout
REAL(KIND=rk), DIMENSION(3) :: dig, dog
REAL(KIND=rk), DIMENSION(:,:,:), ALLOCATABLE :: crit

CONTAINS

!------------------------------------------------------------------------------
! SUBROUTINE: opt_stiff
!------------------------------------------------------------------------------
!> @author Johannes Gebert - HLRS - NUM - gebert@hlrs.de
!
!> @Brief:
!> Rotate around all 3 rot dof until objective function is min.
!> Initial degrees must be integer!
!
!> @param[in] mode "monotropic" or "orthotropic"
!> @param[in] steps Steps to calculate with intervall
!> @param[in] intervall Distance between steps
!------------------------------------------------------------------------------
SUBROUTINE opt_stiff(mode, steps, intervall, stage)

CHARACTER(LEN=*), INTENT(IN) :: mode
INTEGER(KIND=ik), DIMENSION(3) :: steps
INTEGER(KIND=ik), INTENT(IN) :: stage
REAL(KIND=rk), DIMENSION(3) :: intervall

INTEGER(KIND=ik), DIMENSION(3) :: best_position
INTEGER(KIND=ik) :: ii, jj, kk

REAL(KIND=rk) :: min, alpha, eta, phi
REAL(KIND=rk), DIMENSION(6,6) :: tmp_r6x6, mask

!----------------------------------------------------------------------------------------------
! Initialize variables
!----------------------------------------------------------------------------------------------
mask = 0_ik
min = 10E09_ik

!----------------------------------------------------------------------------------------------
! Copy input to output tensor. Important to keep domain data etc.
!----------------------------------------------------------------------------------------------
tout = tin

!----------------------------------------------------------------------------------------------
! Choose between monotropic or orthotropic optimization
!----------------------------------------------------------------------------------------------
SELECT CASE(TRIM(ADJUSTL(mode)))
    !----------------------------------------------------------------------------------------------
    !
    ! General stiffness matrix, S_ij = S_ji:             Convention:
    !
    ! [ S_11, S_12, S_13, | S_14, S_15, S_16 ]           [            |            ]
    ! [ S_21, S_22, S_23, | S_24, S_25, S_26 ]           [ Quadrant 2 | Quadrant 1 ]
    ! [ S_31, S_32, S_33, | S_34, S_35, S_36 ]           [            |            ]
    ! --------------------|-------------------           ---------------------------
    ! [ S_41, S_42, S_43, | S_44, S_45, S_46 ]           [            |            ]
    ! [ S_51, S_52, S_53, | S_54, S_55, S_56 ]           [ Quadrant 3 | Quadrant 4 ]
    ! [ S_61, S_62, S_63, | S_64, S_65, S_66 ]           [            |            ]
    !
    !
    ! Monotropic, case 1 of 3.                           Monotropic, case 2 of 3.
    !
    ! [ S_11, S_12, S_13, | S_14,   0 ,   0  ]           [ S_11, S_12, S_13, |   0 , S_15,   0  ]
    ! [ S_21, S_22, S_23, | S_24,   0 ,   0  ]           [ S_21, S_22, S_23, |   0 , S_25,   0  ]
    ! [ S_31, S_32, S_33, | S_34,   0 ,   0  ]           [ S_31, S_32, S_33, |   0 , S_35,   0  ]
    ! --------------------|-------------------           --------------------|-------------------
    ! [ S_41, S_42, S_43, | S_44,   0 ,   0  ]           [   0 ,   0 ,   0 , | S_44,   0 , S_46 ]
    ! [   0 ,   0 ,   0 , |   0 , S_55, S_56 ]           [ S_51, S_52, S_53, |   0 , S_55,   0  ]
    ! [   0 ,   0 ,   0 , |   0 , S_65, S_66 ]           [   0 ,   0 ,   0 , | S_64,   0 , S_66 ]
    !
    ! Monotropic, case 3 of 3.                           Orthotropic:
    !
    ! [ S_11, S_12, S_13, |   0 ,   0 , S_16 ]           [ S_11, S_12, S_13, |   0 ,   0 ,   0  ]
    ! [ S_21, S_22, S_23, |   0 ,   0 , S_26 ]           [ S_21, S_22, S_23, |   0 ,   0 ,   0  ]
    ! [ S_31, S_32, S_33, |   0 ,   0 , S_36 ]           [ S_31, S_32, S_33, |   0 ,   0 ,   0  ]
    ! --------------------|-------------------           --------------------|-------------------
    ! [   0 ,   0 ,   0 , | S_44, S_45,   0  ]           [   0 ,   0 ,   0 , | S_44,   0 ,   0  ]
    ! [   0 ,   0 ,   0 , | S_54, S_55,   0  ]           [   0 ,   0 ,   0 , |   0 , S_55,   0  ]
    ! [ S_61, S_62, S_63, |   0 ,   0 , S_66 ]           [   0 ,   0 ,   0 , |   0 ,   0 , S_66 ]
    !
    !----------------------------------------------------------------------------------------------
    ! At the monotropic and the orthotropic optimization, the zero entries are minimized.
    !
    ! Example orthotropic: The 1st and the 3rd quadrant must be 0.
    ! Additionally, the minor diagonals of the 4th quadrant must be 0. Therefore, each of the zero-
    ! entries occurs in the upper right and the lower left triangle of the R6x6 matrix.
    ! Subsequently, the mask multiplies them by 2_ik.
    !
    ! The matrix mask needs no recurring initialization, as the values mustn't change during
    ! optimization (program runtime in general).
    !----------------------------------------------------------------------------------------------
    CASE('monotropic')
        mask(1:4,5:6) = 2_ik

    CASE('orthotropic')
        ! Explanation @ opt_stiff_mono
        mask(1:3,4:6) = 2_ik
        mask(  4,5:6) = 2_ik
        mask(  5,  6) = 2_ik

    CASE('anisotropic1')
        ! S11 available only once.
        mask(1,1) = 1_ik

    CASE('anisotropic2')
        ! S_11, S_22, S_33 available only once.
        mask(1,1) = 1_ik
        mask(2,2) = 1_ik
        mask(3,3) = 1_ik

    CASE DEFAULT
        mssg = "No valid optimization chosen. Check your implementation!"
        CALL print_err_stop(std_out, mssg, 1)

END SELECT

!----------------------------------------------------------------------------------------------
! Allocate memory for angle +/- steps.
! Each call, the system checks whether the space of criteria is allocated.
! If not --> allocate
! If ttl_steps changes --> reallocate
! If Program closes --> Everything alright
! Nothing gets deallocated without explicit order. This feature enables writing the
! space of criteria to a vtk (i.e., scalar field).
! @MPI: With mpi, crit may have a different state across the ranks!
!----------------------------------------------------------------------------------------------
IF(ALLOCATED(crit)) THEN
    IF((SIZE(crit, DIM=1) /= (steps(1)+1)) .OR. &
       (SIZE(crit, DIM=2) /= (steps(2)+1)) .OR. &
       (SIZE(crit, DIM=3) /= (steps(3)+1))) THEN

        DEALLOCATE(crit)
    END IF
END IF

IF(.NOT. ALLOCATED(crit)) THEN
    ALLOCATE(crit(steps(1)+1, steps(2)+1, steps(3)+1))
END IF

!----------------------------------------------------------------------------------------------
! alpha = input angle - 1/2 of the swept angles.
!----------------------------------------------------------------------------------------------
phi = dig(3) - ((intervall(3) * steps(3)) / 2._rk)
DO kk = 1_ik, steps(3)+1_ik

    IF(out_amount == "DEBUG") THEN
        WRITE(std_out, "(A, '-- ', A, I0, 3A, F0.2, A)", ADVANCE='NO') creturn , &
            "Progress of stage ", stage, " of ",TRIM(ADJUSTL(mode)), " mode: ", &
            REAL(kk, KIND=rk)/(steps(3)+1)*100._rk, " %"
    END IF

    eta = dig(2) - ((intervall(2) * steps(2)) / 2._rk)
    DO jj = 1_ik, steps(2)+1_ik

        alpha = dig(1) - ((intervall(1) * steps(1)) / 2._rk)
        DO ii = 1_ik, steps(1)+1_ik

            CALL transpose_mat (tin%mat, [ alpha, eta, phi ] , tmp_r6x6)

            !-------------------------------------------------------------------------------
            ! Criteria: Entries that are not zero after multiplication with mask are
            ! squared and summed up. Integer power (**2) is quicker than real power (**2.0).
            ! https://twitter.com/fortrantip/status/1478765410405298176?s=24
            !-------------------------------------------------------------------------------
            crit(ii, jj, kk) = SUM((tmp_r6x6 * mask)**2_ik)

        alpha = alpha + intervall(1)
        END DO

    eta = eta + intervall(2)
    END DO

phi = phi + intervall(3)
END DO

!-------------------------------------------------------------------------------
! Find the best position (min or max) within the criteria space.
!-------------------------------------------------------------------------------
SELECT CASE(TRIM(ADJUSTL(mode)))
    CASE('monotropic', 'orthotropic')
        best_position = MINLOC(crit(:, :, :))

    CASE('anisotropic1', 'anisotropic2')
        best_position = MAXLOC(crit(:, :, :))
END SELECT

!-------------------------------------------------------------------------------
! Calculate the angles of the best position again.
! Alternatively the angles can be stored in an array like crit(:,:,:), but with
! KIND=REAL64. But storing this array and the if/else within the nested loops of
! ii, jj, kk will consume lots of memory and compute time within the array.
!-------------------------------------------------------------------------------
alpha = dig(1) - ((intervall(1) * steps(1)) / 2._rk) + (intervall(1) * best_position(1))
eta   = dig(2) - ((intervall(2) * steps(2)) / 2._rk) + (intervall(2) * best_position(2))
phi   = dig(3) - ((intervall(3) * steps(3)) / 2._rk) + (intervall(3) * best_position(3))

tout%pos = [ alpha, eta, phi ]

!----------------------------------------------------------------------------------------------
! Recalculate the best combination of angles.
!----------------------------------------------------------------------------------------------
CALL transpose_mat (tin%mat, tout%pos, tout%mat)

END SUBROUTINE opt_stiff

!------------------------------------------------------------------------------
! FUNCTION: DoAM
!------------------------------------------------------------------------------
!> @author Johannes Gebert - HLRS - NUM - gebert@hlrs.de
!
!> @brief:
!> Degree of anisotropy - monotropic
!
!> @param[in] mat 2nd rank R6x6 tensor
!> @return[out] res Result
!------------------------------------------------------------------------------
FUNCTION DoAM(mat) RESULT(res)
    REAL(KIND=rk), DIMENSION(6,6), INTENT(in) :: mat
    REAL(KIND=rk) :: res

    !------------------------------------------------------------------------------
    !-- Degree of Anisotropy - average 0 divided by average non-zero entry
    !------------------------------------------------------------------------------
    res= (( + ABS(mat(1,5)) +    ABS(mat(1,6)) &
            + ABS(mat(2,5)) +    ABS(mat(2,6)) &
            + ABS(mat(3,5)) +    ABS(mat(3,6)) &
            + ABS(mat(4,5)) +    ABS(mat(4,6)) )/8.0_rk ) / &
            ((ABS(mat(1,1)) + 2* ABS(mat(1,2)) + 2* ABS(mat(1,3)) + 2* ABS(mat(1,4)) &
            + ABS(mat(2,2)) + 2* ABS(mat(2,3)) + 2* ABS(mat(2,4)) &
            + ABS(mat(3,3)) + 2* ABS(mat(3,4)) &
            + ABS(mat(4,4)) &
            + ABS(mat(5,5)) + 2* ABS(mat(5,6)) &
            + ABS(mat(6,6)))/20.0_rk)*100.0_rk
END FUNCTION DoAM

!------------------------------------------------------------------------------
! FUNCTION: DoAO
!------------------------------------------------------------------------------
!> @author Johannes Gebert - HLRS - NUM - gebert@hlrs.de
!
!> @brief:
!> Degree of anisotropy - orthotropic
!
!> @param[in] mat 2nd rank R6x6 tensor
!> @return[out] res Result
!------------------------------------------------------------------------------
FUNCTION DoAO(mat) RESULT(res)
    REAL(KIND=rk), DIMENSION(6,6), INTENT(in) :: mat
    REAL(KIND=rk) :: res

    !------------------------------------------------------------------------------
    !-- Degree of Anisotropy - average 0 divided by average non-zero entry
    !------------------------------------------------------------------------------
    res=(( ABS(mat(1,4)) + ABS(mat(1,5)) + ABS(mat(1,6)) &
        + ABS(mat(2,4)) + ABS(mat(2,5)) + ABS(mat(2,6)) &
        + ABS(mat(3,4)) + ABS(mat(3,5)) + ABS(mat(3,6)) &
        + ABS(mat(4,5)) + ABS(mat(4,6)) &
        + ABS(mat(5,6)) )/12.0_rk ) / &
        ((ABS(mat(1,1)) +  2*ABS(mat(1,2)) +  2*ABS(mat(1,3)) &
        + ABS(mat(2,2)) +  2*ABS(mat(2,3)) &
        + ABS(mat(3,3)) &
        + ABS(mat(4,4)) + ABS(mat(5,5)) + ABS(mat(6,6)) )/12.0_rk)*100.0_rk
END FUNCTION DoAO

!------------------------------------------------------------------------------
! SUBROUTINE: tilt_tensor
!------------------------------------------------------------------------------
!> @author Johannes Gebert - HLRS - NUM - gebert@hlrs.de
!
!> @brief
!> Tilts a tin by 90° around axis x, y or z - respectively 1, 2 or 3
!> other angles are not implemented. Changes S11, S22 and S33 to get
!> an S11 > S22 > S33 ordering.
!
!> @param[in] mat Input tensor
!------------------------------------------------------------------------------
SUBROUTINE tilt_tensor(mat)

REAL(KIND=rk), DIMENSION(6,6), INTENT(INOUT)  :: mat ! input tensor

REAL(KIND=rk), DIMENSION(6,6) :: BB1, BB2, BB3, tmp6x6
REAL(KIND=rk), DIMENSION(3)   :: n1, n2, n3
REAL(KIND=rk), DIMENSION(3,3) :: aa1, aa2, aa3

n1 = [1,0,0]
n2 = [0,1,0]
n3 = [0,0,1]
aa1 = rot_alg (n1, pihalf)
aa2 = rot_alg (n2, pihalf)
aa3 = rot_alg (n3, pihalf)
BB1 = tra_R6 (aa1)
BB2 = tra_R6 (aa2)
BB3 = tra_R6 (aa3)

     IF((mat(1,1)>mat(2,2)) .AND. (mat(1,1)>mat(3,3)) .AND. (mat(2,2)>mat(3,3))) THEN
    mat = mat !-- 123
ELSE IF((mat(1,1)>mat(2,2)) .AND. (mat(1,1)>mat(3,3)) .AND. (mat(2,2)<mat(3,3))) THEN
    mat  = MATMUL(MATMUL(TRANSPOSE(BB1),mat),BB1) !-- 132 => 123
ELSE IF((mat(1,1)<mat(2,2)) .AND. (mat(1,1)<mat(3,3)) .AND. (mat(2,2)>mat(3,3))) THEN
    tmp6x6 = MATMUL(MATMUL(TRANSPOSE(BB3),mat),BB3) !-- 231 => 132

    mat  = MATMUL(MATMUL(TRANSPOSE(BB1),tmp6x6),BB1) !-- 132 => 123
ELSE IF((mat(1,1)<mat(2,2)) .AND. (mat(1,1)>mat(3,3)) .AND. (mat(2,2)>mat(3,3))) THEN
    mat  = MATMUL(MATMUL(TRANSPOSE(BB3),mat),BB3) !-- 213 => 123
ELSE IF((mat(1,1)>mat(2,2)) .AND. (mat(1,1)<mat(3,3)) .AND. (mat(2,2)<mat(3,3))) THEN
    tmp6x6 = MATMUL(MATMUL(TRANSPOSE(BB2),mat),BB2) !-- 312 => 132

    mat  = MATMUL(MATMUL(TRANSPOSE(BB1),tmp6x6),BB1) !-- 132 => 123
ELSE IF((mat(1,1)<mat(2,2)) .AND. (mat(1,1)<mat(3,3)) .AND. (mat(2,2)<mat(3,3))) THEN
    mat  = MATMUL(MATMUL(TRANSPOSE(BB2),mat),BB2) !-- 321 => 123
END IF

END SUBROUTINE tilt_tensor

END MODULE opt_stiffness


c -----------------------------------------------------------------
c       This is the spectral code for kolmogorov flow
c       using the message-passing in Origin 2000 machines
c       isg, oct 26 98 REV aug 99
c       nx = ny = nz = 2**n   ( n = 4,5,...)
c       nx = x-diretion mesh size in each node
c       ny = y-diretion mesh size in each node
c       nz = z-direction mesh size in each node
c       nproc = number of process (nodes)
c       lx=nx,ly=ny,lz=nz/nproc
c -----------------------------------------------------------------

       program kolflow

       include 'include.h'

       complex,allocatable,dimension(:,:,:)::vx,vy,vz
       complex,allocatable,dimension(:,:,:)::wx,wy,wz
       complex,allocatable,dimension(:,:,:)::ox,oy,oz
       real,allocatable,dimension(:,:,:)::kx,ky,kz,k2
       real,allocatable,dimension(:,:,:)::tmp,tmp1,k2_e
       integer,allocatable,dimension(:)::iseed
       complex, allocatable, dimension(:)::coeffy
       real, allocatable, dimension(:)::coeffxz
       complex ff
       real ffr

c  setup MPP environment

       call mpi_init(ierror)
       call mpi_comm_rank(mpi_comm_world,id,ierror)
       call mpi_comm_size(mpi_comm_world,nproc,ierror)
       nallgrp = mpi_comm_world

c   read parameters from node id = 0

       ak0 = 4.75683

       if(id.eq.0)then
         open(90,file='parameter.d',status='unknown')
         write(*,*)'enter nx,ny,nz'
         read(90,*)nx,ny,nz
         write(*,*)'enter number of processors'
         read(90,*)nprocs
         write(*,*) 'enter seed (321)'
         read(90,*) iseed_m0
         write(*,*)'enter nstep'
         read(90,*) nstep
         write(*,*)'enter tout'
         read(90,*) itout
         write(*,*)'enter eout'
         read(90,*) ieout
         write(*,*)'enter dt'
         read(90,*) dt
         write(*,*)'enter rnu'
         read(90,*) rnu
         write(*,*)'enter u0'
         read(90,*) u0
         write(*,*)'enter time'
         read(90,*) time
         write(*,*)'enter new'
         read(90,*) new
         write(*,*)'enter idp for rerun'
         read(90,*) idp
         write(*,*)'enter force'
         read(90,*) ffr
         write(*,*)'enter kolmogorov forcing mode'
         read(90,*) is
         write(*,*)nx,ny,nz,nprocs,iseed_m0,nstep,itout,ieout,dt,rnu,
     1     u0,time,new,idp,ffr,is
       endif

       call mpi_bcast(nx,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(ny,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(nz,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(nprocs,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(iseed_m0,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(nstep,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(itout,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(ieout,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(dt,1,MPI_REAL,0,nallgrp,ierror)
       call mpi_bcast(rnu,1,MPI_REAL,0,nallgrp,ierror)
       call mpi_bcast(u0,1,MPI_REAL,0,nallgrp,ierror)
       call mpi_bcast(time,1,MPI_REAL,0,nallgrp,ierror)
       call mpi_bcast(new,1,MPI_LOGICAL,0,nallgrp,ierror)
       call mpi_bcast(idp,1,MPI_INTEGER,0,nallgrp,ierror)
       call mpi_bcast(ffr,1,MPI_REAL,0,nallgrp,ierror)
       call mpi_bcast(is,1,MPI_INTEGER,0,nallgrp,ierror)

       lx = nx
       ly = ny
       lz = nz/nproc
       lx1=lx+1
       lx2= lx*2
       lly=ly/nproc
       pi=4.*atan(1.)
       scale = 1.0/(nx*ny*nz)

c allocate memory

       allocate( vx(lx1,ly,lz) )
       allocate( vy(lx1,ly,lz) )
       allocate( vz(lx1,ly,lz) )
       allocate( wx(lx1,ly,lz) )
       allocate( wy(lx1,ly,lz) )
       allocate( wz(lx1,ly,lz) )
       allocate( ox(lx1,ly,lz) )
       allocate( oy(lx1,ly,lz) )
       allocate( oz(lx1,ly,lz) )
       allocate( kx(lx1,ly,lz) )
       allocate( ky(lx1,ly,lz) )
       allocate( kz(lx1,ly,lz) )
       allocate( k2(lx1,ly,lz) )
       allocate( k2_e(lx1,ly,lz) )
       allocate( tmp(lx1,ly,lz) )
       allocate( tmp1(lx1,ly,lz) )
       allocate( iseed(nproc) )
       allocate( coeffy( ly+15 ) )
       allocate( coeffxz( (lx2+15)+2*(nz+15) ) )

       if(id.eq.0)print*,' memory allocated'

       ff = cmplx(ffr,0.0)
       iii = 1
       jjj = 1
       kkk = 1

       eout = ieout * dt
       tout = itout * dt
       nek=0.9*0.5*sqrt(4.0*nx*nx+ny*ny+nz*nz)

       if(id.eq.0)then
         write(*,*)ieout,itout,nek
         open(20,file='spectrum.d',status='unknown')
         open(22,file='transfer.d',status='unknown')
         open(70,file='initial.sp',status='unknown')
       end if

c---Generate initial condition -------

       if(new) then

c---Generate random number seed for each process ---

         if(id.eq.0)then
           call srand(iseed_m0)
           do i=1,nproc
             iseed(i) = irand()
           end do
         end if
         call mpi_bcast(iseed,nproc,MPI_INTEGER,0,nallgrp,ierror)
         irr = iseed(id+1)
         call srand(irr)

         call gaussian(vx,pi)
         call gaussian(vy,pi)
         call gaussian(vz,pi)

         call wavenumber(kx,ky,kz,k2,k2_e,dt,rnu,id)

         tmp = (kx*real(vx) + ky*real(vy) + kz*real(vz))/k2
         vx = cmplx(real(vx) - kx*tmp, aimag(vx))
         vy = cmplx(real(vy) - ky*tmp, aimag(vy))
         vz = cmplx(real(vz) - kz*tmp, aimag(vz))
         tmp = (kx*aimag(vx) + ky*aimag(vy) + kz*aimag(vz))/k2
         vx = cmplx(real(vx), aimag(vx) - kx*tmp)
         vy = cmplx(real(vy), aimag(vy) - ky*tmp)
         vz = cmplx(real(vz), aimag(vz) - kz*tmp)

         cc1 = u0 * sqrt(8.*sqrt(2./pi)/(3.*pi*ak0**5))
         tmp = cc1 * sqrt(k2) * exp (-k2/(ak0*ak0))

         vx = vx * tmp
         vy = vy * tmp
         vz = vz * tmp

         call symmetrize(vx,id)
         call symmetrize(vy,id)
         call symmetrize(vz,id)

          call dealiasing(vx,k2)
          call dealiasing(vy,k2)
          call dealiasing(vz,k2)

c     calculating initial spectrum

         tmp = vx*conjg(vx) + vy*conjg(vy) + vz*conjg(vz)
         tmp(1,:,:) = 0.5*tmp(1,:,:)
         do i=1,nek
           ek = 0.0
           e_t = 0.0
           ek=sum(tmp(1:lx,:,:),mask=(abs(sqrt(k2)-i).lt.0.5))
           call mpi_reduce(ek,e_t,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
           if(id.eq.0)then
             write(70,*)i,e_t
           end if
         end do
         if(id.eq.0)close(70)
          if (id.eq.0)write(*,*)'initial spec done'
       else

c ------ Rreading from DISK ------

        call input(vx,vy,vz,idp,id,nallgrp)
cc         read(40+id)(((vx(i,j,k),i=1,lx1),j=1,ly),k=1,lz)
cc         read(40+id)(((vy(i,j,k),i=1,lx1),j=1,ly),k=1,lz)
cc         read(40+id)(((vz(i,j,k),i=1,lx1),j=1,ly),k=1,lz)
         call symmetrize(vx,id)
         call symmetrize(vy,id)
         call symmetrize(vz,id)

c        write(*,*)id,sum(vx*conjg(vx)),
c    1     sum(vy*conjg(vy)),sum(vz*conjg(vz))
c       if(id.eq.0)then
C for testing
c       write(*,*)(vx(i,5,2),i=1,lx1)
c       write(*,*)(vy(i,5,2),i=1,lx1)
c       write(*,*)(vz(i,5,2),i=1,lx1)
c       endif

         if(id.eq.0)then
           write(*,*)'after reading'
         end if
c
      endif

      call wavenumber(kx,ky,kz,k2,k2_e,dt,rnu,id)

      call cfft1di(ly,coeffy)
      call scfft2dui(lx2,nz,coeffxz)

      ox=vx
      oy=vy
      oz=vz

      dt_h=.5*dt
      istep=-1

      if (id.eq.0) write(*,*)'begin main loop'
1     istep = istep + 1

c   write out the total energy in K space

       time1 = mpi_wtime()
       tmp = vx*conjg(vx) + vy*conjg(vy) + vz*conjg(vz)
       tmp(1,:,:)=0.5*tmp(1,:,:)
       ek = sum(tmp)
       call mpi_reduce(ek,e_t,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
       call mpi_bcast(e_t,1,MPI_REAL,0,nallgrp,ierror)
	if(id.eq.0)then
         write(*,*)istep,time,e_t
         write(76,*)time,e_t
       end if
       if (e_t.gt.100.)   then
         write(*,*)'id',id, 'et way up',e_t
          call MPI_FINALIZE(ierror)
       endif
       if(mod(istep,itout).eq.0.and.istep.ne.0) then
         call output(vx,vy,vz,iii,id,nallgrp)
         iii = iii + 1
       endif

       wx = (0.,1.) * (ky*vz - kz*vy)
       wy = (0.,1.) * (kz*vx - kx*vz)
       wz = (0.,1.) * (kx*vy - ky*vx)

c     energy spectrum calculation

      if(mod(istep,ieout).eq.0) then
        tmp = vx*conjg(vx) + vy*conjg(vy) + vz*conjg(vz)
        tmp(1,:,:)=0.5*tmp(1,:,:)
        if(id.eq.0)then
          write(20,*)(kkk-1)
        end if
        do i=1,nek
          ek = 0.0
          e_t = 0.0
          ek=sum(tmp,mask=(abs(sqrt(k2)-i).lt.0.5))
          call mpi_reduce(ek,e_t,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
          if(id.eq.0)then
            write(20,*)i,e_t
          end if
        end do
        kkk = kkk + 1
      end if

      kx = real(vx)
      ky = aimag(vx)
      kz = real(vy)
      k2 = aimag(vy)
      k2_e = real(vz)
      tmp1 = aimag(vz)
c
      isign = 1
c
      call mpifft(vx,isign,coeffy,coeffxz,id,nallgrp)
      call mpifft(vy,isign,coeffy,coeffxz,id,nallgrp)
      call mpifft(vz,isign,coeffy,coeffxz,id,nallgrp)

      call mpifft(wx,isign,coeffy,coeffxz,id,nallgrp)
      call mpifft(wy,isign,coeffy,coeffxz,id,nallgrp)
      call mpifft(wz,isign,coeffy,coeffxz,id,nallgrp)

c  ---Check energy and vorticity in physical space -----

c      t_ex = 0.
c      t_ey = 0.
c      t_ez = 0.

c      t_wx = 0.
c      t_wy = 0.
c      t_wz = 0.

c      t_ex=sum(vx(1:lx,:,:)*vx(1:lx,:,:))
c      t_ey=sum(vy(1:lx,:,:)*vy(1:lx,:,:))
c      t_ez=sum(vz(1:lx,:,:)*vz(1:lx,:,:))

c      t_wx=sum(wx(1:lx,:,:)*wx(1:lx,:,:))
c      t_wy=sum(wy(1:lx,:,:)*wy(1:lx,:,:))
c      t_wz=sum(wz(1:lx,:,:)*wz(1:lx,:,:))

c      call mpi_reduce(t_ex,to_ex,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
c      call mpi_reduce(t_ey,to_ey,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
c      call mpi_reduce(t_ez,to_ez,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)

c      call mp_reduce(t_wx,to_wx,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
c      call mp_reduce(t_wy,to_wy,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
c      call mp_reduce(t_wz,to_wz,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)

c      if(id.eq.0)then
c        to_ex = to_ex*scale
c        to_ey = to_ey*scale
c        to_ez = to_ez*scale
c
c        to_wx = to_wx*scale
c        to_wy = to_wy*scale
c        to_wz = to_wz*scale
c
c        to_energy = to_ex + to_ey + to_ez
c        to_omega  = to_wx + to_wy + to_wz
c
c        write(11,*)time,to_ex,to_ey,to_ez,0.5*to_energy
c        write(11,*)time,to_wx,to_wy,to_wz,0.5*to_omega
c      end if

c      average over x,z & t
c
c      do i=1,ly
c        avx(i) = avx(i) + sum(vx(:,i,:))
c        avv(i) = avv(i) + sum(vy(:,i,:))
c        avw(i) = avw(i) + sum(vz(:,i,:))
c      enddo
       tmp = real(vy)*real(wz) - real(vz)*real(wy)
       wz = cmplx(real(vz)*real(wx) - real(vx)*real(wz), aimag(wz))
       wx = cmplx(real(vx)*real(wy) - real(vy)*real(wx), aimag(wx))
       wy = cmplx(tmp, aimag(wy))
       tmp = aimag(vy)*aimag(wz) - aimag(vz)*aimag(wy)
       wz = cmplx(real(wz), aimag(vz)*aimag(wx) - aimag(vx)*aimag(wz))
       wx = cmplx(real(wx), aimag(vx)*aimag(wy) - aimag(vy)*aimag(wx))
       wy = cmplx(real(wy), tmp)

       vx = cmplx(kx,ky)
       vy = cmplx(kz,k2)
       vz = cmplx(k2_e,tmp1)

       call wavenumber(kx,ky,kz,k2,k2_e,dt,rnu,id)

       isign = -1

       call mpifft(wx,isign,coeffy,coeffxz,id,nallgrp)
       call mpifft(wy,isign,coeffy,coeffxz,id,nallgrp)
       call mpifft(wz,isign,coeffy,coeffxz,id,nallgrp)

       call symmetrize(wx,id)
       call symmetrize(wy,id)
       call symmetrize(wz,id)

       if (id.eq.0) then
         wy(1,is+1,1) = wy(1,is+1,1) + ff     ! kolmogorov force now
         wy(1,ly-is+1,1) = wy(1,ly-is+1,1) + ff  ! fx=cos(is*y)
       endif
       call dealiasing(vx,k2)
       call dealiasing(vy,k2)
       call dealiasing(vz,k2)

       tmp = (kx*real(wy) + ky*real(wz) + kz*real(wx))/k2
       wy = cmplx(real(wy) - kx*tmp, aimag(wy))
       wz = cmplx(real(wz) - ky*tmp, aimag(wz))
       wx = cmplx(real(wx) - kz*tmp, aimag(wx))
       tmp = (kx*aimag(wy) + ky*aimag(wz) + kz*aimag(wx))/k2
       wy = cmplx(real(wy), aimag(wy) - kx*tmp)
       wz = cmplx(real(wz), aimag(wz) - ky*tmp)
       wx = cmplx(real(wx), aimag(wx) - kz*tmp)

       if(mod(istep,ieout).eq.0) then
         tmp = real(vx)*real(wy) + real(vy)*real(wz) +
     &        real(vz)*real(wx) +
     &        aimag(vx)*aimag(wy) + aimag(vy)*aimag(wz) +
     &        aimag(vz)*aimag(wx)
         tmp(1,:,:) = 0.5*tmp(1,:,:)
         if(id.eq.0)then
           write(22,*)(jjj-1)
         end if

         do i=1,nek
           ek = 0.
           e_t = 0.
           ek=sum(tmp(1:lx,:,:),mask=(abs(sqrt(k2)-i).lt.0.5))
           call mpi_reduce(ek,e_t,1,MPI_REAL,MPI_SUM,0,nallgrp,ierror)
           if(id.eq.0)then
             write(22,*) i,ek
           end if
         end do
         jjj = jjj + 1
       endif

      if(istep.eq.0) then
        vx = vx * (1.-rnu*dt_h*k2) + dt_h*wy
        vy = vy * (1.-rnu*dt_h*k2) + dt_h*wz
        vz = vz * (1.-rnu*dt_h*k2) + dt_h*wx

        time2=mpi_wtime()
        if(id.eq.0) then
          write(*,fmt='(5X,A,F20.4,A)')
     &   'elapsed time =',1.0D3*(time2-time1),' msec'
        end if
        call output(wx,wy,wz,0,id,nallgrp)
        time = time + dt_h
      elseif(istep.eq.1) then
        vx = ox + dt*wy - rnu*dt*k2*vx
        vy = oy + dt*wz - rnu*dt*k2*vy
        vz = oz + dt*wx - rnu*dt*k2*vz

        time2=mpi_wtime()
        if(id.eq.0) then
          write(*,fmt='(5X,A,F20.4,A)')
     &   'elapsed time =',1.0D3*(time2-time1),' msec'
        end if
        call input(ox,oy,oz,0,id,nallgrp)
        time = time + dt_h
      else
         vx = vx + dt_h*(3.*wy - k2_e*ox)
         vy = vy + dt_h*(3.*wz - k2_e*oy)
         vz = vz + dt_h*(3.*wx - k2_e*oz)
         vx = vx*k2_e
         vy = vy*k2_e
         vz = vz*k2_e
         ox = wy
         oy = wz
         oz = wx

         time = time + dt
         time2 = MPI_wtime()

        if(id.eq.0) then
          write(*,fmt='(5X,A,F20.4,A)')
     &   'elapsed time =',1.0D3*(time2-time1),' msec'
        end if

      endif

      if(istep.lt.nstep) goto 1
      write(*,*)id,'finished '

      if(id.eq.0)then
        close(11)
        close(20)
        close(22)
      end if

      stop

100   format(' step',i4,':  t = ',e14.6,' E = ',e14.6,' W = ',e14.6)
200   format(8e13.5)
102   format(2(1x,E16.7))

      call MPI_FINALIZE(ierror)
      stop
      end

c --------------------------------------------------------------
       subroutine wavenumber(kx,ky,kz,k2,k2_e,dt,rnu,id)

       include 'include.h'

       real,dimension(lx1,ly,lz)::kx,ky,kz,k2,k2_e

       do i=1,lx1
         kx(i,:,:)= i-1
       end do

       do j=1,ly
         ky(:,j,:) = mod(j-1+ly/2,ly)-ly/2
       end do

       do k=1,lz
         k1=id*lz+k
         kz(:,:,k) = mod(k1-1+nz/2,nz)-nz/2
       end do

       k2=kx*kx+ky*ky+kz*kz
       k2_e = exp(-k2*dt*rnu)

       if(id.eq.0)then
         k2(1,1,1)=1.e-5
         k2_e(1,1,1) = 1.0
       end if

       return
       end

c --------------------------------------------------------------------
      subroutine gaussian(u,pi)

      include 'include.h'

      complex,dimension(lx1,ly,lz) :: u

      u = 0.0

      do i=1,lx1
       do j=1,ly
        do k=1,lz
          t1 = rand()
          t2 = rand()
          if(t1.le.1.e-10) t1 = 1.e-10
          if(t2.le.1.e-10) t2 = 1.e-10
          t2 = 2*pi*t2
          u(i,j,k)=sqrt(-2.0*log(t1))*cmplx(cos(t2),sin(t2))
        end do
       end do
      end do

      return
      end

c ---------------------------------------------------------------------
      subroutine symmetrize(c,id)

      include 'include.h'

      complex,dimension(lx1,ly,lz)::c
      complex,dimension(ly,lz)::cc

c   zero k2=ny/2 modes
      c(:,ly/2+1,:) = (0.0,0.0)
c   zero k3=nz/2 modes
      nid=(nz/2+1)/lz
      nip=nz/2+1-nid*lz
      if(id.eq.nid) c(:,:,nip) = (0.0,0.0)
c   zero k1=nx modes
      c(lx1,:,:)=(0.0,0.0)
c  zero k1=k2=k3=0 mode
      if(id.eq.0) c(1,1,1) = (0.0,0.0)

      cc=c(1,:,:)

c for k1=0 plane, zero all modes with k2<0

c     do j = ly/2+2,ly
c       c(1,j,:) = (0.0,0.0)
c     end do

c for the line k1=0 and k2=0,  zero all modes with k3<0
c     do k = 1,lz
c       k1=k+id*lz
c       if(k1.gt.nz/2.and.k1.le.nz) c(1,1,k)=(0.0,0.0)
c     end do

      return
      end

c ----------------------------------------------------------------------
c   2/3 rule
c ----------------------------------------------------------------------
      subroutine dealiasing(vx,k2)

      include 'include.h'

      complex,dimension(lx1,ly,lz)::vx
      real,dimension(lx1,ly,lz)::k2
      double precision ktr

      ktr = nz/3.0

      where(sqrt(k2).ge.ktr)vx = (0.0,0.0)

      return
      end

c ----------------------------------------------------------------------
      subroutine output(ux,uy,uz,idump,id,nallgrp)

      include "include.h"

      complex,dimension(lx1,ly,lz)::ux,uy,uz
      integer i1d,i2d,i3d,i4d,i5d

      character*60 fnm1

      i1d=int(id/100)
      i2d=int((id-i1d*100)/10)
      i3d=id-i1d*100-i2d*10

      if(idump.lt.10) then
        fnm1='/scratch.o3.1/eneko/kol256/vel'//char(idump+48)//'.'
     .        //char(i1d+48)//char(i2d+48)//char(i3d+48)
      else
        i4d=int(idump/10)
        i5d=mod(idump,10)
        fnm1='/scratch.o3.1/eneko/kol256/vel'//char(i4d+48)
     .        //char(i5d+48)//'.'//char(i1d+48)
     .        //char(i2d+48)//char(i3d+48)
      endif

      open(10,file=fnm1,status='unknown',form='unformatted')
      write(10)ux
      write(10)uy
      write(10)uz
      close(10)

      return
      end

c ---------------------------------------------------------------------
      subroutine input(ux,uy,uz,idp,id,nallgrp)

      include "include.h"

      complex,dimension(lx1,ly,lz)::ux,uy,uz
      integer i1d,i2d,i3d,i4d,i5d

      character*60 fnm1

      i1d=int(id/100)
      i2d=int((id-i1d*100)/10)
      i3d=id-i1d*100-i2d*10
      if(idp.lt.10) then
        fnm1='/scratch.o3.1/eneko/kol256/vel'//char(idp+48)//'.'
     .        //char(i1d+48)//char(i2d+48)//char(i3d+48)
      else
        i4d=int(idp/10)
        i5d=mod(idp,10)
        fnm1='/scratch.o3.1/eneko/kol256/vel'//char(i4d+48)
     .        //char(i5d+48)//'.'//char(i1d+48)
     .        //char(i2d+48)//char(i3d+48)
      endif

      open(10,file=fnm1,status='unknown',form='unformatted')
      read(10)ux
      read(10)uy
      read(10)uz
      close(10)

      return
      end

c -------------------------------------------------------------------
        subroutine transpose(ux,id,nallgrp)

        include 'include.h'

        complex,dimension(lx+1,ly,lz)::ux
        complex,dimension(lx+1,ly/nproc,lz):: tmp1,tmp2
        complex,dimension(lx+1,ly/nproc,lz*nproc)::tmp
        integer isize,nzm,nzp,status(MPI_STATUS_SIZE,2),req(2)

        isize=(lx+1)*ly*lz/nproc
        lly=ly/nproc
        do i=1,nproc-1
          nzp=mod(id+i,nproc)
          nzm=mod(id-i+nproc,nproc)
          js=nzp*lly
          do j=1,lly
            j1=js+j
            tmp1(:,j,:)=ux(:,j1,:)
          end do
          call mpi_isend(tmp1,isize,MPI_COMPLEX,nzp,i,nallgrp,req(1),
     .                   ierror)
          call mpi_irecv(tmp2,isize,MPI_COMPLEX,nzm,i,
     .               nallgrp,req(2),ierror)
          call MPi_Waitall(2,req,status,ierror)
          ks=nzm*lz
          do k=1,lz
            k1=ks+k
            tmp(:,:,k1)=tmp2(:,:,k)
          end do
        end do

         ks=id*lz
         js=id*lly
        do k=1,lz
          k1=ks+k
          do j=1,ly/nproc
            j1=js+j
            tmp(:,j,k1)=ux(:,j1,k)
          end do
        end do

        do k=1,lz
          do j=1,ly
             ux(:,j,k)=tmp(:,k,j)
           end do
        end do

        return
        end

c ------------------------------------------------------------------------
       subroutine mpifft(ux,isign,coeffz,coeffxy,id,nallgrp)

       include "include.h"

       complex,dimension(lx+1,ly,lz)::ux
       complex,dimension(lx+1,ly/nproc,lz):: tmp1,tmp2
       complex,dimension(lx+1,ly/nproc,lz*nproc)::tmp
       complex, dimension(nz+15)::coeffz
       real, dimension((lx2+15)+2*(ly+15))::coeffxy
       real alpha1,alpha2

       if(isign.eq.-1) then

C      Step 1, 2D fft, real-to-complex in x, complex-to-complex in y

        alpha2=1.0/lx2/ly
c       time1=mpi_wtime()
        do k=1,lz
          call scfft2du(isign,lx2,ly,ux(:,:,k),lx2+2,coeffxy)
        end do
        ux=ux*alpha2
c       time=mpi_wtime()-time1
c       if(id.eq.0)write(*,*)'time for 2-d fft',time

c      Step 2, Transpose between z and y

c       time2=mpi_wtime()
        call transpose(ux,id,nallgrp)
c       time=mpi_wtime()-time2
c       if(id.eq.0)write(*,*)'time for transpose',time

C      Step 3, 1d complex-to-complex in z

c       time3=mpi_wtime()
        do j=1,ly/nproc
          do i=1,lx
            call cfft1d(isign,nz,ux(i,:,j),1,coeffz)
          end do
        end do
        ux=ux/nz
c       time=mpi_wtime()-time3
c       if(id.eq.0)write(*,*)'time for 1-d fft',time

       else if(isign.eq.1)then

C      Step 1, 1d complex-to-complex in z

c       time1=mpi_wtime()
        do j=1,ly/nproc
        do i=1,lx
            call cfft1d(isign,nz,ux(i,:,j),1,coeffz)
c           ux(i,:,j)=ux(i,:,j)/nz
          end do
        end do
c       time=mpi_wtime()-time1
c       if(id.eq.0)write(*,*)'time for 1-d fft',time

c      Step 2, Reverse transpose between z and y

c       time2=mpi_wtime()
        call transpose(ux,id,nallgrp)
c       time=mpi_wtime()-time2
c       if(id.eq.0)write(*,*)'time for transpose',time

C      Step 3, 2D fft, complex-to-complex in y, complex-to-real in x

        alpha2=1.0/lx2/ly
c       time3=mpi_wtime()
        do k=1,lz
          call csfft2du(isign,lx2,ly,ux(:,:,k),lx2+2,coeffxy)
c         ux(:,:,k)=ux(:,:,k)*alpha2
c         call sscal2d(lx,ly,alpha2,ux(:,:,k),lx+2)
        end do
c       time=mpi_wtime()-time3
c       if(id.eq.0)write(*,*)'time for 2-d fft',time

       end if

       return
       end


      function RS1D(tau, taup, term, j_chi, coupling_Array)
      implicit none
      double precision :: RS1D, j_chi
      double precision :: coupling_Array(14,2)
      integer :: tau,taup
      integer :: term
      integer :: c
      double precision :: c5, c4p
      double precision :: c8, c9p
c
      c = 0
c
      c5 = coupling_Array(4,tau)
      c4p = coupling_Array(3,taup)
      c8 = coupling_Array(7,tau)
      c9p = coupling_Array(8,taup)
c
      if (term.eq.c) then
         RS1D = (j_chi*(j_chi+1))/3. * (c5*c4p - c8*c9p)
      else
         RS1D = 0.
      end if
c
      end function RS1D